#pragma once
#include <Arduino.h>
#include <Wire.h>

// ============================================================================
// Pin Configuration
// ============================================================================

namespace SensorPins {
  // Radar (RD-03D) on Serial2
  constexpr int RADAR_RX = 4;
  constexpr int RADAR_TX = 5;
  
  // IMU (BNO055) on I2C
  constexpr int IMU_SDA = 8;
  constexpr int IMU_SCL = 9;
}

// ============================================================================
// Radar - RD-03D mmWave Sensor
// ============================================================================

constexpr int RADAR_MAX_TARGETS = 3;
constexpr uint32_t RADAR_BAUD = 256000;

struct RadarTarget {
  float x;        // X position in cm (negative=left, positive=right)
  float y;        // Y distance in cm (forward)
  float speed;    // Speed in cm/s
  bool valid;     // Target is valid
};

class Radar {
public:
  void init();
  bool update();  // Returns true if new data parsed
  
  const RadarTarget& getTarget(uint8_t index) const;
  uint8_t getTargetCount() const;
  
  // Pack all targets into a payload buffer, returns length
  uint16_t packPayload(uint8_t* buffer) const;
  
private:
  RadarTarget targets[RADAR_MAX_TARGETS] = {};
  uint8_t rxBuf[64];
  uint8_t bufIdx = 0;
  uint8_t headerMatch = 0;
  bool inFrame = false;
  
  void parseFrame();
  static int16_t decodeSignMag(uint16_t raw);
};

// ============================================================================
// IMU - BNO055 9-DOF Sensor
// ============================================================================

class IMU {
public:
  IMU(uint8_t addr = 0x29);
  
  bool init();
  bool update();  // Read latest values
  
  // Get euler angles in degrees
  float getHeading() const { return heading; }
  float getRoll() const { return roll; }
  float getPitch() const { return pitch; }
  
  bool isReady() const { return ready; }
  
  // Pack into payload buffer (6 bytes: h, r, p as int16 * 100)
  uint16_t packPayload(uint8_t* buffer) const;
  
private:
  uint8_t addr;
  bool ready = false;
  float heading = 0, roll = 0, pitch = 0;
  
  void write8(uint8_t reg, uint8_t value);
  uint8_t read8(uint8_t reg);
};

// ============================================================================
// Global Instances
// ============================================================================

extern Radar radar;
extern IMU imu;

// ============================================================================
// Sensor Manager
// ============================================================================

class SensorManager {
public:
  void init();
  void update();
  
  // Streaming control
  void enableIMUStream(bool enable, uint16_t intervalMs = 100);
  void enableRadarStream(bool enable, uint16_t intervalMs = 100);
  
  bool isIMUStreamEnabled() const { return imuStreamEnabled; }
  bool isRadarStreamEnabled() const { return radarStreamEnabled; }
  
private:
  bool imuStreamEnabled = false;
  bool radarStreamEnabled = false;
  uint16_t imuInterval = 100;
  uint16_t radarInterval = 100;
  unsigned long lastIMUSend = 0;
  unsigned long lastRadarSend = 0;
  
  void sendIMUPacket();
  void sendRadarPacket();
};

extern SensorManager sensors;