// Rotator_V2_ESP32S3_Board – YAESU GS-232 / WiFi + USB

#include <WiFi.h>
#include <ESPping.h>
#include <esp_task_wdt.h>
#include <esp_wifi.h>
#include <esp_adc/adc_cali.h>
#include <esp_adc/adc_cali_scheme.h>
#include <esp_adc/adc_oneshot.h>
#include <sys/socket.h>
#include <netinet/tcp.h>

// =============================================================================
// USER CONFIG
// =============================================================================

#define WIFI_SSID        "YourWiFiSSID"
#define WIFI_PASSWORD    "YourWiFiPassword"

IPAddress WIFI_IP(192, 168, 1, 186);
IPAddress WIFI_GATEWAY(192, 168, 1, 1);
IPAddress WIFI_SUBNET(255, 255, 255, 0);
IPAddress WIFI_DNS(192, 168, 1, 1);

#define WIFI_TCP_PORT           2823
#define ETHERNET_MESSAGE_TIMEOUT_MS 5000UL
#define COMMAND_BUFFER_SIZE     50

#define USB_COMMAND_BUFFER_SIZE 50
#define USB_MESSAGE_TIMEOUT_MS  1000UL

#define PIN_ROTATE_CW        3
#define PIN_ROTATE_CCW      14
#define PIN_ADC_AZ           2
// ESP32-S3 ADC1 mapovanie: GPIO1=CH0, GPIO2=CH1, GPIO3=CH2, GPIO4=CH3, GPIO5=CH4, ...
// PIN_ADC_AZ = 2 → ADC1_CH1
#define ADC_AZ_CHANNEL        ADC_CHANNEL_1
#define PIN_LED_LOCKOUT      8

#define ROTATION_RANGE_DEG  450

// Calibration: measure the actual voltage (mV) at the ADC input at 0° and at 360°
// and enter the values directly here. The conversion to degrees is linear (mv_to_deg()).
#define ADC_MV_0DEG          10 //mV at 0
#define ADC_MV_360DEG      2200 //mV at 360
#define ADC_AVG_SAMPLES     10

// =============================================================================
// CONSTANTS
// =============================================================================

#define AZ_MEAS_INTERVAL_MS      100
#define AZ_TOLERANCE             1.5f
#define MOTOR_STARTUP_GRACE_MS   1500UL
#define MOTION_EPS_DEG           0.7f
#define STALL_TIMEOUT_MS         4000UL
#define T1_MAX_DIR_MS          180000UL
#define T2_DIR_CHANGE_DELAY_MS   2000UL
#define T3_TOTAL_CYCLE_MS      225000UL
#define T3_IDLE_RESET_MS        15000UL
#define MOTOR_COOLDOWN_MS       15000UL
#define FAULT_WINDOW_MS         70000UL
#define FAULTS_FOR_EXT_COOLDOWN     3
#define EXTENDED_COOLDOWN_MS   300000UL
#define FAULT_DEBOUNCE_MS         500UL
#define RELAY_SAFE_DELAY_US       100
#define REQUEST_DEBOUNCE_MS       100UL
#define ERR_BLINK_MS              100UL
#define SETTLING_TIME_MS          250UL
#define WDT_TIMEOUT_SEC             8

#define WIFI_CHECK_MS            2000UL
#define WIFI_RECONNECT_DELAY_MS  5000UL
#define WIFI_RECONNECT_TIMEOUT_MS 15000UL
#define WIFI_CLIENT_IDLE_MS    300000UL
#define WIFI_STARTUP_DELAY_MS   10000UL
#define WIFI_PING_INTERVAL_MS   25000UL
#define WIFI_HEARTBEAT_MS       30000UL

// =============================================================================
// ENUMS
// =============================================================================

enum AzState : byte { IDLE=0, INIT_CW, RUN_CW, INIT_CCW, RUN_CCW, WAIT_DIR_CHANGE };
enum ReqQState : byte { REQ_NONE=0, REQ_IN_QUEUE, REQ_IN_PROGRESS };
enum ReqType : byte { REQUEST_STOP=0, REQUEST_AZIMUTH, REQUEST_RESET_LOCKOUT };

// =============================================================================
// STATE
// =============================================================================

