#include "imu.h"
#include "Wire.h"
#include <math.h>

LSM6DS3 imu(I2C_MODE, 0x6A);

float rollSin = 0.0f;   // identity: no rotation
float rollCos = 1.0f;

// ─── 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;
  double sax=0, say=0;
  for (int i=0; i<BIAS_SAMPLES; i++) {
    sx += imu.readFloatGyroX(); sy += imu.readFloatGyroY(); sz += imu.readFloatGyroZ();
    sax += imu.readFloatAccelX(); say += imu.readFloatAccelY();
    digitalWrite(LED_GREEN, (i%20 < 10)); delay(5);  // green flutter during calibration
  }
  biasGX = (float)(sx/BIAS_SAMPLES);
  biasGY = (float)(sy/BIAS_SAMPLES);
  biasGZ = (float)(sz/BIAS_SAMPLES);
  calTempC = readIMUTemp();
  angleX = angleY = accumX = accumY = 0.0f;

  // Roll compensation: compute device yaw on the table from gravity's horizontal components.
  // ax/ay are small when flat; their ratio gives the rotation angle θ.
  // Firmware maps: screenX ← -gz, screenY ← -gy.
  // With device rotated θ CW: screenX ← -(gz·cosθ + gy·sinθ), screenY ← -(gy·cosθ - gz·sinθ).
  float ax_avg = (float)(sax / BIAS_SAMPLES);
  float ay_avg = (float)(say / BIAS_SAMPLES);
  float norm = sqrtf(ax_avg*ax_avg + ay_avg*ay_avg);
  if (norm > 0.05f) {         // only update if we can see meaningful tilt (>~3°)
    rollSin =  ax_avg / norm;
    rollCos = -ay_avg / norm; // negative: gravity pulls in -Y when flat and nominal
  } else {
    rollSin = 0.0f;
    rollCos = 1.0f;
  }
  Serial.print("[CAL] roll="); Serial.print(atan2f(rollSin, rollCos)*(180.f/PI), 1); Serial.println("deg");

  #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_GREEN, HIGH);  // off after calibration
  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); }