#![no_std]
#![no_main]

use core::f32::consts::PI;
use core::fmt::Write;

use arduino_mkrzero::atsamd21g18a::{CorePeripherals, Peripherals};
use arduino_mkrzero::clock::{GenericClockController, ClockGenId, ClockSource};
use arduino_mkrzero::delay::Delay;
use arduino_mkrzero::prelude::*;
use arduino_mkrzero::sercom::{self, I2CMaster0, PadPin, UART3, UART5, UART3Pinout, UART5Pinout};
use arduino_mkrzero::entry;
use arduino_mkrzero::gclk::clkctrl::GENR::*;
use arduino_mkrzero::gclk::genctrl::SRCR::*;

use nb;
//use libm::F32Ext;

//use crate::consts::*;

//const MPU9250_ADDRESS: u8 = 0x68;
//const MPU_WHOAMI_REGISTER: u8 = 117;
//const MAG_ADDRESS: u8 = 0x0C;
//const MAG_WHOAMI_REGISTER: u8 = 0x0;

//mod stepper;
//mod consts;

#[entry]
fn main() -> ! {
    let mut peripherals = Peripherals::take().unwrap();
    let mut core = CorePeripherals::take().unwrap();
    let mut clocks = GenericClockController::with_internal_32kosc(
        peripherals.GCLK,
        &mut peripherals.PM,
        &mut peripherals.SYSCTRL,
        &mut peripherals.NVMCTRL,
    );

    let mut pins = arduino_mkrzero::Pins::new(peripherals.PORT);
    let gclk0 = clocks.gclk0().clone();

    let rx_pin = pins.rx.into_pull_down_input(&mut pins.port).into_pad(&mut pins.port);
    let tx_pin = pins.tx.into_push_pull_output(&mut pins.port).into_pad(&mut pins.port);
    let mut serial = UART5::new(
        &clocks.sercom5_core(&gclk0).unwrap(),
        9600.hz(),
        peripherals.SERCOM5,
        &mut core.NVIC,
        &mut peripherals.PM,
        UART5Pinout::Rx3Tx2 {rx: rx_pin, tx: tx_pin},
    );

    let mut builtin_led = pins.led_builtin.into_open_drain_output(&mut pins.port);
    let mut led_state = true;
    let mut delay = Delay::new(core.SYST, &mut clocks);

    serial.write_str("Initialised\n").unwrap();

    let gclk2 = clocks
        .configure_gclk_divider_and_source(GCLK2, 1, DPLL96M, false)
        .unwrap();
    let gps_rx_pin: sercom::Sercom0Pad1 = pins.scl.into_pull_down_input(&mut pins.port).into_pad(&mut pins.port);
    let gps_tx_pin: sercom::Sercom0Pad0 = pins.sda.into_push_pull_output(&mut pins.port).into_pad(&mut pins.port);
    let mut gps_serial = sercom::UART0::new(
        &clocks.sercom0_core(&gclk2).unwrap(),
        9600.hz(),
        peripherals.SERCOM0,
        &mut core.NVIC,
        &mut peripherals.PM,
        sercom::UART0Pinout::Rx1Tx0 {
            rx: gps_rx_pin,
            tx: gps_tx_pin,
        },
    );

    serial.write_str("GPS Initialised\n").unwrap();

    serial.write_str("\n").unwrap();

    loop {
        serial.write_str("Loop start").unwrap();
        'inner: loop {
            match gps_serial.read() {
                Result::Ok(byte) => {
                    serial.write_str("Byte found: ").unwrap();
                    serial.write(byte).unwrap();
                    serial.write_str("\n").unwrap();
                },
                Result::Err(nb::Error::WouldBlock) => {
                    serial.write_str("Would Block\n").unwrap();
                    break 'inner;
                },
                Result::Err(nb::Error::Other(())) => {
                    serial.write_str("Other error\n").unwrap();
                    break 'inner;
                },
            }
        }

        if led_state {
            builtin_led.set_high();
        } else {
            builtin_led.set_low();
        }
        led_state = !led_state;
        delay.delay_ms(200u8);
    }
}