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

#include <FFat.h>
#include "protocol.h"
#include "commands.h"
#include "motorcontrol.h"
#include "animation.h"
#include "nodegraph.h"
#include "RobotConfig.h"
#include "sensors.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);
}

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

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

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

void runNodeAnimation() {
  static uint32_t lastTickTime = 0;
  static uint32_t currentTick = 0;
  static bool wasActive = false;

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

  config.enableAllMotors();

  uint32_t now = millis();
  if (now - lastTickTime < FRAME_INTERVAL_MS) 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;
    std::vector<uint16_t> speeds;

    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);
      config.setMotorEnabled(motorID, true);
      } else {
      // Only disable torque for motors that should be limp
        if (config.setMotorEnabled(motorID, false)) {
        servoManager[0]->disableTorque(motorID);
      }
    }
  }

  // Send all positions in one sync write - motors move together!
  if (!motorIDs.empty()) {
    servoManager.syncWritePositions(
      motorIDs.data(),
      positions.data(),
      speeds.data(),
      motorIDs.size(),
      config,
      0
    );
  }

    currentTick++;

  // Handle animation end
  if (currentTick > animState.current->getFrameCount()) {
    switch (animState.playMode) {
        case PLAY_ONCE:
        animState.stop();
          break;
        case PLAY_LOOP:
        // Continue looping
          break;
        case PLAY_REPEAT:
        if (--animState.repeatsRemaining == 0) {
          animState.stop();
          }
          break;
      default:
          break;
      }
      currentTick = 0;
  }
}

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

void updateMotorPositions() {
  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);
  }
}

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

// ============================================================================
// 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 (imu.isReady()) flags |= 0x01;
  if (animState.current && animState.current->isActive()) flags |= 0x02;
  if (motorStream.active) flags |= 0x04;
  if (sensors.isIMUStreamEnabled()) 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);
}

// ============================================================================
// Debug Test Mode
// ============================================================================

// Set to true to run IMU test on startup
#define DEBUG_IMU_TEST false

void runIMUTest() {
  static unsigned long lastPrint = 0;
  
  if (millis() - lastPrint < 200) return;  // Print every 200ms
  lastPrint = millis();
  
  Serial.println("--- IMU TEST ---");
  
  // Check IMU status
  Serial.print("IMU Ready: ");
  Serial.println(imu.isReady() ? "YES" : "NO");
  
  if (imu.isReady()) {
    Serial.print("  Heading: ");
    Serial.print(imu.getHeading(), 1);
    Serial.print("°  Pitch: ");
    Serial.print(imu.getPitch(), 1);
    Serial.print("°  Roll: ");
    Serial.print(imu.getRoll(), 1);
    Serial.println("°");
    
    // Calculate what the node would output
    float pitch = constrain(imu.getPitch(), -90.0f, 90.0f);
    uint16_t pitchValue = (uint16_t)(((pitch + 90.0f) / 180.0f) * 4095.0f);
    Serial.print("  Pitch as node value: ");
    Serial.println(pitchValue);
  }
  
  // Check if animation is loaded
  Serial.print("Animation Active: ");
  Serial.println((animState.current && animState.current->isActive()) ? "YES" : "NO");
  
  if (animState.current) {
    Serial.print("  Nodes: ");
    Serial.print(animState.current->nodeGraph.nodes.size());
    Serial.print("  Connections: ");
    Serial.println(animState.current->nodeGraph.connections.size());
    
    // Print each node's output
    for (Node* node : animState.current->nodeGraph.nodes) {
      Serial.print("  Node ");
      Serial.print(node->id);
      Serial.print(" (type ");
      Serial.print(node->type);
      Serial.print("): in=");
      Serial.print(node->inputValue);
      Serial.print(" out=");
      Serial.println(node->outputValue);
    }
    
    // Print servo outputs
    auto outputs = animState.current->nodeGraph.getServoOutputs();
    Serial.print("  Servo outputs: ");
    for (const auto& [motorID, value] : outputs) {
      Serial.print("M");
      Serial.print(motorID);
      Serial.print("=");
      Serial.print(value);
    Serial.print(" ");
  }
  Serial.println();
}

  Serial.println();
}

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

void setup() {
  // Serial setup (buffer size must be set before begin)
  Serial.setRxBufferSize(8192);
  Serial.begin(1000000);
  
  // 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");

  // Load or create robot config
  if (config.loadOrCreateDefault()) {
    Serial.println("[HansonServo] Config loaded: " + config.deviceName);
  } else {
    Serial.println("[HansonServo] Config init failed");
  }

  // DEBUG: Load and play IMU test animation
  #if DEBUG_IMU_TEST
  Serial.println("[DEBUG] Loading /imutest.anim...");
  if (animState.animation.loadFromFile("/imutest.anim")) {
    Serial.println("[DEBUG] Animation loaded successfully!");
    Serial.print("[DEBUG] Nodes: ");
    Serial.println(animState.animation.nodeGraph.nodes.size());
    Serial.print("[DEBUG] Connections: ");
    Serial.println(animState.animation.nodeGraph.connections.size());
    
    // Print node details
    for (Node* node : animState.animation.nodeGraph.nodes) {
      Serial.print("[DEBUG] Node ");
      Serial.print(node->id);
      Serial.print(": type=");
      Serial.println(node->type);
    }
    
    animState.play(PLAY_LOOP);
    Serial.println("[DEBUG] Animation started in LOOP mode");
  } else {
    Serial.println("[DEBUG] Failed to load animation!");
  }
  #endif

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

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

void loop() {
  // Protocol handling
  handleProtocol();
  
  // Animation playback
  runNodeAnimation();
  
  // Motor position updates
  updateMotorPositions();
  handleMotorStreaming();
  
  // Sensor updates and streaming
  sensors.update();
  
  // Heartbeat
  sendHeartbeat();
  
  // DEBUG: Print IMU test info
  #if DEBUG_IMU_TEST
  runIMUTest();
  #endif
}