HansonServo/ls_firmware.ino

/**
 * HansonServo Firmware
 *
 * Unified robot controller with:
 * - Feetech servo control (SCS/STS)
 * - Animation playback via node graphs
 * - ADXL345 IMU
 * - RD-03D mmWave radar
 * - CRC16 tagged packet protocol
 *
 * Protocol: 0xA5 0x5A [TAG 4B][LEN 2B][SEQ 2B][PAYLOAD][CRC16 2B]
 */

// Configuration defines
#define ENABLE_SERIAL_PASSTHROUGH                                              \
  false // Set true to use Feetech app comms straight to servos

#include "robotconfig.h"
#include "animation.h"
#include "commands.h"
#include "motorcontrol.h"
#include "motoraid.h"
#include "nodegraph.h"
#include "protocol.h"
#include "sensors.h"
#include "behaviors.h"
#include <FFat.h>


// ============================================================================
// Global State
// ============================================================================

RobotConfig config;

// Timing constants
constexpr uint16_t FRAME_RATE = 48;
constexpr uint16_t FRAME_INTERVAL_MS = 1000 / FRAME_RATE;
constexpr uint16_t MOTOR_UPDATE_INTERVAL_MS = 50;
constexpr uint32_t HEARTBEAT_INTERVAL_MS = 1000;

// ============================================================================
// Utility Functions
// ============================================================================

uint16_t getSineWaveValue(unsigned long centiseconds) {
  constexpr uint16_t WAVE_PERIOD_CS = 400; // 4 seconds
  constexpr uint16_t WAVE_MAX = 4095;

  float theta = (2.0 * PI * centiseconds) / WAVE_PERIOD_CS;
  float sine = sin(theta);
  float scaled = (sine + 1.0) * (WAVE_MAX / 2.0);
  return (uint16_t)round(scaled);
}

// ============================================================================
// Unified Motor Command Application
// ============================================================================

// Apply motor commands from behaviors and/or animations
// This function works whether an animation is active or not
// Behaviors have priority - they override animation positions
void applyMotorCommands(const std::vector<uint8_t>& motorIDs, 
                        const std::vector<uint16_t>& positions) {
  if (motorIDs.empty()) return;
  
  std::vector<uint8_t> finalMotorIDs;
  std::vector<uint16_t> finalPositions;
  std::vector<uint16_t> speeds;
  
  for (size_t i = 0; i < motorIDs.size(); i++) {
    uint8_t motorID = motorIDs[i];
    uint16_t finalPosition = positions[i];
    
    // Check if a behavior wants to control this motor (behaviors have priority)
    uint16_t behaviorPosition;
    if (behaviorManager.getMotorPosition(motorID, behaviorPosition)) {
      // Behavior is controlling this motor, use its position instead
      finalPosition = behaviorPosition;
    }
    
    finalMotorIDs.push_back(motorID);
    finalPositions.push_back(finalPosition);
    speeds.push_back(0);
    config.setMotorPosition(motorID, finalPosition);
    config.setMotorEnabled(motorID, true);
  }
  
  // Send all positions in one sync write
  if (!finalMotorIDs.empty()) {
    servoManager.syncWritePositions(finalMotorIDs.data(), finalPositions.data(),
                                    speeds.data(), finalMotorIDs.size(), config, 0);
  }
}

// Apply motor commands from behaviors only (when no animation is active)
void applyBehaviorMotorCommands() {
  // Get all motors that behaviors want to control
  std::vector<uint8_t> motorIDs;
  std::vector<uint16_t> positions;
  
  // Check all configured motors to see if any behavior wants to control them
  for (const Motor& motor : config.motors) {
    uint16_t position;
    if (behaviorManager.getMotorPosition(motor.motorID, position)) {
      motorIDs.push_back(motor.motorID);
      positions.push_back(position);
    }
  }
  
  // Apply the behavior-controlled motors
  if (!motorIDs.empty()) {
    applyMotorCommands(motorIDs, positions);
  }
}

// ============================================================================
// Protocol Handler
// ============================================================================

void handleProtocol() {
  if (receivePacket()) {
    dispatchCommand();
  }
}

// ============================================================================
// Serial Passthrough (USB ↔ Servo Serial)
// ============================================================================

