#![no_main]
#![no_std]

// extern crate cortex_m_rt as rt;
use core::sync::atomic::{AtomicU32, Ordering};

//#[macro_use]
//extern crate log;

// extern crate cortex_m;
use panic_semihosting as _;

use cortex_m;
use cortex_m::asm::nop;
use cortex_m_rt::{
    entry,
    exception,
};
use cortex_m_semihosting::hprintln;

extern crate smoltcp;

// Ethernet crate for STM32H7 has been merged into HAL in the latest commit
extern crate stm32h7_ethernet as ethernet;

use stm32h7xx_hal::gpio::Speed;
use stm32h7xx_hal::hal::digital::v2::{
    OutputPin,
    InputPin,
};
use stm32h7xx_hal::rcc::CoreClocks;
use stm32h7xx_hal::{pac, prelude::*, spi, stm32, stm32::interrupt};
use Speed::*;

use libm::round;

/*
#[cfg(feature = "itm")]
use cortex_m_log::log::{trick_init, Logger};

#[cfg(feature = "itm")]
use cortex_m_log::{
    destination::Itm, printer::itm::InterruptSync as InterruptSyncItm,
};
*/

use core::{
    str,
    fmt::Write
};
use core::mem::uninitialized;

// Exception: no phy::wait
//use smoltcp::phy::wait as phy_wait;
use smoltcp::wire::{EthernetAddress, IpAddress, IpCidr, Ipv4Address};
use smoltcp::iface::{NeighborCache, EthernetInterfaceBuilder, Routes};
use smoltcp::socket::SocketSet;
use smoltcp::socket::{SocketHandle, TcpSocket, TcpSocketBuffer};
use smoltcp::time::{Duration, Instant};

// Use embedded-nal to access smoltcp
use embedded_nal::TcpStack;

use firmware;
use firmware::{
	attenuator::Attenuator,
	config_register::{
		ConfigRegister,
		CFGMask,
		StatusMask,
	},
	dds::{
		DDS,
		DDSCFRMask,
	},
	cpld::{
		CPLD,
    },
    scpi::{ HelloWorldCommand, SwitchOnCommand },
    Urukul,
};
use scpi::prelude::*;
use scpi::ieee488::commands::*;
use scpi::scpi::commands::*;
use scpi::{
    ieee488_cls,
    ieee488_ese,
    ieee488_esr,
    ieee488_idn,
    ieee488_opc,
    ieee488_rst,
    ieee488_sre,
    ieee488_stb,
    ieee488_tst,
    ieee488_wai,
    nquery,
    //Helpers
    qonly,
    scpi_crate_version,
    scpi_status,
    scpi_system,
    scpi_tree,
};

/// Configure SYSTICK for 1ms timebase
fn systick_init(syst: &mut stm32::SYST, clocks: CoreClocks) {
    let c_ck_mhz = clocks.c_ck().0 / 1_000_000;

    let syst_calib = 0x3E8;

    syst.set_clock_source(cortex_m::peripheral::syst::SystClkSource::Core);
    syst.set_reload((syst_calib * c_ck_mhz) - 1);
    syst.enable_interrupt();
    syst.enable_counter();
}

/// ======================================================================
/// Entry point
/// ======================================================================

/// TIME is an atomic u32 that counts milliseconds. Although not used
/// here, it is very useful to have for network protocols
static TIME: AtomicU32 = AtomicU32::new(0);

/// Locally administered MAC address
const MAC_ADDRESS: [u8; 6] = [0x02, 0x00, 0x11, 0x22, 0x33, 0x44];

/// Ethernet descriptor rings are a global singleton
#[link_section = ".sram3.eth"]
static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new();

// Theoratical maximum number of socket that can be handled
const SOCKET_COUNT: usize = 2;

// Give buffer sizes of transmitting and receiving TCP packets
const BUFFER_SIZE: usize = 2048;

