HansonServo/HansonServo.ino

#include <base64.h>
#include "feetech.h"
#include "animation.h"
#include "nodegraph.h"
#include "RobotConfig.h"

RobotConfig config;

#define DEVICE_NAME "Little Sophia"
#define FIRMWARE_VERSION "0.0.1"

#define HEADER1 0xAA
#define HEADER2 0x55
#define MAX_PAYLOAD_SIZE 6000       // 10 KB
uint8_t payload[MAX_PAYLOAD_SIZE];  // Global or static

#define CMD_ID_REQUEST 0x01
#define CMD_FILE_LIST 0x02
#define CMD_LOAD_FILE 0x03
#define CMD_DELETE_FILE 0x04
#define CMD_SAVE_FILE 0x05
#define CMD_MESSAGE 0x06
#define CMD_SET_POSITION 0x07
#define CMD_PLAY_FILE 0x08
#define CMD_SCAN_CHANNEL 0x09
#define CMD_WRITE_DATA 0x10
#define CMD_WRITE_CONFIG_UPDATE 0x12
#define CMD_START_POSITION_STREAM 0x14
#define POSITION_STREAM 0x15


// ESP32 S2 PINOUT
#define CH0_RX_PIN 13
#define CH0_TX_PIN 12

// #define RX_PIN 18  // RO
// #define TX_PIN 17  // DI
#define CH1_RX_PIN 11  // RO
#define CH1_TX_PIN 10  // DI
#define DE_PIN 7       // Driver Enable
#define RE_PIN 8       // Receiver Enable

#define PLAY_IDLE 0x00
#define PLAY_ONCE 0x01
#define PLAY_LOOP 0x02
#define PLAY_REPEAT 0x03
uint8_t playMode = PLAY_IDLE;
uint8_t repeatsRemaining = 0;

Animation anim;
Animation* currentAnimation;

bool streamPositions = false;
unsigned long lastStreamPositions = 0;

Feetech* servos[2];

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


uint8_t ids[NUM_CHANNELS] = { 10, 11, 12, 13, 14 };
uint16_t pos1[] = { 0, 0, 0, 0, 0 };
uint16_t pos2[] = { 1023, 1023, 1023, 1023, 1023 };

uint8_t idsSTS[NUM_CHANNELS] = { 15, 103 };
uint16_t pos1STS[] = { 0, 0 };
uint16_t pos2STS[] = { 1023, 1023 };

#define WAVE_PERIOD_CS 400  // 4 seconds = 400 centiseconds
#define WAVE_MAX 4095

uint16_t getSineWaveValue(unsigned long centiseconds) {
  float theta = (2.0 * PI * centiseconds) / WAVE_PERIOD_CS;
  float sine = sin(theta);                         // ranges from -1 to +1
  float scaled = (sine + 1.0) * (WAVE_MAX / 2.0);  // scale to 0–4095
  return (uint16_t)round(scaled);
}


void setup() {
  Serial.begin(1000000);
  Serial.setRxBufferSize(1024);
  for (int i = 0; i < 5; i++) {
    Serial.println(i);
    delay(500);
  }

  // config.deviceName = "Little Elephant";
  // config.firmwareVersion.major = 0;
  // config.firmwareVersion.minor = 2;

  // Add a few motors
  // config.motors.push_back({ "Ankle Left", 10, 2048 });
  // config.motors.push_back({ "Ankle Right", 11, 2048 });
  // config.motors.push_back({ "Ankle Center", 12, 2048 });
  // config.motors.push_back({ "Knee", 13, 2048 });
  // config.motors.push_back({ "Upper Leg", 14, 2048 });



  pinMode(6, INPUT);
  pinMode(7, INPUT);

  servos[0] = new Feetech(Serial1, DE_PIN, RE_PIN, CH0_TX_PIN, CH0_RX_PIN);  // SCS
  servos[0]->setFeetechMode(Feetech::MODE_SCS);
  servos[1] = new Feetech(Serial2, DE_PIN, RE_PIN, CH1_TX_PIN, CH1_RX_PIN);  // STS
  servos[1]->setFeetechMode(Feetech::MODE_STS);

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


  pos2[3] = flipBytes(pos2[3]);

  servos[0]->begin();
  servos[1]->begin();

  if (!FFat.begin(true)) {
    Serial.println("FFat mount failed");
    return;
  } else {
    Serial.println("FFat mount successful");
  }

  if (config.loadOrCreateDefault()) {
    Serial.println("Robot config ready.");
  } else {
    Serial.println("Failed to initialize robot config.");
  }

  // Print serialized config
  Serial.println(config.serializeJSON());
  std::vector<uint8_t> bytes = config.serializeToBytes();
  String output = "";

  for (size_t i = 0; i < bytes.size(); ++i) {
    if (bytes[i] < 16) output += "0";  // pad single-digit hex
    output += String(bytes[i], HEX);
    if (i < bytes.size() - 1) output += " ";
  }
  Serial.println(output);


  // PLAY ANIMATION
  // anim.loadFromFile("/asdf.anim");
  // currentAnimation = &anim;
  // currentAnimation->setActive(true);
  // playMode = PLAY_ONCE;
}

