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 scpi::{
    nquery,
    qonly,
};
use scpi::suffix::{Amplitude, Db};
use uom::si::frequency::{gigahertz, hertz, kilohertz, megahertz, Frequency};
use uom::si::angle::{degree, gon, minute as aminute, radian, revolution, Angle};
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)=>*),
                )*
            ]
        }
    };
}

#[macro_export]
macro_rules! scpi_tree {
    () => {
        scpi_root!(
            "CHANNEL0" => {
                "SWitch" => Channel0SwitchCommand,
                "ATTenuation" => Channel0AttenuationCommand,
                "SYSCLOCK" => Channel0SystemClockCommand,
                "PROFILE0" => {
                    "SINGLEtone" => {
                        "FREQuency" => Channel0Profile0SingletoneFrequencyCommand,
                        "PHASE" => Channel0Profile0SingletonePhaseCommand,
                        "AMPlitude" => Channel0Profile0SingletoneAmplitudeCommand,
                        ["Setup"] => Channel0Profile0SingletoneCommand
                    }
                }
            },
            "CHANNEL1" => {
                "SWitch" => Channel1SwitchCommand,
                "ATTenuation" => Channel1AttenuationCommand
            },
            "CHANNEL2" => {
                "SWitch" => Channel2SwitchCommand,
                "ATTenuation" => Channel2AttenuationCommand
            },
            "CHANNEL3" => {
                "SWitch" => Channel3SwitchCommand,
                "ATTenuation" => Channel3AttenuationCommand
            },
            "CLOCK" => {
                "SOURCE" => ClockSourceCommand,
                "DIVision" => ClockDivisionCommand
            },
            "PROFILE" => ProfileCommand,
            ["EXAMple"] => {
                "HELLO" => {
                    "WORLD" => HelloWorldCommand
                }
            }
        );
    };
}

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 {}

macro_rules! impl_channel_switch_command {
    ($($channel: literal => $command_struct: ty),*) => {
        $(
            impl<T: Device + UrukulTraits> Command<T> for $command_struct {
                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($channel)
                                                                    .map(|current| !current)
                                                                    .map_err(|_| Error::new(ErrorCode::HardwareError)),
                                                    |token| token.try_into()
                                                )?;
                    context.device.set_channel_switch($channel, next_state).map_err(|_| Error::new(ErrorCode::HardwareError))
                }
            }
        )*
    };
}

impl_channel_switch_command!(
    0 => Channel0SwitchCommand,
    1 => Channel1SwitchCommand,
    2 => Channel2SwitchCommand,
    3 => Channel3SwitchCommand
);

pub struct ClockSourceCommand {}
pub struct ClockDivisionCommand {}
pub struct Channel0SystemClockCommand {}
pub struct ProfileCommand {}
pub struct Channel0Profile0SingletoneCommand {}
pub struct Channel0Profile0SingletoneFrequencyCommand {}
pub struct Channel0Profile0SingletonePhaseCommand {}
pub struct Channel0Profile0SingletoneAmplitudeCommand {}

// 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 master clock 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, frequency.get::<hertz>())
                        .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);
        if div == 1.0 || div == 2.0 || div == 4.0 {
            debug!("Set master clock division as {}", div);
            context.device.set_clock_division(div as u8)
                            .map_err(|_| Error::new(ErrorCode::HardwareError))
        } else {
            Err(Error::new(ErrorCode::IllegalParameterValue))
        }
    }
}

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

    // Param: <frequency>
    // The exact method of generating this frequency is auto-decided
    // The process is delegated to individual DDS chip
    fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
        
        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 channel 0 system clock frequency: {:?}", frequency);

        // Setup sys_clk through urukul interface
        context.device.set_channel_sys_clk(0, frequency.get::<hertz>()).map_err(|_| Error::new(ErrorCode::IllegalParameterValue))
    }
}

pub struct Channel0AttenuationCommand {}
pub struct Channel1AttenuationCommand {}
pub struct Channel2AttenuationCommand {}
pub struct Channel3AttenuationCommand {}

macro_rules! impl_channel_attenuation_command {
    ($($channel: literal => $command_struct: ty),*) => {
        $(
            impl<T:Device + UrukulTraits> Command<T> for $command_struct {
                nquery!();
            
                fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
                    let attenuation: f32 = args.next_data(false)?
                                                .map_or(Err(Error::new(ErrorCode::IllegalParameterValue)),
                                                        |token| token.try_into())?;
                    trace!("Received channel {} attenuation input: {}", $channel, attenuation);
                    context.device.set_channel_attenuation($channel, attenuation)
                                    .map_err(|_| Error::new(ErrorCode::HardwareError))
                }
            }
        )*
    };
}

