/**
 * WijiBoard – BLE Stepper Controller Firmware
 * src/main.cpp
 *
 * Board  : Tenstar Robot ESP32-C3 Super Mini (or compatible ESP32-C3)
 * Steppers: Two 28BYJ-48 via ULN2003 (AccelStepper, FULL4WIRE mode)
 *
 * BLE Command Characteristic (WRITE_NR):
 *   S1+<n>   → Step motor 1 CW   n steps
 *   S1-<n>   → Step motor 1 CCW  n steps
 *   S2+<n>   → Step motor 2 CW   n steps
 *   S2-<n>   → Step motor 2 CCW  n steps
 *   SPD:<n>  → Set max speed for both motors (steps/sec)
 *   ACC:<n>  → Set acceleration for both motors (steps/sec²)
 *   HOME1    → Home motor 1 (Right)
 *   HOME2    → Home motor 2 (Left)
 *   HOMEALL  → Home both motors simultaneously
 *   DISABLE  → Disable steppers (un-hold) and require rehoming
 *   POS      → Request current positions (triggers NOTIFY)
 *
 * BLE Status Characteristic (NOTIFY):
 *   P:<s1>,<s2>  → Current step positions for motor 1 & 2
 *
 * UUIDs – must match web/js/ble.js exactly:
 *   Service  : a0b1c2d3-e4f5-6789-abcd-ef0123456700
 *   Command  : a0b1c2d3-e4f5-6789-abcd-ef0123456701
 *   Status   : a0b1c2d3-e4f5-6789-abcd-ef0123456702
 */

#include <AccelStepper.h>
#include <Arduino.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>

// ─── Motor pin mapping (28BYJ-48 / ULN2003) ──────────────────────
// Motor 1 – Shoulder
#define M1_IN1 0
#define M1_IN2 1
#define M1_IN3 3
#define M1_IN4 4

// Motor 2 – Elbow
#define M2_IN1 5
#define M2_IN2 6
#define M2_IN3 7
#define M2_IN4 10

// ─── Stepper constants ────────────────────────────────────────────
#define STEPS_PER_REV 2048
#define DEFAULT_SPEED 600.0f // steps/sec
#define DEFAULT_ACCEL 100.0f // steps/sec²
float currentMaxSpeed = DEFAULT_SPEED;
float currentAccel = DEFAULT_ACCEL;

// ─── BLE UUIDs ────────────────────────────────────────────────────
#define SERVICE_UUID "18f3b235-9831-4c75-8ec0-210469b820a0"
#define CMD_UUID "cd083b06-4447-4cf3-a7c3-322ecf802ce4"
#define STATUS_UUID "82e38c5b-d3ab-41d1-861c-b84dc6bb1e03"
#define DEVICE_NAME "WijiBoard"

// ─── Stepper objects ──────────────────────────────────────────────
AccelStepper stepper1(AccelStepper::FULL4WIRE, M1_IN1, M1_IN2, M1_IN3, M1_IN4);
AccelStepper stepper2(AccelStepper::FULL4WIRE, M2_IN1, M2_IN2, M2_IN3, M2_IN4);

// ─── BLE globals ─────────────────────────────────────────────────
BLEServer *pServer = nullptr;
BLECharacteristic *pCmdChar = nullptr;
BLECharacteristic *pStatusChar = nullptr;
bool bleConnected = false;

// ─── Status notify throttle ───────────────────────────────────────
unsigned long lastNotify = 0;
const unsigned long NOTIFY_INTERVAL_MS = 200;

// ─── Idle timeout tracker ─────────────────────────────────────────
unsigned long lastMotorActive = 0;
const unsigned long IDLE_TIMEOUT_MS = 30 * 60 * 1000; // 30 minutes
bool motorsDisabled = true; // Assume disabled at startup until moved

// ─── Forward declarations ─────────────────────────────────────────
void parseCommand(const String &cmd);
void sendPosition();
void performHomingAll();
void performHoming1();
void performHoming2();

// ─── Homing Routines ──────────────────────────────────────────────

void runSteppersWithNotify() {
  stepper1.run();
  stepper2.run();
  if (bleConnected) {
    unsigned long now = millis();
    if (now - lastNotify >= NOTIFY_INTERVAL_MS) {
      lastNotify = now;
      sendPosition();
    }
  }
}

// ─── BLE Server Callbacks ─────────────────────────────────────────
class ServerCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer *) override {
    bleConnected = true;
    Serial.println("[BLE] Client connected");
  }
  void onDisconnect(BLEServer *) override {
    bleConnected = false;
    Serial.println("[BLE] Client disconnected – restarting advertising");
    BLEDevice::startAdvertising();
  }
};

// ─── Command Characteristic Callbacks ────────────────────────────
class CmdCallbacks : public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic *pChar) override {
    String val = pChar->getValue().c_str();
    val.trim();
    if (val.length() == 0)
      return;
    Serial.printf("[CMD] Received: '%s'\n", val.c_str());
    parseCommand(val);
  }
};

