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

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

Radar radar;
IMU imu;
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);
  }
}

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)] * 3 = 22 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);
    
    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;
  }
  
  return offset;
}

// ============================================================================
// IMU Implementation
// ============================================================================

IMU::IMU(uint8_t addr) : addr(addr) {}

bool IMU::init() {
  Wire.begin(SensorPins::IMU_SDA, SensorPins::IMU_SCL);
  delay(100);
  
  uint8_t id = read8(0x00);  // CHIP_ID register
  if (id != 0xA0) {
    ready = false;
    return false;
  }
  
  // Enter config mode
  write8(0x3D, 0x00);
  delay(25);
  
  // Set NDOF fusion mode
  write8(0x3D, 0x0C);
  delay(25);
  
  ready = true;
  return true;
}

bool IMU::update() {
  if (!ready) return false;
  
  Wire.beginTransmission(addr);
  Wire.write(0x1A);  // Euler angles start register
  Wire.endTransmission();
  
  Wire.requestFrom(addr, (uint8_t)6);
  if (Wire.available() < 6) return false;
  
  int16_t rawHeading = Wire.read() | (Wire.read() << 8);
  int16_t rawPitch = Wire.read() | (Wire.read() << 8);
  int16_t rawRoll = Wire.read() | (Wire.read() << 8);
  
  // Convert from 1/16 degree units
  heading = rawHeading / 16.0f;
  roll = -(rawRoll / 16.0f);  // Inverted so right roll is positive
  pitch = rawPitch / 16.0f;
  
  return true;
}

uint16_t IMU::packPayload(uint8_t* buffer) const {
  // Format: heading(2) + roll(2) + pitch(2) as int16 * 100
  int16_t h = (int16_t)(heading * 100.0f);
  int16_t r = (int16_t)(roll * 100.0f);
  int16_t p = (int16_t)(pitch * 100.0f);
  
  buffer[0] = h & 0xFF;
  buffer[1] = (h >> 8) & 0xFF;
  buffer[2] = r & 0xFF;
  buffer[3] = (r >> 8) & 0xFF;
  buffer[4] = p & 0xFF;
  buffer[5] = (p >> 8) & 0xFF;
  
  return 6;
}

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

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

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

void SensorManager::init() {
  radar.init();
  
  if (imu.init()) {
    Serial.println("[Sensors] IMU initialized");
  } else {
    Serial.println("[Sensors] IMU not detected");
  }
  
  Serial.println("[Sensors] Radar initialized");
}

void SensorManager::update() {
  // Update sensors
  radar.update();
  if (imu.isReady()) {
    imu.update();
  }
  
  // Handle streaming
  unsigned long now = millis();
  
  if (imuStreamEnabled && imu.isReady() && (now - lastIMUSend >= imuInterval)) {
    sendIMUPacket();
    lastIMUSend = now;
  }
  
  if (radarStreamEnabled && (now - lastRadarSend >= radarInterval)) {
    sendRadarPacket();
    lastRadarSend = now;
  }
}

void SensorManager::enableIMUStream(bool enable, uint16_t intervalMs) {
  imuStreamEnabled = enable;
  imuInterval = intervalMs;
  lastIMUSend = millis();
}

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

void SensorManager::sendIMUPacket() {
  uint8_t payload[6];
  uint16_t len = imu.packPayload(payload);
  sendPacket(Tag::IMU, payload, len);
}

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