//! Template project for Rust on ESP32 (`no_std`) based on [`esp-hal`](https://github.com/esp-rs/esp-hal)
//!
//! Useful resources:
//! - [The Rust on ESP Book](https://docs.esp-rs.org/book/)
//! - [Embedded Rust (no_std) on Espressif](https://docs.esp-rs.org/no_std-training/)
//! - [Matrix channel](https://matrix.to/#/#esp-rs:matrix.org)

#![no_std]
#![no_main]

use esp_idf_svc::hal::{i2c, gpio};
use esp_idf_svc::hal::delay::Delay;
use esp_idf_svc::sys::EspError;
use esp_println::println;
use std::thread;
use std::time::Duration;

const MPU6050_ADDRESS: u8 = 0x68;
const ACCEL_XOUT_H: u8 = 0x3B;
const PWR_MGMT_1: u8 = 0x6B;

fn main() -> Result<(), EspError> {
    esp_idf_svc::sys::esp!(esp_idf_svc::sys::esp_wifi_set_mode(
        esp_idf_svc::sys::wifi_mode_t_WIFI_MODE_STA
    ))?;

    println!("MPU6050 Accelerometer Example");

    let peripherals = esp_idf_svc::hal::peripherals::Peripherals::take().unwrap();
    let sda = peripherals.pins.gpio21; // Adjust if you used a different SDA pin
    let scl = peripherals.pins.gpio22; // Adjust if you used a different SCL pin
    let i2c = i2c::I2cDriver::new(peripherals.i2c0, sda, scl, 100_kHz)?;

    // Wake up the MPU6050 (it starts in sleep mode)
    i2c.write(MPU6050_ADDRESS, &[PWR_MGMT_1, 0x00], 100)?;
    Delay::new(&()).delay_ms(100);

    loop {
        let mut accel_data = [0u8; 6];
        match i2c.read(MPU6050_ADDRESS, ACCEL_XOUT_H, &mut accel_data, 100) {
            Ok(_) => {
                let acc_x = (accel_data[0] as i16) << 8 | (accel_data[1] as i16);
                let acc_y = (accel_data[2] as i16) << 8 | (accel_data[3] as i16);
                let acc_z = (accel_data[4] as i16) << 8 | (accel_data[5] as i16);

                println!("Raw Accel X: {}", acc_x);
                println!("Raw Accel Y: {}", acc_y);
                println!("Raw Accel Z: {}", acc_z);
            }
            Err(e) => println!("Error reading accelerometer data: {:?}", e),
        }

        thread::sleep(Duration::from_millis(500));
    }

    Ok(())
}