static float            az_raw = 0.0f;
static float            az_deg = 0.0f;
static float            az_target = 0.0f;
static AzState          az_state = IDLE;
static AzState          pending_state = IDLE;
static ReqQState        az_req_state = REQ_NONE;
static byte             az_req = REQUEST_STOP;
static float            az_req_parm = 0.0f;
static bool             az_initialized = false;
static unsigned long    settling_start = 0;
static bool             wifi_ready = false;
static volatile bool    tcp_drop_client = false;
static volatile unsigned long last_tcp_activity = 0;
static volatile bool    wifi_event_disc = false;

static unsigned long    motor_start_ms = 0;
static unsigned long    last_progress_ms = 0;
static float            last_progress_raw = 0.0f;
static byte             progress_hits = 0;
static unsigned long    t1_dir_start = 0;
static unsigned long    t3_cycle_start = 0;
static unsigned long    t3_last_idle = 0;
static unsigned long    dir_change_until = 0;
static unsigned long    cooldown_until = 0;
static unsigned long    last_req_time = 0;

static unsigned long    fault_times[FAULTS_FOR_EXT_COOLDOWN] = {0,0,0};
static byte             fault_wr = 0;
static bool             hard_lockout = false;

WiFiServer server(WIFI_TCP_PORT);

static adc_oneshot_unit_handle_t adc_handle   = nullptr;
static adc_cali_handle_t         adc_cali_hdl = nullptr;
static bool                      adc_cali_ok  = false;

// =============================================================================
// ADC
// =============================================================================

static void adc_init() {
  adc_oneshot_unit_init_cfg_t ucfg = {};
  ucfg.unit_id  = ADC_UNIT_1;
  ucfg.ulp_mode = ADC_ULP_MODE_DISABLE;
  if (adc_oneshot_new_unit(&ucfg, &adc_handle) != ESP_OK) return;

  adc_oneshot_chan_cfg_t ccfg = {};
  ccfg.atten    = ADC_ATTEN_DB_12;
  ccfg.bitwidth = ADC_BITWIDTH_12;
  if (adc_oneshot_config_channel(adc_handle, ADC_AZ_CHANNEL, &ccfg) != ESP_OK) return;

  adc_cali_curve_fitting_config_t cali = {};
  cali.unit_id  = ADC_UNIT_1;
  cali.atten    = ADC_ATTEN_DB_12;
  cali.bitwidth = ADC_BITWIDTH_12;
  adc_cali_ok = (adc_cali_create_scheme_curve_fitting(&cali, &adc_cali_hdl) == ESP_OK);
}

static int adc_read_mv() {
  if (!adc_handle) return 0;
  int dummy = 0;
  for (byte i = 0; i < 3; i++) { adc_oneshot_read(adc_handle, ADC_AZ_CHANNEL, &dummy); delayMicroseconds(500); }
  long sum = 0;
  for (byte i = 0; i < ADC_AVG_SAMPLES; i++) {
    int raw = 0;
    adc_oneshot_read(adc_handle, ADC_AZ_CHANNEL, &raw);
    if (adc_cali_ok) { int mv = 0; adc_cali_raw_to_voltage(adc_cali_hdl, raw, &mv); sum += mv; }
    else             { sum += (long)raw * 3300 / 4095; }
    delayMicroseconds(500);
  }
  return (int)(sum / ADC_AVG_SAMPLES);
}

static float mv_to_deg(int mv) {
  constexpr float span = (float)(ADC_MV_360DEG - ADC_MV_0DEG);
  float deg = ((float)(mv - ADC_MV_0DEG) / span) * 360.0f;
  if (deg < 0.0f) deg = 0.0f;
  if (deg > ROTATION_RANGE_DEG) deg = ROTATION_RANGE_DEG;
  return deg;
}

// =============================================================================
// HELPERS
// =============================================================================

static float clamp_0_359(float a) {
  float r = fmodf(a, 360.0f);
  if (r < 0.0f) r += 360.0f;
  return r;
}

static float best_target(float target, float current) {
  float best = target, best_dist = fabsf(current - target);
  float cand = target + 360.0f;
  if (cand <= ROTATION_RANGE_DEG) {
    float d = fabsf(current - cand);
    if (d < best_dist) best = cand;
  }
  return best;
}

// =============================================================================
// MOTOR
// =============================================================================

