/*
   Hybrid quadruped hardware tester
   Interactive serial tool to verify the servos (PCA9685) and the 4 DC motors
   (2x TB6612) on an Arduino Nano ESP32 before running gait code.
   Test one channel at a time with the robot supported.

   Board:  Arduino Nano ESP32 (ABX00083)
   Open Serial Monitor @ 115200, line ending = "Newline" (or "Both NL & CR").

   Pin map (must match how you wired it):
     Servos:  PCA9685 on I2C  -> SDA = A4 (GPIO11), SCL = A5 (GPIO12)  [Wire default]
     Motor 0: IN1=D2  IN2=D3  PWM=A0
     Motor 1: IN1=D4  IN2=D5  PWM=A1
     Motor 2: IN1=D6  IN2=D7  PWM=A3
     Motor 3: IN1=D8  IN2=D9  PWM=A6
     Both TB6612 STBY -> 3V3,  VM -> battery+,  VCC -> 3V3,  GND -> common
   */

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

// Servo config
#define PCA9685_ADDR  0x40
#define SERVO_FREQ    60
#define SERVOMIN      170     // pulse count ~0 deg   (generic, not per-servo cal)
#define SERVOMAX      595     // pulse count ~180 deg
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(PCA9685_ADDR);

// Motor pins
const int MOT_IN1[4] = { D2, D4, D6, D8 };
const int MOT_IN2[4] = { D3, D5, D7, D9 };
const int MOT_PWM[4] = { A0, A1, A3, A6 };
int motorSpeed = 180;                 // 0-255 test speed (analogWrite duty)

void setup()
{
  Serial.begin(115200);
  while (!Serial && millis() < 3000) {}

  // Motor pins
  for (int i = 0; i < 4; i++)
  {
    pinMode(MOT_IN1[i], OUTPUT);
    pinMode(MOT_IN2[i], OUTPUT);
    digitalWrite(MOT_IN1[i], LOW);
    digitalWrite(MOT_IN2[i], LOW);
    analogWrite(MOT_PWM[i], 0);
  }

  // Servos
  Wire.begin();
  pwm.begin();
  pwm.setPWMFreq(SERVO_FREQ);

  Serial.println();
  Serial.println("=== Spiderbot hardware tester ===");
  i2cScan();
  printMenu();
}

void loop()
{
  if (Serial.available())
  {
    String line = Serial.readStringUntil('\n');
    line.trim();
    if (line.length() > 0)
      handleCommand(line);
  }
}

void printMenu()
{
  Serial.println(F("\nCommands (type + Enter):"));
  Serial.println(F("  help            show this menu"));
  Serial.println(F("  scan            I2C scan (find PCA9685 @ 0x40)"));
  Serial.println(F("  -- SERVOS --"));
  Serial.println(F("  c               center ALL servos to 90 deg"));
  Serial.println(F("  s <ch> <deg>    set servo channel ch (0-15) to deg (0-180)"));
  Serial.println(F("  sweep <ch>      sweep channel: 45 -> 135 -> 90"));
  Serial.println(F("  -- MOTORS --"));
  Serial.println(F("  f <m>           motor m (0-3) forward burst"));
  Serial.println(F("  b <m>           motor m (0-3) backward burst"));
  Serial.println(F("  mall            run all 4 motors fwd then back (wiring check)"));
  Serial.println(F("  x               STOP all motors"));
  Serial.println(F("  spd <0-255>     set motor test speed"));
  Serial.println();
}

