//! This example uses the RP Pico W board Wifi chip (cyw43).
//! Creates an Access point Wifi network and creates a TCP endpoint on port 1234.

#![no_std]
#![no_main]
#![allow(async_fn_in_trait)]

mod sensor_manager;

use core::array;
use core::borrow::BorrowMut;
use core::cell::RefCell;
use core::fmt::Formatter;
use core::panic::PanicInfo;
use core::str::from_utf8;

use bt_hci::cmd::info;
use cyw43_pio::PioSpi;
use embassy_rp::i2c::{Async, I2c};
use embassy_rp::multicore::spawn_core1;
use embassy_rp::pwm::{self, Pwm};
use embedded_hal_bus::i2c::RefCellDevice;
use fixed::FixedU16;
use log::*;
//use embassy_rp::i2c::InterruptHandler;
use embassy_executor::Spawner;
use embassy_net::tcp::TcpSocket;
use embassy_net::{Config, StackResources};
use embassy_rp::bind_interrupts;
use embassy_rp::clocks::RoscRng;
use embassy_rp::gpio::{Level, Output};
use embassy_rp::peripherals::{DMA_CH0, PIO0, USB};
use embassy_rp::pio::{InterruptHandler, Pio};
use embassy_rp::usb::Driver;
use embassy_time::{Delay, Duration, Timer};
use embedded_io_async::Write;
use mpu6050::Mpu6050;
use rand::RngCore;
use reqwless::response;
use sensor_manager::{sensor_manager, SensorData};
use static_cell::StaticCell;
use defmt_rtt as _;

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

static mut CORE1_STACK: Stack<4096> = Stack::new();
static CHANNEL: Channel<CriticalSectionRawMutex, SensorData, 1> = Channel::new();

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

#[embassy_executor::task]
async fn cyw43_task(runner: cyw43::Runner<'static, Output<'static>, PioSpi<'static, PIO0, 0, DMA_CH0>>) -> ! {
    runner.run().await
}

#[embassy_executor::task]
async fn net_task(mut runner: embassy_net::Runner<'static, cyw43::NetDriver<'static>>) -> ! {
    runner.run().await
}

#[panic_handler]
fn panic( info: &PanicInfo) -> ! {
    error!("{}", info);
    loop { }
}

