HansonServo/commands.cpp

#include "commands.h"
#include "nodegraph.h"
#include "sensors.h"
#include "behaviors.h"
#include "esp_system.h"
#include "soc/rtc_cntl_reg.h"
#include <vector>
#include <unordered_map>

// External references
extern RobotConfig config;
extern ServoManager servoManager;

// Global state instances
AnimationState animState;
MotorStreamState motorStream;

// ============================================================================
// Chunked File Save Session (FSAV)
// ============================================================================
struct SaveSession {
  bool active = false;
  uint16_t totalChunks = 0;
  uint16_t receivedChunks = 0;
  uint32_t totalSize = 0;
  uint16_t chunkSize = 0;  // size of non-final chunks
  std::vector<uint8_t> buffer;
  std::vector<bool> received;
} g_save;

// Helper: reset save session
static void resetSaveSession() {
  g_save.active = false;
  g_save.totalChunks = 0;
  g_save.receivedChunks = 0;
  g_save.totalSize = 0;
  g_save.chunkSize = 0;
  g_save.buffer.clear();
  g_save.received.clear();
}

// ============================================================================
// AnimationState
// ============================================================================

void AnimationState::play(PlayMode mode, uint8_t repeats, uint16_t startFrame) {
  current = &animation;
  current->setActive(true);
  playMode = mode;
  repeatsRemaining = repeats;
  this->startFrame = startFrame;
  playGeneration++;  // Signal that a new animation has started
}

void AnimationState::stop() {
  if (current) {
    current->setActive(false);
  }
  playMode = PLAY_IDLE;
}

// ============================================================================
// MotorStreamState
// ============================================================================

void MotorStreamState::start() {
  active = true;
  lastStreamTime = millis();
}

void MotorStreamState::stop() {
  active = false;
}

bool MotorStreamState::shouldStream() {
  if (!active) return false;
  if (millis() - lastStreamTime >= STREAM_INTERVAL_MS) {
    lastStreamTime = millis();
    return true;
  }
  return false;
}

// ============================================================================
// Command Dispatcher
// ============================================================================

void dispatchCommand() {
  const char* tag = getReceivedTag();
  const uint8_t* payload = getReceivedPayload();
  uint16_t len = getReceivedPayloadLen();
  
  // Identity & Config
  if (tagMatches(tag, Tag::IDENT)) {
    handleIdent(payload, len);
  }
  else if (tagMatches(tag, Tag::CONFG)) {
    handleConfig(payload, len);
  }
  // File Operations
  else if (tagMatches(tag, Tag::FLIST)) {
    handleFileList(payload, len);
  }
  else if (tagMatches(tag, Tag::FLOAD)) {
    handleFileLoad(payload, len);
  }
  else if (tagMatches(tag, Tag::FSAVE)) {
    handleFileSave(payload, len);
  }
  else if (tagMatches(tag, Tag::FDELE)) {
    handleFileDelete(payload, len);
  }
  else if (tagMatches(tag, Tag::FPLAY)) {
    handleFilePlay(payload, len);
  }
  else if (tagMatches(tag, Tag::FSTP)) {
    handleFileStop(payload, len);
  }
  // Motor Control
  else if (tagMatches(tag, Tag::MSET)) {
    handleMotorSet(payload, len);
  }
  else if (tagMatches(tag, Tag::MSCAN)) {
    handleMotorScan(payload, len);
  }
  else if (tagMatches(tag, Tag::MWRIT)) {
    handleMotorWrite(payload, len);
  }
  else if (tagMatches(tag, Tag::MSTRM)) {
    handleMotorStream(payload, len);
  }
  // Behaviors
  else if (tagMatches(tag, Tag::BHVR)) {
    handleBehavior(payload, len);
  }
  else if (tagMatches(tag, Tag::BLST)) {
    handleBehaviorList(payload, len);
  }
  // Visemes
  else if (tagMatches(tag, Tag::VLST)) {
    handleVisemeList(payload, len);
  }
  else if (tagMatches(tag, Tag::VADD)) {
    handleVisemeAdd(payload, len);
  }
  else if (tagMatches(tag, Tag::VDEL)) {
    handleVisemeDelete(payload, len);
  }
  else if (tagMatches(tag, Tag::VSET)) {
    handleVisemeSet(payload, len);
  }
  else if (tagMatches(tag, Tag::VSME)) {
    handleVisemeTrigger(payload, len);
  }
  // System
  else if (tagMatches(tag, Tag::BOOT)) {
    handleBootloader(payload, len);
  }
  else {
    char tagStr[5] = {tag[0], tag[1], tag[2], tag[3], 0};
    sendMessage("Unknown tag: " + String(tagStr));
  }
}

