#![no_main]
#![no_std]
#![feature(core_intrinsics)]
#![feature(assoc_char_funcs)]

use log::{ trace, warn };
use stm32h7xx_hal::gpio::Speed;
use stm32h7xx_hal::{pac, prelude::*, spi};
use stm32h7xx_hal::ethernet;

use smoltcp as net;
use minimq::{
    embedded_nal::{ IpAddr, Ipv4Addr },
    MqttClient, QoS
};

use cortex_m;
use cortex_m_rt::entry;
use rtic::cyccnt::{Instant, U32Ext};

use heapless::{ String, consts, consts::* };

use core::convert::TryInto;

#[macro_use]
pub mod bitmask_macro;
pub mod spi_slave;
pub mod cpld;
use crate::cpld::CPLD;
pub mod config_register;
pub mod attenuator;
pub mod dds;
pub mod nal_tcp_client;
use crate::nal_tcp_client::{ NetStorage, NetworkStack };
pub mod flash;
use crate::flash::flash_ice40_fpga;
pub mod mqtt_mux;
use crate::mqtt_mux::MqttMux;
pub mod urukul;
use crate::urukul::Urukul;

mod logger;

static mut NET_STORE: NetStorage = NetStorage {
    // Placeholder for the real IP address, which is initialized at runtime.
    ip_addrs: [net::wire::IpCidr::Ipv6(
        net::wire::Ipv6Cidr::SOLICITED_NODE_PREFIX,
    )],
    neighbor_cache: [None; 8],
    routes_cache: [None; 8],
};

#[link_section = ".sram3.eth"]
static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new();

macro_rules! add_socket {
    ($sockets:ident, $tx_storage:ident, $rx_storage:ident) => {
        let mut $rx_storage = [0; 4096];
        let mut $tx_storage = [0; 4096];

        let tcp_socket = {
            let tx_buffer = net::socket::TcpSocketBuffer::new(&mut $tx_storage[..]);
            let rx_buffer = net::socket::TcpSocketBuffer::new(&mut $rx_storage[..]);

            net::socket::TcpSocket::new(tx_buffer, rx_buffer)
        };

        let _handle = $sockets.add(tcp_socket);
    };
}

