#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();
}

void Feetech::syncWritePos(uint8_t* ids, uint16_t* positions, uint16_t* speeds, uint8_t count) {
  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++] = 0xFF;
  packet[index++] = 0xFF;
  packet[index++] = BROADCAST_ID;
  packet[index++] = packetLen;
  packet[index++] = SYNCWRITE_DATA;
  packet[index++] = REQUEST_GOAL_POSITION;
  packet[index++] = DATA_LEN_PER_SERVO;

  // Servo data
  uint16_t pos;
  uint16_t speed;
  for (uint8_t i = 0; i < count; i++) {
    packet[index++] = ids[i];

    // if (feetechMode == MODE_SCS) {
    //   pos = map(positions[i], 0, 4095, 0, 1023);
    // } else if (feetechMode == MODE_STS) {
    //   pos = positions[i];
    // }
    pos = positions[i];
    speed = speeds[i];

    packet[index++] = (uint8_t)((pos >> 8) & 0xFF);    // High byte
    packet[index++] = (uint8_t)(pos & 0xFF);           // Low byte
    packet[index++] = (uint8_t)((speed >> 8) & 0xFF);  // Speed high byte
    packet[index++] = (uint8_t)(speed & 0xFF);         // 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();

  clearEcho(sizeof(packet));
}



// Send move command to servo id:0-255, position:0-4095
uint8_t 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
  if (feetechMode == MODE_SCS) {
    packet[6] = (position >> 8) & 0xFF;  // High byte
    packet[7] = position & 0xFF;         // Low byte
  } else if (feetechMode == MODE_STS) {
    packet[6] = position & 0xFF;         // Low byte first for STS
    packet[7] = (position >> 8) & 0xFF;  // High byte second
  }
  // 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();
  clearEcho(sizeof(packet));

  // Should later recieve a 1 byte response code packet
  return waitOnData1Byte(10);
}

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

  // memcpy(lastSentPacket, packet, sizeof(packet));
  // lastSentLength = sizeof(packet);




  // Send packet
  //Serial.println("PING");
  setModeTransmit();
  //delay(20);
  serial.write(packet, sizeof(packet));
  serial.flush();
  //delay(20);
  setModeReceive();
  clearEcho(sizeof(packet));
}

void Feetech::clearEcho(uint8_t length) {
  if (filterEcho) {
    for (int i = 0; i < length; i++) {
      if (serial.available()) serial.read();
    }
  }
}

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);
}

uint8_t Feetech::enableTorque(uint8_t id) {
  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] = WRITE_DATA;             // Instruction
  packet[5] = REQUEST_TORQUE_ENABLE;  // Write first address
  packet[6] = 1;                      // Data to write

  // 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, 8);
  serial.flush();
  setModeReceive();

  return waitOnData1Byte(10);
}

uint8_t Feetech::disableTorque(uint8_t id) {
  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] = WRITE_DATA;             // Instruction
  packet[5] = REQUEST_TORQUE_ENABLE;  // Write first address
  packet[6] = 0;                      // Data to write

  // 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, 8);
  serial.flush();
  setModeReceive();

  return waitOnData1Byte(10);
}

uint16_t Feetech::getModel(uint8_t id) {
  sendRequest(id, REQUEST_MODEL, 2);
  return waitOnData2Bytes(50);
}

uint16_t Feetech::getMajorModel(uint8_t id) {
  sendRequest(id, REQUEST_MODEL, 2);
  uint16_t fullModel = waitOnData2Bytes(10);
  return fullModel; // return low byte (major model)
}

uint8_t Feetech::getID(uint8_t id) {
  sendRequest(id, REQUEST_ID, 1);
  return waitOnData1Byte(10);
}

uint8_t Feetech::setID(uint8_t id, uint8_t newId) {
  write1Byte(id, REQUEST_ID, newId);
  return waitOnData1Byte(10);
}

// Performs entire ID changing routine
uint8_t Feetech::changeID(uint8_t id, uint8_t newId) {
  setLock(id, 0);
  setID(id, newId);
  setLock(newId, 1);
}

uint8_t Feetech::getBaudRate(uint8_t id) {
  sendRequest(id, REQUEST_BAUD_RATE, 1);
  return waitOnData1Byte(10);
}

uint16_t Feetech::getMinAngleLimit(uint8_t id) {
  sendRequest(id, REQUEST_MIN_ANGLE_LIMIT, 2);
  if (feetechMode == MODE_STS) {
    return waitOnData2Bytes(10);
  } else if (feetechMode == MODE_SCS) {
    return flipBytes(waitOnData2Bytes(10));
  }
}

