use scpi::error::Result;
use scpi::expression::numeric_list;
use scpi::expression::numeric_list::NumericList;
use scpi::format::{Arbitrary, Character};
use scpi::prelude::*;
use scpi::NumericValues;

use core::convert::{TryFrom, TryInto};
use core::str;
use core::str::Utf8Error;
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,
};

use embedded_hal::blocking::spi::Transfer;
use cortex_m_semihosting::hprintln;

use crate::{
    Urukul,
    UrukulTraits,
    Error as UrukulError,
    ClockSource,
};

#[macro_export]
macro_rules! recursive_scpi_tree {
    // Handle optional headers (end-node)
    ([$header_name: expr] => $handler: ident) => {
        Node {
            name: str::as_bytes($header_name),
            handler: Some(&$handler{}),
            sub: &[],
            optional: true,
        }
    };
    
    // Handle non-optinal header (end-node)
    ($header_name: expr => $handler: ident) => {
        Node {
            name: str::as_bytes($header_name),
            handler: Some(&$handler{}),
            sub: &[],
            optional: false,
        }
    };
    
    // Handle optional header with sub-commands
    ([$header_name: expr] => {$($($rest: tt)=>*),*}) => {
        Node {
            name: str::as_bytes($header_name),
            handler: None,
            sub: &[
                $(
                    recursive_scpi_tree!($($rest)=>*),
                )*
            ],
            optional: true,
        }
    };
    
    // Handle non-optional header with sub-commands
    ($header_name: expr => {$($($rest: tt)=>*),*}) => {
        Node {
            name: str::as_bytes($header_name),
            handler: None,
            sub: &[
                $(
                    recursive_scpi_tree!($($rest)=>*),
                )*
            ],
            optional: false,
        }
    };
}

#[macro_export]
macro_rules! scpi_root {
    ($($($node: tt)=>*),*) => {
        &Node {
            name: b"ROOT",
            optional: false,
            handler: None,
            sub: &[
                // Create default IEEE488 mandated commands
                ieee488_cls!(),
                ieee488_ese!(),
                ieee488_esr!(),
                ieee488_idn!(b"manufacturer", b"model", b"serial", b"0.1.2"),
                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!(),
                $(
                    recursive_scpi_tree!($($node)=>*),
                )*
            ]
        }
    };
}

pub struct HelloWorldCommand {}
impl<T: Device> Command<T> for HelloWorldCommand {
    qonly!();

    fn query(
        &self,
        _context: &mut Context<T>,
        _args: &mut Tokenizer,
        response: &mut ResponseUnit,
    ) -> Result<()> {
        response.data(b"Hello world" as &[u8]).finish()
    }
}

pub struct Channel0SwitchCommand {}
pub struct Channel1SwitchCommand {}
pub struct Channel2SwitchCommand {}
pub struct Channel3SwitchCommand {}
pub struct ClockSourceCommand {}
pub struct ClockDivisionCommand {}

impl<T: Device + UrukulTraits> Command<T> for Channel0SwitchCommand {
    nquery!();

    fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
        match context.device.get_channel_switch_status(0) {
            Ok(status) => {
                let next_state: bool = match args.next_data(true) {
                    Ok(Some(token)) => token.try_into()?,
                    Ok(None) => !status,
                    Err(_) => return Err(ErrorCode::IllegalParameterValue.into()),
                };
                match context.device.set_channel_switch(0, next_state) {
                    Ok(()) => Ok(()),
                    Err(_) => Err(Error::new(ErrorCode::HardwareError)),
                }
            },
            Err(_) => Err(Error::new(ErrorCode::HardwareError)),
        }
    }
}

impl<T: Device + UrukulTraits> Command<T> for Channel1SwitchCommand {
    nquery!();

    fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
        match context.device.get_channel_switch_status(1) {
            Ok(status) => {
                let next_state: bool = match args.next_data(true) {
                    Ok(Some(token)) => token.try_into()?,
                    Ok(None) => !status,
                    Err(_) => return Err(ErrorCode::IllegalParameterValue.into()),
                };
                match context.device.set_channel_switch(1, next_state) {
                    Ok(()) => Ok(()),
                    Err(_) => Err(Error::new(ErrorCode::HardwareError)),
                }
            },
            Err(_) => Err(Error::new(ErrorCode::HardwareError)),
        }
    }
}

