HansonServo/behaviors.h

#pragma once
#include <Arduino.h>
#include <vector>
#include "sensors.h"
#include "robotconfig.h"
#include "noise.h"

// ============================================================================
// Behavior IDs
// ============================================================================

enum BehaviorID : uint8_t {
  BEHAVIOR_FOCUS = 1,  // Focus behavior (face tracking)
  BEHAVIOR_IDLE = 2,   // Idle behavior (perlin noise for all motors)
  BEHAVIOR_VISEME = 3, // Viseme behavior (mouth motor positions)
};

// ============================================================================
// Base Behavior Class
// ============================================================================

class Behavior {
public:
  Behavior();
  virtual ~Behavior() = default;

  // Get list of motor IDs this behavior controls
  const std::vector<uint8_t>& getControlledMotors() const { return controlledMotors; }

  // Add a motor to the controlled list
  void addMotor(uint8_t motorID);

  // Remove a motor from the controlled list
  void removeMotor(uint8_t motorID);

  // Clear all controlled motors
  void clearMotors();

  // Virtual method to update the behavior (called each frame)
  // Returns true if the behavior is active and wants to control motors
  virtual bool update() = 0;

  // Virtual method to get the desired position for a motor
  // Returns true if this behavior wants to control this motor, false otherwise
  virtual bool getMotorPosition(uint8_t motorID, uint16_t& position) = 0;

protected:
  std::vector<uint8_t> controlledMotors;
};

// ============================================================================
// Focus Behavior - Tracks faces with eyes/neck via FaceDetect sensor
// ============================================================================

// Setting IDs for SSET protocol command
// Float values are transmitted as fixed-point × 1000 (e.g., 0.15 → 150)
// Signed values are transmitted as int16 reinterpreted as uint16
namespace SettingID {
  // Focus: Motor IDs
  constexpr uint16_t FOCUS_EYE_MOTOR_1       = 0x0500;
  constexpr uint16_t FOCUS_EYE_MOTOR_2       = 0x0501;
  constexpr uint16_t FOCUS_NECK_MOTOR        = 0x0502;
  // Focus: Eye servo range
  constexpr uint16_t FOCUS_EYE_CENTER        = 0x0503;
  constexpr uint16_t FOCUS_EYE_MIN           = 0x0504;
  constexpr uint16_t FOCUS_EYE_MAX           = 0x0505;
  // Focus: Neck servo range
  constexpr uint16_t FOCUS_NECK_CENTER       = 0x0506;
  constexpr uint16_t FOCUS_NECK_MIN          = 0x0507;
  constexpr uint16_t FOCUS_NECK_MAX          = 0x0508;
  // Focus: Face x range (int16, signed)
  constexpr uint16_t FOCUS_FACE_X_MIN        = 0x0509;
  constexpr uint16_t FOCUS_FACE_X_MAX        = 0x050A;
  // Focus: Interpolation speeds (float × 1000)
  constexpr uint16_t FOCUS_EYE_SPEED         = 0x050B;
  constexpr uint16_t FOCUS_NECK_SPEED        = 0x050C;
  constexpr uint16_t FOCUS_EYE_RETURN_SPEED  = 0x050D;
  // Focus: Neck delay (ms, uint16)
  constexpr uint16_t FOCUS_NECK_DELAY_MS     = 0x050E;
  // Focus: Neck contribution (float × 1000)
  constexpr uint16_t FOCUS_NECK_CONTRIBUTION = 0x050F;
  // Focus: Neck invert (0 or 1)
  constexpr uint16_t FOCUS_NECK_INVERT       = 0x0510;
  // Focus: Centering speeds (float × 1000)
  constexpr uint16_t FOCUS_EYE_CENTERING     = 0x0511;
  constexpr uint16_t FOCUS_NECK_CENTERING    = 0x0512;
  
  constexpr uint16_t FOCUS_FIRST = 0x0500;
  constexpr uint16_t FOCUS_LAST  = 0x0512;
  constexpr uint16_t FOCUS_COUNT = FOCUS_LAST - FOCUS_FIRST + 1;
}

