#![no_std]
#![no_main]

use arrayvec::ArrayVec;
use core::str::FromStr;
use embassy_net::tcp::ConnectError;
use embedded_io::ReadReady;

use ag_lcd::{Blink, Cursor, Display, LcdDisplay};
use as5600::As5600;
use cyw43::{JoinOptions, aligned_bytes};
use cyw43_pio::{DEFAULT_CLOCK_DIVIDER, PioSpi};
use defmt::*;
use embassy_executor::Spawner;
use embassy_futures::yield_now;
use embassy_net::tcp::client::{TcpClient, TcpClientState};
use embassy_net::{Stack, StackResources};
use embassy_rp::clocks::RoscRng;
use embassy_rp::gpio::{Input, Level, Output};
use embassy_rp::i2c::{self, Config, I2c};
use embassy_rp::peripherals::{DMA_CH0, I2C0, PIO0};
use embassy_rp::pio::{InterruptHandler, Pio};
use embassy_rp::{bind_interrupts, dma};
use embassy_rp::{peripherals::USB, usb};
use embassy_time::{Delay, Duration, Timer};
use static_cell::StaticCell;
use ufmt::{uWrite, uwrite};
use {defmt_rtt as _, panic_probe as _};

bind_interrupts!(struct Irqs {
    PIO0_IRQ_0 => InterruptHandler<PIO0>;
    DMA_IRQ_0 => dma::InterruptHandler<DMA_CH0>;
    USBCTRL_IRQ => usb::InterruptHandler<USB>;
    I2C0_IRQ => i2c::InterruptHandler<I2C0>;
});