// ============================================================================
// Identity & Config Handlers
// ============================================================================

void handleIdent(const uint8_t* payload, uint16_t len) {
  std::vector<uint8_t> response = config.serializeToBytes();
  sendPacket(Tag::IDENT, response.data(), response.size());
}

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

  // Decode robot name
  if (offset >= len) { sendNack(Tag::CONFG, "Missing name"); return; }
  uint8_t nameLength = payload[offset++];
  for (uint8_t i = 0; i < nameLength && offset < len; ++i) {
    newConfig.deviceName += (char)payload[offset++];
  }

  // Decode firmware version
  if (offset + 2 > len) { sendNack(Tag::CONFG, "Missing version"); return; }
  newConfig.firmwareVersion.major = payload[offset++];
  newConfig.firmwareVersion.minor = payload[offset++];

  // Decode motor count
  if (offset >= len) { sendNack(Tag::CONFG, "Missing motors"); return; }
  uint8_t motorCount = payload[offset++];

  // Decode motors
  for (uint8_t i = 0; i < motorCount && offset < len; ++i) {
    if (offset + 5 > len) break;

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

  config = newConfig;
  config.saveToFFat();
  
  sendAck(Tag::CONFG);
  sendMessage("Config updated: " + config.deviceName);
}

// ============================================================================
// File Operation Handlers
// ============================================================================

void handleFileList(const uint8_t* payload, uint16_t len) {
  File root = FFat.open("/");
  if (!root || !root.isDirectory()) {
    sendPacket(Tag::FLIST, nullptr, 0);
    return;
  }

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

  sendPacket(Tag::FLIST, (const uint8_t*)fileList.c_str(), fileList.length());
}

void handleFileLoad(const uint8_t* payload, uint16_t len) {
  if (len == 0 || len >= 128) {
    sendNack(Tag::FLOAD, "Invalid filename");
    return;
  }

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

  File file = FFat.open(filename, "r");
  if (!file || !file.available()) {
    sendNack(Tag::FLOAD, "File not found");
    return;
  }

  size_t fileSize = file.size();
  const uint16_t CHUNK_SIZE = 1024;  // match sender chunking
  uint16_t totalChunks = (fileSize + CHUNK_SIZE - 1) / CHUNK_SIZE;

  // Prepare static header fields
  uint8_t header[8];
  header[0] = totalChunks & 0xFF;
  header[1] = (totalChunks >> 8) & 0xFF;
  header[4] = fileSize & 0xFF;
  header[5] = (fileSize >> 8) & 0xFF;
  header[6] = (fileSize >> 16) & 0xFF;
  header[7] = (fileSize >> 24) & 0xFF;

  for (uint16_t chunkIndex = 0; chunkIndex < totalChunks; chunkIndex++) {
    size_t offset = (size_t)chunkIndex * CHUNK_SIZE;
    uint16_t thisChunk = (uint16_t)min((size_t)CHUNK_SIZE, fileSize - offset);

    // Fill per-chunk fields
    header[2] = chunkIndex & 0xFF;
    header[3] = (chunkIndex >> 8) & 0xFF;

    // Build payload: header + chunkData
    std::vector<uint8_t> payloadBuf;
    payloadBuf.reserve(8 + thisChunk);
    payloadBuf.insert(payloadBuf.end(), header, header + 8);

    // Read chunk data directly into payload buffer
    size_t startIdx = payloadBuf.size();
    payloadBuf.resize(startIdx + thisChunk);
    file.seek(offset);
    file.read(payloadBuf.data() + startIdx, thisChunk);

    sendPacket(Tag::FLOAD, payloadBuf.data(), payloadBuf.size());
    delay(2);  // small pacing to avoid overwhelming host
  }

  file.close();
}