// Tuneable settings - all values exposed for external adjustment
struct FocusSettings {
  // Motor IDs
  uint8_t eyeMotor1 = 14;
  uint8_t eyeMotor2 = 15;
  uint8_t neckMotor = 27;

  // Eye motor position range
  uint16_t eyeCenter = 2200;
  uint16_t eyeMin    = 1700;
  uint16_t eyeMax    = 2500;

  // Neck motor position range
  uint16_t neckCenter = 2000;
  uint16_t neckMin    = 1000;
  uint16_t neckMax    = 3000;

  // Face detection x range (pixels, center-relative)
  float faceXMin = -140.0f;
  float faceXMax =  140.0f;

  // Interpolation speeds (0-1 per update, higher = faster)
  float eyeSpeed      = 0.15f;   // Eyes dart quickly to target
  float neckSpeed     = 0.02f;   // Neck follows smoothly / slowly
  float eyeReturnSpeed = 0.05f;  // Speed eyes center as neck catches up

  // Neck starts moving after this delay (ms) from first detecting a target
  unsigned long neckDelayMs = 500;

  // How much of the face offset the neck should try to cover (0-1)
  // 1.0 = neck tries to fully face the target, 0.5 = neck covers half
  float neckContribution = 0.7f;

  // Invert neck direction (set true if neck motor is wired backwards)
  bool neckInvert = true;

  // Return-to-center speeds when no face detected
  float eyeCenteringSpeed = 0.03f;
  float neckCenteringSpeed = 0.02f;
};

class FocusBehavior : public Behavior {
public:
  FocusBehavior();

  // Update behavior - check face detection for targets
  bool update() override;

  // Get motor position for a controlled motor
  bool getMotorPosition(uint8_t motorID, uint16_t& position) override;

  // Access settings for external tuning
  FocusSettings& getSettings() { return settings; }
  const FocusSettings& getSettings() const { return settings; }

private:
  FocusSettings settings;

  bool isActive;
  
  // Current smoothed positions (servo units)
  uint16_t eyePosition;
  uint16_t neckPosition;
  
  // Current normalized offset the neck has reached (-1 to +1)
  // This tracks how far the neck has rotated toward the target
  float neckNormalized;

  // Timing
  unsigned long faceDetectedTime;  // When face was first seen (for neck delay)
  bool faceWasPresent;             // Was a face present last frame?

  // Map a normalized value (-1..+1) to an asymmetric servo range
  uint16_t normalizedToServo(float n, uint16_t center, uint16_t min, uint16_t max) const;

  // Smooth interpolation helpers
  static float lerpf(float current, float target, float t);
  static uint16_t lerp(uint16_t current, uint16_t target, float t);
};

// ============================================================================
// Idle Behavior - Adds perlin noise to all motors for natural idle motion
// ============================================================================

class IdleBehavior : public Behavior {
public:
  IdleBehavior();

  // Initialize with list of motor IDs to control
  void initMotors(const std::vector<uint8_t>& motorIDs);

  // Update behavior - calculates new noise positions
  bool update() override;

  // Get motor position for a controlled motor
  bool getMotorPosition(uint8_t motorID, uint16_t& position) override;

private:
  // Store current positions for each motor (indexed by motor ID)
  uint16_t motorPositions[256];
  
  // Time offset for perlin noise animation
  unsigned long startTime;
  
  // Configuration
  static constexpr uint16_t POSITION_CENTER = 2047;
  static constexpr uint16_t NOISE_RANGE = 100;       // ±500 from center
  static constexpr float NOISE_SPEED = 0.000125f;    // How fast noise evolves (slower = smoother, 4x slower)
  static constexpr uint16_t MOTOR_SEED_OFFSET = 100; // Seed offset between motors for variety
};

// ============================================================================
// Viseme Behavior - Controls mouth motors for speech
// ============================================================================

// Motor position within a viseme
struct VisemeMotorPosition {
  uint8_t motorID;
  uint16_t position;
};

// Viseme definition: ID, label (3 chars), and motor positions
struct Viseme {
  uint8_t id;
  char label[4];  // 3 characters + null terminator
  std::vector<VisemeMotorPosition> motorPositions;
};