void SetID(uint8_t oldID, uint8_t newID) {
  Serial.println("Setting Lock to 0");
  Serial.println(servos[1]->setLock(oldID, 0));
  delay(1000);
  Serial.print("Changing ID ");
  Serial.print(oldID);
  Serial.print(" to ");
  Serial.println(newID);
  Serial.println(servos[1]->setID(oldID, newID));
  delay(1000);
  Serial.println("Setting Lock to 1");
  Serial.println(servos[1]->setLock(newID, 1));
  delay(1000);
}

void HandleSerialRequests() {
  if (Serial.available() >= 6) {  // 2 headers + 1 command + 2 length + 1 checksum minimum
    if (Serial.read() == HEADER1 && Serial.read() == HEADER2) {
      uint8_t command = Serial.read();
      uint16_t length = (Serial.read() << 8) | Serial.read();

      if (length > MAX_PAYLOAD_SIZE) {
        Serial.println("Payload too large");
        while (Serial.available()) Serial.read();  // flush junk
        return;
      }

      unsigned long start = millis();
      while (Serial.available() < length + 1) {
        if (millis() - start > 1000) {  // 1 second timeout
          Serial.println("Serial timeout");
          return;
        }
      }


      uint8_t checksum = command ^ (length >> 8) ^ (length & 0xFF);

      for (uint16_t i = 0; i < length; i++) {
        payload[i] = Serial.read();
        checksum ^= payload[i];
      }

      uint8_t receivedChecksum = Serial.read();

      if (checksum == receivedChecksum) {
        handleCommand(command, payload, length);
      } else {
        Serial.println("Checksum mismatch");
      }
    }
  }
}
void sendMessage(const String& payload, uint8_t command = CMD_MESSAGE) {
  uint16_t length = payload.length();

  uint8_t checksum = CMD_MESSAGE ^ (length >> 8) ^ (length & 0xFF);
  for (int i = 0; i < length; i++) {
    checksum ^= payload[i];
  }

  Serial.write(HEADER1);
  Serial.write(HEADER2);
  Serial.write(command);
  Serial.write((length >> 8) & 0xFF);
  Serial.write(length & 0xFF);
  Serial.write((const uint8_t*)payload.c_str(), length);
  Serial.write(checksum);
}

void sendMessage(const uint8_t* payload, uint16_t length, uint8_t command = CMD_MESSAGE) {
  uint8_t checksum = command ^ (length >> 8) ^ (length & 0xFF);
  for (uint16_t i = 0; i < length; i++) {
    checksum ^= payload[i];
  }

  Serial.write(HEADER1);
  Serial.write(HEADER2);
  Serial.write(command);
  Serial.write((length >> 8) & 0xFF);
  Serial.write(length & 0xFF);
  Serial.write(payload, length);
  Serial.write(checksum);
}

void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
  switch (command) {
    case CMD_ID_REQUEST:  // CMD_ID_REQUEST
      handleIdRequest();
      break;

    case CMD_FILE_LIST:  //
      handleFileList();
      break;

    case CMD_LOAD_FILE:  //
      handleLoadFile(payload, length);
      break;

    case CMD_DELETE_FILE:  //
      handleDeleteFile(payload, length);
      break;

    case CMD_SAVE_FILE:
      handleSaveFile(payload, length);
      break;

    case CMD_SET_POSITION:
      handleSetPosition(payload, length);
      break;

    case CMD_PLAY_FILE:
      handlePlayAnimation(payload, length);
      break;

    case CMD_SCAN_CHANNEL:
      handleScanChannel(payload, length);
      break;

    case CMD_WRITE_DATA:
      handleDataWrite(payload, length);
      break;

    case CMD_START_POSITION_STREAM:
      startPositionStream(payload, length);
      break;

    case CMD_WRITE_CONFIG_UPDATE:
      handleConfigUpdate(payload, length);
      break;

    default:
      Serial.print("Unknown command: ");
      Serial.println(command, HEX);
      break;
  }
}


void startPositionStream(const uint8_t* payload, uint16_t length) {
  bool start = payload[0] != 0;
  servos[0]->disableTorque(10);
  servos[0]->waitOnData1Byte(10);
  servos[0]->disableTorque(11);
  servos[0]->waitOnData1Byte(10);
  servos[0]->disableTorque(12);
  servos[0]->waitOnData1Byte(10);
  servos[0]->disableTorque(13);
  servos[0]->waitOnData1Byte(10);
  servos[0]->disableTorque(14);
  servos[0]->waitOnData1Byte(10);
  streamPositions = start;
}