void handleFileSave(const uint8_t* payload, uint16_t len) {
  // Chunked protocol:
  // [totalChunks:2][chunkIndex:2][totalSize:4][chunkData:...]
  if (len < 8) {
    sendNack(Tag::FSAVE, "Payload too short");
    return;
  }

  uint16_t totalChunks = payload[0] | (payload[1] << 8);
  uint16_t chunkIndex = payload[2] | (payload[3] << 8);
  uint32_t totalSize = payload[4] | (payload[5] << 8) | (payload[6] << 16) | (payload[7] << 24);
  const uint8_t* chunkData = payload + 8;
  uint16_t chunkLen = len - 8;

  // Basic validation
  if (totalChunks == 0) {
    sendNack(Tag::FSAVE, "totalChunks=0");
    return;
  }
  if (chunkIndex >= totalChunks) {
    sendNack(Tag::FSAVE, "chunkIndex out of range");
    return;
  }
  if (totalSize == 0) {
    sendNack(Tag::FSAVE, "totalSize=0");
    return;
  }

  // Start new session if needed
  if (!g_save.active || g_save.totalSize != totalSize || g_save.totalChunks != totalChunks) {
    g_save.active = true;
    g_save.totalChunks = totalChunks;
    g_save.receivedChunks = 0;
    g_save.totalSize = totalSize;
    g_save.chunkSize = chunkLen;  // assume first chunk size is the standard chunk size
    g_save.buffer.assign(totalSize, 0);
    g_save.received.assign(totalChunks, false);
  }

  // Calculate offset
  uint32_t offset = (uint32_t)chunkIndex * (uint32_t)g_save.chunkSize;
  // For safety, allow last chunk to be smaller
  if (offset + chunkLen > g_save.totalSize) {
    sendNack(Tag::FSAVE, "chunk overflow");
    return;
  }

  // If not already received, copy in
  if (!g_save.received[chunkIndex]) {
    memcpy(g_save.buffer.data() + offset, chunkData, chunkLen);
    g_save.received[chunkIndex] = true;
    g_save.receivedChunks++;
  }

  // If not all chunks yet, ACK chunk and return
  if (g_save.receivedChunks < g_save.totalChunks) {
    sendAck(Tag::FSAVE);  // per-chunk ACK
    return;
  }

  // All chunks received - parse and save animation
  bool ok = parseAndSaveAnimation(g_save.buffer.data(), g_save.buffer.size(), animState.animation);
  g_save.active = false;
  g_save.buffer.clear();
  g_save.received.clear();

  if (ok) {
    sendAck(Tag::FSAVE);
  } else {
    sendNack(Tag::FSAVE, "Parse failed");
  }
}

void handleFileDelete(const uint8_t* payload, uint16_t len) {
  if (len < 2) {
    sendNack(Tag::FDELE, "Invalid request");
    return;
  }
  
  uint16_t filenameLen = payload[0] | (payload[1] << 8);
  if (len < 2 + filenameLen) {
    sendNack(Tag::FDELE, "Invalid filename");
    return;
  }

  char filename[128];
  memcpy(filename, payload + 2, min((uint16_t)127, filenameLen));
  filename[min((uint16_t)127, filenameLen)] = '\0';

  String fullPath = "/" + String(filename);
  deleteFile(FFat, fullPath.c_str());
  
  sendAck(Tag::FDELE);
  sendMessage("Deleted: " + String(filename));
}

void handleFilePlay(const uint8_t* payload, uint16_t len) {
  if (len < 6) {  // Minimum: filenameLen(2) + filename(1) + mode(1) + repeats(1) + startFrame(2)
    sendNack(Tag::FPLAY, "Invalid request");
    return;
  }

  uint16_t filenameLen = payload[0] | (payload[1] << 8);
  if (len < 2 + filenameLen + 4) {  // filenameLen + filename + mode + repeats + startFrame
    sendNack(Tag::FPLAY, "Invalid payload length");
    return;
  }

  char filename[128];
  memcpy(filename, payload + 2, min((uint16_t)127, filenameLen));
  filename[min((uint16_t)127, filenameLen)] = '\0';

  uint16_t offset = 2 + filenameLen;
  PlayMode mode = static_cast<PlayMode>(payload[offset]);
  uint8_t repeats = payload[offset + 1];
  uint16_t startFrame = payload[offset + 2] | (payload[offset + 3] << 8);

  // Debug: show parsed startFrame
  sendMessage("FPLAY parsed - startFrame bytes: [" + String(payload[offset + 2]) + ", " + String(payload[offset + 3]) + "] = " + String(startFrame));

  animState.animation.clear();
  String fullPath = "/" + String(filename);

  if (animState.animation.loadFromFile(fullPath.c_str())) {
    animState.play(mode, repeats, startFrame);
    sendAck(Tag::FPLAY);
    sendMessage("Playing: " + String(filename) + " from frame " + String(startFrame));
    sendMessage("animState.startFrame stored as: " + String(animState.startFrame));
  } else {
    sendNack(Tag::FPLAY, "Load failed");
  }
}

