//https://github.com/rust-embedded/awesome-embedded-rust#driver-crates

use esp_idf_sys as _; // If using the `binstart` feature of `esp-idf-sys`, always keep this module imported

use esp_idf_hal::{
    i2c::{I2cConfig, I2cDriver},
    peripherals::Peripherals,
    prelude::*,
};
//use esp_idf_hal::peripherals::Peripherals;
//use esp_idf_hal::i2c::*;
//use esp_idf_hal::prelude::*; //KHz
use embedded_hal::blocking::delay::DelayMs;
use std::thread;
use std::time::Duration;

use shared_bus;

use mpu6050::*;

use ssd1306::{prelude::*, I2CDisplayInterface, Ssd1306};
use ssd1306::mode::BufferedGraphicsMode;
use embedded_graphics::{
  mono_font::{ascii::FONT_6X10, MonoTextStyleBuilder, MonoTextStyle},
  text::{Baseline, Text},
  pixelcolor::BinaryColor,
  prelude::*,
  primitives::{PrimitiveStyleBuilder, Rectangle, Circle, PrimitiveStyle}
};


//------------------------------------------------------------
struct Delay;

impl DelayMs<u8> for Delay {
    fn delay_ms(&mut self, n:u8) {
        thread::sleep(Duration::from_millis(100));
    }
}

/*
impl DelayUs for Delay {
    fn delay_us(&mut self, n: u32) {
        let secs = n / 1_000_000;
        let nsecs = (n % 1_000_000) * 1_000;

        thread::sleep(Duration::new(u64(secs), nsecs));
    }
}
*/

fn main() {
    esp_idf_sys::link_patches();
    
    let peripherals = Peripherals::take().unwrap();

    let config = I2cConfig::new().baudrate(100.kHz().into());
    let mut i2c = I2cDriver::new(peripherals.i2c0, peripherals.pins.gpio7, peripherals.pins.gpio6, &config).unwrap();

    let bus = shared_bus::BusManagerSimple::new(i2c);

    let mut mpu = Mpu6050::new(bus.acquire_i2c());

    let mut delay  = Delay;
    mpu.init(&mut delay);

    let interface = I2CDisplayInterface::new(bus.acquire_i2c());
    let disp = DisplaySize128x64;
    let rot = DisplayRotation::Rotate0;
    let mut display = Ssd1306::new(interface, disp, rot).into_buffered_graphics_mode();

    let white= PrimitiveStyleBuilder::new()
        .stroke_width(1)
        .stroke_color(BinaryColor::On)
        .build();

    let black= PrimitiveStyleBuilder::new()
        .fill_color(BinaryColor::Off)
        .build();

    let text_style = MonoTextStyleBuilder::new()
        .font(&FONT_6X10)
        .text_color(BinaryColor::On)
        .build();

    display.init().unwrap();

    //thread::sleep(Duration::from_millis(100));

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

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

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

        // get accelerometer data, scaled with sensitivity
        let acc = mpu.get_acc();
        println!("acc: {:?}", acc);

        let acc_str: &str = &acc.unwrap()[0].to_string();

        Rectangle::new(Point::new(0, 0), Size::new(128,64))
        .into_styled(black)
        .draw(&mut display);

        Text::with_baseline(acc_str, Point::zero(), text_style, Baseline::Top)
        .draw(&mut display)
        .unwrap();

        display.flush().unwrap();

        thread::sleep(Duration::from_millis(100));
    }

}