/**
 * Claude's Eyes - IMU Module Header
 *
 * QMI8658 6-axis IMU (accelerometer + gyroscope) on Waveshare board
 */

#ifndef IMU_H
#define IMU_H

#include <Arduino.h>

// IMU data structure
struct IMUData {
    // Accelerometer (m/s²)
    float accel_x;
    float accel_y;
    float accel_z;

    // Gyroscope (degrees/s)
    float gyro_x;
    float gyro_y;
    float gyro_z;

    // Computed values
    float pitch;    // Rotation around X axis (degrees)
    float roll;     // Rotation around Y axis (degrees)
    float yaw;      // Rotation around Z axis (degrees, integrated from gyro)

    // Temperature
    float temperature;
};

class IMUModule {
public:
    IMUModule();

    /**
     * Initialize the IMU
     * @return true if successful
     */
    bool begin();

    /**
     * Update IMU readings
     */
    void update();

    /**
     * Get current IMU data
     */
    const IMUData& getData() { return _data; }

    /**
     * Check if robot is tilted beyond safe angle
     */
    bool isTilted();

    /**
     * Check if robot is upside down
     */
    bool isUpsideDown();

    /**
     * Reset yaw integration
     */
    void resetYaw();

    /**
     * Check if initialized
     */
    bool isInitialized() { return _initialized; }

    /**
     * Get last error
     */
    const char* getLastError() { return _lastError; }

private:
    bool _initialized;
    const char* _lastError;
    IMUData _data;

    unsigned long _lastUpdateTime;
    static const unsigned long UPDATE_INTERVAL = 20;  // 50Hz update rate

    void computeAngles();
};

// Global instance
extern IMUModule IMU;

#endif // IMU_H