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 scpi::suffix::{Amplitude, Db};
use uom::si::frequency::{gigahertz, hertz, kilohertz, megahertz, Frequency};
use uom::si::{f32, f64};

use embedded_hal::blocking::spi::Transfer;

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

use log::{trace, debug, info, warn};

#[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<()> {
        let next_state: bool = args.next_data(true)?
                                    .map_or(
                                        context.device.get_channel_switch_status(0)
                                                        .map(|current| !current)
                                                        .map_err(|_| Error::new(ErrorCode::HardwareError)),
                                        |token| token.try_into()
                                    )?;
        context.device.set_channel_switch(0, next_state).map_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<()> {
        let next_state: bool = args.next_data(true)?
                                    .map_or(
                                        context.device.get_channel_switch_status(1)
                                                        .map(|current| !current)
                                                        .map_err(|_| Error::new(ErrorCode::HardwareError)),
                                        |token| token.try_into()
                                    )?;
        context.device.set_channel_switch(1, next_state).map_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<()> {
        let next_state: bool = args.next_data(true)?
                                    .map_or(
                                        context.device.get_channel_switch_status(2)
                                                        .map(|current| !current)
                                                        .map_err(|_| Error::new(ErrorCode::HardwareError)),
                                        |token| token.try_into()
                                    )?;
        context.device.set_channel_switch(2, next_state).map_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<()> {
        let next_state: bool = args.next_data(true)?
                                    .map_or(
                                        context.device.get_channel_switch_status(3)
                                                        .map(|current| !current)
                                                        .map_err(|_| Error::new(ErrorCode::HardwareError)),
                                        |token| token.try_into()
                                    )?;
        context.device.set_channel_switch(3, next_state).map_err(|_| Error::new(ErrorCode::HardwareError))
    }
}

// Handle CLOCK:SOURCE command, setup the proper source for the system clock
// Leave clock division to CLOCK:DIVision command
impl<T:Device + UrukulTraits> Command<T> for ClockSourceCommand {
    nquery!();

    fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {

        // next_data() fucntion call can never return CharacterProgramData, could be an oversight
        let s: &[u8] = match args.next_data(false)? {
            Some(Token::CharacterProgramData(s)) => s,
            _ => return Err(ErrorCode::IllegalParameterValue.into()),
        };

    	let s_str: &str = str::from_utf8(s)
                                .map_err(|_| ErrorCode::CharacterDataError)?;

		let frequency: f64::Frequency = args.next_data(true)?
											.map_or(Ok(f64::Frequency::new::<hertz>(0.0)), |t| {
												t.numeric(|s| match s {
													NumericValues::Default => Ok(f64::Frequency::new::<hertz>(0.0)),
													_ => Err(ErrorCode::IllegalParameterValue.into()),
												})
											})?;
        trace!("Received frequency: {:?}", frequency);

        let clock_source = match s_str {
			source if source.eq_ignore_ascii_case("OSC") => {
                // If clock source is OSC, it must be 100MHz (not configurable)
                if frequency.get::<megahertz>() != 100.0 {
                    warn!("Clock selection failed! OSC must be 100 MHz");
                    return Err(ErrorCode::IllegalParameterValue.into());
                }
                ClockSource::OSC
			},
			source if source.eq_ignore_ascii_case("MMCX") => {
                // TODO: Implement frequency check for MMCX
                ClockSource::MMCX
			},
			source if source.eq_ignore_ascii_case("SMA") => {
                // TODO: Implement frequency check for SMA
                ClockSource::SMA
			},
			_ => {
				warn!("Clock selection failed! Argument error!");
				return Err(ErrorCode::IllegalParameterValue.into());
            },
        };

        trace!("Changing clock source to {:?} at {:?}", clock_source, frequency);
        context.device.set_clock_source(clock_source)
                        .map_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 div :f32 = args.next_data(false)?
                                .map_or(Err(Error::new(ErrorCode::IllegalParameterValue)),
                                        |token| token.try_into())?;
        trace!("Received master clock division factor: {}", div);
        match div {
            1.0 | 2.0 | 4.0 => {
                debug!("Set master clock division as {}", div);
                context.device.set_clock_division(div as u8)
                                .map_err(|_| Error::new(ErrorCode::HardwareError))
            }
            _ => Err(Error::new(ErrorCode::IllegalParameterValue)),
        }
    }
}

/*
 *  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)),
        }
    }
}