impl<T: Device + UrukulTraits> Command<T> for Channel2SwitchCommand {
    nquery!();

    fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
        match context.device.get_channel_switch_status(2) {
            Ok(status) => {
                let next_state: bool = match args.next_data(true) {
                    Ok(Some(token)) => token.try_into()?,
                    Ok(None) => !status,
                    Err(_) => return Err(ErrorCode::IllegalParameterValue.into()),
                };
                match context.device.set_channel_switch(2, next_state) {
                    Ok(()) => Ok(()),
                    Err(_) => Err(Error::new(ErrorCode::HardwareError)),
                }
            },
            Err(_) => Err(Error::new(ErrorCode::HardwareError)),
        }
    }
}

impl<T: Device + UrukulTraits> Command<T> for Channel3SwitchCommand {
    nquery!();

    fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
        match context.device.get_channel_switch_status(3) {
            Ok(status) => {
                let next_state: bool = match args.next_data(true) {
                    Ok(Some(token)) => token.try_into()?,
                    Ok(None) => !status,
                    Err(_) => return Err(ErrorCode::IllegalParameterValue.into()),
                };
                match context.device.set_channel_switch(3, next_state) {
                    Ok(()) => Ok(()),
                    Err(_) => Err(Error::new(ErrorCode::HardwareError)),
                }
            },
            Err(_) => Err(Error::new(ErrorCode::HardwareError)),
        }
    }
}

impl<T:Device + UrukulTraits> Command<T> for ClockSourceCommand {
    nquery!();

    fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
        let data: &[u8] = match args.next_data(false)? {
            Some(Token::CharacterProgramData(s)) => s,
            _ => return Err(ErrorCode::IllegalParameterValue.into()),
        };
        let result = match str::from_utf8(data) {
            Ok(str_param) => match str_param {
                "OSC" => context.device.set_clock_source(ClockSource::OSC),
                "MMCX" => context.device.set_clock_source(ClockSource::MMCX),
                "SMA" => context.device.set_clock_source(ClockSource::SMA),
                _ => return Err(ErrorCode::IllegalParameterValue.into()),
            }
            _ => return Err(ErrorCode::IllegalParameterValue.into()),
        };
        match result {
            Ok(_) => Ok(()),
            Err(_) => Err(Error::new(ErrorCode::HardwareError)),
        }
    }
}

impl<T:Device + UrukulTraits> Command<T> for ClockDivisionCommand {
    nquery!();

    fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
        let data :u8 = match args.next_data(false)? {
            Some(token) => {
                match f32::try_from(token) {
                    Ok(val) => {
                        hprintln!("{}", val).unwrap();
                        if val == 1.0 || val == 2.0 || val == 4.0 {
                            val as u8
                        } else {
                            return Err(ErrorCode::IllegalParameterValue.into())
                        }
                    },
                    Err(_e) => {
                        hprintln!("Checked numberic error").unwrap();
                        return Err(ErrorCode::IllegalParameterValue.into())
                    },
                }
            },
            _ => return Err(ErrorCode::IllegalParameterValue.into()),
        };
        match context.device.set_clock_division(data) {
            Ok(()) => Ok(()),
            Err(_) => Err(Error::new(ErrorCode::HardwareError)),
        }
    }
}

/*
 *  Implement "Device" trait from SCPI
 *  TODO: Implement mandatory commands
 */
impl<SPI, E> Device for Urukul<SPI> 
where
    SPI: Transfer<u8, Error = E>
{
    fn cls(&mut self) -> Result<()> {
        Ok(())
    }

    fn rst(&mut self) -> Result<()> {
        match self.reset() {
            Ok(_) => Ok(()),
            Err(_) => Err(Error::new(ErrorCode::HardwareError))
        }
    }

    fn tst(&mut self) -> Result<()> {
        match self.test() {
            Ok(0) => Ok(()),
            Ok(_) => Err(Error::new(ErrorCode::SelfTestFailed)),
            Err(_) => Err(Error::new(ErrorCode::SelfTestFailed)),
        }
    }
}