static void motor_off() {
  digitalWrite(PIN_ROTATE_CW,  LOW);
  digitalWrite(PIN_ROTATE_CCW, LOW);
}

static void motor_on(bool cw) {
  motor_off();
  delayMicroseconds(RELAY_SAFE_DELAY_US);
  digitalWrite(cw ? PIN_ROTATE_CW : PIN_ROTATE_CCW, HIGH);
}

static void motor_fault() {
  unsigned long now = millis();
  motor_off();
  az_state = IDLE; pending_state = IDLE; az_req_state = REQ_NONE; dir_change_until = 0;

  if ((now - motor_start_ms) < FAULT_DEBOUNCE_MS) {
    cooldown_until = now + MOTOR_COOLDOWN_MS;
    t3_last_idle = now;
    return;
  }

  fault_times[fault_wr] = now;
  fault_wr = (fault_wr + 1) % FAULTS_FOR_EXT_COOLDOWN;

  byte recent = 0;
  for (byte i = 0; i < FAULTS_FOR_EXT_COOLDOWN; i++)
    if (fault_times[i] && (now - fault_times[i]) <= FAULT_WINDOW_MS) recent++;

  if (recent >= FAULTS_FOR_EXT_COOLDOWN) {
    cooldown_until = now + EXTENDED_COOLDOWN_MS;
    hard_lockout = true;
    digitalWrite(PIN_LED_LOCKOUT, HIGH);
  } else {
    cooldown_until = now + MOTOR_COOLDOWN_MS;
  }
  t3_last_idle = now;
}

// =============================================================================
// AZIMUTH READ
// =============================================================================

static void driver_read_azimuth(byte force) {
  static unsigned long last = 0;
  static float last_ok = 0.0f;
  unsigned long now = millis();
  if (!force && (now - last) < AZ_MEAS_INTERVAL_MS) return;

  float raw = mv_to_deg(adc_read_mv());
  if (!isnan(raw) && !isinf(raw)) { last_ok = raw; az_initialized = true; }
  az_raw = last_ok;
  az_deg = clamp_0_359(az_raw);
  last = now;
}

// =============================================================================
// REQUEST HANDLING
// =============================================================================

void driver_submit_request(byte r, float p, bool from_wifi = false) {
  unsigned long now = millis();

  if (r == REQUEST_RESET_LOCKOUT) {
    if (hard_lockout) {
      hard_lockout = false;
      digitalWrite(PIN_LED_LOCKOUT, LOW);
      for (byte i = 0; i < FAULTS_FOR_EXT_COOLDOWN; i++) fault_times[i] = 0;
      fault_wr = 0; cooldown_until = 0;
    }
    return;
  }

  if (r != REQUEST_STOP && (now - last_req_time) < REQUEST_DEBOUNCE_MS) return;
  last_req_time = now;

  if (r == REQUEST_STOP) { az_req = r; az_req_state = REQ_IN_QUEUE; settling_start = 0; return; }

  if (!az_initialized || (long)(now - cooldown_until) < 0) return;
  if (from_wifi && !wifi_ready) return;

  az_req = r; az_req_parm = p; az_req_state = REQ_IN_QUEUE; settling_start = 0;
}

static void driver_service_requests() {
  if (az_req_state != REQ_IN_QUEUE) return;
  unsigned long now = millis();

  if (az_req == REQUEST_STOP) {
    motor_off(); az_state = IDLE; pending_state = IDLE;
    az_req_state = REQ_NONE; t3_last_idle = now; settling_start = 0;
    return;
  }

  az_target = best_target(az_req_parm, az_raw);

  if (fabsf(az_raw - az_target) < AZ_TOLERANCE) {
    motor_off(); az_state = IDLE; az_req_state = REQ_NONE; t3_last_idle = now; settling_start = 0;
    return;
  }

  AzState wanted = (az_target > az_raw) ? INIT_CW : INIT_CCW;

  if (az_state == WAIT_DIR_CHANGE) { pending_state = wanted; az_req_state = REQ_IN_PROGRESS; return; }

  if ((az_state == RUN_CW  && wanted == INIT_CCW) ||
      (az_state == RUN_CCW && wanted == INIT_CW)) {
    motor_off(); az_state = WAIT_DIR_CHANGE; pending_state = wanted;
    dir_change_until = now + T2_DIR_CHANGE_DELAY_MS; settling_start = 0;
  } else {
    az_state = wanted;
  }
  az_req_state = REQ_IN_PROGRESS;
}

