/*
   Spiderbot V2 servo calibration
   Records PCA9685 pulse counts at joint angles -90/-45/0/90/135 degrees.
   Seed values come from the original SunFounder build and must be calibrated
   for this robot. Set each reference angle with a protractor, save its pulse
   count, and use dump to print the s_servos[] table for the gait firmware.

   Board:  Arduino Nano ESP32   Driver: PCA9685 @ 0x40   Serial: 115200

   Calibration commands:
     1) ch 5          select the channel (PCA9685 output, 0-15)
     2) a 0           select the target angle you'll set it to (-90/-45/0/90/135)
     3) n 5 / n -5    nudge the pulse until the horn/leg is exactly at that angle
     4) save          record the current pulse for (channel, angle)
   Repeat for each angle a servo uses (see which columns are non -1 for it), for
   all 12 leg servos (0,1,2, 4,5,6, 8,9,10, 12,13,14). Then:
     dump             prints the full s_servos[] array -> paste into the gait code
   */

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

#define PCA9685_ADDR 0x40
#define SERVO_FREQ   60
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(PCA9685_ADDR);

// angle columns:            -90   -45    0    90   135
const int ANGLES[5]     = {  -90,  -45,   0,   90,  135 };
const char *ANGLE_STR[5] = { "-90","-45","0", "90","135" };

// working copy of the table, seeded with the current (original) values.
// columns: dn90, dn45, d0, d90, d135   (-1 = unused / not calibrated)
int cal[16][5] = {
  { 145, 253, 360, 577,  -1 },  // 0
  { 618, 502, 385, 155,  -1 },  // 1
  {  -1, 573, 470, 262, 155 },  // 2
  {  -1,  -1,  -1,  -1,  -1 },  // 3
  { 163, 267, 370, 590,  -1 },  // 4
  { 175, 284, 393, 630,  -1 },  // 5
  {  -1, 155, 257, 475, 590 },  // 6
  {  -1,  -1,  -1,  -1,  -1 },  // 7
  { 145, 251, 357, 575,  -1 },  // 8
  { 610, 503, 395, 187,  -1 },  // 9
  {  -1, 605, 490, 275, 160 },  // 10
  {  -1,  -1,  -1,  -1,  -1 },  // 11
  { 173, 282, 390, 610,  -1 },  // 12
  { 163, 265, 367, 590,  -1 },  // 13
  {  -1, 165, 270, 487, 605 },  // 14
  {  -1,  -1,  -1,  -1,  -1 },  // 15
};

int curCh    = 0;      // selected channel
int curAngle = 2;      // selected angle index (default 0 deg)
int curPulse = 360;    // live pulse count being sent

void setup()
{
  Serial.begin(115200);
  while (!Serial && millis() < 3000) {}
  Wire.begin();
  pwm.begin();
  pwm.setPWMFreq(SERVO_FREQ);
  Serial.println("\n=== Spiderbot servo calibration ===");
  printHelp();
  applyPulse();
}

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

void printHelp()
{
  Serial.println(F("\nCommands:"));
  Serial.println(F("  ch <0-15>   select channel"));
  Serial.println(F("  a <angle>   select target angle: -90 -45 0 90 135"));
  Serial.println(F("  p <count>   set raw pulse (approx 150-600) and drive there"));
  Serial.println(F("  n <delta>   nudge pulse by delta (e.g. n 5,  n -1)"));
  Serial.println(F("  save        record current pulse for (channel, angle)"));
  Serial.println(F("  go          drive to the SAVED pulse for (channel, angle)"));
  Serial.println(F("  show        print the saved row for the current channel"));
  Serial.println(F("  home        drive every channel to its saved 90-deg value"));
  Serial.println(F("  dump        print the full s_servos[] array to paste"));
  Serial.println(F("  help        this menu"));
  Serial.println();
}

void status()
{
  Serial.printf("ch %d  angle %s  pulse %d  (saved=%d)\n",
                curCh, ANGLE_STR[curAngle], curPulse, cal[curCh][curAngle]);
}

void applyPulse()
{
  curPulse = constrain(curPulse, 80, 620);
  pwm.setPWM(curCh, 0, curPulse);
  status();
}

int angleToIndex(int deg)
{
  for (int i = 0; i < 5; i++) if (ANGLES[i] == deg) return i;
  return -1;
}

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

  if (!strcmp(cmd, "help")) { printHelp(); }
  else if (!strcmp(cmd, "ch"))
  {
    if (!arg) { Serial.println("usage: ch <0-15>"); return; }
    curCh = constrain(atoi(arg), 0, 15);
    // Start from a saved pulse when switching channels.
    int seed = cal[curCh][curAngle];
    if (seed < 0) seed = cal[curCh][2];      // fall back to d0
    if (seed < 0) seed = 360;
    curPulse = seed;
    applyPulse();
  }
  else if (!strcmp(cmd, "a"))
  {
    if (!arg) { Serial.println("usage: a <-90|-45|0|90|135>"); return; }
    int idx = angleToIndex(atoi(arg));
    if (idx < 0) { Serial.println("angle must be -90 -45 0 90 or 135"); return; }
    curAngle = idx;
    int seed = cal[curCh][curAngle];
    if (seed > 0) curPulse = seed;
    applyPulse();
  }
  else if (!strcmp(cmd, "p"))
  {
    if (!arg) { Serial.println("usage: p <count>"); return; }
    curPulse = atoi(arg);
    applyPulse();
  }
  else if (!strcmp(cmd, "n"))
  {
    if (!arg) { Serial.println("usage: n <delta>"); return; }
    curPulse += atoi(arg);
    applyPulse();
  }
  else if (!strcmp(cmd, "save"))
  {
    cal[curCh][curAngle] = curPulse;
    Serial.printf(">> saved ch %d  %s deg = %d\n", curCh, ANGLE_STR[curAngle], curPulse);
  }
  else if (!strcmp(cmd, "go"))
  {
    int v = cal[curCh][curAngle];
    if (v < 0) { Serial.println("nothing saved for this angle yet"); return; }
    curPulse = v;
    applyPulse();
  }
  else if (!strcmp(cmd, "show"))
  {
    Serial.printf("ch %d:  dn90=%d dn45=%d d0=%d d90=%d d135=%d\n",
                  curCh, cal[curCh][0], cal[curCh][1], cal[curCh][2],
                  cal[curCh][3], cal[curCh][4]);
  }
  else if (!strcmp(cmd, "home"))
  {
    for (int c = 0; c < 16; c++)
      if (cal[c][3] > 0) pwm.setPWM(c, 0, cal[c][3]);   // d90 column
    Serial.println("all channels driven to their saved 90-deg value");
  }
  else if (!strcmp(cmd, "dump"))
  {
    dumpTable();
  }
  else
  {
    Serial.printf("unknown: '%s' (type help)\n", cmd);
  }
}

void dumpTable()
{
  Serial.println(F("\n// ---- paste this over s_servos[] in spider_servo_only.ino ----"));
  Serial.println(F("servos_t s_servos[16] = {"));
  for (int c = 0; c < 16; c++)
  {
    Serial.printf("  {%d , %d , %d , %d , %d , %d },\n",
                  c, cal[c][0], cal[c][1], cal[c][2], cal[c][3], cal[c][4]);
  }
  Serial.println(F("};\n"));
}