class VisemeBehavior : public Behavior {
public:
  VisemeBehavior();

  // Add a viseme with a 3-char label (auto-assigns ID)
  // Returns the assigned viseme ID
  uint8_t addViseme(const char* label);
  
  // Legacy: Add a viseme with specific ID and motor positions (for backwards compatibility)
  void addViseme(uint8_t id, uint16_t pos40, uint16_t pos43, uint16_t pos44);
  
  // Add a viseme with specific ID, label, and motor positions
  void addViseme(uint8_t id, const char* label, uint16_t pos40, uint16_t pos43, uint16_t pos44);
  
  // Delete a viseme by ID
  // Returns true if deleted, false if not found
  bool deleteViseme(uint8_t visemeID);
  
  // Set motor positions for a viseme
  // Returns true if viseme found and updated, false otherwise
  bool setVisemeMotors(uint8_t visemeID, const std::vector<VisemeMotorPosition>& positions);
  
  // Set motor positions and label for a viseme
  // Returns true if viseme found and updated, false otherwise
  bool setVisemeMotorsAndLabel(uint8_t visemeID, const char* label, const std::vector<VisemeMotorPosition>& positions);
  
  // Create or update a viseme with specific ID, label, and motor positions
  // Returns true on success
  bool createOrUpdateViseme(uint8_t visemeID, const char* label, const std::vector<VisemeMotorPosition>& positions);
  
  // Get all visemes (for VLST command)
  const std::vector<Viseme>& getVisemes() const { return visemes; }
  
  // Trigger a viseme by ID - activates the behavior and sets positions
  // Returns true if viseme was found, false otherwise
  bool triggerViseme(uint8_t visemeID);

  // Update behavior - checks for timeout and deactivates
  bool update() override;

  // Get motor position for a controlled motor
  bool getMotorPosition(uint8_t motorID, uint16_t& position) override;

private:
  bool isActive;
  unsigned long lastTriggerTime;
  uint8_t nextVisemeID;  // Auto-increment ID for new visemes
  
  // Current active motor positions (when triggered)
  std::vector<VisemeMotorPosition> currentPositions;
  
  // Registered visemes
  std::vector<Viseme> visemes;
  
  // Configuration
  static constexpr unsigned long TIMEOUT_MS = 3000;  // 3 second timeout
  static constexpr uint16_t DEFAULT_POSITION = 2047; // Center/rest position
  
  // Helper to find viseme by ID
  Viseme* findViseme(uint8_t id);
};

// ============================================================================
// Behavior Manager - Manages active behaviors and resolves motor conflicts
// ============================================================================

class BehaviorManager {
public:
  BehaviorManager();
  
  // Add a behavior to the manager with an ID
  void addBehavior(BehaviorID id, Behavior* behavior);
  
  // Remove a behavior from the manager
  void removeBehavior(Behavior* behavior);
  
  // Enable/disable a behavior by ID
  void setBehaviorEnabled(BehaviorID id, bool enabled);
  
  // Check if a behavior is enabled
  bool isBehaviorEnabled(BehaviorID id) const;
  
  // Get count of registered behaviors
  uint8_t getBehaviorCount() const;
  
  // Get behavior info at index (for iteration)
  // Returns true if index is valid and fills out id and enabled
  bool getBehaviorInfo(uint8_t index, BehaviorID& id, bool& enabled) const;
  
  // Update all enabled behaviors (call each frame)
  void update();
  
  // Check if a behavior wants to control a specific motor
  // Returns true if a behavior provides a position, false otherwise
  bool getMotorPosition(uint8_t motorID, uint16_t& position);

private:
  struct BehaviorEntry {
    BehaviorID id;
    Behavior* behavior;
  };
  
  std::vector<BehaviorEntry> behaviors;
  bool enabledStates[256] = {false};  // Track enabled state by ID
};

// Global behavior manager instance
extern BehaviorManager behaviorManager;

// Global behavior instances (for command/config access)
extern FocusBehavior focusBehavior;
extern VisemeBehavior visemeBehavior;