HansonServo/behaviors.cpp

#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;
  }

  // Always provide position for controlled motors (active by default)
  // When not tracking a target, currentPosition is set to center in update()
  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();
  // 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
}