// the program entry point
#[entry]
fn main() -> ! {

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

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

    // Initialise SRAM3
    dp.RCC.ahb2enr.modify(|_, w| w.sram3en().set_bit());

    // Initialise clocks...
    let rcc = dp.RCC.constrain();
    let ccdr = rcc
        .sys_ck(200.mhz())
        .hclk(200.mhz())
        .pll1_r_ck(100.mhz())   // for TRACECK
        .pll1_q_ck(48.mhz())    // for SPI
        .freeze(vos, &dp.SYSCFG);

    // Get the delay provider.
    let delay = cp.SYST.delay(ccdr.clocks);

    // Initialise system...
    cp.SCB.invalidate_icache();
    cp.SCB.enable_icache();
    // TODO: ETH DMA coherence issues
    // cp.SCB.enable_dcache(&mut cp.CPUID);
    cp.DWT.enable_cycle_counter();

    // Initialise IO...
    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);
    // let mut link_led = gpiob.pb0.into_push_pull_output(); // LED1, green
    // let mut status_led = gpioe.pe1.into_push_pull_output(); // LD2, yellow
    // let mut listen_led = gpiob.pb14.into_push_pull_output(); // LD3, red
    // link_led.set_low().ok();
    // status_led.set_low().ok();
    // listen_led.set_low().ok();

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

	match fpga_cdone.is_high() {
		Ok(true)	=> hprintln!("FPGA is ready."),
		Ok(_)		=> hprintln!("FPGA is in reset state."),
		Err(_)		=> hprintln!("Error: Cannot read C_DONE"),
    }.unwrap();
    
    // Setup Urukul
	/*
	 *	Using SPI1, AF5
	 *  SCLK -> PA5
	 *	MOSI -> PB5
	 *	MISO -> PA6
	 *	CS -> 0: PB12, 1: PA15, 2: PC7
	 */

    let sclk = gpioa.pa5.into_alternate_af5();
    let mosi = gpiob.pb5.into_alternate_af5();
    let miso = gpioa.pa6.into_alternate_af5();


    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.SPI1.spi(
        (sclk, miso, mosi),
        spi::MODE_0,
        3.mhz(),
        ccdr.peripheral.SPI1,
        &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,
        [25_000_000, 25_000_000, 25_000_000, 25_000_000]
    );

    // Setup ethernet pins
    setup_ethernet_pins(
        gpioa.pa1, gpioa.pa2, gpioc.pc1, gpioa.pa7, gpioc.pc4,
        gpioc.pc5, gpiog.pg11, gpiog.pg13, gpiob.pb13
    );

    // Initialise ethernet...
    assert_eq!(ccdr.clocks.hclk().0, 200_000_000); // HCLK 200MHz
    assert_eq!(ccdr.clocks.pclk1().0, 100_000_000); // PCLK 100MHz
    assert_eq!(ccdr.clocks.pclk2().0, 100_000_000); // PCLK 100MHz
    assert_eq!(ccdr.clocks.pclk4().0, 100_000_000); // PCLK 100MHz

    let mac_addr = smoltcp::wire::EthernetAddress::from_bytes(&MAC_ADDRESS);
    let (_eth_dma, mut eth_mac) = unsafe {
        ethernet::ethernet_init(
            dp.ETHERNET_MAC,
            dp.ETHERNET_MTL,
            dp.ETHERNET_DMA,
            &mut DES_RING,
            mac_addr.clone(),
        )
    };
    unsafe {
        ethernet::enable_interrupt();
        cp.NVIC.set_priority(stm32::Interrupt::ETH, 196); // Mid prio
        cortex_m::peripheral::NVIC::unmask(stm32::Interrupt::ETH);
    }

    // ----------------------------------------------------------
    // Begin periodic tasks

    systick_init(&mut delay.free(), ccdr.clocks);
    unsafe {
        cp.SCB.shpr[15 - 4].write(128);
    } // systick exception priority

    // ----------------------------------------------------------
    // Main application loop

    // Setup addresses, maybe not MAC?
    // MAC is set up in prior
    let local_addr = IpAddress::v4(192, 168, 1, 200);
    let mut ip_addrs = [IpCidr::new(local_addr, 24)];

    // let neighbor_cache = NeighborCache::new(BTreeMap::new());
    let mut neighbor_storage = [None; 16];
    let neighbor_cache = NeighborCache::new(&mut neighbor_storage[..]);

    // Routes
    let default_v4_gw = Ipv4Address::new(192, 168, 1, 1);
    let mut routes_storage = [None; 8];
    let mut routes = Routes::new(&mut routes_storage[..]);
    routes.add_default_ipv4_route(default_v4_gw).unwrap();

    // Device? _eth_dma, as it implements phy::device
    let mut iface = EthernetInterfaceBuilder::new(_eth_dma)
            .ethernet_addr(mac_addr)
            .neighbor_cache(neighbor_cache)
            .ip_addrs(&mut ip_addrs[..])
            .routes(routes)
            .finalize();

    // SCPI configs

    // TODO: String for *idn? mandated command

    // SCPI tree
    let tree = scpi_tree![
        // Create default IEEE488 mandated commands
        ieee488_cls!(),
        ieee488_ese!(),
        ieee488_esr!(),
        // ieee488_idn!(b"manufacturer", b"model", b"serial", "0.1.2".as_bytes()),
        ieee488_opc!(),
        ieee488_rst!(),
        ieee488_sre!(),
        ieee488_stb!(),
        ieee488_tst!(),
        ieee488_wai!(),
        // Create default SCPI mandated STATus subsystem
        scpi_status!(),
        // Create default SCPI mandated SYSTem subsystem
        scpi_system!(),
        //
        scpi_crate_version!(),
        //Test
        Node {
            name: b"ABORt",
            handler: None,
            optional: false,
            sub: &[],
        },
        Node {
            name: b"INITiate",
            handler: None,
            optional: false,
            sub: &[
                Node {
                    name: b"IMMediate",
                    handler: None,
                    optional: true,
                    sub: &[],
                }
            ],
        },
        Node {
            name: b"EXAMple",
            optional: true,
            handler: None,
            sub: &[
                Node {
                    name: b"HELLO",
                    optional: false,
                    handler: None,
                    sub: &[
                        Node {
                            name: b"WORLD",
                            optional: true,
                            handler: Some(&HelloWorldCommand {}),
                            sub: &[],
                        }
                    ],
                },
            ],
        },
        Node {
            name: b"SWitch",
            optional: false,
            handler: Some(&SwitchOnCommand {}),
            sub: &[],
        }
    ];

    // Device was declared in prior
    let mut errors = ArrayErrorQueue::<[Error; 10]>::new();
    let mut context = Context::new(&mut urukul, &mut errors, tree);

    //Response bytebuffer
    let mut buf = ArrayVecFormatter::<[u8; 256]>::new();
    // SCPI configs END

    // TCP socket buffers
    let mut rx_storage = [0; BUFFER_SIZE];
    let mut tx_storage = [0; BUFFER_SIZE];

    // Setup TCP sockets
    let tcp1_rx_buffer = TcpSocketBuffer::new(&mut rx_storage[..]);
    let tcp1_tx_buffer = TcpSocketBuffer::new(&mut tx_storage[..]);
    let mut tcp1_socket = TcpSocket::new(tcp1_rx_buffer, tcp1_tx_buffer);

    // Setup a silent socket
    let mut silent_rx_storage = [0; BUFFER_SIZE];
    let mut silent_tx_storage = [0; BUFFER_SIZE];
    let silent_rx_buffer = TcpSocketBuffer::new(&mut silent_rx_storage[..]);
    let silent_tx_buffer = TcpSocketBuffer::new(&mut silent_tx_storage[..]);
    let mut silent_socket = TcpSocket::new(silent_rx_buffer, silent_tx_buffer);

    // Socket storage
    let mut sockets_storage = [ None, None ];

    let mut sockets = SocketSet::new(&mut sockets_storage[..]);
    let tcp1_handle = sockets.add(tcp1_socket);
    let silent_handle = sockets.add(silent_socket);
    let mut handles: [SocketHandle; SOCKET_COUNT] = unsafe {
        uninitialized()
    };

    let mut eth_up = false;

    // Record activeness of silent socket, init as false
    let mut silent_socket_active = false;

    loop {
        let _time = TIME.load(Ordering::Relaxed);
        let eth_last = eth_up;
        match iface.poll(&mut sockets, Instant::from_millis(_time as i64)) {
            Ok(_) => {
                eth_up = true;
            },
            Err(e) => {
                eth_up = false;
            },
        };

	    // Float rounding test socket (:6969)
        {
            let mut socket = sockets.get::<TcpSocket>(tcp1_handle);
            if !socket.is_open() {
                socket.listen(6969).unwrap();
                socket.set_timeout(Some(Duration::from_millis(5000)));
            }

            if socket.can_recv() {
                let data = socket.recv(|buffer| {
                    (buffer.len(), buffer)
                }).unwrap();
                hprintln!("{:?}", data).unwrap();
                let result = lexical_core::parse_partial::<f64>(data).unwrap();
                writeln!(socket, "{}", round(result.0 * 2.0));
            }
        }
		//  SCPI interaction socket (:7000)
		{
		    let mut socket = sockets.get::<TcpSocket>(silent_handle);
		    if !socket.is_open() {
				socket.listen(7000).unwrap();
				socket.set_timeout(Some(Duration::from_millis(1000000)));
		    }

		    if socket.can_recv() {
                let mut data = socket.recv(|buffer| {
                    (buffer.len(), buffer)
                }).unwrap();
                if str::from_utf8(data).unwrap().trim() == "quit" {
                    socket.close();
                    socket.abort();
                    continue;
                }
                let result = context.run(data, &mut buf);
                if let Err(err) = result {
                    writeln!(socket, "{}", str::from_utf8(err.get_message()).unwrap());
                } else {
                    write!(socket, "{}", str::from_utf8(buf.as_slice()).unwrap());
                }
		    }
		}
    }
}

