#include "feetech.h"



bool PRINT_SENT_PACKETS = false;
// S3 setup
// #define RX_PIN 4  // DI
// #define DE_PIN 5  // Driver Enable
// #define RE_PIN 6  // Receiver Enable
// #define TX_PIN 7  // RO


// C3 setup
#define DE_PIN 20
#define RE_PIN 10
#define TX_PIN 21
#define RX_PIN 9

Feetech servos = Feetech(Serial1, DE_PIN, RE_PIN, TX_PIN, RX_PIN);

// SCS are BIG endian    (high byte first)
// SMS are little endian (low byte first)


#define PING 0x01            // QUERY THE WORKING STATUS
#define READ_DATA 0x02       // READ DATA
#define WRITE_DATA 0x03      // WRITE DATA
#define REGWRITE_DATA 0x04   // QUEUES MOVES FOR ACTION COMMAND
#define ACTION 0x05          // TRIGGERS REG WRITE WRITES
#define SYNCWRITE_DATA 0x83  // SIMULTANEOUS CONTROL OF MULTIPLE SERVOS
#define RESET 0x06           // RESET TO FACTORY DEFAULT

void setup() {
  Serial.begin(115200);
  servos.begin();


  // // while (true){
  // //   digitalWrite(TX_PIN, !digitalRead(TX_PIN));
  // //   Serial.println(digitalRead(TX_PIN));
  // //   delay(2000);
  // // }

  // Serial1.begin(1000000, SERIAL_8N1, RX_PIN, TX_PIN);
  //  pinMode(DE_PIN, OUTPUT);
  //  pinMode(RE_PIN, OUTPUT);
  //  SetModeReceive();
  // delay(1000);
}

// Send move command to servo id:0-255, position:0-4095
void sendWritePos(uint8_t id, uint16_t position) {
  uint8_t packet[9];

  packet[0] = 0xFF;                    // Header
  packet[1] = 0xFF;                    // Header
  packet[2] = id;                      // Servo ID
  packet[3] = 5;                       // Length = instruction + address + 2 bytes + checksum
  packet[4] = WRITE_DATA;              // Instruction: WRITE
  packet[5] = 0x2A;                    // Address: Goal Position
  packet[7] = (position >> 8) & 0xFF;  // High byte
  packet[6] = position & 0xFF;         // Low byte

  // Calculate checksum
  uint8_t sum = 0;
  for (int i = 2; i < 8; i++) sum += packet[i];
  packet[8] = ~sum;

  // Send packet
  SetModeTransmit();
  Serial1.write(packet, sizeof(packet));
  delay(10);
  //Serial1.flush();
  SetModeReceive();
}


void syncWriteTwoServos(uint16_t pos1, uint16_t pos2) {
  Serial.println(pos1);
  const uint8_t START_BYTE = 0xFF;
  const uint8_t BROADCAST_ID = 0xFE;
  const uint8_t SYNC_WRITE = 0x83;
  const uint8_t START_ADDR = 0x2A;
  const uint8_t DATA_LEN_PER_SERVO = 6;  // pos(2) + time(2) + speed(2)
  const uint8_t NUM_SERVOS = 2;

  uint8_t packetLen = 4 + (NUM_SERVOS * (1 + DATA_LEN_PER_SERVO));  // instruction + addr + len + servo data
  uint8_t packet[3 + packetLen];                                    // header + payload
  uint8_t index = 0;

  // Header
  packet[index++] = START_BYTE;
  packet[index++] = START_BYTE;
  packet[index++] = BROADCAST_ID;
  packet[index++] = packetLen;
  packet[index++] = SYNC_WRITE;
  packet[index++] = START_ADDR;
  packet[index++] = DATA_LEN_PER_SERVO;

  // Servo 1
  packet[index++] = 0x01;                // ID
  packet[index++] = (pos1 >> 8) & 0xFF;  // Position high
  packet[index++] = pos1 & 0xFF;         // Position low
  packet[index++] = 0x00;                // Time high
  packet[index++] = 0x00;                // Time low
  packet[index++] = 0x07;                // High byte
  packet[index++] = 0xD0;                // Low byte 2000


  // Servo 2
  packet[index++] = 0x02;  // ID
  packet[index++] = pos2 & 0xFF;
  packet[index++] = (pos2 >> 8) & 0xFF;
  packet[index++] = 0x00;
  packet[index++] = 0x00;
  packet[index++] = 0xE8;
  packet[index++] = 0x03;

  // Checksum
  uint8_t checksum = 0;
  for (uint8_t i = 2; i < index; i++) {
    checksum += packet[i];
  }
  checksum = ~checksum & 0xFF;

  // Send packet
  SetModeTransmit();
  Serial1.write(packet, index);
  Serial1.write(checksum);
  Serial1.flush();
  delay(2);
  SetModeReceive();
}