static void driver_service_rotation() {
  unsigned long now = millis();

  if (az_state == IDLE && t3_cycle_start && (now - t3_last_idle) > T3_IDLE_RESET_MS)
    t3_cycle_start = 0;

  if (az_state == WAIT_DIR_CHANGE) {
    if (t3_cycle_start && (now - t3_cycle_start) > T3_TOTAL_CYCLE_MS) { motor_fault(); return; }
    if ((long)(now - dir_change_until) >= 0) az_state = pending_state;
    return;
  }

  if (az_state == INIT_CW || az_state == INIT_CCW) {
    motor_on(az_state == INIT_CW);
    az_state = (az_state == INIT_CW) ? RUN_CW : RUN_CCW;
    motor_start_ms = last_progress_ms = now;
    last_progress_raw = az_raw; progress_hits = 0;
    t1_dir_start = now; settling_start = 0;
    if (!t3_cycle_start) t3_cycle_start = now;
    return;
  }

  if (az_state == RUN_CW || az_state == RUN_CCW) {
    if ((now - t1_dir_start) > T1_MAX_DIR_MS) { motor_fault(); return; }
    if (t3_cycle_start && (now - t3_cycle_start) > T3_TOTAL_CYCLE_MS) { motor_fault(); return; }

    float delta = (az_state == RUN_CW) ? az_raw - last_progress_raw : last_progress_raw - az_raw;
    if (delta >= MOTION_EPS_DEG) {
      last_progress_raw = az_raw;
      if (progress_hits < 2) progress_hits++;
      if (progress_hits >= 2) last_progress_ms = now;
    }

    if ((now - motor_start_ms) > MOTOR_STARTUP_GRACE_MS &&
        (now - last_progress_ms) > STALL_TIMEOUT_MS) { motor_fault(); return; }

    if (az_req_state == REQ_IN_PROGRESS) {
      if (fabsf(az_raw - az_target) < AZ_TOLERANCE) {
        if (!settling_start) { settling_start = now; }
        else if ((now - settling_start) >= SETTLING_TIME_MS) {
          motor_off(); az_state = IDLE; az_req_state = REQ_NONE; t3_last_idle = now; settling_start = 0;
        }
      } else { settling_start = 0; }
    }
  }
}

void driver_service() {
  driver_read_azimuth(0);
  unsigned long now = millis();
  if (hard_lockout && (long)(now - cooldown_until) >= 0) {
    hard_lockout = false; digitalWrite(PIN_LED_LOCKOUT, LOW);
    for (byte i = 0; i < FAULTS_FOR_EXT_COOLDOWN; i++) fault_times[i] = 0;
    fault_wr = 0;
  }
  driver_service_requests();
  driver_service_rotation();
}

// =============================================================================
// GS-232 PROTOCOL
// =============================================================================

static void gs232_format_C(char *out, size_t sz) {
  int v = (int)lroundf(az_deg);
  if (v >= 360) v = 359;
  snprintf(out, sz, "+0%03d", v);
}

static void gs232_process(Stream &in, Stream &out, byte *buf, int &idx,
                           unsigned long &last_rx, size_t bufsz,
                           unsigned long timeout_ms, bool from_wifi = false) {
  while (in.available()) {
    byte b = (byte)in.read();
    last_rx = millis();
    if (from_wifi) last_tcp_activity = last_rx;
    if (b >= 'a' && b <= 'z') b -= 32;
    if (b != 10 && b != 13 && idx < (int)bufsz) buf[idx++] = b;

    if ((b == 13 || idx >= (int)bufsz) && idx) {
      bool valid = false;
      if (from_wifi) last_tcp_activity = millis();

      if (buf[0] == 'C') {
        valid = true;
        char r[16]; gs232_format_C(r, sizeof(r)); out.println(r);
      } else if (buf[0] == 'S') {
        valid = true;
        driver_submit_request(REQUEST_STOP, 0, from_wifi);
      } else if (buf[0] == 'R') {
        valid = true;
        driver_submit_request(REQUEST_RESET_LOCKOUT, 0, from_wifi);
        out.println("LOCKOUT RESET");
      } else if (buf[0] == 'M' && idx >= 4 &&
                 isdigit(buf[1]) && isdigit(buf[2]) && isdigit(buf[3])) {
        int v = (buf[1]-'0')*100 + (buf[2]-'0')*10 + (buf[3]-'0');
        if (v >= 0 && v <= 450) { valid = true; driver_submit_request(REQUEST_AZIMUTH, (float)v, from_wifi); }
      }
      if (from_wifi && valid) wifi_ready = true;
      idx = 0;
    }
  }
  if (idx && (millis() - last_rx > timeout_ms)) idx = 0;
}