void handleFileStop(const uint8_t* payload, uint16_t len) {
  // FSTP has no payload (len should be 0, but we don't strictly require it)
  animState.stop();
  sendAck(Tag::FSTP);
  sendMessage("Animation stopped");
}

// ============================================================================
// Behavior Handler
// ============================================================================

void handleBehavior(const uint8_t* payload, uint16_t len) {
  // BHVR payload: [behaviorID (1 byte)][enable (1 byte)]
  if (len < 2) {
    sendNack(Tag::BHVR, "Invalid payload length");
    return;
  }
  
  uint8_t behaviorID = payload[0];
  uint8_t enable = payload[1];
  
  // Validate behavior ID
  if (behaviorID == 0 || behaviorID > 255) {
    sendNack(Tag::BHVR, "Invalid behavior ID");
    return;
  }
  
  // Enable/disable the behavior
  bool enabled = (enable != 0);
  behaviorManager.setBehaviorEnabled(static_cast<BehaviorID>(behaviorID), enabled);
  
  // Send acknowledgment
  sendAck(Tag::BHVR);
  
  // Send status message
  String status = enabled ? "enabled" : "disabled";
  sendMessage("Behavior " + String(behaviorID) + " " + status);
}

void handleBehaviorList(const uint8_t* payload, uint16_t len) {
  // BLST payload: none (can be empty)
  // Response: [count (1 byte)][behaviorID1 (1 byte)][enabled1 (1 byte)]...
  
  uint8_t count = behaviorManager.getBehaviorCount();
  
  // Build response payload: [count][id1][enabled1][id2][enabled2]...
  std::vector<uint8_t> response;
  response.push_back(count);
  
  for (uint8_t i = 0; i < count; i++) {
    BehaviorID id;
    bool enabled;
    if (behaviorManager.getBehaviorInfo(i, id, enabled)) {
      response.push_back(static_cast<uint8_t>(id));
      response.push_back(enabled ? 1 : 0);
    }
  }
  
  // Send response packet
  sendPacket(Tag::BLST, response.data(), response.size());
  
  // Also send debug message with behavior names
  String msg = "Behaviors: ";
  for (uint8_t i = 0; i < count; i++) {
    BehaviorID id;
    bool enabled;
    if (behaviorManager.getBehaviorInfo(i, id, enabled)) {
      if (i > 0) msg += ", ";
      String name;
      switch (id) {
        case BEHAVIOR_FOCUS:
          name = "Focus";
          break;
        case BEHAVIOR_IDLE:
          name = "Idle";
          break;
        case BEHAVIOR_VISEME:
          name = "Viseme";
          break;
        default:
          name = "Unknown(" + String(static_cast<uint8_t>(id)) + ")";
          break;
      }
      msg += name + "(" + String(static_cast<uint8_t>(id)) + ")=";
      msg += enabled ? "ON" : "OFF";
    }
  }
  sendMessage(msg);
}

// ============================================================================
// Viseme Handlers
// ============================================================================

void handleVisemeList(const uint8_t* payload, uint16_t len) {
  // VLST - returns all visemes with their labels and motor positions
  const std::vector<Viseme>& visemes = visemeBehavior.getVisemes();
  
  // Build response payload
  std::vector<uint8_t> response;
  response.push_back(visemes.size());  // count
  
  for (const Viseme& v : visemes) {
    response.push_back(v.id);  // viseme_id
    
    // Label (3 bytes)
    response.push_back(v.label[0]);
    response.push_back(v.label[1]);
    response.push_back(v.label[2]);
    
    // Motor positions
    response.push_back(v.motorPositions.size());  // motor_count
    for (const VisemeMotorPosition& mp : v.motorPositions) {
      response.push_back(mp.motorID);
      response.push_back(mp.position & 0xFF);         // position_low
      response.push_back((mp.position >> 8) & 0xFF);  // position_high
    }
  }
  
  sendPacket(Tag::VLST, response.data(), response.size());
}

