#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 currentPosition;  // Current position for motors 14 and 15

  // Configuration
  static constexpr uint8_t FOCUS_MOTOR_1 = 14;
  static constexpr uint8_t FOCUS_MOTOR_2 = 15;
  static constexpr uint16_t POSITION_CENTER = 2047;
  static constexpr uint16_t POSITION_MIN = 1700;
  static constexpr uint16_t POSITION_MAX = 2300;
  static constexpr float RADAR_X_MIN = -80.0f;
  static constexpr float RADAR_X_MAX = 80.0f;

  // Calculate motor position from radar x coordinate
  uint16_t calculatePositionFromRadarX(float radarX);
};

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