// ─── Command parser ───────────────────────────────────────────────
void parseCommand(const String &cmd) {
  // S1+<n> or S1-<n> (Relative)
  if (cmd.startsWith("S1") && cmd.length() > 2) {
    long steps = cmd.substring(2).toInt();
    stepper1.move(steps);
    Serial.printf("[S1] Move relative %+ld steps\n", steps);
    return;
  }
  // S2+<n> or S2-<n> (Relative)
  if (cmd.startsWith("S2") && cmd.length() > 2) {
    long steps = cmd.substring(2).toInt();
    stepper2.move(steps);
    Serial.printf("[S2] Move relative %+ld steps\n", steps);
    return;
  }
  // X:<n1>,<n2> (Absolute combined)
  if (cmd.startsWith("X:")) {
    int commaIdx = cmd.indexOf(',');
    if (commaIdx != -1) {
      long target1 = cmd.substring(2, commaIdx).toInt();
      long target2 = cmd.substring(commaIdx + 1).toInt();
      stepper1.moveTo(target1);
      stepper2.moveTo(target2);
      Serial.printf("[X] Move absolute to %ld, %ld\n", target1, target2);
    }
    return;
  }
  // SPD:<n>
  if (cmd.startsWith("SPD:")) {
    float spd = cmd.substring(4).toFloat();
    currentMaxSpeed = spd;
    stepper1.setMaxSpeed(spd);
    stepper2.setMaxSpeed(spd);
    Serial.printf("[CFG] Max speed → %.0f steps/sec\n", spd);
    return;
  }
  // ACC:<n>
  if (cmd.startsWith("ACC:")) {
    float acc = cmd.substring(4).toFloat();
    currentAccel = acc;
    stepper1.setAcceleration(acc);
    stepper2.setAcceleration(acc);
    Serial.printf("[CFG] Acceleration → %.0f steps/sec²\n", acc);
    return;
  }
  // HOME1
  if (cmd == "HOME1") {
    Serial.println("[S1] Homing Motor 1...");
    performHoming1();
    sendPosition();
    return;
  }
  // HOME2
  if (cmd == "HOME2") {
    Serial.println("[S2] Homing Motor 2...");
    performHoming2();
    sendPosition();
    return;
  }
  // HOMEALL
  if (cmd == "HOMEALL") {
    Serial.println("[SYS] Homing both motors...");
    performHomingAll();
    sendPosition();
    return;
  }
  // ZEROALL
  if (cmd == "ZEROALL") {
    stepper1.setCurrentPosition(1024);
    stepper2.setCurrentPosition(0);
    Serial.println("[SYS] Position blindly reset to home (1024, 0).");
    sendPosition();
    return;
  }
  // DISABLE
  if (cmd == "DISABLE") {
    stepper1.disableOutputs();
    stepper2.disableOutputs();
    motorsDisabled = true;
    Serial.println("[SYS] Steppers manually disabled.");
    return;
  }
  // POS – explicit position request
  if (cmd == "POS") {
    sendPosition();
    return;
  }
  Serial.printf("[CMD] Unknown command: '%s'\n", cmd.c_str());
}

// ─── Send current positions via BLE NOTIFY ────────────────────────
void sendPosition() {
  if (!bleConnected || !pStatusChar)
    return;
  String pos = "P:" + String(stepper1.currentPosition()) + "," +
               String(stepper2.currentPosition());
  pStatusChar->setValue(pos.c_str());
  pStatusChar->notify();
  Serial.printf("[POS] %s\n", pos.c_str());
}

// ─── Setup ────────────────────────────────────────────────────────
void setup() {
  Serial.setTxTimeoutMs(0);
  Serial.begin(115200);
  delay(1000); // Wait for USB CDC to enumerate
  Serial.println("\n[BOOT] WijiBoard Stepper Controller");

  // ── Init steppers ──────────────────────────────────────────
  stepper1.setMaxSpeed(DEFAULT_SPEED);
  stepper1.setAcceleration(DEFAULT_ACCEL);
  stepper1.setCurrentPosition(0);

  stepper2.setMaxSpeed(DEFAULT_SPEED);
  stepper2.setAcceleration(DEFAULT_ACCEL);
  stepper2.setCurrentPosition(0);

  Serial.println("[STEP] Steppers initialized");

  // ── Init BLE ───────────────────────────────────────────────
  BLEDevice::init(DEVICE_NAME);

  pServer = BLEDevice::createServer();
  pServer->setCallbacks(new ServerCallbacks());

  // Service
  BLEService *pService = pServer->createService(SERVICE_UUID);

  // Command characteristic (Write without response)
  pCmdChar = pService->createCharacteristic(CMD_UUID,
                                            BLECharacteristic::PROPERTY_WRITE);
  pCmdChar->setCallbacks(new CmdCallbacks());

  // Status characteristic (Notify)
  pStatusChar = pService->createCharacteristic(
      STATUS_UUID, BLECharacteristic::PROPERTY_NOTIFY);
  // pStatusChar->addDescriptor(new BLE2902()); // Removed due to
  // deprecation/NimBLE conflicts

  pService->start();

  // Advertising
  BLEAdvertising *pAdv = BLEDevice::getAdvertising();
  pAdv->addServiceUUID(SERVICE_UUID);
  pAdv->setScanResponse(true);
  pAdv->setMinPreferred(0x06); // 7.5ms
  pAdv->setMaxPreferred(0x0C); // 15ms - CRITICAL for Linux/BlueZ stability
  BLEDevice::startAdvertising();

  Serial.printf("[BLE] Advertising as '%s' – ready!\n", DEVICE_NAME);
}