#[entry]
fn main() -> ! {

    let mut cp = cortex_m::Peripherals::take().unwrap();
    let dp = pac::Peripherals::take().unwrap();

    unsafe {
        logger::enable_itm(&dp.DBGMCU, &mut cp.DCB, &mut cp.ITM);
    }
    logger::init();

    // Enable SRAM3 for the descriptor ring.
    dp.RCC.ahb2enr.modify(|_, w| w.sram3en().set_bit());
    // // Reset RCC clock
    // dp.RCC.rsr.write(|w| w.rmvf().set_bit());

    let pwr = dp.PWR.constrain();
    let vos = pwr.freeze();

    let rcc = dp.RCC.constrain();
    let ccdr = rcc
        .use_hse(8.mhz())
        .sys_ck(400.mhz())
        .hclk(200.mhz())
        .pll1_q_ck(48.mhz())
        .pll1_r_ck(400.mhz())
        .freeze(vos, &dp.SYSCFG);
    
    let delay = cp.SYST.delay(ccdr.clocks);
    
    cp.SCB.invalidate_icache();
    cp.SCB.enable_icache();

    cp.DWT.enable_cycle_counter();

    let gpioa = dp.GPIOA.split(ccdr.peripheral.GPIOA);
    let gpiob = dp.GPIOB.split(ccdr.peripheral.GPIOB);
    let gpioc = dp.GPIOC.split(ccdr.peripheral.GPIOC);
    let gpiod = dp.GPIOD.split(ccdr.peripheral.GPIOD);
    let _gpioe = dp.GPIOE.split(ccdr.peripheral.GPIOE);
    let gpiof = dp.GPIOF.split(ccdr.peripheral.GPIOF);
    let gpiog = dp.GPIOG.split(ccdr.peripheral.GPIOG);

    // Acquire client/broker IP Address, client MAC address from flash memory
    let ipv4_addr_cidr = unsafe {
        let ipv4_bits = core::ptr::read(0x08100000 as *const u64);
        net::wire::IpCidr::new(
            net::wire::IpAddress::v4(
                ((ipv4_bits >> 32) & 0xFF).try_into().unwrap(),
                ((ipv4_bits >> 24) & 0xFF).try_into().unwrap(),
                ((ipv4_bits >> 16) & 0xFF).try_into().unwrap(),
                ((ipv4_bits >>  8) & 0xFF).try_into().unwrap()
            ),
            ((ipv4_bits >>  0) & 0xFF).try_into().unwrap()
        )
    };

    let mac_addr = unsafe {
        let mac_bits = core::ptr::read(0x08100020 as *const u64);
        net::wire::EthernetAddress([
            ((mac_bits >> 40) & 0xFF).try_into().unwrap(),
            ((mac_bits >> 32) & 0xFF).try_into().unwrap(),
            ((mac_bits >> 24) & 0xFF).try_into().unwrap(),
            ((mac_bits >> 16) & 0xFF).try_into().unwrap(),
            ((mac_bits >>  8) & 0xFF).try_into().unwrap(),
            (mac_bits & 0xFF).try_into().unwrap(),
        ])
    };

    let broker_ipv4_addr = unsafe {
        let ipv4_bits = core::ptr::read(0x08100040 as *const u64);
        Ipv4Addr::new(
            ((ipv4_bits >> 24) & 0xFF).try_into().unwrap(),
            ((ipv4_bits >> 16) & 0xFF).try_into().unwrap(),
            ((ipv4_bits >>  8) & 0xFF).try_into().unwrap(),
            ((ipv4_bits >>  0) & 0xFF).try_into().unwrap()
        )
    };

    let device_name: String<U32> = unsafe {
        let mut name = String::new();
        let mut addr = 0x08100060;
        loop {
            let c = core::ptr::read(addr as *const u8);
            if c == 4 {
                break;
            } else {
                name.push(c as char).unwrap();
                addr += 1;
            }
        }
        name
    };


    // Note: ITM doesn't work beyond this, due to a pin conflict between:
    // - FPGA_SPI: SCK  (af5)
    // - ST_LINK SWO    (af0)
    // Both demands PB3
    trace!("Flashing configuration bitstream to iCE40 HX8K on Humpback.");

    // Using SPI_1 alternate functions (af5)
    let fpga_sck = gpiob.pb3.into_alternate_af5();
    let fpga_sdo = gpiob.pb4.into_alternate_af5();
    let fpga_sdi = gpiob.pb5.into_alternate_af5();

    // Setup SPI_SS_B and CRESET_B
    let fpga_ss = gpioa.pa4.into_push_pull_output();
    let fpga_creset = gpiof.pf3.into_open_drain_output();

    // Setup CDONE
    let fpga_cdone = gpiod.pd15.into_pull_up_input();

    // Setup SPI interface
    let fpga_cfg_spi = dp.SPI1.spi(
        (fpga_sck, fpga_sdo, fpga_sdi),
        spi::MODE_3,
        12.mhz(),
        ccdr.peripheral.SPI1,
        &ccdr.clocks,
    );

    flash_ice40_fpga(fpga_cfg_spi, fpga_ss, fpga_creset, fpga_cdone, delay).unwrap();

    // Configure ethernet IO
    {
        let _rmii_refclk = gpioa.pa1.into_alternate_af11().set_speed(Speed::VeryHigh);
        let _rmii_mdio = gpioa.pa2.into_alternate_af11().set_speed(Speed::VeryHigh);
        let _rmii_mdc = gpioc.pc1.into_alternate_af11().set_speed(Speed::VeryHigh);
        let _rmii_crs_dv = gpioa.pa7.into_alternate_af11().set_speed(Speed::VeryHigh);
        let _rmii_rxd0 = gpioc.pc4.into_alternate_af11().set_speed(Speed::VeryHigh);
        let _rmii_rxd1 = gpioc.pc5.into_alternate_af11().set_speed(Speed::VeryHigh);
        let _rmii_tx_en = gpiog.pg11.into_alternate_af11().set_speed(Speed::VeryHigh);
        let _rmii_txd0 = gpiog.pg13.into_alternate_af11().set_speed(Speed::VeryHigh);
        let _rmii_txd1 = gpiob.pb13.into_alternate_af11().set_speed(Speed::VeryHigh);
    }

    // Configure ethernet
    // let mac_addr = net::wire::EthernetAddress([0xAC, 0x6F, 0x7A, 0xDE, 0xD6, 0xC8]);
    let (eth_dma, mut eth_mac) = unsafe {
        ethernet::new_unchecked(
            dp.ETHERNET_MAC,
            dp.ETHERNET_MTL,
            dp.ETHERNET_DMA,
            &mut DES_RING,
            mac_addr.clone(),
        )
    };

    unsafe { ethernet::enable_interrupt() }

    let store = unsafe { &mut NET_STORE };

    store.ip_addrs[0] = ipv4_addr_cidr;

    let neighbor_cache = net::iface::NeighborCache::new(&mut store.neighbor_cache[..]);

    let mut routes = net::iface::Routes::new(&mut store.routes_cache[..]);
    let default_v4_gw = net::wire::Ipv4Address::new(192, 168, 1, 1);
    routes.add_default_ipv4_route(default_v4_gw).unwrap();

    let mut net_interface = net::iface::EthernetInterfaceBuilder::new(eth_dma)
        .ethernet_addr(mac_addr)
        .neighbor_cache(neighbor_cache)
        .ip_addrs(&mut store.ip_addrs[..])
        .routes(routes)
        .finalize();

    /*
     *    Using SPI6
     *    SCLK -> PA5        (af8)
     *    MOSI -> PG14       (af5)
     *    MISO -> PA6        (af8)
     *    CS -> 0: PB12, 1: PA15, 2: PC7
     */
    let sclk = gpioa.pa5.into_alternate_af8().set_speed(Speed::VeryHigh);
    let mosi = gpiog.pg14.into_alternate_af5().set_speed(Speed::VeryHigh);
    let miso = gpioa.pa6.into_alternate_af8().set_speed(Speed::VeryHigh);
    let (cs0, cs1, cs2) = (
        gpiob.pb12.into_push_pull_output(),
        gpioa.pa15.into_push_pull_output(),
        gpioc.pc7.into_push_pull_output(),
    );

    /*
     *  I/O_Update -> PB15
     */
    let io_update = gpiob.pb15.into_push_pull_output();

    let spi = dp.SPI6.spi(
        (sclk, miso, mosi),
        spi::MODE_0,
        2.mhz(),
        ccdr.peripheral.SPI6,
        &ccdr.clocks,
    );

    let switch = CPLD::new(spi, (cs0, cs1, cs2), io_update);
    let parts = switch.split();

    let mut urukul = Urukul::new(
        parts.spi1, parts.spi2, parts.spi3, parts.spi4, parts.spi5, parts.spi6, parts.spi7
    );
    urukul.reset().unwrap();

    let mut mqtt_mux = MqttMux::new(urukul, device_name.as_str());

    // Time unit in ms
    let mut time: u32 = 0;

    // Cycle counter for 1 ms
    // This effectively provides a conversion from rtic unit to ms
    let mut next_ms = Instant::now();
    next_ms += 400_000.cycles();

    let mut socket_set_entries: [_; 8] = Default::default();
    let mut sockets = net::socket::SocketSet::new(&mut socket_set_entries[..]);
    add_socket!(sockets, rx_storage, tx_storage);

    let tcp_stack = NetworkStack::new(&mut net_interface, sockets);

    let mut client = MqttClient::<consts::U256, _>::new(
        IpAddr::V4(broker_ipv4_addr),
        device_name.as_str(),
        tcp_stack,
    )
    .unwrap();

    let mut tick = false;
    let mut has_subscribed = false;

    loop {
        // Update time accumulator in ms
        // Tick once every ms
        if Instant::now() > next_ms {
            tick = true;
            time += 1;
            next_ms += 400_000.cycles();
        }

        // eth Poll if necessary
        // Do not poll if eth link is down
        if tick && client.network_stack.update_delay(time) == 0 && eth_mac.phy_poll_link() {
            client.network_stack.update(time);
        }

        // Process MQTT messages about Urukul/Control
        let connection = match client
            .poll(|_client, topic, message, _properties| {
                // Why is topic a string while message is a slice?
                mqtt_mux.process_mqtt_ingress(topic, message);
            }) {
                Ok(_) => true,
                Err(e) => {
                    warn!("{:?}", e);
                    false
                },
            };
        
        // Process MQTT response messages about Urukul
        for (topic, message) in mqtt_mux.process_mqtt_egress().unwrap() {
            client.publish(
                topic.as_str(),
                message.as_bytes(),
                QoS::AtMostOnce,
                &[]
            ).unwrap();
        }
        
        if connection && !has_subscribed && tick {
            let mut str_builder: String<U128> = String::from(device_name.as_str());
            str_builder.push_str("/Control/#").unwrap();
            match client.subscribe(str_builder.as_str(), &[]) {
                Ok(()) => has_subscribed = true,
                Err(minimq::Error::NotReady) => {},
                _e => {},
            };
        }

        // Reset tick flag
        tick = false;
    }
}