void handleCommand(String line)
{
  char buf[64];
  line.toCharArray(buf, sizeof(buf));
  char *cmd = strtok(buf, " ");
  char *a1  = strtok(NULL, " ");
  char *a2  = strtok(NULL, " ");
  if (!cmd) return;

  if (!strcmp(cmd, "help"))       { printMenu(); }
  else if (!strcmp(cmd, "scan"))  { i2cScan(); }

  // servos
  else if (!strcmp(cmd, "c"))
  {
    Serial.println("Centering all 16 channels to 90 deg");
    for (uint8_t ch = 0; ch < 16; ch++) setServoAngle(ch, 90);
  }
  else if (!strcmp(cmd, "s"))
  {
    if (!a1 || !a2) { Serial.println("usage: s <ch> <deg>"); return; }
    int ch = atoi(a1), deg = atoi(a2);
    setServoAngle(ch, deg);
    Serial.printf("servo ch %d -> %d deg\n", ch, constrain(deg, 0, 180));
  }
  else if (!strcmp(cmd, "sweep"))
  {
    if (!a1) { Serial.println("usage: sweep <ch>"); return; }
    int ch = atoi(a1);
    Serial.printf("sweeping ch %d: 45 -> 135 -> 90\n", ch);
    setServoAngle(ch, 45);  delay(500);
    setServoAngle(ch, 135); delay(500);
    setServoAngle(ch, 90);
  }

  // motors
  else if (!strcmp(cmd, "f"))
  {
    if (!a1) { Serial.println("usage: f <m>"); return; }
    int m = atoi(a1);
    Serial.printf("motor %d FORWARD\n", m);
    motorBurst(m, +1, 1200);
  }
  else if (!strcmp(cmd, "b"))
  {
    if (!a1) { Serial.println("usage: b <m>"); return; }
    int m = atoi(a1);
    Serial.printf("motor %d BACKWARD\n", m);
    motorBurst(m, -1, 1200);
  }
  else if (!strcmp(cmd, "mall"))
  {
    Serial.println("Testing all motors: each fwd then back...");
    for (int m = 0; m < 4; m++)
    {
      Serial.printf("  motor %d fwd\n", m);
      motorBurst(m, +1, 800);
      delay(250);
      Serial.printf("  motor %d back\n", m);
      motorBurst(m, -1, 800);
      delay(400);
    }
    Serial.println("done.");
  }
  else if (!strcmp(cmd, "x"))
  {
    motorStopAll();
    Serial.println("ALL MOTORS STOPPED");
  }
  else if (!strcmp(cmd, "spd"))
  {
    if (!a1) { Serial.println("usage: spd <0-255>"); return; }
    motorSpeed = constrain(atoi(a1), 0, 255);
    Serial.printf("motor test speed = %d\n", motorSpeed);
  }
  else
  {
    Serial.printf("unknown: '%s'  (type 'help')\n", cmd);
  }
}

// servo helpers
void setServoAngle(uint8_t ch, int deg)
{
  deg = constrain(deg, 0, 180);
  int pulse = map(deg, 0, 180, SERVOMIN, SERVOMAX);
  pwm.setPWM(ch, 0, pulse);
}

// motor helpers
void motorDrive(int m, int dir)   // dir: +1 fwd, -1 rev, 0 stop
{
  if (m < 0 || m > 3) return;
  digitalWrite(MOT_IN1[m], dir > 0 ? HIGH : LOW);
  digitalWrite(MOT_IN2[m], dir < 0 ? HIGH : LOW);
  analogWrite(MOT_PWM[m], dir == 0 ? 0 : motorSpeed);
}

void motorBurst(int m, int dir, int ms)
{
  motorDrive(m, dir);
  delay(ms);
  motorDrive(m, 0);
}

void motorStopAll()
{
  for (int i = 0; i < 4; i++) motorDrive(i, 0);
}

// I2C scan
void i2cScan()
{
  Serial.println("I2C scan...");
  byte count = 0;
  for (byte addr = 1; addr < 127; addr++)
  {
    Wire.beginTransmission(addr);
    if (Wire.endTransmission() == 0)
    {
      Serial.printf("  found 0x%02X", addr);
      if (addr == 0x40) Serial.print("  <- PCA9685 servo driver");
      Serial.println();
      count++;
    }
  }
  if (count == 0)
    Serial.println("  NONE found - check SDA/SCL/power/ground!");
}