// =============================================================================
// TCP PROTOCOL SERVICE
// =============================================================================

static void protocol_service() {
  static byte buf[COMMAND_BUFFER_SIZE];
  static int idx = 0;
  static unsigned long last_rx = 0;
  static unsigned long last_heartbeat = 0;
  static WiFiClient c;

  if (tcp_drop_client) {
    tcp_drop_client = false;
    if (c) { c.stop(); idx = 0; last_rx = 0; last_heartbeat = 0; }
  }

  if (!c || !c.connected()) {
    if (c) { c.stop(); idx = 0; last_rx = 0; last_heartbeat = 0; }
    c = server.available();
    if (!c) return;

    int fd = c.fd(), ka=1, ki=30, kp=10, kc=3, nd=1;
    setsockopt(fd, SOL_SOCKET,  SO_KEEPALIVE,  &ka, sizeof(int));
    setsockopt(fd, IPPROTO_TCP, TCP_KEEPIDLE,  &ki, sizeof(int));
    setsockopt(fd, IPPROTO_TCP, TCP_KEEPINTVL, &kp, sizeof(int));
    setsockopt(fd, IPPROTO_TCP, TCP_KEEPCNT,   &kc, sizeof(int));
    setsockopt(fd, IPPROTO_TCP, TCP_NODELAY,   &nd, sizeof(int));
    last_rx = last_heartbeat = millis(); idx = 0;
  }

  gs232_process(c, c, buf, idx, last_rx, COMMAND_BUFFER_SIZE, ETHERNET_MESSAGE_TIMEOUT_MS, true);

  unsigned long now = millis();
  if (c && c.connected() && last_rx > 0 &&
      (now - last_rx)        > WIFI_HEARTBEAT_MS &&
      (now - last_heartbeat) > WIFI_HEARTBEAT_MS) {
    last_heartbeat = now;
    char hb[16]; gs232_format_C(hb, sizeof(hb)); c.println(hb);
    last_tcp_activity = now;
  }

  if (last_rx > 0 && (now - last_rx) > WIFI_CLIENT_IDLE_MS) {
    if (c.connected()) { last_rx = now; }
    else { c.stop(); idx = 0; last_rx = 0; last_heartbeat = 0; }
  }
}

// =============================================================================
// USB SERIAL SERVICE
// =============================================================================

static void usb_service() {
  static byte buf[USB_COMMAND_BUFFER_SIZE];
  static int idx = 0;
  static unsigned long last_rx = 0;
  gs232_process(Serial, Serial, buf, idx, last_rx, USB_COMMAND_BUFFER_SIZE, USB_MESSAGE_TIMEOUT_MS);
}

// =============================================================================
// WIFI SERVICE
// =============================================================================

static void wifi_event_handler(WiFiEvent_t event) {
  if (event == ARDUINO_EVENT_WIFI_STA_DISCONNECTED ||
      event == ARDUINO_EVENT_WIFI_STA_LOST_IP)
    wifi_event_disc = true;
}

static void wifi_connect() {
  WiFi.persistent(false);
  WiFi.setSleep(false);
  WiFi.setAutoReconnect(false);
  WiFi.config(WIFI_IP, WIFI_GATEWAY, WIFI_SUBNET, WIFI_DNS);
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
}