void handleConfigUpdate(const uint8_t* payload, uint16_t length) {
  RobotConfig newConfig;
  uint16_t offset = 0;

  // 🔹 Decode robot name
  uint8_t nameLength = payload[offset++];
  newConfig.deviceName = "";

  for (uint8_t i = 0; i < nameLength && offset < length; ++i) {
    newConfig.deviceName += (char)payload[offset++];
  }

  // 🔹 Decode firmware version
  if (offset + 2 > length) return;
  newConfig.firmwareVersion.major = payload[offset++];
  newConfig.firmwareVersion.minor = payload[offset++];

  // 🔹 Decode motor count
  if (offset >= length) return;
  uint8_t motorCount = payload[offset++];

  // 🔹 Decode motors
  for (uint8_t i = 0; i < motorCount && offset < length; ++i) {
    if (offset + 5 > length) break;  // MODEL(2) + ID(2) + nameLength(1)

    ServoModel model;
    model.major = payload[offset++];
    model.minor = payload[offset++];

    uint16_t motorID = payload[offset++] | (payload[offset++] << 8);

    uint8_t motorNameLength = payload[offset++];
    String motorName = "";

    for (uint8_t j = 0; j < motorNameLength && offset < length; ++j) {
      motorName += (char)payload[offset++];
    }

    Motor motor;
    motor.motorID = motorID;
    motor.servoModel = model;
    motor.name = motorName;
    motor.position = 0;
    motor.isEnabled = true;

    newConfig.motors.push_back(motor);
  }

  // 🔧 Now you can use `config` however you like
  Serial.println("Robot name: " + newConfig.deviceName);
  Serial.printf("Firmware: %d.%d\n", newConfig.firmwareVersion.major, config.firmwareVersion.minor);
  Serial.printf("Motors: %d\n", newConfig.motors.size());

  for (const Motor& m : newConfig.motors) {
    Serial.printf("Motor %d (%s) - Model %d.%d\n", m.motorID, m.name.c_str(), m.servoModel.major, m.servoModel.minor);
  }

  sendMessage(newConfig.deviceName);
  config = newConfig;
  config.saveToFFat();
}



void handleDataWrite(const uint8_t* payload, uint16_t length) {
  uint8_t channel = payload[0];
  uint8_t id = payload[1];
  uint8_t writeByte = payload[2];

  uint8_t major = config.getMotorModel(id);
  servos[payload[0]]->setFeetechMode(major);  // put feetech interface in correct mode


  // Special case for ID changes, perform unlock, change, lock routine
  if (writeByte == 5) {
    sendMessage("CHANGING ID");
    servos[channel]->changeID(id, payload[4]);
    return;
  }

  // payload[4] indicates if its a single byte, or 2 bytes of information
  if (payload[4] == 1) {
    servos[channel]->write1Byte(id, writeByte, payload[4]);
    uint8_t response = servos[channel]->waitOnData1Byte(10);
    sendMessage(&response, 1, CMD_WRITE_DATA);
  } else {
    servos[channel]->write2Bytes(id, writeByte, payload[4] | (payload[5] << 8));
    uint16_t response = servos[channel]->waitOnData2Bytes(10);
    uint8_t buffer[2];
    buffer[0] = (response >> 8) & 0xFF;  // high byte
    buffer[1] = response & 0xFF;         // low byte
    sendMessage(buffer, 2, CMD_WRITE_DATA);
  }
}

void handleIdRequest() {
  std::vector<uint8_t> payload = config.serializeToBytes();
  uint16_t length = payload.size();

  // Compute checksum: XOR of CMD_ID_REQUEST, length bytes, and all payload bytes
  uint8_t checksum = CMD_ID_REQUEST ^ (length >> 8) ^ (length & 0xFF);
  for (uint8_t byte : payload) {
    checksum ^= byte;
  }

  // Send header
  Serial.write(HEADER1);
  Serial.write(HEADER2);
  Serial.write(CMD_ID_REQUEST);
  Serial.write((length >> 8) & 0xFF);
  Serial.write(length & 0xFF);

  // Send payload
  for (uint8_t byte : payload) {
    Serial.write(byte);
  }

  // Send checksum
  Serial.write(checksum);
}


void handleFileList() {
  File root = FFat.open("/");
  if (!root || !root.isDirectory()) {
    sendFileListResponse("");  // empty payload
    return;
  }

  String payload = "";
  File file = root.openNextFile();

  while (file) {
    if (!file.isDirectory()) {
      payload += String(file.name()) + "\n";
    }
    file = root.openNextFile();
  }

  sendFileListResponse(payload);
}

void sendFileListResponse(const String& payloadStr) {
  uint16_t length = payloadStr.length();
  if (length > MAX_PAYLOAD_SIZE) {
    Serial.println("File list too large");
    return;
  }

  uint8_t checksum = CMD_FILE_LIST ^ (length >> 8) ^ (length & 0xFF);
  for (uint16_t i = 0; i < length; i++) {
    checksum ^= payloadStr[i];
  }

  Serial.write(HEADER1);
  Serial.write(HEADER2);
  Serial.write(CMD_FILE_LIST);
  Serial.write((length >> 8) & 0xFF);
  Serial.write(length & 0xFF);
  Serial.write((const uint8_t*)payloadStr.c_str(), length);
  Serial.write(checksum);
}

