use std::any::type_name;
use std::fmt::{Debug, Formatter};
use crate::hardware::a4963::{A4963Diagnostic, A4963};
use anyhow::{ensure, Result};
use embedded_hal::spi::SpiDevice;
use log::trace;
use crate::hardware::a4963::register::{A4963Register, Configuration0Register, Configuration1Register, Configuration2Register, Configuration3Register, Configuration4Register, Configuration5Register, Direction, MaskRegister, MotorControlMode, RunRegister, SpeedOutputSelection, WRITE_BIT};
use crate::hardware::error::WrappingError;

pub struct A4963Driver<SPI> {
    spi: SPI,
    mask_register: MaskRegister
}

impl<SPI> A4963Driver<SPI>
where
    SPI: SpiDevice<u8>,
    SPI::Error: Send,
    SPI::Error: Sync,
    SPI::Error: 'static, {
    pub fn new(spi: SPI) -> Self {
        trace!(
            "A4963Driver<SPI={}>::new(spi)",
            type_name::<SPI>()
        );
        let mut result = Self {
            spi,
            mask_register: MaskRegister {
                ..Default::default()
            },
        };

        let _ = result.write(RunRegister {
            enable: false,
            ..Default::default()
        });

        result
    }

    fn read_diagnostic_register(&mut self) -> Result<A4963Diagnostic> {
        self.write(self.mask_register.clone())
    }

    fn write<Register: A4963Register>(&mut self, register: Register) -> Result<A4963Diagnostic> {
        trace!("A4963Driver::write(self: {self:?}, register: {register:?})");
        let mut rx_buffer = [0u8; 2];
        let tx_data = register.encode() | Register::ADDRESS | WRITE_BIT;
        trace!("A4963Driver::write - tx {} = {tx_data:016b}", type_name::<Register>());
        let tx_buffer = tx_data.to_be_bytes();
        self.spi.transfer(&mut rx_buffer, &tx_buffer).map_err(WrappingError)?;
        let rx_data = u16::from_be_bytes(rx_buffer);
        trace!("A4963Driver::write - rx = {rx_data:016b}");
        Ok(A4963Diagnostic::decode(rx_data))
    }

    fn read<Register: A4963Register>(&mut self) -> Result<Register> {
        trace!("A4963Driver::read<{}>(self: {self:?})", type_name::<Register>());
        let mut rx_buffer = [0u8; 2];
        let tx_buffer = Register::ADDRESS.to_be_bytes();
        trace!("A4963Driver::read - tx = {:016b}", Register::ADDRESS);
        self.spi.transfer(&mut rx_buffer, &tx_buffer).map_err(WrappingError)?;
        let rx_data = u16::from_be_bytes(rx_buffer);
        trace!("A4963Driver::read - rx {} = {rx_data:016b}", type_name::<Register>());
        A4963Diagnostic::decode(rx_data & 0b1110_0000_0000_0000)
            .assert_healthy()?;
        Ok(Register::decode(rx_data))
    }

    fn write_verify<Register: A4963Register>(&mut self, register: Register) -> Result<A4963Diagnostic> {
        let result = self.write(register.clone())?;
        let readback = self.read()?;

        ensure!(register == readback, "Register did not match readback.\nregister = {register:?},\nreadback = {readback:?}");

        Ok(result)
    }
}

impl<SPI> A4963 for A4963Driver<SPI>
where
    SPI: SpiDevice<u8>,
    SPI::Error: Send,
    SPI::Error: Sync,
    SPI::Error: 'static, {

    fn check_health(&mut self) -> Result<A4963Diagnostic> {
        trace!("A4963Driver::check_health(self: {self:?})");
        self.read_diagnostic_register()
    }

    fn write_configuration(&mut self) -> Result<()> {
        trace!("A4963Driver::write_configuration(self: {self:?})");

        self.check_health()?.assert_healthy()?;
        // Write our configurations
        self.write_verify(Configuration0Register {
            ..Default::default()
        })?.assert_healthy()?;
        self.write_verify(Configuration1Register {
            // I_lim = (n + 1) * 0.0125 / 0.007 = 8.9285714286A
            current_sense_threshold: 4,
            ..Default::default()
        })?.assert_healthy()?;
        self.write_verify(Configuration2Register {
            ..Default::default()
        })?.assert_healthy()?;
        self.write_verify(Configuration3Register {
            position_integral_gain: 0b1001,
            ..Default::default()
        })?.assert_healthy()?;
        self.write_verify(Configuration4Register {
            forced_startup_torque_duty_cycle: 0b1111,
            start_speed: 0b1111,
            ..Default::default()
        })?.assert_healthy()?;
        self.write_verify(Configuration5Register {
            speed_integral_gain: 0b0111,
            speed_output_selection: SpeedOutputSelection::ElectricalFrequency,
            // 819.1Hz = 49146 rpm max speed (divide by number of motor pole pairs)
            // Measured maximum of the motor is ~840Hz
            maximum_electrical_cycle_frequency: 0b101,
            phase_advance: 0b0100,
            ..Default::default()
        })?.assert_healthy()?;
        // Set up the run mode for the motor (this enables the motor)
        self.write_verify(RunRegister {
            motor_control_mode: MotorControlMode::ClosedLoopSpeed,
            restart_after_loss_of_sync: false, // I want to handle this in my code
            direction: Direction::Forward, // A = Black, B = Yellow, C = Red
            enable: true,
            ..Default::default()
        })?.assert_healthy()?;

        Ok(())
    }
}

impl<SPI> Debug for A4963Driver<SPI> {
    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
        write!(
            f,
            "A4963Driver<SPI={}> {{ }}",
            type_name::<SPI>(),
        )
    }
}