static void wifi_service() {
  static unsigned long last_check = 0;
  static unsigned long disconnected_ms = 0;
  static bool was_connected = false;
  static bool begin_pending = false;
  static bool full_reset_done = false;
  static unsigned long last_ping_ms = 0;
  static bool last_ping_ok = true;
  static bool prev_ping_ok = true;

  unsigned long now = millis();
  if ((now - last_check) < WIFI_CHECK_MS) return;
  last_check = now;

  if (wifi_event_disc) wifi_event_disc = false;

  if (WiFi.status() == WL_CONNECTED) {
    bool recent = (last_tcp_activity > 0 && (now - last_tcp_activity) < WIFI_PING_INTERVAL_MS);
    if (recent) { last_ping_ms = now; last_ping_ok = true; }
    else if ((now - last_ping_ms) >= WIFI_PING_INTERVAL_MS) {
      last_ping_ms = now;
      esp_task_wdt_reset();
      bool ok = Ping.ping(WIFI_GATEWAY, 1);
      esp_task_wdt_reset();
      if (!ok) {
        if (!prev_ping_ok) last_ping_ok = false;
      } else { last_ping_ok = true; }
      prev_ping_ok = ok;
    }
  } else {
    last_ping_ok = false; last_ping_ms = 0; prev_ping_ok = true;
  }

  bool connected = (WiFi.status() == WL_CONNECTED) && last_ping_ok;

  if (connected) {
    begin_pending = false; full_reset_done = false;
    if (!was_connected) {
      server.end(); server.begin(); server.setNoDelay(true);
      tcp_drop_client = true; wifi_ready = true;
      esp_wifi_set_ps(WIFI_PS_NONE);
    }
    was_connected = true; disconnected_ms = 0;
    return;
  }

  if (wifi_ready) {
    wifi_ready = false; was_connected = false;
    driver_submit_request(REQUEST_STOP, 0);
  }
  if (!disconnected_ms) disconnected_ms = now;

  if (begin_pending) {
    if ((now - disconnected_ms) > WIFI_RECONNECT_TIMEOUT_MS) {
      if (!full_reset_done) {
        WiFi.disconnect(true); WiFi.mode(WIFI_OFF); delay(300);
        esp_task_wdt_reset(); WiFi.mode(WIFI_STA); delay(300);
        esp_task_wdt_reset(); full_reset_done = true;
      } else { WiFi.disconnect(false); }
      begin_pending = false; disconnected_ms = 0;
    }
    return;
  }

  if ((now - disconnected_ms) < WIFI_RECONNECT_DELAY_MS) return;
  wifi_connect();
  begin_pending = true; disconnected_ms = now;
}

// =============================================================================
// SETUP & LOOP
// =============================================================================

void setup() {
  esp_task_wdt_config_t wdt = { .timeout_ms = WDT_TIMEOUT_SEC * 1000, .idle_core_mask = 0, .trigger_panic = true };
  esp_task_wdt_reconfigure(&wdt);
  esp_task_wdt_add(NULL);

  pinMode(PIN_ROTATE_CW,  OUTPUT);
  pinMode(PIN_ROTATE_CCW, OUTPUT);
  pinMode(PIN_ADC_AZ,     INPUT);
  pinMode(PIN_LED_LOCKOUT, OUTPUT);
  digitalWrite(PIN_LED_LOCKOUT, LOW);
  motor_off();

  analogReadResolution(12);
  adc_init();

  Serial.begin(115200);

  if ((float)(ADC_MV_360DEG - ADC_MV_0DEG) < 1.0f) {
    while (1) { digitalWrite(PIN_LED_LOCKOUT, HIGH); delay(ERR_BLINK_MS); digitalWrite(PIN_LED_LOCKOUT, LOW); delay(ERR_BLINK_MS); }
  }

  driver_read_azimuth(1);

  unsigned long t = millis();
  while ((millis() - t) < WIFI_STARTUP_DELAY_MS) { esp_task_wdt_reset(); delay(200); }

  WiFi.onEvent(wifi_event_handler);
  WiFi.setHostname("rotator-sa7kza");
  wifi_connect();
  esp_wifi_set_ps(WIFI_PS_NONE);

  unsigned long ws = millis();
  while (WiFi.status() != WL_CONNECTED) {
    delay(500); esp_task_wdt_reset();
    if ((millis() - ws) > 15000UL) break;
  }

  if (WiFi.status() == WL_CONNECTED) {
    server.begin(); server.setNoDelay(true);
    wifi_ready = true; esp_wifi_set_ps(WIFI_PS_NONE);
  }
}

void loop() {
  esp_task_wdt_reset();
  wifi_service();
  driver_service();
  protocol_service();
  usb_service();
}