#![no_std]
#![no_main]

use embassy_executor::Spawner;
use embassy_rp::{
    bind_interrupts,
    i2c::{self, I2c},
    peripherals::{self, USB},
    usb::Driver,
};
use embassy_time::{Delay, Timer};
use log::{error, info};
use mmc56x3::MMC56X3;

#[panic_handler]
fn panic(_info: &core::panic::PanicInfo) -> ! {
    embassy_rp::rom_data::reset_to_usb_boot(0, 0);

    loop {}
}

bind_interrupts!(struct Irqs {
    USBCTRL_IRQ => embassy_rp::usb::InterruptHandler<USB>;
    I2C0_IRQ => i2c::InterruptHandler<peripherals::I2C0>;
});

#[embassy_executor::task]
async fn logger_task(driver: Driver<'static, USB>) {
    embassy_usb_logger::run!(1024, log::LevelFilter::Info, driver);
}

#[embassy_executor::main]
async fn main(spawner: Spawner) {
    let p = embassy_rp::init(Default::default());

    let driver = Driver::new(p.USB, Irqs);
    spawner.must_spawn(logger_task(driver));

    let sda = p.PIN_4;
    let scl = p.PIN_5;

    let i2c = I2c::new_blocking(p.I2C0, scl, sda, i2c::Config::default());

    let mut device = MMC56X3::new(i2c, Delay);
    if let Err(e) = device.init() {
        error!("Failed to init {:?}", e);
        reboot().await;
    };

    if let Err(e) = device.set_data_rate(mmc56x3::DataRate::Max1000Hz) {
        error!("Failed to set data rate {:?}", e);
        reboot().await;
    }

    if let Err(e) = device.set_continuous_mode(true) {
        error!("Failed to set_continuous_mode {:?}", e);
        reboot().await;
    }

    for _ in 0..200 {
        Timer::after_millis(50).await;

        // let result = device.read_temperature().await;
        // info!("T: {:?}", result);
        // device
        //     .trigger_messurement()
        //     .await
        //     .expect("Failed to trigger trigger_messurement");

        match device.read_messurement() {
            Ok(d) => info!("Got: {:?}", d),
            Err(e) => error!("Error: {:?}", e),
        }
    }

    reboot().await;
}

async fn reboot() {
    info!("Doing cold boot");
    Timer::after_secs(1).await;
    embassy_rp::rom_data::reset_to_usb_boot(0, 0);
}