void handleLoadFile(const uint8_t* payload, uint16_t length) {
  if (length == 0 || length >= 128) {
    Serial.println("Invalid filename");
    return;
  }

  char filename[128];
  memcpy(filename, payload, length);
  filename[length] = '\0';

  File file = FFat.open(filename, "r");
  if (!file || !file.available()) {
    Serial.println("File not found or empty");
    return;
  }

  size_t fileSize = file.size();
  if (fileSize > 65535) {
    Serial.println("File too large for single transfer");
    file.close();
    return;
  }

  uint8_t* buffer = (uint8_t*)malloc(fileSize);
  if (!buffer) {
    Serial.println("Memory allocation failed");
    file.close();
    return;
  }

  size_t bytesRead = file.read(buffer, fileSize);
  file.close();

  if (bytesRead != fileSize) {
    Serial.println("File read error");
    free(buffer);
    return;
  }

  // 🔹 Compute checksum
  uint8_t checksum = CMD_LOAD_FILE ^ (fileSize >> 8) ^ (fileSize & 0xFF);
  for (size_t i = 0; i < fileSize; i++) {
    checksum ^= buffer[i];
  }

  // 🔹 Send packet
  Serial.write(HEADER1);
  Serial.write(HEADER2);
  Serial.write(CMD_LOAD_FILE);
  Serial.write((fileSize >> 8) & 0xFF);
  Serial.write(fileSize & 0xFF);
  Serial.write(buffer, fileSize);
  Serial.write(checksum);

  free(buffer);
  Serial.println("File sent in one go");
}

void handleDeleteFile(const uint8_t* payload, uint16_t length) {
  sendMessage("Deleting FILE");
  if (length < 1) {
    Serial.println("Payload too short for filename length");
    sendMessage("Payload too short for filename length");
    return;
  }

  // 🔹 Parse filename
  uint16_t filenameLength = payload[0] | (payload[1] << 8);
  if (length < 2 + filenameLength) {
    Serial.println("Payload too short for filename");
    sendMessage("Payload too short for filename");
    return;
  }


  char filename[filenameLength + 1];


  memcpy(filename, payload + 2, filenameLength);
  filename[filenameLength] = '\0';

  deleteFile(FFat, ("/" + String(filename)).c_str());


  sendMessage(("File Deleted: " + String(filename)).c_str(), CMD_DELETE_FILE);
}


void handlePlayAnimation(const uint8_t* payload, uint16_t length) {
  sendMessage("Playing FILE");
  if (length < 1) {
    Serial.println("Payload too short for filename length");
    sendMessage("Payload too short for filename length");
    return;
  }

  // 🔹 Parse filename
  uint16_t filenameLength = payload[0] | (payload[1] << 8);
  if (length < 2 + filenameLength) {
    Serial.println("Payload too short for filename");
    sendMessage("Payload too short for filename");
    return;
  }


  char filename[filenameLength + 1];

  memcpy(filename, payload + 2, filenameLength);
  filename[filenameLength] = '\0';

  // 🔹 Extract playback mode and repeat count
  playMode = payload[2 + filenameLength];
  repeatsRemaining = payload[3 + filenameLength];

  //deleteFile(FFat, ("/" + String(filename)).c_str());
  anim.clear();
  anim.loadFromFile(("/" + String(filename)).c_str());
  //playAnimation(anim);

  currentAnimation = &anim;
  currentAnimation->setActive(true);  // ✅ mark it as running

  sendMessage("File Played", CMD_PLAY_FILE);
}

void handleSaveFile(const uint8_t* payload, uint16_t length) {
  bool valid = parseAnimationPayload(payload, length, anim);

  if (valid) {
    //Serial.println("Animation parsed successfully!");
    //anim.printKeyframes();
  } else {
    //Serial.println("Failed to parse animation.");
    length = 1;         // Override length to 1 for fallback response
    payload = nullptr;  // We'll send a single 0x00 instead
  }

  if (length > MAX_PAYLOAD_SIZE) {
    Serial.println("File list too large");
    return;
  }

  // Calculate checksum
  uint8_t checksum = CMD_SAVE_FILE ^ (length >> 8) ^ (length & 0xFF);
  if (valid) {
    for (uint16_t i = 0; i < length; i++) {
      checksum ^= payload[i];
    }
  } else {
    checksum ^= 0x00;
  }

  // Send response
  Serial.write(HEADER1);
  Serial.write(HEADER2);
  Serial.write(CMD_SAVE_FILE);
  Serial.write((length >> 8) & 0xFF);
  Serial.write(length & 0xFF);

  if (valid) {
    for (uint16_t i = 0; i < length; i++) {
      Serial.write(payload[i]);
    }
  } else {
    Serial.write(0x00);
  }

  Serial.write(checksum);
}



bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& animation) {
  sendMessage("SAVING FILE");
  if (length < 1) {
    Serial.println("Payload too short for filename length");
    return false;
  }

  // 🔹 Parse filename
  uint16_t filenameLength = payload[0] | (payload[1] << 8);
  if (length < 2 + filenameLength + 18) {
    Serial.println("Payload too short for filename and header");
    return false;
  }

  char filename[filenameLength + 1];


  memcpy(filename, payload + 2, filenameLength);
  filename[filenameLength] = '\0';


  const uint8_t* ptr = payload + 2 + filenameLength;

  String msg = "SAVING FILE: ";
  msg += filename;
  sendMessage(msg);
  // 🔹 Parse header
  memcpy(animation.header.magic, ptr, 4);
  if (strncmp(animation.header.magic, "ANIM", 4) != 0) {
    Serial.println("Invalid magic header");

    sendMessage("invalid magic header");
    return false;
  }

  animation.header.frameCount = ptr[5] << 8 | ptr[4];  // little-endian ✅

  animation.header.version = ptr[6];
  animation.header.frameRate = ptr[7];
  memcpy(animation.header.reserved, ptr + 8, 8);

  uint16_t curveCount = ptr[17] << 8 | ptr[16];  // little-endian ✅
  Serial.print("curveCount: ");
  Serial.println(curveCount);

  ptr += 18;

  if (length < (ptr - payload) + curveCount * sizeof(CurveSegment)) {
    Serial.println("Payload too short for curve segments");
    sendMessage("Payload too short");
    return false;
  }

  // 🔹 Parse curve segments
  animation.clearAllCurves();
  for (uint16_t i = 0; i < curveCount; i++) {
    CurveSegment seg;
    memcpy(&seg, ptr, sizeof(CurveSegment));
    // Serial.print("Segment ");
    // Serial.print(i);
    // Serial.print(": motorID=");
    // Serial.print(seg.motorID);
    // Serial.print(", startTime=");
    // Serial.print(seg.startTime);
    // Serial.print(", endTime=");
    // Serial.print(seg.endTime);
    // Serial.print(", startPointY=");
    // Serial.print(seg.startPointY);
    // Serial.print(", startHandleX=");
    // Serial.print(seg.startHandleX);
    // Serial.print(", startHandleY=");
    // Serial.print(seg.startHandleY);
    // Serial.print(", endHandleX=");
    // Serial.print(seg.endHandleX);
    // Serial.print(", endHandleY=");
    // Serial.print(seg.endHandleY);
    // Serial.print(", endPointY=");
    // Serial.println(seg.endPointY);


    animation.addCurveSegment(seg);
    ptr += sizeof(CurveSegment);
  }
  sendMessage(anim.printCurves());
  // for (int i = 0; i < 800; i++){
  //  sendMessage(String(anim.getMotorPosition(11, i)));
  // }
  // ✅ Advance ptr to node graph payload
  //ptr += curveCount * sizeof(CurveSegment);

  // 🔹 Parse node graph
  uint8_t nodeCount = ptr[0];
  Serial.print("Node count: ");
  Serial.println(nodeCount);
  // sendMessage(String("Node count: ") + String(nodeCount));
  // sendMessage(String("ptr offset: ") + String(ptr - payload));


  uint16_t remainingLength = length - (ptr - payload);
  if (remainingLength > 0) {
    loadNodeGraph(ptr, remainingLength, animation.nodeGraph);
    animation.nodeGraph.bindAnimationContext(&animation);
  } else {
    Serial.println("No node graph data found");
  }
  sendMessage(printNodeGraph(animation.nodeGraph));

  // currentAnimation = &animation;
  // currentAnimation->setActive(true);  // ✅ mark it as running

  // 🔹 Save using received filename
  animation.saveToFile(("/" + String(filename)).c_str());

  sendMessage("SAVED");
  return true;
}


void handleSetPosition(const uint8_t* payload, uint16_t length) {
  if (length % 3 != 0) {
    Serial.println("Invalid sync packet length");
    return;
  }

  uint8_t motorCount = length / 3;

  // Dynamically allocate arrays
  uint8_t ids[motorCount];
  uint16_t positions[motorCount];
  uint16_t speeds[motorCount];

  for (uint8_t i = 0; i < motorCount; ++i) {
    uint8_t motorId = payload[i * 3];
    uint16_t position = (payload[i * 3 + 2] << 8) | payload[i * 3 + 1];

    ids[i] = motorId;
    positions[i] = position;
    speeds[i] = 0;
    // Optional: update internal state or debug
    // Serial.printf("Motor %d → %d\n", motorId, position);
  }

  // FLIP BYTES FOR STS SERVOS
  for (int i = 0; i < motorCount; i++) {
    if (config.getMotorModel(ids[i]) == 9) {
      positions[i] = flipBytes(positions[i]);
      speeds[i] = map(speeds[i], 0, 4095, 0, 254);
      speeds[i] = flipBytes(speeds[i]);
    } else {
      positions[i] = map(positions[i], 0, 4095, 0, 1023);
      speeds[i] = map(speeds[i], 0, 4095, 0, 1000);
    }
  }

  // Send sync write to all motors
  servos[0]->syncWritePos(ids, positions, speeds, motorCount);

  String readablePayload = encodeMotorPositionsReadable(ids, positions, motorCount);
  sendMessage(readablePayload, CMD_MESSAGE);
}


