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 = settings.eyeCenter;
  neckPosition = settings.neckCenter;
  neckNormalized = 0.0f;
  faceDetectedTime = 0;
  faceWasPresent = false;
  
  // Add motors to controlled list
  addMotor(settings.eyeMotor1);
  addMotor(settings.eyeMotor2);
  addMotor(settings.neckMotor);
}

bool FocusBehavior::update() {
  uint8_t faceCount = faceDetect.getFaceCount();
  unsigned long now = millis();
  
  // ---- No face detected ----
  if (faceCount == 0 || !faceDetect.getFace(0).valid) {
    isActive = false;
    faceWasPresent = false;
    
    // Smoothly return eyes and neck to center
    eyePosition = lerp(eyePosition, settings.eyeCenter, settings.eyeCenteringSpeed);
    neckNormalized = lerpf(neckNormalized, 0.0f, settings.neckCenteringSpeed);
    neckPosition = normalizedToServo(
      settings.neckInvert ? -neckNormalized : neckNormalized,
      settings.neckCenter, settings.neckMin, settings.neckMax
    );
    
    return false;
  }

  // ---- Face detected ----
  isActive = true;
  const DetectedFace& face = faceDetect.getFace(0);

  // Track when we first saw a face (for neck delay)
  if (!faceWasPresent) {
    faceDetectedTime = now;
    faceWasPresent = true;
  }

  // Normalize face x to -1..+1
  float faceNorm = (float)face.x / settings.faceXMax;
  if (faceNorm < -1.0f) faceNorm = -1.0f;
  if (faceNorm >  1.0f) faceNorm =  1.0f;

  // ---- Neck: smoothly follow the target after a delay ----
  float neckTarget = faceNorm * settings.neckContribution;
  
  if (now - faceDetectedTime >= settings.neckDelayMs) {
    // Neck is allowed to move - smoothly interpolate toward target
    neckNormalized = lerpf(neckNormalized, neckTarget, settings.neckSpeed);
  }
  // else: neck stays where it is during the delay period

  // Convert neck normalized position to servo units
  neckPosition = normalizedToServo(
    settings.neckInvert ? -neckNormalized : neckNormalized,
    settings.neckCenter, settings.neckMin, settings.neckMax
  );

  // ---- Eyes: dart to the remainder that the neck hasn't covered ----
  // The eyes compensate for whatever offset the neck hasn't reached yet
  // As the neck catches up, this remainder shrinks toward 0 (eyes center)
  float eyeNorm = faceNorm - neckNormalized;
  
  // Clamp eye normalized to -1..+1
  if (eyeNorm < -1.0f) eyeNorm = -1.0f;
  if (eyeNorm >  1.0f) eyeNorm =  1.0f;

  // Convert to servo position and interpolate quickly
  uint16_t eyeTarget = normalizedToServo(eyeNorm, settings.eyeCenter, settings.eyeMin, settings.eyeMax);
  eyePosition = lerp(eyePosition, eyeTarget, settings.eyeSpeed);

  return true;
}

bool FocusBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
  if (motorID == settings.eyeMotor1 || motorID == settings.eyeMotor2) {
    position = eyePosition;
    return true;
  }
  if (motorID == settings.neckMotor) {
    position = neckPosition;
    return true;
  }
  return false;
}

uint16_t FocusBehavior::normalizedToServo(float n, uint16_t center, uint16_t min, uint16_t max) const {
  // Map a normalized value (-1..+1) to servo range, handling asymmetric ranges
  float rangeNeg = (float)(center - min);
  float rangePos = (float)(max - center);

  float posFloat;
  if (n < 0.0f) {
    posFloat = (float)center + (n * rangeNeg);
  } else {
    posFloat = (float)center + (n * rangePos);
  }

  int16_t pos = (int16_t)posFloat;
  if (pos < (int16_t)min) pos = (int16_t)min;
  if (pos > (int16_t)max) pos = (int16_t)max;
  return (uint16_t)pos;
}

float FocusBehavior::lerpf(float current, float target, float t) {
  float diff = target - current;
  if (fabs(diff) < 0.001f) return target;
  return current + diff * t;
}

uint16_t FocusBehavior::lerp(uint16_t current, uint16_t target, float t) {
  int16_t diff = (int16_t)target - (int16_t)current;
  if (abs(diff) < 2) return target;
  return (uint16_t)((int16_t)current + (int16_t)(diff * t));
}

// ============================================================================
// 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::createOrUpdateViseme(uint8_t visemeID, const char* label, const std::vector<VisemeMotorPosition>& positions) {
  Viseme* viseme = findViseme(visemeID);
  
  if (viseme) {
    // Update existing
    return setVisemeMotorsAndLabel(visemeID, label, positions);
  } else {
    // Create new
    Viseme newViseme;
    newViseme.id = visemeID;
    
    // 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 (visemeID < 10) {
        newViseme.label[1] = '0' + visemeID;
        newViseme.label[2] = ' ';
      } else if (visemeID < 100) {
        newViseme.label[1] = '0' + (visemeID / 10);
        newViseme.label[2] = '0' + (visemeID % 10);
      } else {
        newViseme.label[1] = 'X';
        newViseme.label[2] = 'X';
      }
      newViseme.label[3] = '\0';
    }
    
    // Set motor positions
    newViseme.motorPositions = positions;
    
    visemes.push_back(newViseme);
    
    // Update controlled motors list
    for (const auto& pos : positions) {
      addMotor(pos.motorID);
    }
    
    // Update nextVisemeID if needed
    if (visemeID >= nextVisemeID) {
      nextVisemeID = visemeID + 1;
    }
    
    Serial.print("[Viseme] Created viseme ID ");
    Serial.print(visemeID);
    Serial.print(" label '");
    Serial.print(newViseme.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;
FocusBehavior focusBehavior;
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
}