#include "behaviors.h"
#include <algorithm>

// ============================================================================
// Base Behavior Implementation
// ============================================================================

Behavior::Behavior() {
  controlledMotors.clear();
}

void Behavior::addMotor(uint8_t motorID) {
  // Check if motor already in list
  for (uint8_t id : controlledMotors) {
    if (id == motorID) {
      return;  // Already added
    }
  }
  controlledMotors.push_back(motorID);
}

void Behavior::removeMotor(uint8_t motorID) {
  controlledMotors.erase(
    std::remove(controlledMotors.begin(), controlledMotors.end(), motorID),
    controlledMotors.end()
  );
}

void Behavior::clearMotors() {
  controlledMotors.clear();
}

// ============================================================================
// Focus Behavior Implementation
// ============================================================================

FocusBehavior::FocusBehavior() {
  isActive = false;
  eyePosition = EYE_POSITION_CENTER;
  neckPosition = NECK_POSITION_CENTER;
  targetEyePosition = EYE_POSITION_CENTER;
  targetNeckPosition = NECK_POSITION_CENTER;
  targetDetectedTime = 0;
  neckStartTime = 0;
  neckRotating = false;
  
  // Add motors 14, 15, and 27 to controlled list
  addMotor(FOCUS_MOTOR_1);
  addMotor(FOCUS_MOTOR_2);
  addMotor(NECK_MOTOR);
}

bool FocusBehavior::update() {
  // Check radar for valid targets
  uint8_t targetCount = radar.getTargetCount();
  unsigned long now = millis();
  
  if (targetCount == 0 || !radar.getTarget(0).valid) {
    // No target - return to center
    isActive = false;
    targetEyePosition = EYE_POSITION_CENTER;
    targetNeckPosition = NECK_POSITION_CENTER;
    targetDetectedTime = 0;
    neckRotating = false;
    
    // Smoothly interpolate to center
    eyePosition = lerp(eyePosition, EYE_POSITION_CENTER, INTERPOLATION_SPEED);
    neckPosition = lerp(neckPosition, NECK_POSITION_CENTER, INTERPOLATION_SPEED);
    
    return false;
  }

  // Get first valid target
  const RadarTarget& target = radar.getTarget(0);
  
  if (!target.valid) {
    isActive = false;
    targetEyePosition = EYE_POSITION_CENTER;
    targetNeckPosition = NECK_POSITION_CENTER;
    targetDetectedTime = 0;
    neckRotating = false;
    return false;
  }

  // Active tracking - calculate target positions from radar angle
  isActive = true;
  uint16_t targetEyePos = calculateEyePositionFromRadarAngle(target.angle);
  uint16_t targetNeckPos = calculateNeckPositionFromRadarAngle(target.angle);
  
  // Eyes track immediately
  targetEyePosition = targetEyePos;
  
  // Check if this is a new target (angle changed significantly or first detection)
  bool newTarget = (targetDetectedTime == 0) || 
                   (abs((int16_t)targetEyePosition - (int16_t)eyePosition) > 50);
  
  if (newTarget) {
    // Reset timing for new target
    targetDetectedTime = now;
    neckStartTime = now + NECK_DELAY_MS;
    neckRotating = false;
  }
  
  // Check if neck should start rotating
  if (!neckRotating && now >= neckStartTime && targetDetectedTime > 0) {
    neckRotating = true;
    targetNeckPosition = targetNeckPos;
  }
  
  // If neck is rotating, gradually center the eyes as neck approaches target
  if (neckRotating) {
    // Neck tracks the target
    targetNeckPosition = targetNeckPos;
    
    // Calculate how far the neck has moved toward its target (0 = just started, 1 = at target)
    int16_t neckDistanceToTarget = abs((int16_t)targetNeckPosition - (int16_t)neckPosition);
    int16_t totalNeckRange = NECK_POSITION_MAX - NECK_POSITION_MIN;
    float neckProgress = 1.0f - ((float)neckDistanceToTarget / (float)totalNeckRange);
    neckProgress = (neckProgress < 0.0f) ? 0.0f : (neckProgress > 1.0f) ? 1.0f : neckProgress;
    
    // As neck gets closer to target, eyes gradually center
    // Interpolate eye target between current target position and center based on neck progress
    float eyeTargetBlend = 1.0f - neckProgress;  // 1.0 = at target angle, 0.0 = centered
    int16_t eyeTargetOffset = (int16_t)targetEyePos - (int16_t)EYE_POSITION_CENTER;
    targetEyePosition = EYE_POSITION_CENTER + (uint16_t)(eyeTargetOffset * eyeTargetBlend);
  }
  
  // Smoothly interpolate current positions toward targets
  // Use different speeds for eyes and neck (neck is slower/smoother)
  eyePosition = lerp(eyePosition, targetEyePosition, INTERPOLATION_SPEED);
  neckPosition = lerp(neckPosition, targetNeckPosition, NECK_INTERPOLATION_SPEED);
  
  return true;
}

bool FocusBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
  // Provide position for eyes (motors 14 and 15)
  if (motorID == FOCUS_MOTOR_1 || motorID == FOCUS_MOTOR_2) {
    position = eyePosition;
    return true;
  }
  
  // Provide position for neck (motor 27)
  if (motorID == NECK_MOTOR) {
    position = neckPosition;
    return true;
  }
  
  return false;
}

