#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;
  currentPosition = POSITION_CENTER;
  
  // Add motors 14 and 15 to controlled list
  addMotor(FOCUS_MOTOR_1);
  addMotor(FOCUS_MOTOR_2);
}

bool FocusBehavior::update() {
  // Check radar for valid targets
  uint8_t targetCount = radar.getTargetCount();
  
  if (targetCount == 0) {
    // No target - return to center
    isActive = false;
    currentPosition = POSITION_CENTER;
    return false;
  }

  // Get first valid target
  const RadarTarget& target = radar.getTarget(0);
  
  if (!target.valid) {
    isActive = false;
    currentPosition = POSITION_CENTER;
    return false;
  }

  // Active tracking - calculate position from radar x coordinate
  isActive = true;
  currentPosition = calculatePositionFromRadarX(target.x);
  
  return true;
}

bool FocusBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
  // Only provide position for motors we control
  if (motorID != FOCUS_MOTOR_1 && motorID != FOCUS_MOTOR_2) {
    return false;
  }

  // Only provide position if behavior is active
  if (!isActive) {
    return false;
  }

  position = currentPosition;
  return true;
}

uint16_t FocusBehavior::calculatePositionFromRadarX(float radarX) {
  // Clamp radar x to valid range
  if (radarX < RADAR_X_MIN) radarX = RADAR_X_MIN;
  if (radarX > RADAR_X_MAX) radarX = RADAR_X_MAX;

  // Linear mapping from radar x (-80 to 80) to motor position (1700 to 2300)
  // Center (2047) corresponds to radar x = 0
  // Left (-80) maps to 1700, Right (80) maps to 2300
  
  // Normalize radar x to -1.0 to 1.0 range
  float normalizedX = radarX / RADAR_X_MAX;
  
  // Calculate range from center
  // Left side: center - 347 = 1700
  // Right side: center + 253 = 2300
  float rangeLeft = POSITION_CENTER - POSITION_MIN;   // 347
  float rangeRight = POSITION_MAX - POSITION_CENTER;  // 253
  
  uint16_t position;
  if (normalizedX < 0.0f) {
    // Left side: use left range
    position = POSITION_CENTER + (uint16_t)(normalizedX * rangeLeft);
  } else {
    // Right side: use right range
    position = POSITION_CENTER + (uint16_t)(normalizedX * rangeRight);
  }

  // Clamp to valid range
  if (position < POSITION_MIN) position = POSITION_MIN;
  if (position > POSITION_MAX) position = POSITION_MAX;

  return position;
}

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

BehaviorManager behaviorManager;

BehaviorManager::BehaviorManager() {
  behaviors.clear();
}

void BehaviorManager::addBehavior(Behavior* behavior) {
  if (behavior == nullptr) return;
  
  // Check if already added
  for (Behavior* b : behaviors) {
    if (b == behavior) return;
  }
  
  behaviors.push_back(behavior);
}

void BehaviorManager::removeBehavior(Behavior* behavior) {
  behaviors.erase(
    std::remove(behaviors.begin(), behaviors.end(), behavior),
    behaviors.end()
  );
}

void BehaviorManager::update() {
  // Update all behaviors
  for (Behavior* behavior : behaviors) {
    if (behavior) {
      behavior->update();
    }
  }
}

bool BehaviorManager::getMotorPosition(uint8_t motorID, uint16_t& position) {
  // Check all behaviors to see if any wants to control this motor
  for (Behavior* behavior : behaviors) {
    if (behavior && behavior->getMotorPosition(motorID, position)) {
      return true;  // Found a behavior controlling this motor
    }
  }
  return false;  // No behavior controlling this motor
}