void syncWriteServos(uint8_t* ids, uint16_t* positions, uint8_t count) {
  const uint8_t SYNC_WRITE = 0x83;
  const uint8_t START_BYTE = 0xFF;
  const uint8_t BROADCAST_ID = 0xFE;
  const uint8_t START_ADDR = 0x2A;          // Goal Position address
  const uint8_t DATA_LEN_PER_SERVO = 0x04;  // 2 bytes pos + 2 bytes speed

  // Calculate packet length: instruction + address + data length + (count × data per servo)
  uint8_t packetLen = 4 + (count * (1 + DATA_LEN_PER_SERVO));  // instruction + addr + len + servo data

  // Create packet buffer
  uint8_t packet[3 + packetLen];  // 3 header bytes + payload
  uint8_t index = 0;

  // Header
  packet[index++] = START_BYTE;
  packet[index++] = START_BYTE;
  packet[index++] = BROADCAST_ID;
  packet[index++] = packetLen;
  packet[index++] = SYNC_WRITE;
  packet[index++] = START_ADDR;
  packet[index++] = DATA_LEN_PER_SERVO;

  // Servo data
  for (uint8_t i = 0; i < count; i++) {
    packet[index++] = ids[i];
    packet[index++] = (uint8_t)((positions[i] >> 8) & 0xFF);  // High byte
    packet[index++] = (uint8_t)(positions[i] & 0xFF);         // Low byte
    packet[index++] = 0x00;                                   // Speed high byte
    packet[index++] = 0x00;                                   // Speed low byte
  }

  // Checksum
  uint8_t checksum = 0;
  for (uint8_t i = 2; i < index; i++) {
    checksum += packet[i];
  }
  checksum = ~checksum;

  // Send packet
  SetModeTransmit();
  Serial1.write(packet, index);
  Serial1.write(checksum);
  Serial1.flush();
  SetModeReceive();
}

// Send a ping command to a specific servo
void sendPing(uint8_t id) {
  uint8_t packet[6];

  packet[0] = 0xFF;  // Header
  packet[1] = 0xFF;  // Header
  packet[2] = id;    // Servo ID
  packet[3] = 0x02;  // Length = instruction + address + checksum
  packet[4] = PING;  // Instruction: PING

  // Calculate checksum
  uint8_t sum = 0;
  for (int i = 2; i < 5; i++) sum += packet[i];
  packet[5] = ~sum;  // Checksum

  // Send packet
  SetModeTransmit();
  Serial1.write(packet, sizeof(packet));
  Serial1.flush();
  SetModeReceive();

  if (PRINT_SENT_PACKETS) {
    Serial.print("Sent ping packet: ");
    for (int i = 0; i < 6; i++) {
      Serial.print("0x");
      if (packet[i] < 0x10) Serial.print("0");  // Leading zero for single-digit hex
      Serial.print(packet[i], HEX);
      Serial.print(" ");
    }
    Serial.println();
  }
}

void WaitOnReply(unsigned long timeout) {
  unsigned long startTime = millis();  // Record the start time

  while (millis() - startTime < timeout) {  // Loop until timeout
    if (Serial1.available()) {
      uint8_t buffer[32];
      int count = 0;

      // Read all available bytes
      while (Serial1.available() && count < sizeof(buffer)) {
        buffer[count++] = Serial1.read();
      }

      // Display on Serial
      Serial.print("recv: ");
      Serial.print(buffer[2]);
      Serial.print(" ");
      if (buffer[4] == 0x00) {
        Serial.print("OK");
      } else {
        Serial.print("NOK");
      }
      Serial.print("\t");
      for (int i = 0; i < count; i++) {
        Serial.print("0x");
        if (buffer[i] < 0x10) Serial.print("0");
        Serial.print(buffer[i], HEX);
        Serial.print(" ");
      }
      Serial.println();


      break;  // Exit the loop after processing the reply
    }
    delay(10);  // Small delay to prevent busy-waiting
  }
}

