#![no_std]
#![no_main]

use bsp::entry;
use bsp::hal::{
    clocks::init_clocks_and_plls, fugit::RateExtU32, gpio::FunctionI2c, i2c::I2C, pac, sio::Sio,
    watchdog::Watchdog,
};
use rp_pico as bsp;

use defmt::info;
use defmt_rtt as _;
use panic_probe as _;

use accelerometer::{vector::F32x3, Accelerometer};
use stk8ba58::Stk8ba58;

#[entry]
fn main() -> ! {
    info!("Program start");
    let mut pac = pac::Peripherals::take().unwrap();
    let mut watchdog = Watchdog::new(pac.WATCHDOG);
    let sio = Sio::new(pac.SIO);

    let clocks = init_clocks_and_plls(
        12_000_000u32,
        pac.XOSC,
        pac.CLOCKS,
        pac.PLL_SYS,
        pac.PLL_USB,
        &mut pac.RESETS,
        &mut watchdog,
    )
    .ok()
    .unwrap();

    let pins = bsp::Pins::new(
        pac.IO_BANK0,
        pac.PADS_BANK0,
        sio.gpio_bank0,
        &mut pac.RESETS,
    );

    let i2c = I2C::i2c1(
        pac.I2C1,
        pins.gpio18.into_function::<FunctionI2c>(),
        pins.gpio19.into_function::<FunctionI2c>(),
        400u32.kHz(),
        &mut pac.RESETS,
        &clocks.system_clock,
    );

    let mut accelerometer = Stk8ba58::new(i2c);

    loop {
        let F32x3 { x, y, z } = accelerometer.accel_norm().unwrap();
        info!("X axis acceleration = {}", x);
        info!("Y axis acceleration = {}", y);
        info!("Z axis acceleration = {}", z);
    }
}