void handleVisemeAdd(const uint8_t* payload, uint16_t len) {
  // VADD payload: [label: 3 bytes]
  if (len < 3) {
    sendNack(Tag::VADD, "Invalid payload length (need 3-byte label)");
    return;
  }
  
  // Extract label (3 bytes)
  char label[4];
  label[0] = payload[0];
  label[1] = payload[1];
  label[2] = payload[2];
  label[3] = '\0';
  
  // Add the viseme
  uint8_t newID = visemeBehavior.addViseme(label);
  
  // Send ACK with the new ID
  uint8_t ackPayload[1] = { newID };
  sendPacket(Tag::ACK, ackPayload, 1);
}

void handleVisemeDelete(const uint8_t* payload, uint16_t len) {
  // VDEL payload: [viseme_id: 1 byte]
  if (len < 1) {
    sendNack(Tag::VDEL, "Invalid payload length");
    return;
  }
  
  uint8_t visemeID = payload[0];
  
  if (visemeBehavior.deleteViseme(visemeID)) {
    sendAck(Tag::VDEL);
  } else {
    sendNack(Tag::VDEL, "Viseme not found");
  }
}

void handleVisemeSet(const uint8_t* payload, uint16_t len) {
  // VSET payload: [viseme_id: 1][label: 3 bytes][motor_count: 1][motor_id: 1][pos_low: 1][pos_high: 1]...
  if (len < 5) {  // Minimum: viseme_id(1) + label(3) + motor_count(1)
    sendNack(Tag::VSET, "Invalid payload length");
    return;
  }
  
  uint8_t visemeID = payload[0];
  
  // Extract label (3 bytes)
  char label[4];
  label[0] = payload[1];
  label[1] = payload[2];
  label[2] = payload[3];
  label[3] = '\0';
  
  uint8_t motorCount = payload[4];
  
  // Calculate expected length: viseme_id(1) + label(3) + motor_count(1) + motors(motorCount * 3)
  if (len < 5 + motorCount * 3) {
    sendNack(Tag::VSET, "Motor data truncated");
    return;
  }
  
  // Parse motor positions
  std::vector<VisemeMotorPosition> positions;
  for (uint8_t i = 0; i < motorCount; i++) {
    uint16_t offset = 5 + i * 3;  // Start after viseme_id(1) + label(3) + motor_count(1)
    VisemeMotorPosition mp;
    mp.motorID = payload[offset];
    mp.position = payload[offset + 1] | (payload[offset + 2] << 8);  // Little-endian
    positions.push_back(mp);
  }
  
  if (visemeBehavior.setVisemeMotorsAndLabel(visemeID, label, positions)) {
    sendAck(Tag::VSET);
  } else {
    sendNack(Tag::VSET, "Viseme not found");
  }
}

void handleVisemeTrigger(const uint8_t* payload, uint16_t len) {
  // VSME payload: [viseme_id: 1 byte]
  // Fire-and-forget - no response expected
  if (len < 1) {
    return;  // Silent fail for fire-and-forget
  }
  
  uint8_t visemeID = payload[0];
  visemeBehavior.triggerViseme(visemeID);
  // No response sent - fire and forget
}

// ============================================================================
// Motor Control Handlers
// ============================================================================

void handleMotorSet(const uint8_t* payload, uint16_t len) {
  if (len % 3 != 0) {
    sendNack(Tag::MSET, "Invalid length");
    return;
  }

  uint8_t motorCount = len / 3;
  uint8_t ids[motorCount];
  uint16_t positions[motorCount];
  uint16_t speeds[motorCount];

  for (uint8_t i = 0; i < motorCount; ++i) {
    ids[i] = payload[i * 3];
    positions[i] = (payload[i * 3 + 2] << 8) | payload[i * 3 + 1];
    speeds[i] = 0;
  }

  servoManager.syncWritePositions(ids, positions, speeds, motorCount, config, 0);
  sendAck(Tag::MSET);
}