#if ENABLE_SERIAL_PASSTHROUGH
void handleSerialPassthrough() {
  // Forward USB Serial → Servo Serial1
  while (Serial.available()) {
    Serial1.write(Serial.read());
  }

  // Forward Servo Serial1 → USB Serial
  while (Serial1.available()) {
    Serial.write(Serial1.read());
  }
}
#endif

// ============================================================================
// Animation Playback
// ============================================================================

// Dispatcher: calls the appropriate animation function based on version
void runAnimation() {
  if (!animState.current || !animState.current->isActive()) {
    return;
  }

  if (animState.current->header.version == 2) {
    runFrameAnimation();
  } else {
    runNodeAnimation();
  }
}

// Version 2: Frame-by-frame animation playback
void runFrameAnimation() {
  static uint32_t lastTickTime = 0;
  static uint32_t currentTick = 0;
  static uint8_t lastGeneration = 0;

  if (!animState.current || !animState.current->isActive()) {
    return;
  }

  // Reset tick when a new animation starts (detected by generation change)
  if (lastGeneration != animState.playGeneration) {
    currentTick = animState.startFrame;
    lastTickTime = millis();
    lastGeneration = animState.playGeneration;
    sendMessage("V2 Animation started, generation: " + String(lastGeneration) + ", startFrame: " + String(animState.startFrame));
  }

  config.enableAllMotors();

  // Calculate frame interval from animation's frame rate
  uint16_t frameIntervalMs = 1000 / animState.current->header.frameRate;
  if (frameIntervalMs == 0) frameIntervalMs = 1;  // Safety: prevent division by zero

  uint32_t now = millis();
  if (now - lastTickTime < frameIntervalMs)
    return;
  lastTickTime = now;

  // Get frame data for current tick
  const std::vector<MotorPosition>* frameData = animState.current->getFrameData(currentTick);
  
  if (frameData && !frameData->empty()) {
    // Collect motor commands from frame data
    std::vector<uint8_t> motorIDs;
    std::vector<uint16_t> positions;

    for (const auto& motorPos : *frameData) {
      motorIDs.push_back(motorPos.motorID);
      positions.push_back(motorPos.position);
    }

    // Apply motor commands (behaviors will override if needed)
    applyMotorCommands(motorIDs, positions);
    
    // Debug: print frame and motor positions
    // Serial.print("Frame ");
    // Serial.print(currentTick);
    // Serial.print(": ");
    // for (size_t i = 0; i < motorIDs.size(); i++) {
    //   if (i > 0) Serial.print(", ");
    //   Serial.print("M");
    //   Serial.print(motorIDs[i]);
    //   Serial.print("=");
    //   Serial.print(positions[i]);
    // }
    // Serial.println();
  }

  // Emit per-frame event
  {
    uint8_t payload[4];
    payload[0] = currentTick & 0xFF;
    payload[1] = (currentTick >> 8) & 0xFF;
    payload[2] = static_cast<uint8_t>(animState.playMode);
    payload[3] = 0; // in-progress
    sendPacket(Tag::FRAME, payload, 4);
  }

  currentTick++;

  // Handle animation end
  uint16_t totalFrames = animState.current->getFrameCount();
  uint16_t framesPlayed = currentTick - animState.startFrame;
  uint16_t remainingFrames = (totalFrames > animState.startFrame) ? (totalFrames - animState.startFrame) : 0;

  // Debug: log completion check near the end
  if (remainingFrames > 0 && framesPlayed >= remainingFrames - 1) {
    sendMessage("Completion check - currentTick: " + String(currentTick) + ", framesPlayed: " + String(framesPlayed) + ", remainingFrames: " + String(remainingFrames) + ", totalFrames: " + String(totalFrames));
  }

  // Check if we've played all remaining frames (from startFrame to totalFrames-1)
  if (totalFrames > 0 && remainingFrames > 0 && framesPlayed >= remainingFrames) {
    sendMessage("Animation completion triggered! Sending completion packet.");
    switch (animState.playMode) {
      case PLAY_ONCE:
        {
          // Send completion packet BEFORE stopping animation
          uint8_t done[4];
          done[0] = currentTick & 0xFF;
          done[1] = (currentTick >> 8) & 0xFF;
          done[2] = static_cast<uint8_t>(animState.playMode);
          done[3] = 1; // complete
          sendPacket(Tag::FRAME, done, 4);
          animState.stop();
        }
        break;
      case PLAY_LOOP:
        // Reset to start frame for seamless looping
        currentTick = animState.startFrame;
        break;
      case PLAY_REPEAT:
        if (--animState.repeatsRemaining == 0) {
          // Send completion packet BEFORE stopping animation
          uint8_t done[4];
          done[0] = currentTick & 0xFF;
          done[1] = (currentTick >> 8) & 0xFF;
          done[2] = static_cast<uint8_t>(animState.playMode);
          done[3] = 1; // complete
          sendPacket(Tag::FRAME, done, 4);
          animState.stop();
  } else {
          currentTick = animState.startFrame;
        }
        break;
      default:
        break;
      }
  }
}

