#include "feetech.h"

Feetech::Feetech(HardwareSerial& serial, int DE_PIN, int RE_PIN, int TX_PIN, int RX_PIN)
  : serial(serial), DE_PIN(DE_PIN), RE_PIN(RE_PIN), TX_PIN(TX_PIN), RX_PIN(RX_PIN) {
}

void Feetech::begin() {
  serial.begin(1000000, SERIAL_8N1, RX_PIN, TX_PIN);
  pinMode(DE_PIN, OUTPUT);
  pinMode(RE_PIN, OUTPUT);
  setModeReceive();
}

// Send move command to servo id:0-255, position:0-4095
void Feetech::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();
  serial.write(packet, sizeof(packet));
  serial.flush();
  setModeReceive();
}

void Feetech::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();
  serial.write(packet, sizeof(packet));
  serial.flush();
  setModeReceive();
}

void Feetech::testRequest() {
  uint8_t packet[6] = { 0XFF, 0XFF, 0X00, 0X02, 0X06, 0XF7 };
  uint8_t sum = 0;
  for (int i = 2; i < 5; i++) sum += packet[i];  // Include all data bytes for checksum
  packet[5] = ~sum;                              // Checksum

  setModeTransmit();
  serial.write(packet, sizeof(packet));
  serial.flush();
  setModeReceive();
  waitOnReply(100);
}

void Feetech::enableTorque(uint8_t id) {
  uint8_t packet[8];  // Adjust size based on your packet structure

  packet[0] = 255;  // Header
  packet[1] = 255;  // Header
  packet[2] = id;   // Servo ID
  packet[3] = 4;    // Length (instruction + parameters + checksum)
  packet[4] = 3;    // Instruction to enable torque
  packet[5] = 1;    // Parameter to enable torque

  // Calculate checksum
  uint8_t sum = 0;
  for (int i = 2; i < 6; i++) {
    sum += packet[i];
  }
  packet[6] = ~sum;  // Checksum (bitwise NOT)

  setModeTransmit();
  serial.write(packet, sizeof(packet));  // Send the packet
  serial.flush();
  //delay(10); // Short delay
  setModeReceive();
}

uint16_t Feetech::getPosition(uint8_t id){
  sendRequest(id, 0x38);
  waitOnReply(10);
  return 0;
}

void Feetech::sendRequest(uint8_t id, byte instruction) {
  uint8_t packet[8];

  packet[0] = 0xFF;              // Header
  packet[1] = 0xFF;              // Header
  packet[2] = id;                // Servo ID (ensure this matches the expected ID, e.g., 0x02)
  packet[3] = 0x04;              // Length (4 bytes of following data)
  packet[4] = READ_DATA;         // Instruction
  packet[5] = instruction;  // Write first address
  packet[6] = 0x02;              // Number of bytes to read

  // Calculate checksum
  uint8_t sum = 0;
  for (int i = 2; i < 7; i++) sum += packet[i];  // Include all data bytes for checksum
  packet[7] = ~sum;                              // Checksum

  setModeTransmit();
  serial.write(packet, sizeof(packet));
  serial.flush();
  setModeReceive();
}

void Feetech::pingAll() {
  Serial.println("PINGING ALL 0-255");
  for (int i = 0; i < 255; i++) {
    sendPing(i);

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

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

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

      // Read all available bytes
      while (serial.available() && count < sizeof(buffer)) {
        buffer[count++] = serial.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 Feetech::waitOnReply(unsigned long timeout, uint8_t expectedReturnType) {
  unsigned long startTime = millis();  // Record the start time

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

      // Read all available bytes
      while (serial.available() && count < sizeof(buffer)) {
        buffer[count++] = serial.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 Feetech::sendData(const byte* data, size_t length) {
  // digitalWrite(_transmitPin, HIGH); // Enable transmit mode
  // _serial.write(data, length);
  // _serial.flush(); // Wait for transmission to complete
  // digitalWrite(_transmitPin, LOW); // Disable transmit mode
}

size_t Feetech::receiveData(byte* buffer, size_t bufferSize) {
  size_t bytesRead = 0;
  while (serial.available() > 0 && bytesRead < bufferSize) {
    buffer[bytesRead++] = serial.read();
  }
  return bytesRead;
}

void Feetech::setModeTransmit() {
  digitalWrite(DE_PIN, HIGH);
  digitalWrite(RE_PIN, HIGH);
  delay(10);
}

void Feetech::setModeReceive() {
  digitalWrite(DE_PIN, LOW);
  digitalWrite(RE_PIN, LOW);
  delay(10);
}

void Feetech::update() {
  if (Serial.available()) {
    setModeTransmit();
    while (Serial.available()) {
      char incomingByte = Serial.read();  // Read from USB Serial
      serial.write(incomingByte);         // Send to Serial1
    }
    setModeReceive();
  }

  // Pass data from Serial1 (Feetech) to Serial (USB)
  if (serial.available()) {
    while (serial.available()) {
      char incomingByte = serial.read();  // Read from Serial1
      Serial.write(incomingByte);         // Send to USB Serial
    }
  }
}