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

// ============================================================================
// Viseme Behavior Implementation
// ============================================================================

VisemeBehavior::VisemeBehavior() {
  isActive = false;
  lastTriggerTime = 0;
  nextVisemeID = 0;
  currentPositions.clear();
  visemes.clear();
}

Viseme* VisemeBehavior::findViseme(uint8_t id) {
  for (Viseme& v : visemes) {
    if (v.id == id) {
      return &v;
    }
  }
  return nullptr;
}

uint8_t VisemeBehavior::addViseme(const char* label) {
  Viseme newViseme;
  newViseme.id = nextVisemeID++;
  
  // Copy label (3 chars, ensure null-terminated)
  if (label && strlen(label) >= 3) {
    newViseme.label[0] = label[0];
    newViseme.label[1] = label[1];
    newViseme.label[2] = label[2];
    newViseme.label[3] = '\0';
  } else {
    // Default label if not provided or too short
    newViseme.label[0] = 'V';
    newViseme.label[1] = 'I';
    newViseme.label[2] = 'S';
    newViseme.label[3] = '\0';
  }
  
  newViseme.motorPositions.clear();
  visemes.push_back(newViseme);
  
  Serial.print("[Viseme] Added viseme '");
  Serial.print(newViseme.label);
  Serial.print("' with ID ");
  Serial.println(newViseme.id);
  
  return newViseme.id;
}

void VisemeBehavior::addViseme(uint8_t id, uint16_t pos40, uint16_t pos43, uint16_t pos44) {
  // Legacy method for backwards compatibility
  Viseme* existing = findViseme(id);
  
  if (existing) {
    // Update existing viseme
    existing->motorPositions.clear();
    existing->motorPositions.push_back({40, pos40});
    existing->motorPositions.push_back({43, pos43});
    existing->motorPositions.push_back({44, pos44});
  } else {
    // Add new viseme
    Viseme newViseme;
    newViseme.id = id;
    
    // Default label based on ID (V + 2 digit ID)
    newViseme.label[0] = 'V';
    if (id < 10) {
      newViseme.label[1] = '0' + id;
      newViseme.label[2] = ' ';
    } else if (id < 100) {
      newViseme.label[1] = '0' + (id / 10);
      newViseme.label[2] = '0' + (id % 10);
    } else {
      newViseme.label[1] = 'X';
      newViseme.label[2] = 'X';
    }
    newViseme.label[3] = '\0';
    
    newViseme.motorPositions.push_back({40, pos40});
    newViseme.motorPositions.push_back({43, pos43});
    newViseme.motorPositions.push_back({44, pos44});
    visemes.push_back(newViseme);
    
    // Update nextVisemeID if needed
    if (id >= nextVisemeID) {
      nextVisemeID = id + 1;
    }
  }
  
  // Update controlled motors list
  addMotor(40);
  addMotor(43);
  addMotor(44);
}

// Overload to add viseme with explicit label
void VisemeBehavior::addViseme(uint8_t id, const char* label, uint16_t pos40, uint16_t pos43, uint16_t pos44) {
  Viseme* existing = findViseme(id);
  
  if (existing) {
    // Update existing viseme
    if (label) {
      existing->label[0] = label[0];
      existing->label[1] = label[1];
      existing->label[2] = label[2];
      existing->label[3] = '\0';
    }
    existing->motorPositions.clear();
    existing->motorPositions.push_back({40, pos40});
    existing->motorPositions.push_back({43, pos43});
    existing->motorPositions.push_back({44, pos44});
  } else {
    // Add new viseme
    Viseme newViseme;
    newViseme.id = id;
    
    // Set label
    if (label) {
      newViseme.label[0] = label[0];
      newViseme.label[1] = label[1];
      newViseme.label[2] = label[2];
      newViseme.label[3] = '\0';
    } else {
      // Default label
      newViseme.label[0] = 'V';
      if (id < 10) {
        newViseme.label[1] = '0' + id;
        newViseme.label[2] = ' ';
      } else if (id < 100) {
        newViseme.label[1] = '0' + (id / 10);
        newViseme.label[2] = '0' + (id % 10);
      } else {
        newViseme.label[1] = 'X';
        newViseme.label[2] = 'X';
      }
      newViseme.label[3] = '\0';
    }
    
    newViseme.motorPositions.push_back({40, pos40});
    newViseme.motorPositions.push_back({43, pos43});
    newViseme.motorPositions.push_back({44, pos44});
    visemes.push_back(newViseme);
    
    // Update nextVisemeID if needed
    if (id >= nextVisemeID) {
      nextVisemeID = id + 1;
    }
  }
  
  // Update controlled motors list
  addMotor(40);
  addMotor(43);
  addMotor(44);
}