#[embassy_executor::task]
async fn cyw43_task(
    runner: cyw43::Runner<'static, cyw43::SpiBus<Output<'static>, PioSpi<'static, PIO0, 0>>>,
) -> ! {
    runner.run().await
}
#[embassy_executor::task]
async fn logger_task(usb: embassy_rp::Peri<'static, embassy_rp::peripherals::USB>) {
    let driver = embassy_rp::usb::Driver::new(usb, Irqs);

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

const WIFI_NETWORK: &str = "aura";
const WIFI_PASSWORD: &str = "12345678";

#[embassy_executor::main]
async fn main(spawner: Spawner) {
    let p = embassy_rp::init(Default::default());

    let fw = aligned_bytes!("../firmware/43439A0.bin");
    let clm = aligned_bytes!("../firmware/43439A0_clm.bin");
    let nvram = aligned_bytes!("../firmware/nvram_rp2040.bin");
    let mut rng = RoscRng;

    // 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 ../../cyw43-firmware/43439A0.bin --binary-format bin --chip RP2040 --base-address 0x10100000
    //     probe-rs download ../../cyw43-firmware/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,
        DEFAULT_CLOCK_DIVIDER,
        pio.irq0,
        cs,
        p.PIN_24,
        p.PIN_29,
        dma::Channel::new(p.DMA_CH0, Irqs),
    );

    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, nvram).await;

    spawner.spawn(unwrap!(cyw43_task(runner)));
    spawner.spawn(unwrap!(logger_task(p.USB)));

    let edelay = &mut Delay;

    let mut lcd: LcdDisplay<_, _> = LcdDisplay::new(
        Output::new(p.PIN_14, Level::Low),
        Output::new(p.PIN_15, Level::Low),
        edelay,
    )
    .with_half_bus(
        Output::new(p.PIN_10, Level::Low),
        Output::new(p.PIN_11, Level::Low),
        Output::new(p.PIN_12, Level::Low),
        Output::new(p.PIN_13, Level::Low),
    )
    .with_display(Display::On)
    .with_blink(Blink::On)
    .with_cursor(Cursor::On)
    .with_lines(ag_lcd::Lines::TwoLines)
    // .with_autoscroll(ag_lcd::AutoScroll::On)
    .build();
    lcd.set_cursor(Cursor::Off);
    lcd.set_blink(Blink::Off);

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

    static RESOURCES: StaticCell<StackResources<5>> = StaticCell::new();
    let (stack, runner) = embassy_net::new(
        net_device,
        embassy_net::Config::dhcpv4(Default::default()),
        RESOURCES.init(StackResources::new()),
        rng.next_u64(),
    );
    spawner.spawn(unwrap!(net_task(runner)));
    uwrite!(lcd, "con.");

    while let Err(err) = control
        .join(WIFI_NETWORK, JoinOptions::new(WIFI_PASSWORD.as_bytes()))
        .await
    {
        lcd.clear();
        let num = match err {
            cyw43::JoinError::AuthenticationFailure => 200,
            cyw43::JoinError::JoinFailure(e) => e,
            cyw43::JoinError::NetworkNotFound => 201,
        };
        uwrite!(lcd, "join {}", num);
    }

    uwrite!(lcd, "link.");
    stack.wait_link_up().await;

    lcd.clear();
    // lcd.home();
    uwrite!(lcd, "dhcp.");
    stack.wait_config_up().await;
    let cfg = wait_for_config(stack).await;
    let local_addr = cfg.address.address();
    info!("IP address: {:?}", local_addr);
    // uwrite!(lcd, "IP address: {:?}", local_addr.octets());

    let i2c = I2c::new_async(p.I2C0, p.PIN_5, p.PIN_4, Irqs, Config::default());
    let mut as5600 = As5600::new(i2c);

    let mut led = Output::new(p.PIN_1, Level::Low);

    let in1 = Input::new(p.PIN_18, embassy_rp::gpio::Pull::Up);
    let in2 = Input::new(p.PIN_19, embassy_rp::gpio::Pull::Up);
    let in3 = Input::new(p.PIN_20, embassy_rp::gpio::Pull::Up);
    let in4 = Input::new(p.PIN_21, embassy_rp::gpio::Pull::Up);

    let mut rx_buffer = [0; 4096];
    let mut tx_buffer = [0; 4096];
    let mut socket = embassy_net::tcp::TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer);
    socket.set_timeout(Some(Duration::from_secs(10)));

    led.set_low();
    info!("Connecting...");
    uwrite!(lcd, "con2.");
    let host_addr = embassy_net::Ipv4Address::from_str("192.168.12.1").unwrap();
    if let Err(e) = socket.connect((host_addr, 7070)).await {
        lcd.clear();
        uwrite!(lcd, "conerr");
        Timer::after(Duration::from_micros(100)).await;
        lcd.set_position(0, 1);
        let emsg = match e {
            ConnectError::ConnectionReset => "rst",
            ConnectError::InvalidState => "inv",
            ConnectError::TimedOut => "tout",
            ConnectError::NoRoute => "nroute",
        };
        uwrite!(lcd, "{}", emsg);
        warn!("connect error: {:?}", e);
    } else {
        uwrite!(lcd, "conok");
    }
    info!("Connected to {:?}", socket.remote_endpoint());

    let delay = Duration::from_millis(100);
    let mut buffer = ArrayVec::<u8, 512>::new();
    loop {
        let in1 = in1.is_low();
        let in2 = in2.is_low();
        let in3 = in3.is_low();
        let in4 = in4.is_low();
        let angle = as5600.angle().unwrap_or(0);
        {
            use embedded_io::Write;
            let _ = core::writeln!(
                &mut buffer[..],
                "in1={} in2={} in3={} in4={} angle={}",
                in1,
                in2,
                in3,
                in4,
                angle
            );
        }
        {
            use embedded_io_async::Write;
            let _ = socket.write_all(&*buffer).await;
        }
        if socket.read_ready().unwrap_or(false) {
            let mut rx_buffer = [0; 4096];
            let n = socket.read(&mut rx_buffer).await.unwrap_or(0);
            if n > 0 {
                lcd.clear();
                lcd.home();
                let s = core::str::from_utf8(&rx_buffer[..n]).unwrap_or("");
                let npos = s.find('\n').unwrap_or(n);
                let display_text = &s[..npos];
                lcd.write_str(display_text).ok();

                Timer::after(Duration::from_micros(100)).await;
                lcd.set_position(0, 1);
                lcd.write_str(&s[npos..]);
            }
        }
        Timer::after(delay).await;
    }
}

async fn wait_for_config(stack: Stack<'static>) -> embassy_net::StaticConfigV4 {
    loop {
        if let Some(config) = stack.config_v4() {
            return config.clone();
        }
        yield_now().await;
    }
}