use crate::{
    adcon, drate, fsc0, fsc2, gpio, mux, ofc0, opcodes, status, AdControl, Ads1256, AllCalibration,
    BlockingDelay, CalibrationCommand, Config, DataRate, DigitalIo, GainCalibration, Multiplexer,
    OffsetCalibration, Status,
};
use embedded_hal::digital::OutputPin;
use embedded_hal::spi;
use embedded_hal::spi::SpiBus;
use embedded_hal_async::digital::Wait;
use physical_node::GPIO_ERROR_MSG;
use physical_node::spi::{end_spi, end_spi_if_err};

impl<DelayerT, SST, DrdyT> Ads1256<DelayerT, SST, DrdyT>
where
    DelayerT: BlockingDelay,
    SST: OutputPin,
    DrdyT: Wait,
{
    //----- Base register read/write ----------------------------------
    /// [buffer] - The data to send the the ADS1256 starting at index 2, the first two bytes are
    /// reserved as the command bytes.
    #[inline]
    pub fn raw_write_registers<SpiT: SpiBus, const BUF_SIZE: usize>(
        &mut self,
        spi: &mut SpiT,
        start_address: u8,
        mut buffer: [u8; BUF_SIZE],
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        let num_registers = BUF_SIZE - 2;
        assert!(start_address <= fsc2::ADDRESS, "Invalid starting register address.");
        //TODO: Change to compile time assertion or bound in future Rust version.
        assert!(num_registers <= 11, "Cannot write more than the total number of registers.");
        assert!(num_registers >= 1, "Must write at least one register.");
        // num_registers represents the total number of registers to write, including the one at the
        // provided address. Adjust values based on it accordingly.

        // First command byte = 4 bits for the write register opcode, followed by 4 bits for the
        // starting register address.
        let cmd_start = opcodes::WREG | start_address;
        // Second byte = number of registers to write in addition to the register at the starting
        // address.
        let num_additional = num_registers as u8 - 1;
        buffer[0] = cmd_start;
        buffer[1] = num_additional;

        spi.write(&buffer)
    }

    /// [buffer] - The data to send the the ADS1256 starting at index 2, the first two bytes are
    /// reserved as the command bytes.
    #[inline]
    pub fn write_registers<SpiT: SpiBus, const BUF_SIZE: usize>(
        &mut self,
        spi: &mut SpiT,
        start_address: u8,
        buffer: [u8; BUF_SIZE],
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        self.slave_select.set_low().expect(GPIO_ERROR_MSG);
        let spi_op_result = self.raw_write_registers(spi, start_address, buffer);
        end_spi(&mut self.slave_select, spi, spi_op_result)
    }

    fn read_registers<SpiT: SpiBus, const NUM: usize>(
        &mut self,
        spi: &mut SpiT,
        start_address: u8,
    ) -> Result<[u8; NUM], <SpiT as spi::ErrorType>::Error> {
        assert!(start_address <= fsc2::ADDRESS, "Invalid starting register address.");
        //TODO: Change to compile time assertion or bound.
        assert!(NUM <= 11, "Cannot read more than the total number of registers.");
        assert!(NUM >= 1, "Must read at least one register.");
        // NUM represents the total number of registers to read, including the one at the provided
        // address. Adjust values based on it accordingly.

        // First command byte = 4 bits for the read register opcode, followed by 4 bits for the
        // starting register address.
        let cmd_start = opcodes::RREG | start_address;
        // Second byte = number of registers to read in addition to the register at the starting
        // address.
        let num_additional = NUM as u8 - 1;
        let mut buffer = [0u8; NUM];
        self.slave_select.set_low().expect(GPIO_ERROR_MSG);
        let spi_op_result = spi.write(&[cmd_start, num_additional]);
        end_spi_if_err(&mut self.slave_select, spi, spi_op_result)?;
        self.delayer.t6_delay();
        let spi_op_result = spi.read(&mut buffer);
        end_spi(&mut self.slave_select, spi, spi_op_result)?;
        Ok(buffer)
    }

    //----- Standalone commands ----------------------------------
    #[inline]
    fn standalone_command<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
        opcode: u8,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        self.slave_select.set_low().expect(GPIO_ERROR_MSG);
        let spi_op_result = spi.write(&[opcode]);
        end_spi(&mut self.slave_select, spi, spi_op_result)
    }

    #[inline(always)]
    pub fn standby<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        self.standalone_command(spi, opcodes::STANDBY)
    }

    /// Self calibration is performed after reset, therefore additional commands should not be sent
    /// until data ready pin goes low indicating the calibration is complete.
    #[inline(always)]
    pub fn reset<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        self.standalone_command(spi, opcodes::RESET)
    }

    #[inline(always)]
    pub fn wake<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        self.standalone_command(spi, opcodes::WAKEUP)
    }

    /// Perform self offset and gain calibration.
    #[inline]
    pub async fn self_calibrate<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        let result = self.standalone_command(spi, opcodes::SELFCAL);
        self.data_ready.wait_for_low().await.expect(GPIO_ERROR_MSG);
        result
    }

    /// Perform self offset calibration.
    #[inline]
    pub async fn self_offset_calibrate<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        let result = self.standalone_command(spi, opcodes::SELFOCAL);
        self.data_ready.wait_for_low().await.expect(GPIO_ERROR_MSG);
        result
    }

    /// Perform self gain calibration.
    #[inline]
    pub async fn self_gain_calibrate<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        let result = self.standalone_command(spi, opcodes::SELFGCAL);
        self.data_ready.wait_for_low().await.expect(GPIO_ERROR_MSG);
        result
    }

    /// Perform system offset calibration.
    #[inline]
    pub async fn system_offset_calibrate<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        let result = self.standalone_command(spi, opcodes::SYSOCAL);
        self.data_ready.wait_for_low().await.expect(GPIO_ERROR_MSG);
        result
    }

    /// Perform system gain calibration.
    #[inline]
    pub async fn system_gain_calibrate<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        let result = self.standalone_command(spi, opcodes::SYSGCAL);
        self.data_ready.wait_for_low().await.expect(GPIO_ERROR_MSG);
        result
    }

    //----- Public register read/write ----------------------------------
    #[inline]
    pub fn read_status<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
    ) -> Result<Status, <SpiT as spi::ErrorType>::Error> {
        Ok(Status(self.read_registers::<_, 1>(spi, status::ADDRESS)?[0]))
    }

    #[inline]
    pub fn write_status<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
        setting: Status,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        // Create full command buffer, initialize command bytes to 0 and set data value to status
        // byte.
        let buffer: [u8; 3] = [0, 0, setting.0];
        self.write_registers(spi, status::ADDRESS, buffer)
    }

    #[inline]
    pub fn read_multiplexer<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
    ) -> Result<Multiplexer, <SpiT as spi::ErrorType>::Error> {
        Ok(Multiplexer(self.read_registers::<_, 1>(spi, mux::ADDRESS)?[0]))
    }

    #[inline]
    pub fn write_multiplexer<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
        setting: Multiplexer,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        // Create full command buffer, initialize command bytes to 0 and set data value to status
        // byte.
        let buffer: [u8; 3] = [0, 0, setting.0];
        self.write_registers(spi, mux::ADDRESS, buffer)
    }

    #[inline]
    pub fn read_ad_control<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
    ) -> Result<AdControl, <SpiT as spi::ErrorType>::Error> {
        Ok(AdControl(self.read_registers::<_, 1>(spi, adcon::ADDRESS)?[0]))
    }

    #[inline]
    pub fn write_ad_control<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
        setting: AdControl,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        // Create full command buffer, initialize command bytes to 0 and set data value to status
        // byte.
        let buffer: [u8; 3] = [0, 0, setting.0];
        self.write_registers(spi, adcon::ADDRESS, buffer)
    }

    /// Combined function to write the ADC and multiplexer registers at the same time since this is such a common
    /// occurrence.
    #[inline]
    pub fn write_mux_adc<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
        input: Multiplexer,
        ad_control: AdControl,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        let buffer = [0u8, 0u8, input.0, ad_control.0];
        self.write_registers(spi, mux::ADDRESS, buffer)
    }

    #[inline]
    pub fn read_data_rate<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
    ) -> Result<DataRate, <SpiT as spi::ErrorType>::Error> {
        Ok(DataRate::from_byte(self.read_registers::<_, 1>(spi, drate::ADDRESS)?[0]))
    }

    #[inline]
    pub fn write_data_rate<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
        setting: DataRate,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        // Create full command buffer, initialize command bytes to 0 and set data value to status
        // byte.
        let buffer: [u8; 3] = [0, 0, setting as u8];
        self.write_registers(spi, drate::ADDRESS, buffer)
    }

    #[inline]
    pub fn read_gpio<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
    ) -> Result<DigitalIo, <SpiT as spi::ErrorType>::Error> {
        Ok(DigitalIo(self.read_registers::<_, 1>(spi, gpio::ADDRESS)?[0]))
    }

    #[inline]
    pub fn write_gpio<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
        setting: DigitalIo,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        // Create full command buffer, initialize command bytes to 0 and set data value to status
        // byte.
        let buffer: [u8; 3] = [0, 0, setting.0];
        self.write_registers(spi, gpio::ADDRESS, buffer)
    }

    #[inline]
    pub fn read_offset_calibration<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
    ) -> Result<OffsetCalibration, <SpiT as spi::ErrorType>::Error> {
        Ok(OffsetCalibration(self.read_registers(spi, ofc0::ADDRESS)?))
    }

    #[inline]
    pub fn write_offset_calibration<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
        setting: OffsetCalibration,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        // Create full command buffer, initialize command bytes to 0 and set data values to offset
        // calibration bytes
        let buffer = [0, 0, setting.0[0], setting.0[1], setting.0[2]];
        self.write_registers(spi, ofc0::ADDRESS, buffer)
    }

    #[inline]
    pub fn read_gain_calibration<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
    ) -> Result<GainCalibration, <SpiT as spi::ErrorType>::Error> {
        Ok(GainCalibration(self.read_registers(spi, fsc0::ADDRESS)?))
    }

    #[inline]
    pub fn write_gain_calibration<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
        setting: GainCalibration,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        // Create full command buffer, initialize command bytes to 0 and set data values to gain
        // calibration bytes
        let buffer = [0, 0, setting.0[0], setting.0[1], setting.0[2]];
        self.write_registers(spi, fsc0::ADDRESS, buffer)
    }

    /// Reads all calibration registers. Bytes 0 to 2 are offset calibration, bytes 3 to 5 are gain
    /// calibration.
    #[inline]
    pub fn read_all_calibration<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
    ) -> Result<AllCalibration, <SpiT as spi::ErrorType>::Error> {
        Ok(self.read_registers(spi, ofc0::ADDRESS)?.into())
    }

    #[inline]
    pub fn exec_cal_command<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
        command: &CalibrationCommand,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        let spi_op_result = spi.write(command.into());
        end_spi(&mut self.slave_select, spi, spi_op_result)
    }

    #[inline]
    pub fn read_config<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
    ) -> Result<Config, <SpiT as spi::ErrorType>::Error> {
        let bytes = self.read_registers::<_, 5>(spi, status::ADDRESS)?;
        Ok(Config::from_bytes(bytes))
    }

    #[inline]
    pub fn write_config<SpiT: SpiBus>(
        &mut self,
        spi: &mut SpiT,
        setting: Config,
    ) -> Result<(), <SpiT as spi::ErrorType>::Error> {
        // Create full command buffer, initialize command bytes to 0 and set data value to status
        // byte.
        let buffer: [u8; 7] = [
            0,
            0,
            setting.status.0,
            setting.multiplexer.0,
            setting.ad_control.0,
            setting.data_rate as u8,
            setting.digital_io.0,
        ];
        self.write_registers(spi, status::ADDRESS, buffer)
    }
}