mod shtp;

use crate::data::reader::{DataBufferReader, DataReader};
use crate::data::writer::{DataBufferWriter, DataWriter};
use crate::hardware::error::{NotAvailableError, SpiError};
use crate::hardware::imu::{Imu, ImuMeasurement};
use anyhow::{anyhow, bail, ensure, Result};
use chrono::TimeDelta;
use embedded_hal::spi::SpiDevice;
use log::info;
use num_traits::ToPrimitive;

const RX_BUFFER_LENGTH: usize = 1024;

const SHTP_VERSION_CHANNEL: u8 = 0;
const SHTP_VERSION_TAG: u8 = 0x80;

const EXECUTABLE_CHANNEL: u8 = 1;
const EXECUTABLE_RESET: u8 = 1;

const CONTROL_CHANNEL: u8 = 2;
const INITIALIZE_RESPONSE_ID: u8 = 0xF1;
const UNSOLICITED_INITIALIZE_COMMAND: u8 = 0x84;
const SET_FEATURE_REPORT_ID: u8 = 0xFD;

pub struct Bno085<SPI> {
    spi: SPI,
    sequence_number: u8,
}

impl<SPI> Bno085<SPI>
where
    SPI: SpiDevice<u8>,
    SPI::Error: Send,
    SPI::Error: Sync,
    SPI::Error: 'static,
{
    pub fn new(spi: SPI) -> Self {
        Self {
            spi,
            sequence_number: 0u8,
        }
    }

    pub fn init(&mut self) -> Result<()> {
        let mut rx = DataBufferReader::<RX_BUFFER_LENGTH>::new();

        self.spi.read(rx.get_write_buffer(shtp::Header::SIZE)).map_err(SpiError)?;
        let header = shtp::Header::parse(&mut rx)?;
        ensure!(header.channel == SHTP_VERSION_CHANNEL, "SHTP Advertisement Expected on Channel 0");
        ensure!(header.sequence_number == 0, "First Message Sequence Number");
        self.spi.read(rx.get_write_buffer(header.length as usize)).map_err(SpiError)?;
        let header = shtp::Header::parse(&mut rx)?;
        ensure!(header.channel == SHTP_VERSION_CHANNEL, "SHTP Advertisement Expected on Channel 0");
        ensure!(header.sequence_number == 1, "Second Message Sequence Number");
        println!("advertisement: {}", hex::encode(&rx.get_buffer()[..(header.length as usize)]));
        self.handle_generic_message(&mut rx)?;

        // Executable Reset Message
        self.spi.read(rx.get_write_buffer(shtp::Header::SIZE)).map_err(SpiError)?;
        let header = shtp::Header::parse(&mut rx)?;
        ensure!(header.channel == EXECUTABLE_CHANNEL, "SHTP Advertisement Expected on Channel 1");
        ensure!(header.sequence_number == 2, "Third Message Sequence Number");
        self.spi.read(rx.get_write_buffer(header.length as usize)).map_err(SpiError)?;
        let header = shtp::Header::parse(&mut rx)?;
        ensure!(header.channel == EXECUTABLE_CHANNEL, "SHTP Advertisement Expected on Channel 1");
        ensure!(header.sequence_number == 3, "Fourth Message Sequence Number");
        ensure!(rx.read_be::<u8>().ok_or(NotAvailableError)? == EXECUTABLE_RESET, "Executable Reset Complete");
        println!("reset_message: {}", hex::encode(&rx.get_buffer()[..(header.length as usize)]));

        // Initialization Message
        self.spi.read(rx.get_write_buffer(shtp::Header::SIZE)).map_err(SpiError)?;
        let header = shtp::Header::parse(&mut rx)?;
        ensure!(header.channel == CONTROL_CHANNEL, "SHTP Advertisement Expected on Channel 0");
        ensure!(header.sequence_number == 4, "Fifth Message Sequence Number");
        self.spi.read(rx.get_write_buffer(header.length as usize)).map_err(SpiError)?;
        let header = shtp::Header::parse(&mut rx)?;
        ensure!(header.channel == CONTROL_CHANNEL, "SHTP Advertisement Expected on Channel 0");
        ensure!(header.sequence_number == 5, "Sixth Message Sequence Number");
        println!("init_message : {}", hex::encode(&rx.get_buffer()[..(header.length as usize)]));

        let mut tx = DataBufferWriter::<128>::new();
        self.produce_sensor_enable(&tx)?;
        self.spi.write(tx.get_buffer()).map_err(SpiError)?;

        Ok(())
    }

    fn handle_generic_message(&self, rx: &mut impl DataReader) -> Result<()> {
        let tag = rx.read_le::<u8>().ok_or(NotAvailableError)?;
        let length = rx.read_le::<u8>().ok_or(NotAvailableError)?;
        let value = rx.read(length as usize).ok_or(NotAvailableError)?;

        if tag == SHTP_VERSION_TAG {
            let shtp_version = String::from_utf8_lossy(&value[..(length as usize - 1)]);
            info!("BNO085 SHTP Version: {shtp_version}");
            return Ok(());
        }

        Err(anyhow!("Unknown Tag {tag}"))
    }

    fn handle_initialize_response(&self, rx: &mut impl DataReader) -> Result<()> {
        let report_id = rx.read_le::<u8>().ok_or(NotAvailableError)?;

        if report_id != INITIALIZE_RESPONSE_ID {
            bail!("Unknown Report ID {report_id}");
        }

        let _sequence_number = rx.read_le::<u8>().ok_or(NotAvailableError)?;
        let command = rx.read_le::<u8>().ok_or(NotAvailableError)?;
        ensure!(command == UNSOLICITED_INITIALIZE_COMMAND, "");
        let _command_sequence_number = rx.read_le::<u8>().ok_or(NotAvailableError)?;
        let _response_sequence_number = rx.read_le::<u8>().ok_or(NotAvailableError)?;
        let status = rx.read_le::<u8>().ok_or(NotAvailableError)?;
        ensure!(status == 0, "Status (0 – successful. 1 – Operation failed)");
        let subsystem = rx.read_le::<u8>().ok_or(NotAvailableError)?;
        info!("BNO085 Subsystem: {subsystem}");
        let _r2 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
        let _r3 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
        let _r4 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
        let _r5 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
        let _r6 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
        let _r7 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
        let _r8 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
        let _r9 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
        let _r10 = rx.read_le::<u8>().ok_or(NotAvailableError)?;

        Ok(())
    }

    fn produce_sensor_enable(
        &mut self,
        tx: &impl DataWriter,
    ) -> Result<()> {
        let starting_position = tx.get_position();
        let length = tx.defer::<u16>()?;
        tx.push_be(CONTROL_CHANNEL)?;
        tx.push_be(self.sequence_number)?;
        self.sequence_number = self.sequence_number.wrapping_add(1);

        self.produce_set_feature(
            tx,
            0x01, // Accelerometer,
            0,
            0,
            TimeDelta::seconds(1) / 100, // 100Hz
            TimeDelta::zero(),
            0,
        )?;
        self.produce_set_feature(
            tx,
            0x02, // Gyroscope (Calibrated)
            0,
            0,
            TimeDelta::seconds(1) / 100, // 100Hz
            TimeDelta::zero(),
            0,
        )?;
        self.produce_set_feature(
            tx,
            0x03, // Magnetic Field (Calibrated)
            0,
            0,
            TimeDelta::seconds(1) / 100, // 100Hz
            TimeDelta::zero(),
            0,
        )?;

        length.set_le((tx.get_position() - starting_position) as u16); // Fill in the length as we finish
        Ok(())
    }

    fn produce_set_feature(
        &self,
        tx: &impl DataWriter,
        feature_report_id: u8,
        feature_flags: u8,
        change_sensitivity: u16,
        report_interval: TimeDelta,
        batch_interval: TimeDelta,
        configuration_word: u32,
    ) -> Result<()> {
        tx.push_le(SET_FEATURE_REPORT_ID)?;
        tx.push_le(feature_report_id)?;
        tx.push_le(feature_flags)?;
        tx.push_le(change_sensitivity)?;
        let report_interval = report_interval.num_microseconds()
            .and_then(|us| us.to_u32())
            .ok_or(anyhow!("Could Not Convert Report Interval to Microseconds"))?;
        tx.push_le(report_interval)?;
        let batch_interval = batch_interval.num_microseconds()
            .and_then(|us| us.to_u32())
            .ok_or(anyhow!("Could Not Convert Batch Interval to Microseconds"))?;
        tx.push_le(batch_interval)?;
        tx.push_le(configuration_word)?;
        Ok(())
    }
}

