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 (radar 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 radar targets with eyes/neck
// ============================================================================

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

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

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

private:
  bool isActive;
  uint16_t eyePosition;      // Current interpolated position for motors 14 and 15
  uint16_t neckPosition;     // Current interpolated position for motor 27
  
  // Target positions
  uint16_t targetEyePosition;
  uint16_t targetNeckPosition;
  
  // Timing
  unsigned long targetDetectedTime;  // When target was first detected
  unsigned long neckStartTime;       // When neck rotation should start
  bool neckRotating;                 // Whether neck is actively rotating
  
  // Configuration
  static constexpr uint8_t FOCUS_MOTOR_1 = 14;  // Eye motor 1
  static constexpr uint8_t FOCUS_MOTOR_2 = 15;  // Eye motor 2
  static constexpr uint8_t NECK_MOTOR = 27;     // Neck motor
  
  // Eye motor position range (motors 14 and 15)
  static constexpr uint16_t EYE_POSITION_CENTER = 2200;
  static constexpr uint16_t EYE_POSITION_MIN = 1700;
  static constexpr uint16_t EYE_POSITION_MAX = 2500;
  
  // Neck motor position range (motor 27)
  static constexpr uint16_t NECK_POSITION_CENTER = 2000;
  static constexpr uint16_t NECK_POSITION_MIN = 1000;
  static constexpr uint16_t NECK_POSITION_MAX = 3000;
  
  static constexpr unsigned long NECK_DELAY_MS = 1000;  // 1 second delay before neck moves
  static constexpr float INTERPOLATION_SPEED = 0.05f;   // Smooth interpolation factor for eyes (0-1, higher = faster)
  static constexpr float NECK_INTERPOLATION_SPEED = 0.03f;  // Slower interpolation for neck (smoother motion)
  static constexpr float EYE_CENTERING_SPEED = 0.03f;   // Speed at which eyes center when neck rotates

  // Calculate motor position from radar angle (in degrees)
  uint16_t calculateEyePositionFromRadarAngle(float radarAngle);
  uint16_t calculateNeckPositionFromRadarAngle(float radarAngle);
  
  // Smooth interpolation helper (linear interpolation)
  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);
  
  // 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);
  
  // 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 viseme behavior instance (for command access)
extern VisemeBehavior visemeBehavior;