// Version 1: Node graph animation playback
void runNodeAnimation() {
  static uint32_t lastTickTime = 0;
  static uint32_t currentTick = 0;
  static uint8_t lastGeneration = 0;

  if (!animState.current || !animState.current->isActive()) {
    return;
  }

  // Reset tick when a new animation starts (detected by generation change)
  if (lastGeneration != animState.playGeneration) {
    currentTick = animState.startFrame;  // Start from specified frame
    lastTickTime = millis();
    lastGeneration = animState.playGeneration;
    // Debug: send startFrame via MSGE
    sendMessage("Animation startFrame: " + String(animState.startFrame) + ", currentTick: " + String(currentTick));
  }

  config.enableAllMotors();

  // Calculate frame interval from animation's frame rate
  uint16_t frameIntervalMs = 1000 / animState.current->header.frameRate;
  if (frameIntervalMs == 0) frameIntervalMs = 1;  // Safety: prevent division by zero

  uint32_t now = millis();
  if (now - lastTickTime < frameIntervalMs)
    return;
    lastTickTime = now;

  // Tick the node graph
  animState.current->nodeGraph.tick(currentTick, *animState.current);
  auto outputs = animState.current->nodeGraph.getServoOutputs();

  // Collect motor commands
    std::vector<uint8_t> motorIDs;
    std::vector<uint16_t> positions;

  for (const auto &[motorID, value] : outputs) {
      if (value != 65535) {
        motorIDs.push_back(motorID);
        positions.push_back(value);
  } else {
      // Only disable torque for motors that should be limp
        if (config.setMotorEnabled(motorID, false)) {
        servoManager[0]->disableTorque(motorID);
      }
    }
  }

  // Apply motor commands (behaviors will override if needed)
  applyMotorCommands(motorIDs, positions);

  // Emit per-frame event: [frameLo, frameHi, playMode, status=0]
  // Send actual frame number (currentTick), not relative frame
  {
    uint8_t payload[4];
    payload[0] = currentTick & 0xFF;
    payload[1] = (currentTick >> 8) & 0xFF;
    payload[2] = static_cast<uint8_t>(animState.playMode);
    payload[3] = 0; // in-progress
    sendPacket(Tag::FRAME, payload, 4);
  }

    currentTick++;

  // Handle animation end (0 = run indefinitely for variable-only animations)
  // Calculate total frames played: currentTick - startFrame
  uint16_t framesPlayed = currentTick - animState.startFrame;
  // Calculate remaining frames: total frames minus startFrame
  // If animation has 100 frames and we start at 50, we should only play 50 frames
  uint16_t totalFrames = animState.current->getFrameCount();
  uint16_t remainingFrames = (totalFrames > animState.startFrame) ? (totalFrames - animState.startFrame) : 0;
  
  // Debug: show completion check values every 10 frames
  if (framesPlayed % 10 == 0 || framesPlayed >= remainingFrames - 1) {
    sendMessage("Frame check - played: " + String(framesPlayed) + ", remaining: " + String(remainingFrames) + ", total: " + String(totalFrames) + ", startFrame: " + String(animState.startFrame));
  }
  
  if (totalFrames > 0 && remainingFrames > 0 && framesPlayed >= remainingFrames) {
    switch (animState.playMode) {
        case PLAY_ONCE:
      animState.stop();
        {
          uint8_t done[4];
          done[0] = currentTick & 0xFF;
          done[1] = (currentTick >> 8) & 0xFF;
          done[2] = static_cast<uint8_t>(animState.playMode);
          done[3] = 1; // complete
          sendPacket(Tag::FRAME, done, 4);
        }
          break;
        case PLAY_LOOP:
      // Reset to start frame for seamless looping
      currentTick = animState.startFrame;
      sendMessage("Looping back to startFrame: " + String(animState.startFrame));
          break;
        case PLAY_REPEAT:
      if (--animState.repeatsRemaining == 0) {
        animState.stop();
        uint8_t done[4];
        done[0] = currentTick & 0xFF;
        done[1] = (currentTick >> 8) & 0xFF;
        done[2] = static_cast<uint8_t>(animState.playMode);
        done[3] = 1; // complete
        sendPacket(Tag::FRAME, done, 4);
        // Animation stopped, don't reset tick
      } else {
        // Reset to start frame for next repeat
        currentTick = animState.startFrame;
          }
          break;
    default:
      break;
    }
    // Don't reset currentTick here - each case handles it if needed
  }
}