impl<SPI: SpiDevice> Imu for Bno085<SPI> {
    fn step(&mut self) -> Result<ImuMeasurement> {
        todo!("asd")
    }
}

#[cfg(test)]
mod tests {
    use crate::test_utils::SilentDrop;
    use embedded_hal_mock::eh1::spi::Mock as SpiMock;
    use embedded_hal_mock::eh1::spi::Transaction as SpiTransaction;

    #[test]
    fn test_init() -> Result<()> {
        let expectations = vec![
            SpiTransaction::transaction_start(),
            SpiTransaction::read_vec(vec![
                0x0C, 0x00, 0x00, 0x00
            ]),
            SpiTransaction::transaction_end(),
            SpiTransaction::transaction_start(),
            SpiTransaction::read_vec(vec![
                0x0C, 0x00, 0x00, 0x01, 0x80, 0x06, 0x30, 0x2E, 0x30, 0x2E, 0x30, 0x00,
            ]),
            SpiTransaction::transaction_end(),
            SpiTransaction::transaction_start(),
            SpiTransaction::read_vec(vec![
                0x05, 0x00, 0x01, 0x02
            ]),
            SpiTransaction::transaction_end(),
            SpiTransaction::transaction_start(),
            SpiTransaction::read_vec(vec![
                0x05, 0x00, 0x01, 0x03, 1
            ]),
            SpiTransaction::transaction_end(),
            SpiTransaction::transaction_start(),
            SpiTransaction::read_vec(vec![
                20, 0x00, 0x02, 0x04
            ]),
            SpiTransaction::transaction_end(),
            SpiTransaction::transaction_start(),
            SpiTransaction::read_vec(vec![
                20, 0x00, 0x02, 0x05, 0xF1, 0x00, 0x84, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
            ]),
            SpiTransaction::transaction_end(),
            SpiTransaction::transaction_start(),
            SpiTransaction::write_vec(vec![55, 0, 2, 0, 253, 1, 0, 0, 0, 16, 39, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 253, 2, 0, 0, 0, 16, 39, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 253, 3, 0, 0, 0, 16, 39, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]),
            SpiTransaction::transaction_end(),
        ];

        let spi = SpiMock::new(&expectations);

        let mut bno = SilentDrop::new(Bno085::new(spi));

        bno.init()?;

        bno.spi.done();

        Ok(())
    }
}

