#![no_std]
#![no_main]

use esp_backtrace as _;
use esp_hal::{
    clock::ClockControl,
    delay::Delay,
    i2c::I2C,
    peripherals::Peripherals,
    prelude::*,
    system::SystemControl,
};
use mpu6050::{Mpu6050, TempSensor};
use panic_halt as _;

#[entry]
fn main() -> ! {
    // Initialize peripherals
    let peripherals = Peripherals::take();
    let system = SystemControl::new(peripherals.SYSTEM);
    let clocks = ClockControl::max(system.clock_control).freeze();

    let mut delay = Delay::new(&clocks);

    // Initialize I2C
    let i2c = I2C::new(
        peripherals.I2C0,
        peripherals.pins.gpio21, // SDA (I2C Data)
        peripherals.pins.gpio22, // SCL (I2C Clock)
        400.kHz(),
        &clocks,
    );

    // Initialize MPU6050 Sensor
    let mut mpu = Mpu6050::new(i2c);
    mpu.init(&mut delay).unwrap();

    loop {
        let accel = mpu.get_acc().unwrap();
        let gyro = mpu.get_gyro().unwrap();

        esp_println::println!(
            "Accel: X={} Y={} Z={} | Gyro: X={} Y={} Z={}",
            accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z
        );

        delay.delay_ms(500u32);
    }
}