impl_channel_attenuation_command!(
    0 => Channel0AttenuationCommand,
    1 => Channel1AttenuationCommand,
    2 => Channel2AttenuationCommand,
    3 => Channel3AttenuationCommand
);

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

    fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
        let profile :f32 = args.next_data(false)?
                                .map_or(Err(Error::new(ErrorCode::IllegalParameterValue)),
                                        |token| token.try_into())?;
        if ((profile as u8) as f32) != profile {
            return Err(Error::new(ErrorCode::IllegalParameterValue));
        }

        trace!("Selected Profile :{}", profile);
        let profile = profile as u8;
        if profile >= 8 {
            Err(Error::new(ErrorCode::IllegalParameterValue))
        } else {
            context.device.set_profile(profile)
                            .map_err(|_| Error::new(ErrorCode::HardwareError))
        }
    }
}

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

    // Params: frequency, phase, amplitude (all mandatory)
    fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {

        // Read output frequency
		let frequency: f64::Frequency = args.next_data(false)?
											.map_or(Err(Error::new(ErrorCode::MissingParameter)), |t| {
												t.numeric(|s| match s {
													NumericValues::Default => Ok(f64::Frequency::new::<hertz>(0.0)),
													_ => Err(ErrorCode::IllegalParameterValue.into()),
												})
                                            })?;
        trace!("Received channel 0 profile 0 output single tone frequency: {:?}", frequency);
        // Handle negative frequency
        if frequency.get::<hertz>() < 0.0 {
            return Err(ErrorCode::DataOutOfRange.into());
        }

        // Read phase offset
        let phase: f64::Angle = args.next_data(false)?
                                    .map_or(Err(Error::new(ErrorCode::MissingParameter)), |t| {
                                        t.numeric(
                                            |s| match s {
                                            NumericValues::Default => Ok(f64::Angle::new::<degree>(0.0)),
                                            _ => Err(ErrorCode::IllegalParameterValue.into()),
                                        })
                                    })?;
        trace!("Received channel 0 profile 0 output single tone phase offset: {:?}", phase);
        // Handle out-of-bound phase offset
        if phase.get::<degree>() < 0.0 || phase.get::<degree>() >= 360.0 {
            return Err(ErrorCode::DataOutOfRange.into());
        }

        // Read amplitude offset
        let amplitude: f64 = args.next_data(false)?
                                    .map_or(Err(Error::new(ErrorCode::MissingParameter)),
                                            |token| token.try_into())?;
        trace!("Received channel 0 profile 0 output single tone amplitude offset: {:?}", amplitude);
        // Handle out-of-bound phase offset
        if amplitude < 0.0 || amplitude > 1.0 {
            return Err(ErrorCode::DataOutOfRange.into());
        }

        trace!("Set up a single tone on channel 0, profile 0");
        context.device.set_channel_single_tone_profile(0, 0, frequency.get::<hertz>(), phase.get::<degree>(), amplitude)
                        .map_err(|_| Error::new(ErrorCode::HardwareError))
    }
}

impl<T:Device + UrukulTraits> Command<T> for Channel0Profile0SingletoneFrequencyCommand {
    // TODO: Implement query for publishing
    nquery!();

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

        // Read output frequency
		let frequency: f64::Frequency = args.next_data(false)?
                                            .map_or(Err(Error::new(ErrorCode::MissingParameter)), |t| {
                                                t.numeric(|s| match s {
                                                    NumericValues::Default => Ok(f64::Frequency::new::<hertz>(0.0)),
                                                    _ => Err(ErrorCode::IllegalParameterValue.into()),
                                                })
                                            })?;
        trace!("Received channel 0 profile 0 output single tone frequency: {:?}", frequency);
        // Handle negative frequency
        if frequency.get::<hertz>() < 0.0 {
            return Err(ErrorCode::DataOutOfRange.into());
        }

        context.device.set_channel_single_tone_profile_frequency(0, 0, frequency.get::<hertz>())
                        .map_err(|_| Error::new(ErrorCode::HardwareError))
    }
}

impl<T:Device + UrukulTraits> Command<T> for Channel0Profile0SingletonePhaseCommand {
    // TODO: Implement query for publishing
    nquery!();

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

        // Read phase offset
        let phase: f64::Angle = args.next_data(false)?
                                    .map_or(Err(Error::new(ErrorCode::MissingParameter)), |t| {
                                        t.numeric(
                                            |s| match s {
                                            NumericValues::Default => Ok(f64::Angle::new::<degree>(0.0)),
                                            _ => Err(ErrorCode::IllegalParameterValue.into()),
                                        })
                                    })?;
        trace!("Received channel 0 profile 0 output single tone phase offset: {:?}", phase);
        // Handle out-of-bound phase offset
        if phase.get::<degree>() < 0.0 || phase.get::<degree>() >= 360.0 {
            return Err(ErrorCode::DataOutOfRange.into());
        }
        context.device.set_channel_single_tone_profile_phase(0, 0, phase.get::<degree>())
                        .map_err(|_| Error::new(ErrorCode::HardwareError))
    }
}

impl<T:Device + UrukulTraits> Command<T> for Channel0Profile0SingletoneAmplitudeCommand {
    // TODO: Implement query for publishing
    nquery!();

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

        // Read amplitude offset
        let amplitude: f64 = args.next_data(false)?
                                    .map_or(Err(Error::new(ErrorCode::MissingParameter)),
                                            |token| token.try_into())?;
        trace!("Received channel 0 profile 0 output single tone amplitude offset: {:?}", amplitude);
        // Handle out-of-bound phase offset
        if amplitude < 0.0 || amplitude > 1.0 {
            return Err(ErrorCode::DataOutOfRange.into());
        }
        context.device.set_channel_single_tone_profile_amplitude(0, 0, amplitude)
                        .map_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)),
        }
    }
}