/**
 * Claude's Eyes - IMU Module Implementation
 *
 * QMI8658 6-axis IMU using SensorLib
 */

#include "imu.h"
#include "config.h"
#include <Wire.h>
#include <SensorQMI8658.hpp>

// Global instance
IMUModule IMU;

// Sensor instance
static SensorQMI8658 qmi;

IMUModule::IMUModule()
    : _initialized(false)
    , _lastError(nullptr)
    , _lastUpdateTime(0)
{
    memset(&_data, 0, sizeof(_data));
}

bool IMUModule::begin() {
#if !ENABLE_IMU
    Serial.println("[IMU] Disabled in config");
    return false;
#endif

    // Initialize I2C (shared with touch on Waveshare board)
    Wire.begin(IMU_SDA_PIN, IMU_SCL_PIN);

    // Initialize QMI8658
    if (!qmi.begin(Wire, QMI8658_L_SLAVE_ADDRESS, IMU_SDA_PIN, IMU_SCL_PIN)) {
        // Try alternate address
        if (!qmi.begin(Wire, QMI8658_H_SLAVE_ADDRESS, IMU_SDA_PIN, IMU_SCL_PIN)) {
            _lastError = "QMI8658 not found";
            Serial.println("[IMU] QMI8658 not found on I2C");
            return false;
        }
    }

    Serial.printf("[IMU] QMI8658 found, Chip ID: 0x%02X\n", qmi.getChipID());

    // Configure accelerometer
    // Range: ±8g, ODR: 896.8Hz
    qmi.configAccelerometer(
        SensorQMI8658::ACC_RANGE_8G,
        SensorQMI8658::ACC_ODR_896_8Hz,
        SensorQMI8658::LPF_MODE_0
    );

    // Configure gyroscope
    // Range: ±512dps, ODR: 896.8Hz
    qmi.configGyroscope(
        SensorQMI8658::GYR_RANGE_512DPS,
        SensorQMI8658::GYR_ODR_896_8Hz,
        SensorQMI8658::LPF_MODE_0
    );

    // Enable sensors
    qmi.enableAccelerometer();
    qmi.enableGyroscope();

    _initialized = true;
    Serial.println("[IMU] Initialized successfully");

    // Take initial reading
    update();

    return true;
}

void IMUModule::update() {
    if (!_initialized) return;

    unsigned long now = millis();
    if (now - _lastUpdateTime < UPDATE_INTERVAL) {
        return;
    }

    float dt = (now - _lastUpdateTime) / 1000.0f;
    _lastUpdateTime = now;

    // Read sensor data
    if (qmi.getDataReady()) {
        float ax, ay, az;
        float gx, gy, gz;

        if (qmi.getAccelerometer(ax, ay, az)) {
            _data.accel_x = ax;
            _data.accel_y = ay;
            _data.accel_z = az;
        }

        if (qmi.getGyroscope(gx, gy, gz)) {
            _data.gyro_x = gx;
            _data.gyro_y = gy;
            _data.gyro_z = gz;
        }

        _data.temperature = qmi.getTemperature_C();

        // Compute orientation angles
        computeAngles();

        // Integrate yaw from gyroscope
        if (dt > 0 && dt < 1.0f) {  // Sanity check
            _data.yaw += _data.gyro_z * dt;

            // Keep yaw in -180 to 180 range
            while (_data.yaw > 180.0f) _data.yaw -= 360.0f;
            while (_data.yaw < -180.0f) _data.yaw += 360.0f;
        }
    }
}

void IMUModule::computeAngles() {
    // Calculate pitch and roll from accelerometer
    // Assumes sensor is mounted level when robot is flat

    // Pitch: rotation around X axis
    // tan(pitch) = -ax / sqrt(ay² + az²)
    _data.pitch = atan2(-_data.accel_x,
                        sqrt(_data.accel_y * _data.accel_y +
                             _data.accel_z * _data.accel_z)) * 180.0f / PI;

    // Roll: rotation around Y axis
    // tan(roll) = ay / az
    _data.roll = atan2(_data.accel_y, _data.accel_z) * 180.0f / PI;
}

bool IMUModule::isTilted() {
    // Check if tilted more than 30 degrees in any direction
    const float TILT_THRESHOLD = 30.0f;
    return (abs(_data.pitch) > TILT_THRESHOLD ||
            abs(_data.roll) > TILT_THRESHOLD);
}

bool IMUModule::isUpsideDown() {
    // Z-axis should be positive (pointing up) when right-side up
    // If Z is negative, robot is flipped
    return _data.accel_z < -5.0f;  // Threshold accounts for noise
}

void IMUModule::resetYaw() {
    _data.yaw = 0;
}