#[embassy_executor::main]
async fn main(spawner: Spawner) {
    info!("Hello World!");

    let p = embassy_rp::init(Default::default());
    let mut rng = RoscRng;

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

    let mut flipper = Output::new(p.PIN_22, Level::Low);

    let mut c: pwm::Config = Default::default();
    c.divider = 255.into();
    c.top = 8715;
    c.compare_b = 7210;
    c.compare_a = 2240;
    let mut pwm = Pwm::new_output_ab(p.PWM_SLICE1, p.PIN_18, p.PIN_19, c.clone());
    flipper.set_high();
    
    let sda = p.PIN_20;
    let scl = p.PIN_21;
    let config = embassy_rp::i2c::Config::default();
    let bus = embassy_rp::i2c::I2c::new_async(p.I2C0, scl, sda, Irqs, config);

    let sda = p.PIN_26;
    let scl = p.PIN_27;
    let config = embassy_rp::i2c::Config::default();
    let bus1 = embassy_rp::i2c::I2c::new_async(p.I2C1, scl, sda, Irqs, config);
    let bus1 = RefCell::new(bus1);
    let mut tof2enable = Output::new(p.PIN_16, Level::Low);


    spawn_core1(
        p.CORE1, 
        unsafe { &mut *core::ptr::addr_of_mut!(CORE1_STACK) }, 
        move || {
            sensor_manager(bus, tof2enable)
        }
    );

    info!("made RefCell");

    info!("2");
    Timer::after_millis(1).await;
    let mut tof = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus1)).unwrap();
    info!("3");
    tof.set_address(0x32).unwrap();
    info!("4");
    tof2enable.set_high();
    Timer::after_micros(1200).await; // DS11555 3.2
    info!("5");
    let mut tof2 = vl53l0x::VL53L0x::new(RefCellDevice::new(&bus1)).unwrap();
    info!("6");


    let mut gyro = Mpu6050::new_with_addr(bus,0x68);

    Timer::after_millis(1300).await;
    gyro.init(&mut Delay).unwrap();

    gyro.set_gyro_range(mpu6050::device::GyroRange::D2000).unwrap();
    gyro.set_accel_range(mpu6050::device::AccelRange::G16).unwrap();
    info!("pinidtd");
    Timer::after_millis(1300).await;

    loop {
        //info!("pos: {}", gyro.get_gyro().unwrap().x);
        //info!("jerk: {}", gyro.get_acc().unwrap().x);
        //info!("temp: {}", gyro.get_temp().unwrap() * (9./5.) + 32.);
        if let Ok(dist) = tof.read_range_single_millimeters_blocking() {
            info!("dist1: {dist}mm");
        }
        Timer::after_millis(3).await;
        if let Ok(dist) = tof2.read_range_single_millimeters_blocking() {
            info!("dist2: {dist}mm");
        }
        Timer::after_millis(23).await;
    }

    let fw = include_bytes!("../../cyw43-firmware/43439A0.bin");
    let clm = include_bytes!("../../cyw43-firmware/43439A0_clm.bin");

    // To make flashing faster for development, you may want to flash the firmwares independently
    // at hardcoded addresses, instead of baking them into the program with `include_bytes!`:
    //     probe-rs download 43439A0.bin --binary-format bin --chip RP2040 --base-address 0x10100000
    //     probe-rs download 43439A0_clm.bin --binary-format bin --chip RP2040 --base-address 0x10140000
    //let fw = unsafe { core::slice::from_raw_parts(0x10100000 as *const u8, 230321) };
    //let clm = unsafe { core::slice::from_raw_parts(0x10140000 as *const u8, 4752) };

    let pwr = Output::new(p.PIN_23, Level::Low);
    let cs = Output::new(p.PIN_25, Level::High);
    let mut pio = Pio::new(p.PIO0, Irqs);
    let spi = PioSpi::new(&mut pio.common, pio.sm0, pio.irq0, cs, p.PIN_24, p.PIN_29, p.DMA_CH0);

    static STATE: StaticCell<cyw43::State> = StaticCell::new();
    let state = STATE.init(cyw43::State::new());
    let (net_device, mut control, runner) = cyw43::new(state, pwr, spi, fw).await;
    defmt::unwrap!(spawner.spawn(cyw43_task(runner)));

    control.init(clm).await;
    control
        .set_power_management(cyw43::PowerManagementMode::PowerSave)
        .await;

    // Use a link-local address for communication without DHCP server
    let config = Config::ipv4_static(embassy_net::StaticConfigV4 {
        address: embassy_net::Ipv4Cidr::new(embassy_net::Ipv4Address::new(169, 254, 1, 1), 16),
        dns_servers: heapless::Vec::new(),
        gateway: None,
    });

    // Generate random seed
    let seed = rng.next_u64();

    // Init network stack
    static RESOURCES: StaticCell<StackResources<3>> = StaticCell::new();
    let (stack, runner) = embassy_net::new(net_device, config, RESOURCES.init(StackResources::new()), seed);

    defmt::unwrap!(spawner.spawn(net_task(runner)));

    //control.start_ap_open("cyw43", 5).await;
    control.start_ap_wpa2("lovesense_setup2", "password123", 5).await;

    // And now we can use it!

    let mut rx_buffer = [0; 4096];
    let mut tx_buffer = [0; 4096];
    let mut buf = [0; 4096];

    //embassy_time::Timer::after_millis(7000).await;

    //async fn write_to_device<'a, T,const N: usize>(bus: &mut embassy_rp::i2c::I2c<'a, T,Async>, addr: u16, data: [[u8;2];N])
    //where T: embassy_rp::i2c::Instance { 
    //    for transaction in data {
    //        let _ = bus.write_async(addr, transaction).await;
    //    }
    //}

    //async fn write_flag<'a, T>(bus: &mut embassy_rp::i2c::I2c<'a, T,Async>, addr: u16, reg: u8, bit: u8, value: bool)
    //where T: embassy_rp::i2c::Instance { 
    //    let mut initial: [u8;1] = [0];
    //    let _ = bus.write_read_async(addr, [reg], &mut initial).await;
    //    let mask = 1 << bit;
    //    if value {
    //        initial[0] |= mask;
    //    } else {
    //        initial[0] &= !mask;
    //    }
    //    let _ = bus.write_async(addr, [reg, initial[0]]).await;
    //}

    //let id = 0x29;
    //Timer::after_millis(50).await; // sensor boot
    //write_to_device(&mut bus, id, [
    //    [0x88, 0x00],
    //    [0x80, 0x01],
    //    [0xFF, 0x01],
    //    [0x00, 0x00],
    //]).await;
    //let mut stop: [u8;1] = [0];
    //let _ = bus.write_read_async(id, [0x91], &mut stop);
    //write_to_device(&mut bus, id, [
    //    [0x00, 0x01],
    //    [0xFF, 0x00],
    //    [0x80, 0x00],
    //]).await;
    //
    //// disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
    //write_flag(&mut bus, id, 0x60, 1, true).await;
    //write_flag(&mut bus, id, 0x60, 4, true).await;

    //let mega_counts_per_second = 0.25;
    //let mega_counts_per_second: u16 = (mega_counts_per_second * (1<<7) as f64) as u16;
    //let _ = bus.write_async(id, [FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT as u8,
    //    (mega_counts_per_second >> 8) as u8,
    //    (mega_counts_per_second & 255) as u8
    //]).await;

    //// get spad info
    //let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0xFF]).await;

    //write_to_device(&mut bus, id , [
    //        [0x80, 0x01],
    //        [0xff, 0x01],
    //        [0x00, 0x00],
    //        [0xff, 0x06],
    //]).await;
    //write_flag(&mut bus, id, 0x83, 3, true).await;
    //write_to_device(&mut bus, id , [
    //        [0xff, 0x07],
    //        [0x81, 0x01],
    //        [0x80, 0x01],
    //        [0x94, 0x6b],
    //        [0x83, 0x00],
    //]).await;
    //debug!("starting spad wait");
    //loop {
    //    let mut wait: [u8;1] = [0];
    //    let _ = bus.write_read_async(id, [0x83], &mut wait).await;
    //    if wait[0] != 0 {
    //        break;
    //    }
    //    Timer::after_micros(5).await;
    //}
    //debug!("ended spad wait");

    //let _ = bus.write_async(id, [0x83, 0x01]).await;

    //let mut value: [u8;1] = [0];
    //let _ = bus.write_read_async(id, [0x92], &mut value).await;
    //write_to_device(&mut bus, id, [
    //        [0x81, 0x00],
    //        [0xff, 0x06],
    //]).await;
    //write_flag(&mut bus, id, 0x83, 3, false).await;
    //write_to_device(&mut bus, id, [
    //    [0xff, 0x01],
    //    [0x00, 0x01],
    //    [0xff, 0x00],
    //    [0x80, 0x00],
    //]).await;
    //let count = value[0] & 0x7f;
    //let is_aperture = value[0] & 0b10000000;
    //let is_aperture = is_aperture != 0;
    //// TODO: vl53l0x.py post line 200

    //let mut spad_map: [u8;6]=[0;6];
    //let _ = bus.write_read_async(id, [GLOBAL_CONFIG_SPAD_ENABLES_REF_0 as u8], &mut spad_map);

    //write_to_device(&mut bus, id, [
    //    [0xff, 0x01],
    //    [DYNAMIC_SPAD_REF_EN_START_OFFSET as u8, 0x00],
    //    [DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD as u8, 0x2c],
    //    [0xff, 0x00],
    //    [DYNAMIC_SPAD_REF_EN_START_OFFSET as u8, 0xb4],
    //]).await;

    //let mut spads_enabled = 0;
    //for i in 0..48 {
    //    if i < 12 && is_aperture || spads_enabled >= count {
    //        spad_map[i/8] &= !(1<< (i>>2));
    //    } else if (spad_map[i/8] & (1<< (i>>2))) != 0 {
    //        spads_enabled += 1;
    //    }
    //}

    //let mut spad_write: [u8;7] = [0;7];
    //spad_write[0] = GLOBAL_CONFIG_SPAD_ENABLES_REF_0 as u8;
    //spad_write[1..].clone_from_slice(&spad_map);
    //let _ = bus.write_async(id, spad_write).await;

    //
    //write_to_device(&mut bus, id, [
    //    [0xFF, 0x01],
    //    [0x00, 0x00],
    //    [0xFF, 0x00],
    //    [0x09, 0x00],
    //    [0x10, 0x00],
    //    [0x11, 0x00],
    //    [0x24, 0x01],
    //    [0x25, 0xFF],
    //    [0x75, 0x00],
    //    [0xFF, 0x01],
    //    [0x4E, 0x2C],
    //    [0x48, 0x00],
    //    [0x30, 0x20],
    //    [0xFF, 0x00],
    //    [0x30, 0x09],
    //    [0x54, 0x00],
    //    [0x31, 0x04],
    //    [0x32, 0x03],
    //    [0x40, 0x83],
    //    [0x46, 0x25],
    //    [0x60, 0x00],
    //    [0x27, 0x00],
    //    [0x50, 0x06],
    //    [0x51, 0x00],
    //    [0x52, 0x96],
    //    [0x56, 0x08],
    //    [0x57, 0x30],
    //    [0x61, 0x00],
    //    [0x62, 0x00],
    //    [0x64, 0x00],
    //    [0x65, 0x00],
    //    [0x66, 0xA0],
    //    [0xFF, 0x01],
    //    [0x22, 0x32],
    //    [0x47, 0x14],
    //    [0x49, 0xFF],
    //    [0x4A, 0x00],
    //    [0xFF, 0x00],
    //    [0x7A, 0x0A],
    //    [0x7B, 0x00],
    //    [0x78, 0x21],
    //    [0xFF, 0x01],
    //    [0x23, 0x34],
    //    [0x42, 0x00],
    //    [0x44, 0xFF],
    //    [0x45, 0x26],
    //    [0x46, 0x05],
    //    [0x40, 0x40],
    //    [0x0E, 0x06],
    //    [0x20, 0x1A],
    //    [0x43, 0x40],
    //    [0xFF, 0x00],
    //    [0x34, 0x03],
    //    [0x35, 0x44],
    //    [0xFF, 0x01],
    //    [0x31, 0x04],
    //    [0x4B, 0x09],
    //    [0x4C, 0x05],
    //    [0x4D, 0x04],
    //    [0xFF, 0x00],
    //    [0x44, 0x00],
    //    [0x45, 0x20],
    //    [0x47, 0x08],
    //    [0x48, 0x28],
    //    [0x67, 0x00],
    //    [0x70, 0x04],
    //    [0x71, 0x01],
    //    [0x72, 0xFE],
    //    [0x76, 0x00],
    //    [0x77, 0x00],
    //    [0xFF, 0x01],
    //    [0x0D, 0x01],
    //    [0xFF, 0x00],
    //    [0x80, 0x01],
    //    [0x01, 0xF8],
    //    [0xFF, 0x01],
    //    [0x8E, 0x01],
    //    [0x00, 0x01],
    //    [0xFF, 0x00],
    //    [0x80, 0x00],

    //    [SYSTEM_INTERRUPT_CONFIG_GPIO as u8, 0x04],
    //]).await;
    ////write_flag(&mut bus, id, GPIO_HV_MUX_ACTIVE_HIGH as u8, 4, false).await;
    ////let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0x01]).await;
    ////calibrate(&mut bus, id, 0x40).await;
    ////let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0x02]).await;
    ////calibrate(&mut bus, id, 0x00).await;
    ////let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0xe8]).await;

    //async fn calibrate<'a, T>(bus: &mut embassy_rp::i2c::I2c<'a, T,Async>, addr: u16, data: u8)
    //where T: embassy_rp::i2c::Instance { 
    //    let _ = bus.write_async(addr, [SYSRANGE_START as u8 ,data | 0x01]).await;
    //    debug!("started calib wait");
    //    loop {
    //        let mut wait: [u8;1] = [0];
    //        let _ = bus.write_read_async(addr, [0x13], &mut wait).await;
    //        if wait[0] & 0x07 != 0 {
    //            break;
    //        }
    //        Timer::after_micros(5).await;
    //    }
    //    debug!("ended calib wait");
    //    write_to_device(bus, addr, [
    //        [SYSTEM_INTERRUPT_CLEAR as u8, 0x01],
    //        [SYSRANGE_START as u8, 0x00],
    //    ]).await;
    //}


    ////TODO VL53L0X.cpp L 236-280


    //// start continuous mode
    //write_to_device(&mut bus, id, [
    //    [0x80, 0x01],
    //    [0xFF, 0x01],
    //    [0x00, 0x00],
    //    [0x91, stop[0]],
    //    [0x00, 0x01],
    //    [0xFF, 0x00],
    //    [0x80, 0x00],
    //    //[0x04, 0x00], // measurement delay
    //    [0x00, 0x02], // back to back shots
    //]).await;

    ////let _ = bus.write_async(0x88u16, [0x00]).await;
    ////let _ = bus.write_async(0x80u16, [0x01]).await;
    ////let _ = bus.write_async(0xFFu16, [0x01]).await;
    ////let _ = bus.write_async(0x00u16, [0x00]).await;
    ////let _ = bus.write_async(0x00u16, [0x01]).await;
    ////let _ = bus.write_async(0xFFu16, [0x00]).await;
    ////let _ = bus.write_async(0x80u16, [0x00]).await;
    //
    ////loop {
    ////    debug!("starting wait part 1");
    ////    loop {
    ////        let mut wait: [u8;1] = [0];
    ////        let _ = bus.write_read_async(id, [SYSRANGE_START as u8], &mut wait).await;
    ////        debug!("{wait:?}");
    ////        if wait[0] & 0x01 != 0 {
    ////            break;
    ////        }
    ////        Timer::after_micros(5).await;
    ////    }
    ////    debug!("starting wait part 2");
    ////    loop {
    ////        let mut wait: [u8;1] = [0];
    ////        let _ = bus.write_read_async(id, [RESULT_INTERRUPT_STATUS as u8], &mut wait).await;
    ////        if wait[0] & 0x07 != 0 {
    ////            break;
    ////        }
    ////        Timer::after_micros(5).await;
    ////    }
    ////    debug!("ending wait part 2");
    ////    let mut output: [u8; 2] = [0;2];
    ////    let _ = bus.write_read_async(id, [0x14], &mut output).await;
    ////    let _ = bus.write_async(id, [0x0B, 0x01]).await;
    ////    info!("out: {:?}",output);
    ////    Timer::after_millis(20).await;
    ////}

    loop {
        let mut socket = TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer);
        socket.set_timeout(Some(Duration::from_secs(3)));

        let mut c: pwm::Config = Default::default();
        c.divider = 255.into();
        c.top = 8715;
        c.compare_b = 0;
        c.compare_a = 0;
        pwm.set_config(&c);

        flipper.set_low();

        control.gpio_set(0, false).await;
        info!("Listening on TCP:1234...");
        if let Err(e) = socket.accept(1234).await {
            warn!("accept error: {:?}", e);
            continue;
        }

        info!("Received connection from {:?}", socket.remote_endpoint());
        control.gpio_set(0, true).await;

        loop {
            let mut n = match socket.read(&mut buf).await {
                Ok(0) => {
                    warn!("read EOF");
                    break;
                }
                Ok(n) => n,
                Err(e) => {
                    warn!("read error: {:?}", e);
                    break;
                }
            };
            

            info!("rxd {}", from_utf8(&buf[..n]).unwrap());
            Timer::after_millis(30).await;

            let mut segs = buf[..n].trim_ascii().split(|c| *c == ' ' as u8);

            match char::from_u32(segs.next().unwrap()[0] as u32).unwrap() {
                'W' => {
                    let addr: [u8;1] =  hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
                    let reg: [u8;1] =  hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
                    let data: [u8;1] =  hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
                    info!("writing {:?}", reg);
                    Timer::after_millis(30).await;

                    bus.write_async(addr[0] as u16, [reg[0], data[0]]).await.unwrap();

                    buf[0] = b"O"[0];
                    buf[1] = b"K"[0];
                    n=2;
                },
                'R' => {
                    let addr: [u8;1] =  hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
                    let reg: [u8;1] =  hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
                    info!("writing {:?}, addr {addr:?}", reg);

                    let mut response: [u8;2] = [0;2];

                    let _ = bus.write_read_async(addr[0] as u16, reg, &mut response).await;
                    info!("recd {:?}", response);
                    info!("recd {:#02x}", response[0]);
                    let _ = hex::encode_to_slice(response, &mut buf);
                    n = 4;
                },
                'D' => {
                    let left: [u8;2] =  hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
                    let right: [u8;2] =  hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
                    info!("left {:?}, right {left:?}", right);

                    let left = u16::from_be_bytes(left);
                    c.compare_a = left;
                    let right = u16::from_be_bytes(right);
                    c.compare_b = right;
                    info!("left {:?}, right {left:?}", right);
                    pwm.set_config(&c);
                },
                'F' => {
                    flipper.set_high();
                },
                'L' => {
                    flipper.set_low();
                },
                _ => {}
                
            }

            //let mut response: [u8;2] = [0;2];

            //let _ = bus.read_async(0xC0u16, &mut response).await;
            //let _ = hex::encode_to_slice(response, &mut buf);

            match socket.write_all(&buf[..n]).await {
                Ok(()) => {}
                Err(e) => {
                    warn!("write error: {:?}", e);
                    break;
                }
            };
        }
    }
}