void handleMotorScan(const uint8_t* payload, uint16_t len) {
  if (len < 1) {
    sendNack(Tag::MSCAN, "Invalid request");
    return;
  }

  uint8_t channel = payload[0];
  Feetech* servo = servoManager[channel];

  for (int i = 0; i < 254; i++) {
    servo->sendPing(i);
    uint8_t val = servo->waitOnData1Byte(10);

    if (val != 0) {
      uint8_t response[33];
      response[0] = channel;
      response[1] = i;

      uint16_t model = servo->getModel(i);
      servo->setFeetechMode(model);
      
      uint16_t minAngle = servo->getMinAngleLimit(i);
      uint16_t maxAngle = servo->getMaxAngleLimit(i);
      uint16_t position = servo->getPosition(i);
      uint8_t cwDead = servo->getCWDeadZone(i);
      uint8_t ccwDead = servo->getCCWDeadZone(i);
      uint16_t offset = servo->getOffset(i);
      uint8_t mode = servo->getMode(i);
      uint8_t torque = servo->getTorqueEnable(i);
      uint8_t accel = servo->getAcceleration(i);
      uint16_t goalPos = servo->getGoalPosition(i);
      uint16_t goalTime = servo->getGoalTime(i);
      uint16_t goalSpeed = servo->getGoalSpeed(i);
      uint8_t lock = servo->getLock(i);
      int16_t speed = servo->getSpeed(i);
      uint16_t load = servo->getLoad(i);
      uint8_t temp = servo->getTemperature(i);
      uint8_t moving = servo->getMoving(i);
      uint8_t current = servo->getCurrent(i);
      uint8_t voltage = servo->getVoltage(i);

      // Pack response
      response[2] = (model >> 8) & 0xFF;
      response[3] = model & 0xFF;
      response[4] = (minAngle >> 8) & 0xFF;
      response[5] = minAngle & 0xFF;
      response[6] = (maxAngle >> 8) & 0xFF;
      response[7] = maxAngle & 0xFF;
      response[8] = (position >> 8) & 0xFF;
      response[9] = position & 0xFF;
      response[10] = cwDead;
      response[11] = ccwDead;
      response[12] = (offset >> 8) & 0xFF;
      response[13] = offset & 0xFF;
      response[14] = mode;
      response[15] = torque;
      response[16] = accel;
      response[17] = (goalPos >> 8) & 0xFF;
      response[18] = goalPos & 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] = temp;
      response[29] = moving;
      response[30] = (current >> 8) & 0xFF;
      response[31] = current & 0xFF;
      response[32] = voltage;

      sendPacket(Tag::MSCAN, response, 33);
    }
  }

  // Signal end of scan
  uint8_t endResponse[2] = { channel, 255 };
  sendPacket(Tag::MSCAN, endResponse, 2);
}

void handleMotorWrite(const uint8_t* payload, uint16_t len) {
  if (len < 5) {
    sendNack(Tag::MWRIT, "Invalid request");
    return;
  }

  uint8_t channel = payload[0];
  uint8_t id = payload[1];
  uint8_t reg = payload[2];
  uint8_t dataLen = payload[3];

  Feetech* servo = servoManager[channel];
  uint8_t model = servo->getMajorModel(id);
  servo->setFeetechMode(model);

  // Special case for ID changes (reg 5)
  if (reg == 5 && dataLen == 1) {
    servo->changeID(id, payload[4]);
    sendAck(Tag::MWRIT);
    sendMessage("ID changed");
    return;
  }

  if (dataLen == 1) {
    servo->write1Byte(id, reg, payload[4]);
    uint8_t response = servo->waitOnData1Byte(10);
    sendPacket(Tag::MWRIT, &response, 1);
  } else {
    servo->write2Bytes(id, reg, payload[4] | (payload[5] << 8));
    uint16_t response = servo->waitOnData2Bytes(10);
    uint8_t respBuf[2] = { (uint8_t)(response & 0xFF), (uint8_t)(response >> 8) };
    sendPacket(Tag::MWRIT, respBuf, 2);
  }
}

void handleMotorStream(const uint8_t* payload, uint16_t len) {
  if (len < 1) {
    sendNack(Tag::MSTRM, "Invalid request");
    return;
  }

  bool enable = payload[0] != 0;

  if (enable) {
    // Disable torque for position reading
    for (const Motor& motor : config.motors) {
      servoManager[0]->setFeetechMode(motor.servoModel.major);
      servoManager[0]->disableTorque(motor.motorID);
    }
    motorStream.start();
  } else {
    motorStream.stop();
  }

  sendAck(Tag::MSTRM);
}

// ============================================================================
// System Handlers
// ============================================================================

void handleBootloader(const uint8_t* payload, uint16_t len) {
  sendMessage("Entering bootloader...");
  Serial.flush();
  delay(100);
  
  REG_WRITE(RTC_CNTL_OPTION1_REG, RTC_CNTL_FORCE_DOWNLOAD_BOOT);
  esp_restart();
}