uint16_t FocusBehavior::calculateEyePositionFromRadarAngle(float radarAngle) {
  // Calculate eye motor position from radar angle (in degrees)
  // Angle range: approximately -45 to +45 degrees (typical radar FOV)
  // Map to eye motor position range: 1700 to 2300 (center 2047)
  
  // Clamp angle to reasonable range (can extend later if needed)
  constexpr float ANGLE_MIN = -45.0f;
  constexpr float ANGLE_MAX = 45.0f;
  
  if (radarAngle < ANGLE_MIN) radarAngle = ANGLE_MIN;
  if (radarAngle > ANGLE_MAX) radarAngle = ANGLE_MAX;
  
  // Normalize angle to -1.0 to 1.0 range
  float normalizedAngle = radarAngle / ANGLE_MAX;
  
  // Calculate range from center
  float rangeLeft = EYE_POSITION_CENTER - EYE_POSITION_MIN;   // 347
  float rangeRight = EYE_POSITION_MAX - EYE_POSITION_CENTER;  // 253
  
  uint16_t position;
  if (normalizedAngle < 0.0f) {
    // Left side: use left range
    position = EYE_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeLeft);
  } else {
    // Right side: use right range
    position = EYE_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeRight);
  }

  // Clamp to valid range
  if (position < EYE_POSITION_MIN) position = EYE_POSITION_MIN;
  if (position > EYE_POSITION_MAX) position = EYE_POSITION_MAX;

  return position;
}

uint16_t FocusBehavior::calculateNeckPositionFromRadarAngle(float radarAngle) {
  // Calculate neck motor position from radar angle (in degrees)
  // Angle range: approximately -45 to +45 degrees (typical radar FOV)
  // Map to neck motor position range: 1000 to 3000 (center 2000)
  // NOTE: Rotation is inverted for neck motor
  
  // Clamp angle to reasonable range (can extend later if needed)
  constexpr float ANGLE_MIN = -45.0f;
  constexpr float ANGLE_MAX = 45.0f;
  
  if (radarAngle < ANGLE_MIN) radarAngle = ANGLE_MIN;
  if (radarAngle > ANGLE_MAX) radarAngle = ANGLE_MAX;
  
  // Normalize angle to -1.0 to 1.0 range, then invert for neck motor
  float normalizedAngle = -(radarAngle / ANGLE_MAX);
  
  // Calculate range from center
  float rangeLeft = NECK_POSITION_CENTER - NECK_POSITION_MIN;   // 1000
  float rangeRight = NECK_POSITION_MAX - NECK_POSITION_CENTER;  // 1000
  
  uint16_t position;
  if (normalizedAngle < 0.0f) {
    // Left side: use left range
    position = NECK_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeLeft);
  } else {
    // Right side: use right range
    position = NECK_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeRight);
  }

  // Clamp to valid range
  if (position < NECK_POSITION_MIN) position = NECK_POSITION_MIN;
  if (position > NECK_POSITION_MAX) position = NECK_POSITION_MAX;

  return position;
}

uint16_t FocusBehavior::lerp(uint16_t current, uint16_t target, float t) {
  // Linear interpolation with clamping to prevent overshoot
  int16_t diff = (int16_t)target - (int16_t)current;
  int16_t delta = (int16_t)(diff * t);
  
  // If difference is very small, snap to target
  if (abs(diff) < 2) {
    return target;
  }
  
  return (uint16_t)(current + delta);
}

// ============================================================================
// Behavior Manager Implementation
// ============================================================================

BehaviorManager behaviorManager;

BehaviorManager::BehaviorManager() {
  behaviors.clear();
  // Initialize all enabled states to false
  for (int i = 0; i < 256; i++) {
    enabledStates[i] = false;
  }
}

void BehaviorManager::addBehavior(BehaviorID id, Behavior* behavior) {
  if (behavior == nullptr) return;
  
  // Check if already added
  for (const auto& entry : behaviors) {
    if (entry.behavior == behavior || entry.id == id) return;
  }
  
  behaviors.push_back({id, behavior});
  // New behaviors are enabled by default
  enabledStates[id] = true;
}

void BehaviorManager::removeBehavior(Behavior* behavior) {
  behaviors.erase(
    std::remove_if(behaviors.begin(), behaviors.end(),
                   [behavior](const BehaviorEntry& entry) {
                     return entry.behavior == behavior;
                   }),
    behaviors.end()
  );
}

void BehaviorManager::setBehaviorEnabled(BehaviorID id, bool enabled) {
  enabledStates[id] = enabled;
}

bool BehaviorManager::isBehaviorEnabled(BehaviorID id) const {
  return enabledStates[id];
}

uint8_t BehaviorManager::getBehaviorCount() const {
  return behaviors.size();
}

bool BehaviorManager::getBehaviorInfo(uint8_t index, BehaviorID& id, bool& enabled) const {
  if (index >= behaviors.size()) {
    return false;
  }
  
  id = behaviors[index].id;
  enabled = enabledStates[id];
  return true;
}

void BehaviorManager::update() {
  // Update all enabled behaviors
  for (const auto& entry : behaviors) {
    if (entry.behavior && enabledStates[entry.id]) {
      entry.behavior->update();
    }
  }
}

bool BehaviorManager::getMotorPosition(uint8_t motorID, uint16_t& position) {
  // Check all enabled behaviors to see if any wants to control this motor
  for (const auto& entry : behaviors) {
    if (entry.behavior && enabledStates[entry.id] && 
        entry.behavior->getMotorPosition(motorID, position)) {
      return true;  // Found an enabled behavior controlling this motor
    }
  }
  return false;  // No enabled behavior controlling this motor
}