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

// ============================================================================
// 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
  void addBehavior(Behavior* behavior);
  
  // Remove a behavior from the manager
  void removeBehavior(Behavior* behavior);
  
  // Update all 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:
  std::vector<Behavior*> behaviors;
};

// Global behavior manager instance
extern BehaviorManager behaviorManager;