#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 - ADXL345 3-axis Accelerometer
// ============================================================================

class ADXL345 {
public:
  ADXL345(uint8_t addr = 0x53);

  bool init();
  bool update();  // Read latest values

  // Get acceleration in g-forces
  float getAccelX() const { return accelX; }
  float getAccelY() const { return accelY; }
  float getAccelZ() const { return accelZ; }

  // Get tilt angles in degrees (approximated from gravity)
  float getPitch() const;  // Pitch from Y/Z acceleration (Y=front/back)
  float getRoll() const;   // Roll from X/Z acceleration (X=left/right)

  // Get Euler angles (pitch and roll only, no heading without magnetometer)
  void getEulerAngles(float& pitch, float& roll) const;

  bool isReady() const { return ready; }

  // Pack into payload buffer (10 bytes: x,y,z accel + pitch,roll angles)
  uint16_t packPayload(uint8_t* buffer) const;

private:
  uint8_t addr;
  bool ready = false;
  float accelX = 0, accelY = 0, accelZ = 0;

  void write8(uint8_t reg, uint8_t value);
  uint8_t read8(uint8_t reg);
  void readAccelData();
};

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

extern Radar radar;
extern ADXL345 adxl;

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

class SensorManager {
public:
  void init();
  void update();

  // Streaming control
  void enableADXLStream(bool enable, uint16_t intervalMs = 100);
  void enableRadarStream(bool enable, uint16_t intervalMs = 100);

  bool isADXLStreamEnabled() const { return adxlStreamEnabled; }
  bool isRadarStreamEnabled() const { return radarStreamEnabled; }

private:
  bool adxlStreamEnabled = true;
  bool radarStreamEnabled = true;
  uint16_t adxlInterval = 10;
  uint16_t radarInterval = 10;
  unsigned long lastADXLSend = 0;
  unsigned long lastRadarSend = 0;

  void sendADXLPacket();
  void sendRadarPacket();
};

extern SensorManager sensors;