#include "imu.h"
#include "Wire.h"

LSM6DS3 imu(I2C_MODE, 0x6A);

// ─── I2C helpers ──────────────────────────────────────────────────────────────
void imuWriteReg(uint8_t reg, uint8_t val) {
  // LSM6DS3 is on Wire1 (internal I2C, SDA=P0.17, SCL=P0.16), NOT Wire (external pins 4/5)
  Wire1.beginTransmission(0x6A); Wire1.write(reg); Wire1.write(val); Wire1.endTransmission();
}

uint8_t imuReadReg(uint8_t reg) {
  Wire1.beginTransmission(0x6A); Wire1.write(reg); Wire1.endTransmission(false);
  Wire1.requestFrom((uint8_t)0x6A, (uint8_t)1);
  return Wire1.available() ? Wire1.read() : 0;
}

// ─── Temperature ──────────────────────────────────────────────────────────────
float readIMUTemp() {
  int16_t raw = (int16_t)((imuReadReg(REG_OUT_TEMP_H) << 8) | imuReadReg(REG_OUT_TEMP_L));
  return 25.0f + (float)raw / 256.0f;
}

// ─── Calibration ──────────────────────────────────────────────────────────────
void calibrateGyroBias() {
  Serial.println("[CAL] Hold still...");
  double sx=0, sy=0, sz=0;
  for (int i=0; i<BIAS_SAMPLES; i++) {
    sx += imu.readFloatGyroX(); sy += imu.readFloatGyroY(); sz += imu.readFloatGyroZ();
    digitalWrite(LED_BLUE, (i%20 < 10)); delay(5);
  }
  biasGX = (float)(sx/BIAS_SAMPLES);
  biasGY = (float)(sy/BIAS_SAMPLES);
  biasGZ = (float)(sz/BIAS_SAMPLES);
  calTempC = readIMUTemp();
  angleX = angleY = accumX = accumY = 0.0f;

  #ifdef FEATURE_TELEMETRY
    statRecalCount++;
    float bxr = biasGX*(PI/180.f), byr = biasGY*(PI/180.f), bzr = biasGZ*(PI/180.f);
    statBiasRms = sqrtf((bxr*bxr + byr*byr + bzr*bzr) / 3.0f);
  #endif

  digitalWrite(LED_BLUE, HIGH);
  Serial.print("[CAL] T="); Serial.print(calTempC,1);
  Serial.print("C  bias="); Serial.print(biasGX,4);
  Serial.print(","); Serial.print(biasGY,4);
  Serial.print(","); Serial.println(biasGZ,4);
}

// ─── Motion curve ─────────────────────────────────────────────────────────────
float applyCurve(float v) {
  switch (cfg.curve) {
    case CURVE_SQUARE: return (v >= 0 ? 1.f : -1.f) * v * v;
    case CURVE_SQRT:   return (v >= 0 ? 1.f : -1.f) * sqrtf(fabsf(v));
    default:           return v;
  }
}

float applyAcceleration(float d) { return d * (1.0f + fabsf(d) * cfg.accelStrength); }