/*
Copyright (c) 2020 Todd Stellanova
LICENSE: BSD3 (see LICENSE file)
*/

#![no_main]
#![no_std]

use cortex_m_rt as rt;
use rt::entry;

use panic_rtt_core::{self, rprintln, rtt_init_print};

use embedded_hal::blocking::delay::DelayMs;
use embedded_hal::digital::v2::OutputPin;
use embedded_hal::digital::v2::ToggleableOutputPin;

const IMU_REPORTING_RATE_HZ: u16 = 200;
const IMU_REPORTING_INTERVAL_MS: u16 = 1000 / IMU_REPORTING_RATE_HZ;

/// Sensors
use ms5611::{Ms5611, Oversampling};
use ms5611_spi as ms5611;

use pixracer_bsp::peripherals;

#[entry]
fn main() -> ! {
    rtt_init_print!(NoBlockTrim);
    rprintln!("-- > MAIN --");

    let (
        mut user_leds,
        mut delay_source,
        _rng,
        _i2c1_port,
        _spi1_port,
        spi2_port,
        (_spi_cs_imu, _spi_drdy_imu),
        (_spi_cs_6dof, _spi_drdy_6dof),
        (_spi_cs_mag, _spi_drdy_mag),
        spi_cs_baro,
        _spi_cs_fram,
        mut spi1_power_enable,
        _tim1_pwm_chans,
    ) = peripherals::setup_peripherals();

    let spi_bus2 = shared_bus::CortexMBusManager::new(spi2_port);

    // power cycle spi devices
    let _ = spi1_power_enable.set_low();
    delay_source.delay_ms(250u8);
    let _ = spi1_power_enable.set_high();
    // wait a bit for sensors to power up
    delay_source.delay_ms(250u8);

    // Setup internal barometer
    let mut baro_int_opt = {
        let rc =
            Ms5611::new(spi_bus2.acquire(), spi_cs_baro, &mut delay_source);
        if let Ok(baro) = rc {
            Some(baro)
        } else {
            rprintln!("baro setup failed");
            None
        }
    };

    let loop_interval = IMU_REPORTING_INTERVAL_MS as u8;
    rprintln!("loop_interval: {}", loop_interval);

    let _ = user_leds.0.set_high();
    let _ = user_leds.1.set_low();
    let _ = user_leds.2.set_high();

    loop {
        if baro_int_opt.is_some() {
            if let Ok(sample) =
                baro_int_opt.as_mut().unwrap().get_second_order_sample(
                    Oversampling::OS_2048,
                    &mut delay_source,
                )
            {
                rprintln!("baro: {} ", sample.pressure);
            }
        }

        let _ = user_leds.0.toggle();
        let _ = user_leds.1.toggle();
        //let _ = user_leds.2.toggle();
    }
}