// ESP32 DevKit C V4 with MPU6050 using embedded-hal
// This code initializes the I2C interface and reads acceleration and gyroscope data

use esp_idf_hal::delay::Ets;
use esp_idf_hal::i2c::{I2cConfig, I2cDriver};
use esp_idf_hal::peripherals::Peripherals;
use esp_idf_hal::prelude::*;
use esp_idf_sys as _; // If using the `binstart` feature of `esp-idf-sys`, always keep this import
use log::*;
use std::thread;
use std::time::Duration;

// MPU6050 constants
const MPU6050_ADDR: u8 = 0x68; // MPU6050 device address
const ACCEL_XOUT_H: u8 = 0x3B;  // Accelerometer data registers start
const PWR_MGMT_1: u8 = 0x6B;    // Power management register

// Function to initialize the MPU6050
fn init_mpu6050(i2c: &mut I2cDriver) -> Result<(), anyhow::Error> {
    // Wake up the MPU6050 (it starts in sleep mode)
    i2c.write(MPU6050_ADDR, &[PWR_MGMT_1, 0x00])?;
    
    // Short delay to ensure the device is ready
    thread::sleep(Duration::from_millis(100));
    
    Ok(())
}

// Function to read the raw accelerometer and gyroscope data
fn read_mpu6050_data(i2c: &mut I2cDriver) -> Result<(f32, f32, f32, f32, f32, f32), anyhow::Error> {
    // Read 14 bytes of data starting from ACCEL_XOUT_H register
    // This will give us all accelerometer and gyroscope data
    let mut buffer = [0u8; 14];
    
    // Tell the device which register we want to read from
    i2c.write(MPU6050_ADDR, &[ACCEL_XOUT_H])?;
    
    // Read the data
    i2c.read(MPU6050_ADDR, &mut buffer)?;
    
    // Convert the raw data to 16-bit values
    let accel_x = (((buffer[0] as i16) << 8) | buffer[1] as i16) as f32 / 16384.0;
    let accel_y = (((buffer[2] as i16) << 8) | buffer[3] as i16) as f32 / 16384.0;
    let accel_z = (((buffer[4] as i16) << 8) | buffer[5] as i16) as f32 / 16384.0;
    
    // Temperature is not used in this example, skip 2 bytes
    
    let gyro_x = (((buffer[8] as i16) << 8) | buffer[9] as i16) as f32 / 131.0;
    let gyro_y = (((buffer[10] as i16) << 8) | buffer[11] as i16) as f32 / 131.0;
    let gyro_z = (((buffer[12] as i16) << 8) | buffer[13] as i16) as f32 / 131.0;
    
    Ok((accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z))
}

fn main() -> anyhow::Result<()> {
    // Initialize the logger
    esp_idf_svc::log::EspLogger::initialize_default();
    
    info!("MPU6050 Rust Example on ESP32");
    
    // Get the peripherals
    let peripherals = Peripherals::take().unwrap();
    
    // Configure I2C
    let i2c_config = I2cConfig::new().baudrate(100.kHz().into());
    
    // Initialize I2C on the ESP32 - using GPIO 21 (SDA) and GPIO 22 (SCL)
    let sda = peripherals.pins.gpio21;
    let scl = peripherals.pins.gpio22;
    let i2c = peripherals.i2c0;
    
    let mut i2c_driver = I2cDriver::new(i2c, sda, scl, &i2c_config)?;
    
    // Initialize the MPU6050
    info!("Initializing MPU6050...");
    init_mpu6050(&mut i2c_driver)?;
    info!("MPU6050 initialized successfully!");
    
    // Main loop - continuously read and display sensor data
    loop {
        match read_mpu6050_data(&mut i2c_driver) {
            Ok((ax, ay, az, gx, gy, gz)) => {
                info!("Accelerometer: X={:.2}, Y={:.2}, Z={:.2} g", ax, ay, az);
                info!("Gyroscope: X={:.2}, Y={:.2}, Z={:.2} deg/s", gx, gy, gz);
            },
            Err(e) => {
                error!("Failed to read sensor data: {}", e);
            }
        }
        
        // Wait before next reading
        thread::sleep(Duration::from_millis(500));
    }
}