#include "sensors.h"
#include "protocol.h"

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

Radar radar;
ADXL345 adxl;
SensorManager sensors;

// ============================================================================
// Radar Implementation
// ============================================================================

static const uint8_t RADAR_HEADER[] = {0xAA, 0xFF, 0x03, 0x00};
static const uint8_t RADAR_FOOTER[] = {0x55, 0xCC};
constexpr float RADAR_DISTANCE_SCALE = 0.1f;  // Raw mm to cm
constexpr float RADAR_MIN_VALID_DIST = 30.0f; // Minimum valid distance in cm

int16_t Radar::decodeSignMag(uint16_t raw) {
  int16_t magnitude = raw & 0x7FFF;
  return (raw & 0x8000) ? magnitude : -magnitude;
}

void Radar::init() {
  Serial2.begin(RADAR_BAUD, SERIAL_8N1, SensorPins::RADAR_RX, SensorPins::RADAR_TX);
}

bool Radar::update() {
  bool newData = false;
  
  while (Serial2.available()) {
    uint8_t b = Serial2.read();
    
    if (!inFrame) {
      // Looking for header
      if (b == RADAR_HEADER[headerMatch]) {
        rxBuf[headerMatch] = b;
        headerMatch++;
        if (headerMatch == 4) {
          inFrame = true;
          bufIdx = 4;
          headerMatch = 0;
        }
      } else if (b == RADAR_HEADER[0]) {
        headerMatch = 1;
        rxBuf[0] = b;
      } else {
        headerMatch = 0;
      }
      continue;
    }
    
    // In frame - collect bytes
    if (bufIdx < sizeof(rxBuf)) {
      rxBuf[bufIdx++] = b;
    }
    
    // Check for footer
    if (bufIdx >= 6 && rxBuf[bufIdx - 2] == RADAR_FOOTER[0] && rxBuf[bufIdx - 1] == RADAR_FOOTER[1]) {
      parseFrame();
      newData = true;
      inFrame = false;
      bufIdx = 0;
    }
    
    // Overflow protection
    if (bufIdx >= sizeof(rxBuf)) {
      inFrame = false;
      bufIdx = 0;
    }
  }
  
  return newData;
}

void Radar::parseFrame() {
  for (int i = 0; i < RADAR_MAX_TARGETS; i++) {
    int offset = 4 + (i * 6);
    
    uint16_t x_raw = rxBuf[offset] | (rxBuf[offset + 1] << 8);
    uint16_t y_raw = rxBuf[offset + 2] | (rxBuf[offset + 3] << 8);
    uint16_t spd_raw = rxBuf[offset + 4] | (rxBuf[offset + 5] << 8);
    
    targets[i].x = decodeSignMag(x_raw) * RADAR_DISTANCE_SCALE;
    targets[i].y = (int16_t)(y_raw - 0x8000) * RADAR_DISTANCE_SCALE;
    targets[i].speed = decodeSignMag(spd_raw) * RADAR_DISTANCE_SCALE;
    
    targets[i].valid = (y_raw != 0) && (y_raw != 0x8000) && (targets[i].y >= RADAR_MIN_VALID_DIST);
    
    // Calculate angle from x and y (x is mm from center line, y is distance)
    // atan2(x, y) gives angle in radians, convert to degrees
    if (targets[i].valid && targets[i].y > 0) {
      targets[i].angle = atan2(targets[i].x, targets[i].y) * 180.0f / PI;
    } else {
      targets[i].angle = 0.0f;
    }
  }
}

const RadarTarget& Radar::getTarget(uint8_t index) const {
  if (index >= RADAR_MAX_TARGETS) index = 0;
  return targets[index];
}

uint8_t Radar::getTargetCount() const {
  uint8_t count = 0;
  for (int i = 0; i < RADAR_MAX_TARGETS; i++) {
    if (targets[i].valid) count++;
  }
  return count;
}