// ============================================================================
// Helper Functions
// ============================================================================

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

  for (const Motor& motor : config.motors) {
    payload.push_back(motor.motorID);
    payload.push_back(motor.position & 0xFF);
    payload.push_back((motor.position >> 8) & 0xFF);
  }

  sendPacket(Tag::MPOS, payload.data(), payload.size());
}

bool parseAndSaveAnimation(const uint8_t* payload, uint16_t len, Animation& animation) {
  const uint8_t* ptr = payload;
  uint16_t remaining = len;

  // Optional filename block (only present if sender included it)
  String filename = "received.anim";
  if (remaining >= 4 && strncmp((const char*)ptr, "ANIM", 4) != 0) {
    if (remaining < 2) return false;
    uint16_t filenameLen = ptr[0] | (ptr[1] << 8);
    ptr += 2;
    remaining -= 2;
    if (filenameLen > 127 || remaining < filenameLen) return false;
    char fname[128];
    memcpy(fname, ptr, filenameLen);
    fname[filenameLen] = '\0';
    filename = fname;
    ptr += filenameLen;
    remaining -= filenameLen;
  }

  // Header: 16 bytes
  if (remaining < 16) return false;
  memcpy(animation.header.magic, ptr, 4);
  if (strncmp(animation.header.magic, "ANIM", 4) != 0) {
    sendMessage("Invalid magic header");
    return false;
  }
  animation.header.frameCount = ptr[4] | (ptr[5] << 8);
  animation.header.version = ptr[6];
  animation.header.frameRate = ptr[7];
  memcpy(animation.header.reserved, ptr + 8, 8);
  ptr += 16;
  remaining -= 16;

  if (animation.header.version == 2) {
    // Version 2: Frame data block
    // Calculate motor count from remaining data
    if (animation.header.frameCount == 0) return false;
    uint16_t frameDataSize = remaining;
    uint16_t frameSize = frameDataSize / animation.header.frameCount;
    uint16_t motorCount = frameSize / sizeof(MotorPosition);
    
    if (frameSize % sizeof(MotorPosition) != 0) return false;
    if (frameDataSize < animation.header.frameCount * motorCount * sizeof(MotorPosition)) return false;
    
    animation.clearFrameData();
    
    // Read all frames
    for (uint16_t frameIndex = 0; frameIndex < animation.header.frameCount; frameIndex++) {
      std::vector<MotorPosition> frame;
      frame.reserve(motorCount);
      
      for (uint16_t motorIndex = 0; motorIndex < motorCount; motorIndex++) {
        if (remaining < sizeof(MotorPosition)) return false;
        MotorPosition motorPos;
        memcpy(&motorPos, ptr, sizeof(MotorPosition));
        frame.push_back(motorPos);
        ptr += sizeof(MotorPosition);
        remaining -= sizeof(MotorPosition);
      }
      
      animation.setFrameData(frameIndex, frame);
    }
  } else {
    // Version 1: Curve count (at start of curve block)
    if (remaining < 2) return false;
    uint16_t curveCount = ptr[0] | (ptr[1] << 8);
    ptr += 2;
    remaining -= 2;

    // Curves (17 bytes each, packed)
    uint16_t curveDataSize = curveCount * sizeof(CurveSegment);
    if (remaining < curveDataSize) return false;
    animation.clearAllCurves();
    for (uint16_t i = 0; i < curveCount; i++) {
      CurveSegment seg;
      memcpy(&seg, ptr, sizeof(CurveSegment));
      animation.addCurveSegment(seg);
      ptr += sizeof(CurveSegment);
    }
    remaining -= curveDataSize;

    // Node graph (whatever remains)
    if (remaining > 0) {
      loadNodeGraph(ptr, remaining, animation.nodeGraph);
      animation.nodeGraph.bindAnimationContext(&animation);
    }
  }

  // Save to file
  String fullPath = "/" + filename;
  animation.saveToFile(fullPath.c_str());
  sendMessage("Saved: " + filename);
  return true;
}

void deleteFile(fs::FS& fs, const char* path) {
  if (fs.remove(path)) {
    Serial.printf("Deleted: %s\n", path);
  } else {
    Serial.printf("Delete failed: %s\n", path);
  }
}