// ─── Loop ─────────────────────────────────────────────────────────
// ─── Homing State Machine ─────────────────────────────────────────
enum HomingState {
  HOME_IDLE,
  HOME_ALL_PHASE1,
  HOME_ALL_PHASE2,
  HOME_1_PHASE1,
  HOME_1_PHASE2,
  HOME_2_PHASE1,
  HOME_2_PHASE2
};
HomingState homingState = HOME_IDLE;

void loop() {
  stepper1.run();
  stepper2.run();

  bool moving = stepper1.isRunning() || stepper2.isRunning() || homingState != HOME_IDLE;
  
  if (moving) {
    lastMotorActive = millis();
    motorsDisabled = false;
  } else if (!motorsDisabled && (millis() - lastMotorActive > IDLE_TIMEOUT_MS)) {
    stepper1.disableOutputs();
    stepper2.disableOutputs();
    motorsDisabled = true;
    Serial.println("[SYS] Idle timeout reached. Steppers disabled.");
    if (bleConnected && pStatusChar) {
      pStatusChar->setValue("SYS:IDLE_TIMEOUT");
      pStatusChar->notify();
    }
  }

  if (homingState == HOME_ALL_PHASE1) {
    if (stepper1.distanceToGo() == 0 && stepper2.distanceToGo() == 0) {
      stepper1.setCurrentPosition(1024);
      stepper2.setCurrentPosition(0);
      stepper1.moveTo(1024);
      stepper2.moveTo(0);
      homingState = HOME_ALL_PHASE2;
      Serial.println("[SYS] Homing all: Phase 2 (Hold Outward Home)");
    }
  } else if (homingState == HOME_ALL_PHASE2) {
    if (stepper1.distanceToGo() == 0 && stepper2.distanceToGo() == 0) {
      stepper1.setMaxSpeed(currentMaxSpeed);
      stepper2.setMaxSpeed(currentMaxSpeed);
      stepper1.setAcceleration(currentAccel);
      stepper2.setAcceleration(currentAccel);
      sendPosition();
      homingState = HOME_IDLE;
      Serial.println("[SYS] Homing all complete.");
    }
  } else if (homingState == HOME_1_PHASE1) {
    if (stepper1.distanceToGo() == 0) {
      stepper1.setCurrentPosition(1024);
      stepper1.moveTo(1024);
      homingState = HOME_1_PHASE2;
    }
  } else if (homingState == HOME_1_PHASE2) {
    if (stepper1.distanceToGo() == 0) {
      stepper1.setMaxSpeed(currentMaxSpeed);
      stepper1.setAcceleration(currentAccel);
      homingState = HOME_IDLE;
    }
  } else if (homingState == HOME_2_PHASE1) {
    if (stepper2.distanceToGo() == 0) {
      stepper2.setCurrentPosition(0);
      stepper2.moveTo(0);
      homingState = HOME_2_PHASE2;
    }
  } else if (homingState == HOME_2_PHASE2) {
    if (stepper2.distanceToGo() == 0) {
      stepper2.setMaxSpeed(currentMaxSpeed);
      stepper2.setAcceleration(currentAccel);
      homingState = HOME_IDLE;
    }
  }

  // Periodic position notify while motors are moving
  if (bleConnected) {
    unsigned long now = millis();
    if (moving && (now - lastNotify >= NOTIFY_INTERVAL_MS)) {
      lastNotify = now;
      sendPosition();
    }
  }
}

// ─── Homing Routines ──────────────────────────────────────────────

void performHomingAll() {
  Serial.println("[SYS] Homing all: Phase 1 (Stall OUTWARD)");
  stepper1.setMaxSpeed(1500);
  stepper2.setMaxSpeed(1500);
  stepper1.setAcceleration(1500);
  stepper2.setAcceleration(1500);
  stepper1.move(-2000); 
  stepper2.move(2000);
  homingState = HOME_ALL_PHASE1;
}

void performHoming1() {
  Serial.println("[S1] Homing Motor 1: Phase 1 (Stall OUTWARD)");
  stepper1.setMaxSpeed(1500);
  stepper1.setAcceleration(1500);
  stepper1.move(-2000);
  homingState = HOME_1_PHASE1;
}

void performHoming2() {
  Serial.println("[S2] Homing Motor 2: Phase 1 (Stall OUTWARD)");
  stepper2.setMaxSpeed(1500);
  stepper2.setAcceleration(1500);
  stepper2.move(2000);
  homingState = HOME_2_PHASE1;
}