uint16_t Radar::packPayload(uint8_t* buffer) const {
  // Format: count(1) + [valid(1), x(2), y(2), speed(2), angle(2)] * 3 = 28 bytes
  buffer[0] = getTargetCount();
  uint16_t offset = 1;
  
  for (int i = 0; i < RADAR_MAX_TARGETS; i++) {
    buffer[offset++] = targets[i].valid ? 1 : 0;
    
    int16_t x = (int16_t)(targets[i].x * 10);  // cm * 10 for precision
    int16_t y = (int16_t)(targets[i].y * 10);
    int16_t spd = (int16_t)(targets[i].speed * 10);
    int16_t angle = (int16_t)(targets[i].angle * 10);  // degrees * 10 for precision
    
    buffer[offset++] = x & 0xFF;
    buffer[offset++] = (x >> 8) & 0xFF;
    buffer[offset++] = y & 0xFF;
    buffer[offset++] = (y >> 8) & 0xFF;
    buffer[offset++] = spd & 0xFF;
    buffer[offset++] = (spd >> 8) & 0xFF;
    buffer[offset++] = angle & 0xFF;
    buffer[offset++] = (angle >> 8) & 0xFF;
  }
  
  return offset;
}

// ============================================================================
// ADXL345 Implementation
// ============================================================================

ADXL345::ADXL345(uint8_t addr) : addr(addr) {}

bool ADXL345::init() {
  Wire.begin(SensorPins::IMU_SDA, SensorPins::IMU_SCL);
  delay(100);

  uint8_t id = read8(0x00);  // DEVID register (should be 0xE5)
  if (id != 0xE5) {
    ready = false;
    return false;
  }

  // Enable measurement mode
  write8(0x2D, 0x08);  // POWER_CTL: measure mode

  ready = true;
  return true;
}

bool ADXL345::update() {
  if (!ready) return false;

  readAccelData();
  return true;
}

float ADXL345::getPitch() const {
  // Approximate pitch from Y/Z acceleration (Y = front/back axis)
  // This is not as accurate as a proper IMU with gyroscope
  if (accelZ == 0) return 0;
  return atan2(-accelY, sqrt(accelX * accelX + accelZ * accelZ)) * 180.0f / PI;
}

float ADXL345::getRoll() const {
  // Approximate roll from X/Z acceleration (X = left/right axis)
  if (accelZ == 0) return 0;
  return atan2(accelX, accelZ) * 180.0f / PI;
}

void ADXL345::getEulerAngles(float& pitch, float& roll) const {
  // Calculate Euler angles from accelerometer data
  // Note: Heading (yaw) cannot be determined from accelerometer alone
  // Coordinate system: X=left/right, Y=front/back, Z=up/down

  if (!ready) {
    pitch = 0.0f;
    roll = 0.0f;
    return;
  }

  // Pitch (front/back tilt) = atan2(-accelY, sqrt(accelX² + accelZ²))
  // Roll (left/right tilt) = atan2(accelX, accelZ)

  float accelMagnitude = sqrt(accelX * accelX + accelZ * accelZ);
  if (accelMagnitude > 0.01f) {  // Avoid division by very small numbers
    pitch = atan2(-accelY, accelMagnitude) * 180.0f / PI;
  } else {
    pitch = 0.0f;
  }

  if (fabs(accelZ) > 0.01f) {  // Avoid division by zero
    roll = atan2(accelX, accelZ) * 180.0f / PI;
  } else {
    roll = 0.0f;
  }
}