String encodeMotorPositionsReadable(const uint8_t* ids, const uint16_t* positions, uint8_t motorCount) {
  String payload = "";

  for (uint8_t i = 0; i < motorCount; ++i) {
    payload += "ID:";
    payload += String(ids[i]);
    payload += ",POS:";
    payload += String(positions[i]);

    if (i < motorCount - 1) {
      payload += ";";  // separate entries
    }
  }

  return payload;
}




// Scans 0-254 and responds with the channel and ID as each successful ping is received
// Signals end by responding with channel and 255
void handleScanChannel(const uint8_t* payload, uint16_t length) {
  if (length != 1) {
    sendMessage("Length of scanChannel Request Wrong");
    return;
  }


  for (int i = 0; i < 254; i++) {
    servos[payload[0]]->sendPing(i);
    uint8_t val = servos[payload[0]]->waitOnData1Byte(10);

    if (val != 0) {
      uint8_t response[44];      // Adjusted size to fit all values
      response[0] = payload[0];  // channel
      response[1] = i;           // responding address

      uint16_t model = servos[payload[0]]->getModel(i);
      servos[payload[0]]->setFeetechMode(model);  // put feetech interface in correct mode
      uint16_t minAngleLimit = servos[payload[0]]->getMinAngleLimit(i);
      uint16_t maxAngleLimit = servos[payload[0]]->getMaxAngleLimit(i);
      uint16_t position = servos[payload[0]]->getPosition(i);
      uint8_t CWDeadZone = servos[payload[0]]->getCWDeadZone(i);
      uint8_t CCWDeadZone = servos[payload[0]]->getCCWDeadZone(i);
      uint16_t offset = servos[payload[0]]->getOffset(i);
      uint8_t mode = servos[payload[0]]->getMode(i);
      uint8_t torqueEnable = servos[payload[0]]->getTorqueEnable(i);
      uint8_t acceleration = servos[payload[0]]->getAcceleration(i);
      uint16_t goalPosition = servos[payload[0]]->getGoalPosition(i);
      uint16_t goalTime = servos[payload[0]]->getGoalTime(i);
      uint16_t goalSpeed = servos[payload[0]]->getGoalSpeed(i);
      uint8_t lock = servos[payload[0]]->getLock(i);
      int16_t speed = servos[payload[0]]->getSpeed(i);
      uint16_t load = servos[payload[0]]->getLoad(i);
      uint8_t temperature = servos[payload[0]]->getTemperature(i);
      uint8_t moving = servos[payload[0]]->getMoving(i);
      uint8_t current = servos[payload[0]]->getCurrent(i);
      uint8_t voltage = servos[payload[0]]->getVoltage(i);

      // Pack values into response
      response[2] = (model >> 8) & 0xFF;
      response[3] = model & 0xFF;

      response[4] = (minAngleLimit >> 8) & 0xFF;
      response[5] = minAngleLimit & 0xFF;

      response[6] = (maxAngleLimit >> 8) & 0xFF;
      response[7] = maxAngleLimit & 0xFF;

      response[8] = (position >> 8) & 0xFF;
      response[9] = position & 0xFF;

      response[10] = CWDeadZone;
      response[11] = CCWDeadZone;

      response[12] = (offset >> 8) & 0xFF;
      response[13] = offset & 0xFF;

      response[14] = mode;
      response[15] = torqueEnable;
      response[16] = acceleration;

      response[17] = (goalPosition >> 8) & 0xFF;
      response[18] = goalPosition & 0xFF;

      response[19] = (goalTime >> 8) & 0xFF;
      response[20] = goalTime & 0xFF;

      response[21] = (goalSpeed >> 8) & 0xFF;
      response[22] = goalSpeed & 0xFF;

      response[23] = lock;

      response[24] = (speed >> 8) & 0xFF;
      response[25] = speed & 0xFF;

      response[26] = (load >> 8) & 0xFF;
      response[27] = load & 0xFF;

      response[28] = temperature;
      response[29] = moving;
      response[30] = (current >> 8) & 0xFF;
      response[31] = current & 0xFF;

      response[32] = voltage;


      // You can continue adding more fields here if needed...

      sendMessage(response, 33, CMD_SCAN_CHANNEL);  // updated length
    }
  }
  uint8_t r[2];
  r[0] = payload[0];  // channel
  r[1] = 255;         // responding address

  sendMessage(r, 2, CMD_SCAN_CHANNEL);


  // std::vector<uint8_t> successfulAddresses;
  // servos[payload[0]]->pingAll(successfulAddresses);
  // std::vector<uint8_t> response;
  // response.push_back(payload[0]);                  // channel
  // response.push_back(successfulAddresses.size());  // count

  // for (uint8_t address : successfulAddresses) {
  //   response.push_back(address);
  // }

  // sendMessage(response.data(), response.size(), CMD_SCAN_CHANNEL);
}