void SetModeTransmit() {
  digitalWrite(DE_PIN, HIGH);
  digitalWrite(RE_PIN, HIGH);
  delay(10);
}

void SetModeReceive() {
  digitalWrite(DE_PIN, LOW);
  digitalWrite(RE_PIN, LOW);
  delay(10);
}

void PingAll() {
  Serial.println("PINGING ALL 0-255");
  for (int i = 180; i < 255; i++) {

    sendPing(i);

    WaitOnReply(100);
  }
  Serial.println("PINGING COMPLETE");
}

void TestRange(uint8_t id) {
  for (int i = 0; i < 4095; i += 256) {
    sendWritePos(id, i);
    WaitOnReply(100);
    delay(100);
  }
  for (int i = 4095; i > 0; i -= 256) {

    sendWritePos(id, i);
    WaitOnReply(100);
    delay(100);
  }
}

uint8_t ids[] = { 1, 2 };  //, 10, 15, 40, 41, 43, 44};
uint16_t pos[2];

void loop() {
  //PingAll();

  
  
  if (true) {
    for (int i = 0; i < 4096; i += 128) {
      servos.sendWritePos(103, i);
      servos.waitOnReply(10);
      servos.getPosition(103);
      Serial.println(i);
      delay(2000);
    }
    for (int i = 4095; i > 0; i -= 128) {
      servos.sendWritePos(103, i);
      servos.waitOnReply(10);
      servos.getPosition(103);
      Serial.println(i);
      delay(2000);
    }
  }

  //servos.testRequest();
  //servos.update();
  //servos.enableTorque(1);
  //servos.enableTorque(2);
  // servos.pingAll();
  // delay(2000);
  // for (int i = 0; i < 4096; i += 32) {
  //   servos.sendWritePos(103, i);
  //   servos.waitOnReply(10);
  //   delay(100);
  // }
  return;
  //servos.update();
  // //servos.pingAll();

  // //servos.sendWritePos(1, 100);
  //delay(2000);
  // servos.waitOnReply(10);
  // delay(2000);
  // servos.sendRequest(103, 0x02);
  // servos.waitOnReply(10);
  // delay(2000);
  // for (int i = 0; i < 1024; i++){
  //     servos.sendWritePos(103, i);
  //     servos.waitOnReply(10);
  //     Serial.println(i);
  //     delay(50);
  // }

  // servos.waitOnReply(10);
  // delay(2000);
  // servos.sendWritePos(103, 500);
  // servos.waitOnReply(10);
  // delay(2000);
  // servos.sendWritePos(103, 1000);
  // //servos.sendRequest(103, 0x02);
  // servos.waitOnReply(10);
  // delay(2000);
  return;

  //WaitOnReply(100);
  //PingAll();
  //return;
  // for (uint16_t pos = 0; pos <= 1024; pos += 16) {
  //   syncWriteTwoServos(pos, 1024 - pos);
  //   delay(500);
  // }

  // for (int i = 0; i < 1024; i += 8) {
  //   for (int x = 0; x < 2; x++) {
  //     pos[x] = i;
  //   }
  //   //sendWritePos(1, i);
  //   sendWritePos(103, i);
  //   //syncWriteServos(ids, pos, 2);
  //   Serial.println(i);
  //   //syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]);
  //   delay(10);
  // }

  // for (int i = 1025; i > 0; i -= 8) {
  //   for (int x = 0; x < 2; x++) {
  //     pos[x] = i;
  //   }
  //   //sendWritePos(1, i);
  //   sendWritePos(103, i);
  //   //syncWriteServos(ids, pos, 2);
  //   Serial.println(i);
  //   //syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]);
  //   delay(10);
  // }


  //TestRange(1);
  // TestRange(4);
  // TestRange(10);
  // TestRange(15);
  // TestRange(40);
  // TestRange(41);
  // TestRange(43);
  // TestRange(44);

  // sendWritePos(123, 1000);
  //delay(2000);

  // sendWritePos(123, 2000);
  // delay(2000);



  // sendWritePos(34, 4095);
  // WaitOnReply(50);
  // delay(100);
  // sendWritePos(34, 0);
  // WaitOnReply(50);
  // delay(100);

  // WaitOnReply(1000);
  // delay(1000);
  // delay(1000);
  // sendPing(5);
  // Serial.println("WAITING...");
  // WaitOnReply(1000);
  // delay(1000);
}