uint16_t ADXL345::packPayload(uint8_t* buffer) const {
  // Format: accelX(2) + accelY(2) + accelZ(2) + pitch(2) + roll(2), all ×100
  // Accelerations in g-forces ×100, angles in degrees ×100

  // Pack acceleration data
  int16_t x = (int16_t)(accelX * 100.0f);
  int16_t y = (int16_t)(accelY * 100.0f);
  int16_t z = (int16_t)(accelZ * 100.0f);

  buffer[0] = x & 0xFF;
  buffer[1] = (x >> 8) & 0xFF;
  buffer[2] = y & 0xFF;
  buffer[3] = (y >> 8) & 0xFF;
  buffer[4] = z & 0xFF;
  buffer[5] = (z >> 8) & 0xFF;

  // Calculate and pack Euler angles
  float pitch_deg, roll_deg;
  getEulerAngles(pitch_deg, roll_deg);

  int16_t pitch = (int16_t)(pitch_deg * 100.0f);
  int16_t roll = (int16_t)(roll_deg * 100.0f);

  buffer[6] = pitch & 0xFF;
  buffer[7] = (pitch >> 8) & 0xFF;
  buffer[8] = roll & 0xFF;
  buffer[9] = (roll >> 8) & 0xFF;

  return 10;
}

void ADXL345::write8(uint8_t reg, uint8_t value) {
  Wire.beginTransmission(addr);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

uint8_t ADXL345::read8(uint8_t reg) {
  Wire.beginTransmission(addr);
  Wire.write(reg);
  Wire.endTransmission();
  Wire.requestFrom(addr, (uint8_t)1);
  return Wire.available() ? Wire.read() : 0xFF;
}

void ADXL345::readAccelData() {
  // Read 6 bytes starting from DATAX0 register (0x32)
  Wire.beginTransmission(addr);
  Wire.write(0x32);  // Start at DATAX0
  Wire.endTransmission();

  Wire.requestFrom(addr, (uint8_t)6);
  if (Wire.available() < 6) return;

  // ADXL345 outputs 10-bit values (2 bytes per axis)
  int16_t x_raw = Wire.read() | (Wire.read() << 8);
  int16_t y_raw = Wire.read() | (Wire.read() << 8);
  int16_t z_raw = Wire.read() | (Wire.read() << 8);

  // Convert to g-forces: ±2g range, 10-bit = ±512 LSB, 4mg/LSB
  const float scale = 0.00390625f;  // 4mg/LSB = 0.004g, but we use 1/256 for 10-bit

  accelX = x_raw * scale;
  accelY = y_raw * scale;
  accelZ = z_raw * scale;
}

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

void SensorManager::init() {
  radar.init();

  if (adxl.init()) {
    Serial.println("[Sensors] ADXL345 initialized");
  } else {
    Serial.println("[Sensors] ADXL345 not detected");
  }

  Serial.println("[Sensors] Radar initialized");
}

void SensorManager::update() {
  // Update sensors
  radar.update();
  if (adxl.isReady()) {
    adxl.update();
  }

  // Handle streaming
  unsigned long now = millis();

  if (adxlStreamEnabled && adxl.isReady() && (now - lastADXLSend >= adxlInterval)) {
    sendADXLPacket();
    lastADXLSend = now;
  }

  if (radarStreamEnabled && (now - lastRadarSend >= radarInterval)) {
    sendRadarPacket();
    lastRadarSend = now;
  }
}

void SensorManager::enableADXLStream(bool enable, uint16_t intervalMs) {
  adxlStreamEnabled = enable;
  adxlInterval = intervalMs;
  lastADXLSend = millis();
}

void SensorManager::enableRadarStream(bool enable, uint16_t intervalMs) {
  radarStreamEnabled = enable;
  radarInterval = intervalMs;
  lastRadarSend = millis();
}

void SensorManager::sendADXLPacket() {
  uint8_t payload[32];  // Buffer sized for current/future payload expansion
  uint16_t len = adxl.packPayload(payload);
  sendPacket(Tag::IMU, payload, len);  // Reuse IMU tag for compatibility
}

void SensorManager::sendRadarPacket() {
  uint8_t payload[32];  // Updated size for angle field
  uint16_t len = radar.packPayload(payload);
  sendPacket(Tag::RADAR, payload, len);
}