// ============================================================================
// Test Functions
// ============================================================================

void testSweepMotor40() {
  static unsigned long lastUpdate = 0;
  static uint16_t position = 500;
  static int16_t direction = 1;
  const unsigned long SWEEP_INTERVAL_MS = 20;  // Update every 20ms
  const uint16_t MIN_POS = 500;
  const uint16_t MAX_POS = 2500;
  const uint16_t STEP = 10;
  
  unsigned long now = millis();
  if (now - lastUpdate < SWEEP_INTERVAL_MS) {
    return;
  }
  lastUpdate = now;
  
  // Update position
  position += (direction * STEP);
  
  // Reverse direction at limits
  if (position >= MAX_POS) {
    position = MAX_POS;
    direction = -1;
  } else if (position <= MIN_POS) {
    position = MIN_POS;
    direction = 1;
  }
  
  // Send position to motor 40
  uint8_t motorID = 40;
  uint16_t positions[1] = {position};
  uint16_t speeds[1] = {0};
  uint8_t ids[1] = {motorID};
  
  servoManager.syncWritePositions(ids, positions, speeds, 1, config, 0);
}

// ============================================================================
// Motor Position Updates
// ============================================================================

void updateMotorPositions() {
  // Only read positions when motor streaming is active
  // This prevents blocking the main loop when positions aren't needed
  if (!motorStream.active) {
    return;
  }

  static unsigned long lastUpdate = 0;

  if (millis() - lastUpdate < MOTOR_UPDATE_INTERVAL_MS)
    return;
  lastUpdate = millis();

  for (const Motor &motor : config.motors) {
    servoManager[0]->setFeetechMode(motor.servoModel.major);
    uint16_t position = servoManager[0]->getPosition(motor.motorID);
    config.setMotorPosition(motor.motorID, position);
    uint16_t current = servoManager[0]->getCurrent(motor.motorID);
    config.setMotorCurrent(motor.motorID, current);
  }
}

void printMotorStats() {
  Serial.println("--- Motor stats ---");
  for (const Motor& motor : config.motors) {
    Serial.print("ID:");
    Serial.print(motor.motorID);
    Serial.print(" pos:");
    Serial.print(motor.position);
    Serial.print(" cur:");
    Serial.println(motor.current);
  }
  Serial.println("-------------------");
}

void handleMotorStreaming() {
  if (motorStream.shouldStream()) {
    sendMotorPositions();
  }
}

void printLoopRate() {
  static unsigned long lastPrint = 0;
  static uint32_t loopCount = 0;
  
  loopCount++;
  
  unsigned long now = millis();
  if (now - lastPrint >= 1000) {
    Serial.print("[Loop] ");
    Serial.print(loopCount);
    Serial.println(" Hz");
    loopCount = 0;
    lastPrint = now;
  }
}

// ============================================================================
// Heartbeat
// ============================================================================

