#![no_std]
#![no_main]

use esp_backtrace as _;
use esp_println::println;
use hal::{
    clock::ClockControl,
    gpio::IO,
    i2c::{self, *},
    peripherals::{Peripherals, I2C0},
    prelude::*,
    Delay, Rtc,
};
use mpu6050::*;

fn init_6050<'a>(
    i2c: I2C<'a, I2C0>,
    delay: &mut Delay,
) -> Result<Mpu6050<I2C<'a, I2C0>>, Mpu6050Error<i2c::Error>> {
    let mut mpu = Mpu6050::new(i2c);
    mpu.init(delay)?;
    Ok(mpu)
}

#[entry]
fn main() -> ! {
    let peripherals = Peripherals::take();
    let system = peripherals.SYSTEM.split();

    let clocks = ClockControl::max(system.clock_control).freeze();
    let mut mpu_delay: Delay = Delay::new(&clocks);
    let mut delay: Delay = Delay::new(&clocks);
    let io: IO = IO::new(peripherals.GPIO, peripherals.IO_MUX);
    let i2c = I2C::new(
        peripherals.I2C0,
        io.pins.gpio1,
        io.pins.gpio2,
        100u32.kHz(),
        &clocks,
    );

    let mut mpu = init_6050(i2c, &mut mpu_delay).unwrap();

    println!("Hello world!");
    loop {
        // get roll and pitch estimate
        let acc = mpu.get_acc_angles();
        println!("r/p: {:?}\r\n", acc);

        // get temp
        let temp = mpu.get_temp();
        println!("temp: {:?}c\r\n", temp);

        // get gyro data, scaled with sensitivity
        let gyro = mpu.get_gyro();
        println!("gyro: {:?}\r\n", gyro);

        // get accelerometer data, scaled with sensitivity
        let acc = mpu.get_acc();
        println!("acc: {:?}\r\n", acc);
        delay.delay_ms(500u32);
    }
}