use stm32h7xx_hal::gpio::{
    gpioa::{PA1, PA2, PA7},
    gpiob::{PB13},
    gpioc::{PC1, PC4, PC5},
    gpiog::{PG11, PG13},
    Speed::VeryHigh,
};

/*
 *  Migrated ethernet setup pins
 */
pub fn setup_ethernet_pins<REF_CLK, MDIO, MDC, CRS_DV, RXD0, RXD1, TX_EN, TXD0, TXD1>(
    pa1: PA1<REF_CLK>, pa2: PA2<MDIO>, pc1: PC1<MDC>, pa7: PA7<CRS_DV>, pc4: PC4<RXD0>,
    pc5: PC5<RXD1>, pg11: PG11<TX_EN>, pg13: PG13<TXD0>, pb13: PB13<TXD1>
) {
    pa1.into_alternate_af11().set_speed(VeryHigh);
    pa2.into_alternate_af11().set_speed(VeryHigh);
    pc1.into_alternate_af11().set_speed(VeryHigh);
    pa7.into_alternate_af11().set_speed(VeryHigh);
    pc4.into_alternate_af11().set_speed(VeryHigh);
    pc5.into_alternate_af11().set_speed(VeryHigh);
    pg11.into_alternate_af11().set_speed(VeryHigh);
    pg13.into_alternate_af11().set_speed(VeryHigh);
    pb13.into_alternate_af11().set_speed(VeryHigh);
}

#[interrupt]
fn ETH() {
    unsafe { ethernet::interrupt_handler() }
}

#[exception]
fn SysTick() {
    TIME.fetch_add(1, Ordering::Relaxed);
}

#[exception]
fn HardFault(ef: &cortex_m_rt::ExceptionFrame) -> ! {
    panic!("HardFault at {:#?}", ef);
}

#[exception]
fn DefaultHandler(irqn: i16) {
    panic!("Unhandled exception (IRQn = {})", irqn);
}