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 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,
};

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

use crate::{Urukul, UrukulTraits};

// pub struct MyDevice;

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 SwitchOnCommand {}
impl<T: Device + UrukulTraits> Command<T> for SwitchOnCommand {
    nquery!();

    fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
        let mut channel :u32 = 0;
        let mut channel_index = 0;
        while true {
            match args.next_data(channel_index != 0) {
                Ok(Some(token)) => {
                    match u32::try_from(token) {
                        Ok(shift) => {
                            if shift >= 4 {
                                return Err(ErrorCode::IllegalParameterValue.into());
                            }
                            channel |= (1 << shift);
                        },
                        Err(_) => return Err(ErrorCode::IllegalParameterValue.into()),
                    }
                },
                Ok(None) => break,
                Ok(_) => return Err(ErrorCode::MissingParameter.into()),
                Err(_e) => return Err(_e),
            };
            channel_index += 1;
        }
        match context.device.switch(channel) {
            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)),
        }
    }
}