#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
  float angle;    // Angle in degrees (calculated from x,y using atan2)
  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();
};

// ============================================================================
// Face Detection - via WebSocket from Radxa
// ============================================================================

constexpr int FACE_MAX_FACES = 5;

struct DetectedFace {
  int16_t x;        // Center-relative pixels (0,0 = image center)
  int16_t y;        // Positive = right/down, negative = left/up
  uint16_t w;       // Bounding box width
  uint16_t h;       // Bounding box height
  uint8_t conf;     // Confidence 0-255
  bool valid;
};

class FaceDetect {
public:
  // Feed raw FACE payload (after 4-byte tag) from WebSocket
  void feedPayload(const uint8_t* payload, size_t len);
  
  const DetectedFace& getFace(uint8_t index) const;
  uint8_t getFaceCount() const { return faceCount; }
  bool hasNewData() const { return newData; }
  void clearNewData() { newData = false; }
  
  // Remote alive state
  void setAlive(bool alive) { remoteAlive = alive; lastAliveTime = millis(); }
  bool isAlive() const { return remoteAlive && (millis() - lastAliveTime < 5000); }
  
  // Pack faces into payload buffer, returns length
  uint16_t packPayload(uint8_t* buffer) const;
  
private:
  DetectedFace faces[FACE_MAX_FACES] = {};
  uint8_t faceCount = 0;
  bool newData = false;
  bool remoteAlive = false;
  unsigned long lastAliveTime = 0;
};

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

extern Radar radar;
extern ADXL345 adxl;
extern FaceDetect faceDetect;

// ============================================================================
// 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);
  void enableFaceStream(bool enable, uint16_t intervalMs = 100);

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

private:
  bool adxlStreamEnabled = true;
  bool radarStreamEnabled = true;
  bool faceStreamEnabled = true;
  uint16_t adxlInterval = 500;  // 500ms = 2 packets per second
  uint16_t radarInterval = 10;
  uint16_t faceInterval = 50;   // 20 Hz
  unsigned long lastADXLSend = 0;
  unsigned long lastRadarSend = 0;
  unsigned long lastFaceSend = 0;

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

extern SensorManager sensors;