bool VisemeBehavior::deleteViseme(uint8_t visemeID) {
  for (auto it = visemes.begin(); it != visemes.end(); ++it) {
    if (it->id == visemeID) {
      Serial.print("[Viseme] Deleted viseme ID ");
      Serial.println(visemeID);
      visemes.erase(it);
      return true;
    }
  }
  
  Serial.print("[Viseme] Delete failed - unknown viseme ID ");
  Serial.println(visemeID);
  return false;
}

bool VisemeBehavior::setVisemeMotors(uint8_t visemeID, const std::vector<VisemeMotorPosition>& positions) {
  Viseme* viseme = findViseme(visemeID);
  if (!viseme) {
    Serial.print("[Viseme] setVisemeMotors failed - unknown viseme ID ");
    Serial.println(visemeID);
    return false;
  }
  
  // Update motor positions
  viseme->motorPositions = positions;
  
  // Update controlled motors list
  for (const auto& pos : positions) {
    addMotor(pos.motorID);
  }
  
  Serial.print("[Viseme] Updated viseme ID ");
  Serial.print(visemeID);
  Serial.print(" with ");
  Serial.print(positions.size());
  Serial.println(" motors");
  
  return true;
}

bool VisemeBehavior::setVisemeMotorsAndLabel(uint8_t visemeID, const char* label, const std::vector<VisemeMotorPosition>& positions) {
  Viseme* viseme = findViseme(visemeID);
  if (!viseme) {
    Serial.print("[Viseme] setVisemeMotorsAndLabel failed - unknown viseme ID ");
    Serial.println(visemeID);
    return false;
  }
  
  // Update label (3 bytes)
  if (label) {
    viseme->label[0] = label[0];
    viseme->label[1] = label[1];
    viseme->label[2] = label[2];
    viseme->label[3] = '\0';
  }
  
  // Update motor positions
  viseme->motorPositions = positions;
  
  // Update controlled motors list
  for (const auto& pos : positions) {
    addMotor(pos.motorID);
  }
  
  Serial.print("[Viseme] Updated viseme ID ");
  Serial.print(visemeID);
  Serial.print(" label '");
  Serial.print(viseme->label);
  Serial.print("' with ");
  Serial.print(positions.size());
  Serial.println(" motors");
  
  return true;
}

bool VisemeBehavior::triggerViseme(uint8_t visemeID) {
  Viseme* viseme = findViseme(visemeID);
  if (!viseme) {
    Serial.print("[Viseme] Unknown viseme ID ");
    Serial.println(visemeID);
    return false;
  }
  
  // Copy positions for this viseme
  currentPositions = viseme->motorPositions;
  
  // Activate and reset timer
  isActive = true;
  lastTriggerTime = millis();
  
  Serial.print("[Viseme] Triggered '");
  Serial.print(viseme->label);
  Serial.print("' (ID ");
  Serial.print(visemeID);
  Serial.println(")");
  
  return true;
}

bool VisemeBehavior::update() {
  if (!isActive) {
    return false;
  }
  
  // Check for timeout
  unsigned long now = millis();
  if (now - lastTriggerTime >= TIMEOUT_MS) {
    // Timeout reached - deactivate
    isActive = false;
    currentPositions.clear();
    Serial.println("[Viseme] Timeout - deactivated");
    return false;
  }
  
  return true;
}

bool VisemeBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
  if (!isActive) {
    return false;
  }
  
  // Look up motor in current positions
  for (const auto& pos : currentPositions) {
    if (pos.motorID == motorID) {
      position = pos.position;
      return true;
    }
  }
  
  return false;
}

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

BehaviorManager behaviorManager;
VisemeBehavior visemeBehavior;

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
}