HansonServo/behaviors.h

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

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

enum BehaviorID : uint8_t {
  BEHAVIOR_FOCUS = 1,  // Focus behavior (radar tracking)
  // Add more behavior IDs here as needed
};

// ============================================================================
// 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 = 2047;
  static constexpr uint16_t EYE_POSITION_MIN = 1600;
  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);
};

// ============================================================================
// 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;