uint16_t Feetech::setMinAngleLimit(uint8_t id, uint16_t limit) {
  write2Bytes(id, REQUEST_MIN_ANGLE_LIMIT, limit);
  if (feetechMode == MODE_SCS) {
    return flipBytes(waitOnData2Bytes(10));
  } else if (feetechMode == MODE_STS) {
    return waitOnData2Bytes(10);
  }
}



uint16_t Feetech::getMaxAngleLimit(uint8_t id) {
  sendRequest(id, REQUEST_MAX_ANGLE_LIMIT, 2);
  if (feetechMode == MODE_SCS) {
    return flipBytes(waitOnData2Bytes(10));
  } else if (feetechMode == MODE_STS) {
    return waitOnData2Bytes(10);
  }
}

uint16_t Feetech::setMaxAngleLimit(uint8_t id, uint16_t limit) {
  write2Bytes(id, REQUEST_MAX_ANGLE_LIMIT, limit);
  if (feetechMode == MODE_SCS) {
    return flipBytes(waitOnData2Bytes(10));
  } else if (feetechMode == MODE_STS) {
    return waitOnData2Bytes(10);
  }
}

uint8_t Feetech::getCWDeadZone(uint8_t id) {
  sendRequest(id, REQUEST_CW_DEAD_ZONE, 1);
  return waitOnData1Byte(10);
}

uint8_t Feetech::getCCWDeadZone(uint8_t id) {
  sendRequest(id, REQUEST_CCW_DEAD_ZONE, 1);
  return waitOnData1Byte(10);
}

uint16_t Feetech::getOffset(uint8_t id) {
  sendRequest(id, REQUEST_OFFSET, 2);
  return waitOnData2Bytes(10);
}

uint8_t Feetech::getMode(uint8_t id) {
  sendRequest(id, REQUEST_MODE, 1);
  return waitOnData1Byte(10);
}

uint8_t Feetech::getTorqueEnable(uint8_t id) {
  sendRequest(id, REQUEST_TORQUE_ENABLE, 1);
  return waitOnData1Byte(10);
}

uint8_t Feetech::getAcceleration(uint8_t id) {
  sendRequest(id, REQUEST_ACCELERATION, 1);
  return waitOnData1Byte(10);
}

uint16_t Feetech::getGoalPosition(uint8_t id) {
  sendRequest(id, REQUEST_GOAL_POSITION, 2);
  return waitOnData2Bytes(10);
}

uint16_t Feetech::getGoalTime(uint8_t id) {
  sendRequest(id, REQUEST_GOAL_TIME, 2);
  return waitOnData2Bytes(10);
}

uint16_t Feetech::getGoalSpeed(uint8_t id) {
  sendRequest(id, REQUEST_GOAL_TIME, 2);
  return waitOnData2Bytes(10);
}

uint8_t Feetech::getLock(uint8_t id) {
  if (feetechMode == MODE_SCS) {
    sendRequest(id, REQUEST_LOCK_SCS, 1);
  } else if (feetechMode == MODE_STS) {
    sendRequest(id, REQUEST_LOCK_SMS_STS, 1);
  }
  return waitOnData1Byte(10);
}

uint8_t Feetech::setLock(uint8_t id, uint8_t lockEnabled) {
  if (feetechMode == MODE_SCS) {
    write1Byte(id, REQUEST_LOCK_SCS, lockEnabled);
  } else if (feetechMode == MODE_STS) {
    write1Byte(id, REQUEST_LOCK_SMS_STS, lockEnabled);
  }
  return waitOnData1Byte(10);
}

uint8_t Feetech::getVoltage(uint8_t id) {
  sendRequest(id, REQUEST_VOLTAGE, 1);
  //float voltage = waitOnData1Byte(10) * 0.1;
  return waitOnData1Byte(10);
}

uint16_t Feetech::getPosition(uint8_t id) {
  sendRequest(id, REQUEST_POSITION, 2);
  if (feetechMode == MODE_STS) {
    return waitOnData2Bytes(10);
  } else if (feetechMode == MODE_SCS) {
    return map(flipBytes(waitOnData2Bytes(10)), 0, 1023, 0, 4095);
  }
}

int16_t Feetech::getSpeed(uint8_t id) {
  sendRequest(id, REQUEST_CURRENT_SPEED, 2);
  int16_t val = waitOnData2Bytes(10);
  if (val < 0) {
    val -= 32767;
    val = -val;
  }
  return val;
}