bool flip = false;
unsigned long lastSend = 0;
void loop() {


  runNodeAnimation();


  HandleSerialRequests();




  if (millis() - lastSend > 50) {
    // Update config motor positions
    for (const Motor& motor : config.motors) {
      servos[payload[0]]->setFeetechMode(motor.servoModel.major);  // put feetech interface in correct mode
      uint16_t position = servos[0]->getPosition(motor.motorID);
      config.setMotorPosition(motor.motorID, position);
      //Serial.print(position);
      //Serial.print("\t");
    }
    //Serial.println();
    lastSend = millis();
  }

  if (streamPositions) {
    if (millis() - lastStreamPositions > 50) {
      lastStreamPositions = millis();
      SendMotorPositions();
    }
  }
}



void runNodeAnimation() {
  static uint32_t lastTickTime = 0;
  static uint32_t currentTick = 0;
  const uint16_t FRAME_INTERVAL_MS = 1000 / 48;

  if (!currentAnimation) return;  // ✅ Prevent crash
  if (!currentAnimation->isActive()) return;

  config.enableAllMotors();

  uint32_t now = millis();
  if (now - lastTickTime >= FRAME_INTERVAL_MS) {
    lastTickTime = now;

    currentAnimation->nodeGraph.tick(currentTick, *currentAnimation);

    // if (currentTick == 100){
    //   message += String(anim.getMotorPosition(10, currentTick));
    //   message += "\n";
    // }

    auto outputs = currentAnimation->nodeGraph.getServoOutputs();

    std::vector<uint8_t> motorIDs;
    std::vector<uint16_t> positions;
    std::vector<uint16_t> speeds;
    String message = String(currentTick) + String("\n");

    for (const auto& [motorID, value] : outputs) {
      if (value != 65535) {
        motorIDs.push_back(motorID);
        positions.push_back(value);
        speeds.push_back(0);
        config.setMotorPosition(motorID, value);
        if (config.setMotorEnabled(motorID, true)) {
          servos[0]->enableTorque(motorID);
        }
      } else {
        if (config.setMotorEnabled(motorID, false)) {
          servos[0]->disableTorque(motorID);
        }
      }
      message += "Motor ";
      message += String(motorID);
      message += " → ";
      message += String(value);
      message += "\n";
    }

    uint8_t motorCount = motorIDs.size();  // ✅ number of motors

    if (currentTick % 20 == 0) {
      sendMessage(message);
    }

    // FLIP BYTES FOR STS SERVOS
    for (int i = 0; i < motorCount; i++) {
      if (config.getMotorModel(motorIDs[i]) == 9) {
        positions[i] = flipBytes(positions[i]);
        speeds[i] = map(speeds[i], 0, 4095, 0, 254);
        speeds[i] = flipBytes(speeds[i]);
      } else {
        positions[i] = map(positions[i], 0, 4095, 0, 1023);
        speeds[i] = map(speeds[i], 0, 4095, 0, 1000);
      }
    }


    // ✅ Send to servo controller
    servos[0]->syncWritePos(motorIDs.data(), positions.data(), speeds.data(), motorCount);


    currentTick++;

    if (currentTick > currentAnimation->getFrameCount()) {
      switch (playMode) {
        case PLAY_ONCE:
          currentAnimation->setActive(false);
          break;
        case PLAY_LOOP:

          break;
        case PLAY_REPEAT:
          repeatsRemaining--;
          if (repeatsRemaining == 0) {
            currentAnimation->setActive(false);
          }
          break;
      }
      currentTick = 0;
    }
  }
}



void SendMotorPositions() {
  std::vector<uint8_t> payload;

  for (const Motor& motor : config.motors) {
    uint16_t position = motor.position;
    payload.push_back(position >> 8);    // High byte
    payload.push_back(position & 0xFF);  // Low byte
  }

  sendMessage(payload.data(), payload.size(), POSITION_STREAM);
}


void PrintFileList() {
  File root = FFat.open("/");
  if (!root || !root.isDirectory()) {
    Serial.println("Failed to open FFat root directory");
    return;
  }

  Serial.println("Files in FFat:");

  File file = root.openNextFile();
  while (file) {
    Serial.print("  ");
    Serial.print(file.name());
    Serial.print("  |  Size: ");
    Serial.println(file.size());
    file = root.openNextFile();
  }

  Serial.println("End of file list.");
}

void ClearFiles() {
  File root = FFat.open("/");
  if (!root || !root.isDirectory()) {
    Serial.println("Failed to open FFat root directory");
    return;
  }

  Serial.println("Files in FFat:");

  File file = root.openNextFile();
  while (file) {
    String filename = "/" + String(file.name());  // Add leading slash
    Serial.print("  Deleting: ");
    Serial.println(filename);
    file.close();                        // Close before deleting
    deleteFile(FFat, filename.c_str());  // Use corrected path
    file = root.openNextFile();
  }

  Serial.println("FFat cleanup complete.");
}



void deleteFile(fs::FS& fs, const char* path) {
  Serial.printf("Deleting file: %s\r\n", path);
  if (fs.remove(path)) {
    Serial.println("- file deleted");
  } else {
    Serial.println("- delete failed");
  }
}


