#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 eyes to center
    eyePosition = lerp(eyePosition, EYE_POSITION_CENTER, INTERPOLATION_SPEED);
    
    // Keep neck at center (no movement)
    neckPosition = NECK_POSITION_CENTER;
    
    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);
  
  // Eyes track immediately
  targetEyePosition = targetEyePos;
  
  // Neck disabled for now - keep it centered
  targetNeckPosition = NECK_POSITION_CENTER;
  neckRotating = false;
  
  // Smoothly interpolate eye position toward target
  eyePosition = lerp(eyePosition, targetEyePosition, INTERPOLATION_SPEED);
  
  // Keep neck at center (no movement)
  neckPosition = NECK_POSITION_CENTER;
  
  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: -50 to +50 degrees, mapped to full eye range (1700-2500, center 2200)
  
  constexpr float ANGLE_MIN = -50.0f;
  constexpr float ANGLE_MAX = 50.0f;
  
  // Clamp angle to -50 to +50 range
  if (radarAngle < ANGLE_MIN) radarAngle = ANGLE_MIN;
  if (radarAngle > ANGLE_MAX) radarAngle = ANGLE_MAX;
  
  // Normalize angle to -1.0 to 1.0 range
  float normalized = radarAngle / 50.0f;
  
  // Calculate range from center in each direction
  // Left range: 2200 - 1700 = 500, Right range: 2500 - 2200 = 300
  float rangeLeft = (float)(EYE_POSITION_CENTER - EYE_POSITION_MIN);   // 500
  float rangeRight = (float)(EYE_POSITION_MAX - EYE_POSITION_CENTER);  // 300
  
  // Use different ranges for left (negative) and right (positive) to use full range
  float positionFloat;
  if (normalized < 0.0f) {
    // Negative angle: use left range (500 units)
    positionFloat = (float)EYE_POSITION_CENTER + (normalized * rangeLeft);
  } else {
    // Positive angle: use right range (300 units)
    positionFloat = (float)EYE_POSITION_CENTER + (normalized * rangeRight);
  }
  
  // Convert to int16_t first to handle negative values, then clamp
  int16_t position = (int16_t)positionFloat;

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

  return (uint16_t)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);
}

// ============================================================================
// Idle Behavior Implementation
// ============================================================================

IdleBehavior::IdleBehavior() {
  startTime = millis();
  
  // Initialize all motor positions to center
  for (int i = 0; i < 256; i++) {
    motorPositions[i] = POSITION_CENTER;
  }
}

void IdleBehavior::initMotors(const std::vector<uint8_t>& motorIDs) {
  clearMotors();
  for (uint8_t id : motorIDs) {
    addMotor(id);
    motorPositions[id] = POSITION_CENTER;
  }
}

bool IdleBehavior::update() {
  unsigned long now = millis();
  float timeOffset = (float)(now - startTime) * NOISE_SPEED;
  
  // Update position for each controlled motor using perlin noise
  for (uint8_t motorID : controlledMotors) {
    // Use motor ID as seed offset for variety between motors
    uint16_t seed = motorID * MOTOR_SEED_OFFSET;
    
    // Get perlin noise value (-1 to 1 range approximately)
    float noiseValue = perlin1D_octave(seed, timeOffset, 3, 0.5f);
    
    // Map noise to position range: center ± NOISE_RANGE
    // Perlin noise typically returns values in roughly -1 to 1 range
    int16_t offset = (int16_t)(noiseValue * (float)NOISE_RANGE);
    
    // Calculate final position
    int16_t position = (int16_t)POSITION_CENTER + offset;
    
    // Clamp to valid servo range
    if (position < 1547) position = 1547;  // center - 500
    if (position > 2547) position = 2547;  // center + 500
    
    motorPositions[motorID] = (uint16_t)position;
  }
  
  // Idle behavior is always active (but low priority)
  return true;
}

bool IdleBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
  // Check if we control this motor
  for (uint8_t id : controlledMotors) {
    if (id == motorID) {
      position = motorPositions[motorID];
      return true;
    }
  }
  return false;
}

// ============================================================================
// 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
}