void sendHeartbeat() {
  static unsigned long lastHeartbeat = 0;

  if (millis() - lastHeartbeat < HEARTBEAT_INTERVAL_MS)
    return;
  lastHeartbeat = millis();

  // Pack state: uptime(4) + flags(2)
  uint8_t payload[6];
  uint32_t uptime = millis() / 1000;
  uint16_t flags = 0;

  // Build flags
  if (adxl.isReady())
    flags |= 0x01;
  if (animState.current && animState.current->isActive())
    flags |= 0x02;
  if (motorStream.active)
    flags |= 0x04;
  if (sensors.isADXLStreamEnabled())
    flags |= 0x08;
  if (sensors.isRadarStreamEnabled())
    flags |= 0x10;

  payload[0] = uptime & 0xFF;
  payload[1] = (uptime >> 8) & 0xFF;
  payload[2] = (uptime >> 16) & 0xFF;
  payload[3] = (uptime >> 24) & 0xFF;
  payload[4] = flags & 0xFF;
  payload[5] = (flags >> 8) & 0xFF;

  sendPacket(Tag::STATE, payload, 6);
}

// ============================================================================
// Setup
// ============================================================================

void setup() {
  // Serial setup (buffer size must be set before begin)
  Serial.setRxBufferSize(8192);
  Serial.begin(1000000);

  pinMode(6, OUTPUT);
  digitalWrite(6, 1);

  // Startup delay
  delay(500);
  Serial.println("\n[HansonServo] Starting...");

  // Initialize servo manager
  servoManager.init();
  Serial.println("[HansonServo] Servos initialized");

  // Initialize sensors
  sensors.init();

  // Initialize filesystem
  if (!FFat.begin(true)) {
    Serial.println("[HansonServo] FFat mount failed!");
      return;
    }
  Serial.println("[HansonServo] Filesystem ready");

  

  // Initialize behaviors (order determines priority: first added = highest priority)
  // Priority: Focus > Viseme > Idle
  // NOTE: Don't set enabled state here - let config load restore it, or set defaults after
  
  // 1. Focus behavior (highest priority - radar tracking)
  static FocusBehavior focusBehavior;
  behaviorManager.addBehavior(BEHAVIOR_FOCUS, &focusBehavior);
  
  // 2. Viseme behavior (medium priority - mouth animation for speech)
  behaviorManager.addBehavior(BEHAVIOR_VISEME, &visemeBehavior);
  
  // 3. Idle behavior (lowest priority - perlin noise for all motors)
  static IdleBehavior idleBehavior;
  behaviorManager.addBehavior(BEHAVIOR_IDLE, &idleBehavior);
  
  Serial.println("[HansonServo] Behaviors initialized (focus > viseme > idle)");

  // Check if config file exists before loading
  bool configExisted = FFat.exists("/robot_config.bin");
  
  // Load full config with behaviors and visemes (will restore their state)
  // This must happen BEFORE setting defaults, so saved states aren't overwritten
  bool configLoaded = config.loadOrCreateDefault("/robot_config.bin", &behaviorManager, &visemeBehavior);
  
  if (configLoaded) {
    Serial.println("[HansonServo] Config loaded: " + config.deviceName);
    
    // If config file existed before, behavior states should have been loaded
    // If it's a new file, we need to set defaults
    if (!configExisted) {
      Serial.println("[HansonServo] New config file, setting default behavior states...");
      behaviorManager.setBehaviorEnabled(BEHAVIOR_FOCUS, true);
      behaviorManager.setBehaviorEnabled(BEHAVIOR_VISEME, true);
      behaviorManager.setBehaviorEnabled(BEHAVIOR_IDLE, true);
      // Save the defaults
      config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
    } else {
      Serial.println("[HansonServo] Behavior states loaded from config");
    }
    
    // Check if visemes were loaded from config
    if (visemeBehavior.getVisemes().empty()) {
      Serial.println("[HansonServo] No visemes in config, creating defaults...");
      // Only create defaults if config had no visemes
      visemeBehavior.addViseme(0, "SIL", 2047, 2047, 2047);  // Neutral/rest (sil)
      visemeBehavior.addViseme(1, "AA ", 2200, 1900, 2100);  // AA (as in "father")
      visemeBehavior.addViseme(2, "AE ", 2100, 2000, 2000);  // AE (as in "cat")
      visemeBehavior.addViseme(3, "AH ", 2150, 1950, 2050);  // AH (as in "but")
      visemeBehavior.addViseme(4, "AO ", 2000, 2100, 1950);  // AO (as in "bought")
      visemeBehavior.addViseme(5, "EH ", 1900, 2200, 1900);  // EH (as in "bed")
      visemeBehavior.addViseme(6, "IH ", 1850, 2250, 1850);  // IH (as in "bit")
      visemeBehavior.addViseme(7, "IY ", 1800, 2300, 1800);  // IY (as in "beat")
      visemeBehavior.addViseme(8, "OW ", 2300, 1800, 2200);  // OW (as in "boat")
      visemeBehavior.addViseme(9, "UH ", 2250, 1850, 2150);  // UH (as in "book")
      visemeBehavior.addViseme(10, "UW ", 2200, 1900, 2100); // UW (as in "boot")
      // Save the defaults
      config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
    } else {
      Serial.println("[HansonServo] Visemes loaded from config: " + String(visemeBehavior.getVisemes().size()));
    }
  } else {
    Serial.println("[HansonServo] Config init failed");
    // Set defaults on failure
    behaviorManager.setBehaviorEnabled(BEHAVIOR_FOCUS, true);
    behaviorManager.setBehaviorEnabled(BEHAVIOR_VISEME, true);
    behaviorManager.setBehaviorEnabled(BEHAVIOR_IDLE, true);
  }
  
  // Initialize idle behavior motors (needs config.motors to be loaded)
  std::vector<uint8_t> allMotorIDs;
  for (const Motor& motor : config.motors) {
    allMotorIDs.push_back(motor.motorID);
  }
  idleBehavior.initMotors(allMotorIDs);

  motorStream.start();
  
  // Initialize motor-aided movement
  // Motor 25 - default settings
  motorAid.addMotor(25);
  
  // Motors 30, 31, 35, 36 - high gear ratio, maximum sensitivity
  AidedMotorSettings highGearSettings;
  highGearSettings.velocityThreshold = 8;    // Extremely sensitive trigger
  highGearSettings.resetThreshold = 4;       // Minimal hysteresis
  highGearSettings.leadOffset = 100;         // Smaller lead for fine control
  highGearSettings.assistSpeed = 600;        // Moderate speed
  highGearSettings.assistDuration = 400;     // Shorter burst
  
  motorAid.addMotor(30, highGearSettings);
  motorAid.addMotor(31, highGearSettings);
  motorAid.addMotor(35, highGearSettings);
  motorAid.addMotor(36, highGearSettings);

  Serial.println("[HansonServo] Ready");
  Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16");

  // ---- TEST: Load and play animation ----
  // Serial.println("[TEST] Loading /slow.anim...");
  // if (animState.animation.loadFromFile("/slow.anim")) {
  //   Serial.println("[TEST] Animation loaded successfully");
    
  //   delay(1000);  // Wait 1 second
    
  //   // Print animation info
  //   Serial.println(animState.animation.printAnim());
    
  //   delay(5000);  // Wait 5 seconds
    
  //   // Play the animation
  //   Serial.println("[TEST] Playing animation...");
  //   animState.play(PLAY_ONCE, 1, 0);
  // } else {
  //   Serial.println("[TEST] Failed to load /animation.anim");
  // }
  // ---- END TEST ----
}

// ============================================================================
// Main Loop
// ============================================================================

void loop() {
// Serial passthrough (when enabled)
#if ENABLE_SERIAL_PASSTHROUGH
  handleSerialPassthrough();
    return;
#endif

  // Protocol handling
  handleProtocol();

  // Update behaviors
  behaviorManager.update();

  // Animation playback (auto-selects v1 node or v2 frame based on version)
  runAnimation();
  
  // If no animation is active, still apply behavior motor commands
  if (!animState.current || !animState.current->isActive()) {
    applyBehaviorMotorCommands();
  }

  // Motor position updates
  updateMotorPositions();
  handleMotorStreaming();
  
  // Motor-aided movement (when motorStream is active)
  if (motorStream.active) {
    motorAid.update();
  }
  //printMotorStats();
  // Sensor updates and streaming
  //sensors.update();

  // Heartbeat
  //sendHeartbeat();
  
  // Debug: print loop rate
  printLoopRate();
  
  // ============================================================================
  // TEST: Uncomment to enable motor 40 sweep test
  // ============================================================================
   //testSweepMotor40();
}