// NOT SURE ABOUT THIS ONE from 0-1000 one direction, 0-2024 the other.
uint16_t Feetech::getLoad(uint8_t id) {
  sendRequest(id, REQUEST_CURRENT_LOAD, 2);
  return waitOnData2Bytes(10);
}

uint8_t Feetech::getTemperature(uint8_t id) {
  sendRequest(id, REQUEST_TEMPERATURE, 1);
  return waitOnData1Byte(10);
}

uint8_t Feetech::getMoving(uint8_t id) {
  sendRequest(id, REQUEST_MOVING, 1);
  return waitOnData1Byte(10);
}

// Multiplier could be wrong
uint16_t Feetech::getCurrent(uint8_t id) {
  sendRequest(id, REQUEST_CURRENT_CURRENT, 2);
  //return waitOnData2Bytes(10) * 0.01; FLOAT
  return waitOnData2Bytes(10);
}

void Feetech::sendRequest(uint8_t id, byte instruction, uint8_t byteCount) {
  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] = byteCount;    // 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();

  clearEcho(sizeof(packet));
}

void Feetech::write1Byte(uint8_t id, byte instruction, uint8_t data) {
  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] = WRITE_DATA;   // Instruction
  packet[5] = instruction;  // Write first address
  packet[6] = data;         // Data to write

  // 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, 8);
  serial.flush();
  setModeReceive();
}

void Feetech::write2Bytes(uint8_t id, byte instruction, uint16_t data) {
  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] = instruction;  // Address: Goal Position
  if (feetechMode == MODE_SCS) {
    packet[6] = (data >> 8) & 0xFF;  // High byte
    packet[7] = data & 0xFF;         // Low byte
  } else if (feetechMode == MODE_STS) {
    packet[6] = data & 0xFF;         // Low byte first for STS
    packet[7] = (data >> 8) & 0xFF;  // High byte second
  }
  // 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();
  clearEcho(sizeof(packet));

  // Should later recieve a 1 byte response code packet
  //return waitOnData2Bytes(10);
}

void Feetech::pingAll(std::vector<uint8_t>& successfulAddresses) {
  Serial.println("PINGING ALL 0-255");
  successfulAddresses.clear();  // Clear any previous results
  for (int i = 0; i < 255; i++) {
    //Serial.println(i);
    sendPing(i);
    uint8_t val = waitOnData1Byte(10);
    if (val != 0) {
      //Serial.println(val);
      successfulAddresses.push_back(i);  // Store the successful address
    }
  }
  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();
      }

      //Serial.println(count);

      // 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
  }
}

uint8_t Feetech::waitOnData1Byte(unsigned long timeout) {
  unsigned long startTime = millis();

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

      while (serial.available() && count < sizeof(buffer)) {
        buffer[count++] = serial.read();
      }

      // Look for valid packet starting with 0xFF 0xFF
      for (int i = 0; i <= count - 4; i++) {
        if (buffer[i] == 0xFF && buffer[i + 1] == 0xFF) {
          uint8_t length = buffer[i + 3];
          int packetSize = length + 4;

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

            return buffer[i + 5];  // Return value byte
          }
        }
      }
    }
    delay(1);
  }
  return 0;
}



uint16_t Feetech::waitOnData2Bytes(unsigned long timeout) {
  unsigned long startTime = millis();

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

      while (serial.available() && count < sizeof(buffer)) {
        buffer[count++] = serial.read();
      }

      // Look for valid packet starting with 0xFF 0xFF
      for (int i = 0; i <= count - 5; i++) {
        if (buffer[i] == 0xFF && buffer[i + 1] == 0xFF) {
          uint8_t length = buffer[i + 3];
          int packetSize = length + 4;

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

            // Combine two bytes into a uint16_t (little-endian)
            uint16_t value = buffer[i + 5] | (buffer[i + 6] << 8);
            return value;
          }
        }
      }
    }
    delay(1);
  }
  return 0;
}





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);
}

uint16_t Feetech::flipBytes(uint16_t value) {
  return (value >> 8) | (value << 8);
}

void Feetech::setFeetechMode(FeetechMode newMode) {
  feetechMode = newMode;
}

// Pass motor model number (major) to set SCS/STS mode
void Feetech::setFeetechMode(uint8_t modelMajor) {
  feetechMode = static_cast<FeetechMode>(modelMajor);
}

void Feetech::setFeetechMode(uint16_t model) {
  uint8_t major = model & 0xFF;  // Get major model
  setFeetechMode(major);
}

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
    }
  }
}