/**
 * 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
 *   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²

// ─── 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;

// ─── 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;
  }
  // X1:<n> (Absolute)
  if (cmd.startsWith("X1:")) {
    long target = cmd.substring(3).toInt();
    stepper1.moveTo(target);
    Serial.printf("[S1] Move absolute to %ld\n", target);
    return;
  }
  // X2:<n> (Absolute)
  if (cmd.startsWith("X2:")) {
    long target = cmd.substring(3).toInt();
    stepper2.moveTo(target);
    Serial.printf("[S2] Move absolute to %ld\n", target);
    return;
  }
  // SPD:<n>
  if (cmd.startsWith("SPD:")) {
    float spd = cmd.substring(4).toFloat();
    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();
    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;
  }
  // 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 ─────────────────────────────────────────────────────────
void loop() {
  // Run steppers (non-blocking AccelStepper)
  stepper1.run();
  stepper2.run();

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

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

void performHomingAll() {
  Serial.println("[SYS] Homing all: Phase 1 (Stall UP)");
  
  // 1. Swing BOTH motors UP to hit the mechanism housing (stall point)
  // M1 (Left) swings UP by moving CW (negative)
  // M2 (Right) swings UP by moving CCW (positive)
  stepper1.move(-2048); 
  stepper2.move(2048);
  while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
    runSteppersWithNotify();
  }
  
  // 2. At this point, BOTH are stalled against the mechanism housing.
  // We know the physical angles of these stall points!
  // M1 stall is at +474 steps (83.3 degrees).
  // M2 stall is at +530 steps (93.1 degrees).
  stepper1.setCurrentPosition(474);
  stepper2.setCurrentPosition(530);

  Serial.println("[SYS] Homing all: Phase 2 (Move to Outward Home)");

  // 3. Move BOTH motors to their outward home positions
  // M1 goes to +1024 (180 degrees, pointing Left)
  // M2 goes to 0 (0 degrees, pointing Right)
  stepper1.moveTo(1024);
  stepper2.moveTo(0);
  while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
    runSteppersWithNotify();
  }

  // 4. Disable outputs to rest
  stepper1.disableOutputs();
  stepper2.disableOutputs();
  Serial.println("[SYS] Homing all complete.");
}

void performHoming1() {
  Serial.println("[S1] Homing Motor 1: Phase 1 (Stall UP)");
  stepper1.move(-2048);
  while (stepper1.distanceToGo() != 0) {
    runSteppersWithNotify();
  }
  
  stepper1.setCurrentPosition(474);
  
  Serial.println("[S1] Homing Motor 1: Phase 2 (Move Outward)");
  stepper1.moveTo(1024);
  while (stepper1.distanceToGo() != 0) {
    runSteppersWithNotify();
  }
  
  stepper1.disableOutputs();
  Serial.println("[S1] Homing complete.");
}

void performHoming2() {
  Serial.println("[S2] Homing Motor 2: Phase 1 (Stall UP)");
  stepper2.move(2048);
  while (stepper2.distanceToGo() != 0) {
    runSteppersWithNotify();
  }
  
  stepper2.setCurrentPosition(530);
  
  Serial.println("[S2] Homing Motor 2: Phase 2 (Move Outward)");
  stepper2.moveTo(0);
  while (stepper2.distanceToGo() != 0) {
    runSteppersWithNotify();
  }
  
  stepper2.disableOutputs();
  Serial.println("[S2] Homing complete.");
}