void playAnimation(Animation& animation) {
  uint16_t durationCS = animation.getFrameCount();
  const uint8_t fps = 48;
  const uint32_t frameIntervalMS = 1000 / fps;  // ~20.83 ms
  const uint32_t totalDurationMS = durationCS * 10;

  uint32_t startTime = millis();
  uint32_t nextFrameTime = startTime;

  while (millis() - startTime < totalDurationMS) {
    uint32_t currentTime = millis();

    if (currentTime >= nextFrameTime) {
      uint16_t timeCS = (currentTime - startTime) / 10;

      //for (uint8_t motorID = 0; motorID < NUM_CHANNELS; motorID++) {
      uint16_t pos = animation.getMotorPosition(0, timeCS);
      pos = pos / 4;
      servos[0]->sendWritePos(10, pos);

      Serial.println(pos);
      //}

      nextFrameTime += frameIntervalMS;
    }

    // Optional: yield or small delay to avoid busy loop
    delay(1);
  }

  // Optional: reset motors to center
  // for (uint8_t motorID = 0; motorID < NUM_CHANNELS; motorID++) {
  //   servos[0]->sendWritePos(motorID, 2048);
  // }
}



void playAnimationOLD(Animation& anim) {
  // uint16_t positions[NUM_CHANNELS];
  // const uint16_t frameCount = 400;  //anim.getFrameCount();
  // const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND;
  // uint32_t nextFrameTime = millis();
  // Serial.print("Frame Count: ");
  // Serial.println(frameCount);

  // // Organize keyframes per motor
  // std::vector<Keyframe> motorKeyframes[NUM_CHANNELS];
  // for (const auto& kf : anim.getKeyframes()) {
  //   if (kf.motorId < NUM_CHANNELS) {
  //     motorKeyframes[kf.motorId].push_back(kf);
  //   }
  // }

  // // Sort keyframes per motor by frame
  // for (int ch = 0; ch < NUM_CHANNELS; ch++) {
  //   std::sort(motorKeyframes[ch].begin(), motorKeyframes[ch].end(),
  //             [](const Keyframe& a, const Keyframe& b) {
  //               return a.frame < b.frame;
  //             });
  // }

  // for (uint16_t frame = 0; frame < frameCount; frame++) {
  //   while (millis() < nextFrameTime) {
  //     delay(1);
  //   }

  //   for (int ch = 0; ch < NUM_CHANNELS; ch++) {
  //     const auto& kfs = motorKeyframes[ch];
  //     uint16_t value = 512;  // default position

  //     // Find surrounding keyframes
  //     Keyframe prev = { ch, 0, 512 }, next = { ch, frameCount, 512 };
  //     for (size_t i = 0; i < kfs.size(); i++) {
  //       if (kfs[i].frame <= frame) {
  //         prev = kfs[i];
  //       }
  //       if (kfs[i].frame > frame) {
  //         next = kfs[i];
  //         break;
  //       }
  //     }

  //     // Interpolate
  //     if (prev.frame == next.frame) {
  //       value = prev.position;
  //     } else {
  //       float t = float(frame - prev.frame) / (next.frame - prev.frame);
  //       value = prev.position + t * (next.position - prev.position);
  //     }

  //     positions[ch] = value;
  //   }
  //   Serial.println(positions[0]);
  //   servos[0]->syncWritePos(ids, positions, NUM_CHANNELS);
  //   nextFrameTime += frameDelay;
  // }
}



// void playLayeredAnimation(Animation& base, Animation& overlay) {
//   uint16_t basePositions[NUM_CHANNELS];
//   uint16_t overlayPositions[NUM_CHANNELS];
//   uint16_t finalPositions[NUM_CHANNELS];

//   const uint16_t frameCount = base.getFrameCount();
//   const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND;
//   uint32_t nextFrameTime = millis();

//   for (uint16_t frame = 0; frame < frameCount; frame++) {
//     while (millis() < nextFrameTime) delay(1);

//     base.getFramePositions(frame, basePositions);
//     overlay.getFramePositions(frame, overlayPositions);

//     for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) {
//       finalPositions[ch] = (basePositions[ch] + overlayPositions[ch]) / 2;
//     }

//     servos[0]->syncWritePos(ids, finalPositions, NUM_CHANNELS);
//     nextFrameTime += frameDelay;
//   }
// }


void SCSPingAll() {
  std::vector<uint8_t> successfulAddresses;
  servos[0]->pingAll(successfulAddresses);

  // Now successfulAddresses contains all successful pings
  Serial.println("Successful Addresses:");
  for (uint8_t address : successfulAddresses) {
    Serial.print(address);
    Serial.print(" ");
  }
  Serial.println();
}

void STSPingAll() {
  std::vector<uint8_t> successfulAddresses;
  servos[1]->pingAll(successfulAddresses);

  // Now successfulAddresses contains all successful pings
  Serial.println("Successful Addresses:");
  for (uint8_t address : successfulAddresses) {
    Serial.print(address);
    Serial.print(" ");
  }
  Serial.println();
}