use std::fmt::Debug;
use std::hint::unreachable_unchecked;
use strum_macros::FromRepr;
use anyhow::{ensure, Result};
use thiserror::Error;
use crate::hardware::a4963::A4963Diagnostic;

pub(super) const WRITE_BIT: u16 = 0b0001_0000_0000_0000;

pub(super) trait A4963Register: Clone + PartialEq + Debug {
    const ADDRESS: u16;

    fn encode(self) -> u16;
    fn decode(value: u16) -> Self;
}

#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)]
#[repr(u8)]
pub(super) enum RecirculationMode {
    Auto = 0b00,
    High = 0b01,
    Low = 0b10,
    Off = 0b11,
}

#[derive(Clone, Debug, PartialEq, Eq)]
pub(super) struct Configuration0Register {
    pub recirculation_mode: RecirculationMode,
    /// 4 bits long. Blank time is this * 400ns
    pub blank_time: u8,
    /// 6 bits long. Dead time is this * 50ns
    pub dead_time: u8,
}

impl Default for Configuration0Register {
    fn default() -> Self {
        Self {
            recirculation_mode: RecirculationMode::Auto,
            blank_time: 0b1000,
            dead_time: 0b01_0100,
        }
    }
}

impl A4963Register for Configuration0Register {
    const ADDRESS: u16 = 0b0000_0000_0000_0000;

    fn encode(self) -> u16 {
        assert!(self.blank_time <= 0b1111);
        assert!(self.dead_time <= 0b11_1111);

        let mut result = 0;

        result |= (self.recirculation_mode as u16) << 10;
        result |= (self.blank_time as u16) << 6;
        result |= self.dead_time as u16;

        result
    }

    fn decode(value: u16) -> Self {
        Self {
            recirculation_mode: RecirculationMode::from_repr(((value >> 10) & 0b11) as u8)
                // Safety: the enum covers all permutations
                .unwrap_or_else(|| unsafe { unreachable_unchecked() }),
            blank_time: ((value >> 6) & 0b1111) as u8,
            dead_time: (value & 0b11_1111) as u8,
        }
    }
}

#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)]
#[repr(u8)]
pub(super) enum BemfTimeQualifier {
    Debounce = 0b0,
    Window = 0b1,
}

#[derive(Clone, Debug, PartialEq, Eq)]
pub(super) struct Configuration1Register {
    /// 12.5% if false, 25% if true
    pub enable_fast_decay: bool,
    pub invert_pwm: bool,
    /// 4 bit value. V_ilm = (n + 1) * 12.5mV
    pub current_sense_threshold: u8,
    pub bemf_time_qualifier: BemfTimeQualifier,
    /// 5 bit value. V_dst = n * 50mV
    pub short_detection_threshold: u8,
}

impl Default for Configuration1Register {
    fn default() -> Self {
        Self {
            enable_fast_decay: false,
            invert_pwm: false,
            current_sense_threshold: 0b1111,
            bemf_time_qualifier: BemfTimeQualifier::Debounce,
            short_detection_threshold: 0b1_1111,
        }
    }
}

impl A4963Register for Configuration1Register {
    const ADDRESS: u16 = 0b0010_0000_0000_0000;

    fn encode(self) -> u16 {
        assert!(self.current_sense_threshold <= 0b1111);
        assert!(self.short_detection_threshold <= 0b1_1111);

        let mut result = 0;

        result |= if self.enable_fast_decay { 0b0000_1000_0000_0000 } else {0};
        result |= if self.invert_pwm { 0b0000_0100_0000_0000 } else {0};
        result |= (self.current_sense_threshold as u16) << 6;
        result |= (self.bemf_time_qualifier as u16) << 5;
        result |= self.short_detection_threshold as u16;

        result
    }

    fn decode(value: u16) -> Self {
        Self {
            enable_fast_decay: (value & 0b0000_1000_0000_0000) != 0,
            invert_pwm: (value & 0b0000_0100_0000_0000) != 0,
            current_sense_threshold: ((value >> 6) & 0b1111) as u8,
            bemf_time_qualifier: BemfTimeQualifier::from_repr(((value >> 5) & 0b1) as u8)
                // Safety: the enum covers all permutations
                .unwrap_or_else(|| unsafe { unreachable_unchecked() }),
            short_detection_threshold: (value & 0b1_1111) as u8,
        }
    }
}

#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)]
#[repr(u8)]
pub(super) enum OverspeedLimit {
    Percent100 = 0b00,
    Percent125 = 0b01,
    Percent150 = 0b10,
    Percent200 = 0b11,
}

#[derive(Clone, Debug, PartialEq, Eq)]
pub(super) struct Configuration2Register {
    /// K_CP = 2^(n-7)
    pub position_proportional_gain: u8,
    pub overspeed_limit: OverspeedLimit,
    pub degauss_compensation: bool,
    /// t_PW = 20us + (n * 1.6us)
    pub fixed_period: u8,
}

impl Default for Configuration2Register {
    fn default() -> Self {
        Self {
            position_proportional_gain: 0b1000,
            overspeed_limit: OverspeedLimit::Percent150,
            degauss_compensation: false,
            fixed_period: 0b1_0011,
        }
    }
}

impl A4963Register for Configuration2Register {
    const ADDRESS: u16 = 0b0100_0000_0000_0000;

    fn encode(self) -> u16 {
        assert!(self.position_proportional_gain <= 0b1111);
        assert!(self.fixed_period <= 0b1_1111);

        let mut result = 0;

        result |= (self.position_proportional_gain as u16) << 8;
        result |= (self.overspeed_limit as u16) << 6;
        result |= if self.degauss_compensation {0b0000_0000_0010_0000} else {0};
        result |= self.fixed_period as u16;

        result
    }

    fn decode(value: u16) -> Self {
        Self {
            position_proportional_gain: ((value >> 8) & 0b1111) as u8,
            overspeed_limit: OverspeedLimit::from_repr(((value >> 6) & 0b11) as u8)
                // Safety: the enum covers all permutations
                .unwrap_or_else(|| unsafe { unreachable_unchecked() }),
            degauss_compensation: (value & 0b0000_0000_0010_0000) != 0,
            fixed_period: (value & 0b1_1111) as u8,
        }
    }
}

#[derive(Clone, Debug, PartialEq, Eq)]
pub(super) struct Configuration3Register {
    /// K_CI = 2^(n-7)
    pub position_integral_gain: u8,
    /// D_H = (n + 1) * 6.25%
    pub hold_torque_duty_cycle: u8,
    /// t_HOLD = n * 8ms
    pub hold_time: u8,
}

impl Default for Configuration3Register {
    fn default() -> Self {
        Self {
            position_integral_gain: 0b1000,
            hold_torque_duty_cycle: 0b0101,
            hold_time: 0b0010,
        }
    }
}

impl A4963Register for Configuration3Register {
    const ADDRESS: u16 = 0b0110_0000_0000_0000;

    fn encode(self) -> u16 {
        assert!(self.position_integral_gain <= 0b1111);
        assert!(self.hold_torque_duty_cycle <= 0b1111);
        assert!(self.hold_time <= 0b1111);

        let mut result = 0;

        result |= (self.position_integral_gain as u16) << 8;
        result |= (self.hold_torque_duty_cycle as u16) << 4;
        result |= self.hold_time as u16;

        result
    }

    fn decode(value: u16) -> Self {
        Self {
            position_integral_gain: ((value >> 8) & 0b1111) as u8,
            hold_torque_duty_cycle: ((value >> 4) & 0b1111) as u8,
            hold_time: (value & 0b1111) as u8,
        }
    }
}

#[derive(Clone, Debug, PartialEq, Eq)]
pub(super) struct Configuration4Register {
    /// K_SP = 2^(n-7) * K_NSP
    pub speed_proportional_gain: u8,
    /// D_S = (n + 1) * 6.25%
    pub forced_startup_torque_duty_cycle: u8,
    /// f_ST = (n + 1) * 2Hz
    pub start_speed: u8,
}

impl Default for Configuration4Register {
    fn default() -> Self {
        Self {
            speed_proportional_gain: 0b1000,
            forced_startup_torque_duty_cycle: 0b0111,
            start_speed: 0b0011,
        }
    }
}

impl A4963Register for Configuration4Register {
    const ADDRESS: u16 = 0b1000_0000_0000_0000;

    fn encode(self) -> u16 {
        assert!(self.speed_proportional_gain <= 0b1111);
        assert!(self.forced_startup_torque_duty_cycle <= 0b1111);
        assert!(self.start_speed <= 0b1111);

        let mut result = 0;

        result |= (self.speed_proportional_gain as u16) << 8;
        result |= (self.forced_startup_torque_duty_cycle as u16) << 4;
        result |= self.start_speed as u16;

        result
    }

    fn decode(value: u16) -> Self {
        Self {
            speed_proportional_gain: ((value >> 8) & 0b1111) as u8,
            forced_startup_torque_duty_cycle: ((value >> 4) & 0b1111) as u8,
            start_speed: (value & 0b1111) as u8,
        }
    }
}

#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)]
#[repr(u8)]
pub(super) enum SpeedOutputSelection {
    ElectricalFrequency = 0b0,
    CommutationFrequency = 0b1,
}

#[derive(Clone, Debug, PartialEq, Eq)]
pub(super) struct Configuration5Register {
    /// K_SI = 2^(n-7) * K_NSI
    pub speed_integral_gain: u8,
    pub speed_output_selection: SpeedOutputSelection,
    /// f_MS = (2^(8 + n) - 1) * 0.1Hz
    pub maximum_electrical_cycle_frequency: u8,
    /// theta_ADV = n * 1.875˚ (electrical)
    pub phase_advance: u8,
}

impl Default for Configuration5Register {
    fn default() -> Self {
        Self {
            speed_integral_gain: 0b1000,
            speed_output_selection: SpeedOutputSelection::ElectricalFrequency,
            maximum_electrical_cycle_frequency: 0b101,
            phase_advance: 0b1000,
        }
    }
}

impl A4963Register for Configuration5Register {
    const ADDRESS: u16 = 0b1010_0000_0000_0000;

    fn encode(self) -> u16 {
        assert!(self.speed_integral_gain <= 0b1111);
        assert!(self.maximum_electrical_cycle_frequency <= 0b111);
        assert!(self.phase_advance <= 0b1111);

        let mut result = 0;

        result |= (self.speed_integral_gain as u16) << 8;
        result |= (self.speed_output_selection as u16) << 7;
        result |= (self.maximum_electrical_cycle_frequency as u16) << 4;
        result |= self.phase_advance as u16;

        result
    }

    fn decode(value: u16) -> Self {
        Self {
            speed_integral_gain: ((value >> 8) & 0b1111) as u8,
            speed_output_selection: SpeedOutputSelection::from_repr(((value >> 7) & 0b1) as u8)
                // Safety: the enum covers all permutations
                .unwrap_or_else(|| unsafe { unreachable_unchecked() }),
            maximum_electrical_cycle_frequency: ((value >> 4) & 0b111) as u8,
            phase_advance: (value & 0b1111) as u8,
        }
    }
}

#[derive(Clone, Debug, PartialEq, Eq)]
pub(super) struct MaskRegister {
    pub enable_temperature_warning: bool,
    pub enable_overtemperature: bool,
    pub enable_loss_of_synchronization: bool,
    pub enable_undervoltage: bool,
    pub enable_phase_a_high_side: bool,
    pub enable_phase_a_low_side: bool,
    pub enable_phase_b_high_side: bool,
    pub enable_phase_b_low_side: bool,
    pub enable_phase_c_high_side: bool,
    pub enable_phase_c_low_side: bool,
}

impl Default for MaskRegister {
    fn default() -> Self {
        Self {
            enable_temperature_warning: true,
            enable_overtemperature: true,
            enable_loss_of_synchronization: true,
            enable_undervoltage: true,
            enable_phase_a_high_side: true,
            enable_phase_a_low_side: true,
            enable_phase_b_high_side: true,
            enable_phase_b_low_side: true,
            enable_phase_c_high_side: true,
            enable_phase_c_low_side: true,
        }
    }
}

impl A4963Register for MaskRegister {
    const ADDRESS: u16 = 0b1100_0000_0000_0000;

    fn encode(self) -> u16 {
        let mut result = 0;

        result |= if !self.enable_temperature_warning { 0b0000_1000_0000_0000 } else { 0 };
        result |= if !self.enable_overtemperature { 0b0000_0100_0000_0000 } else { 0 };
        result |= if !self.enable_loss_of_synchronization { 0b0000_0010_0000_0000 } else { 0 };
        result |= if !self.enable_undervoltage { 0b0000_0000_1000_0000 } else { 0 };
        result |= if !self.enable_phase_a_high_side { 0b0000_0000_0010_0000 } else { 0 };
        result |= if !self.enable_phase_a_low_side { 0b0000_0000_0001_0000 } else { 0 };
        result |= if !self.enable_phase_b_high_side { 0b0000_0000_0000_1000 } else { 0 };
        result |= if !self.enable_phase_b_low_side { 0b0000_0000_0000_0100 } else { 0 };
        result |= if !self.enable_phase_c_high_side { 0b0000_0000_0000_0010 } else { 0 };
        result |= if !self.enable_phase_c_low_side { 0b0000_0000_0000_0001 } else { 0 };

        result
    }

    fn decode(value: u16) -> Self {
        Self {
            enable_temperature_warning: (value & 0b0000_1000_0000_0000) == 0,
            enable_overtemperature: (value & 0b0000_0100_0000_0000) == 0,
            enable_loss_of_synchronization: (value & 0b0000_0010_0000_0000) == 0,
            enable_undervoltage: (value & 0b0000_0000_1000_0000) == 0,
            enable_phase_a_high_side: (value & 0b0000_0000_0010_0000) == 0,
            enable_phase_a_low_side: (value & 0b0000_0000_0001_0000) == 0,
            enable_phase_b_high_side: (value & 0b0000_0000_0000_1000) == 0,
            enable_phase_b_low_side: (value & 0b0000_0000_0000_0100) == 0,
            enable_phase_c_high_side: (value & 0b0000_0000_0000_0010) == 0,
            enable_phase_c_low_side: (value & 0b0000_0000_0000_0001) == 0,
        }
    }
}

#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)]
#[repr(u8)]
pub(super) enum MotorControlMode {
    IndirectDutyCycle = 0b00,
    DirectDutyCycle = 0b01,
    ClosedLoopCurrent = 0b10,
    ClosedLoopSpeed = 0b11,
}

#[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)]
#[repr(u8)]
pub(super) enum Direction {
    Forward = 0b0,
    Reverse = 0b1,
}

#[derive(Clone, Debug, PartialEq, Eq)]
pub(super) struct RunRegister {
    pub motor_control_mode: MotorControlMode,
    pub stop_on_fail: bool,
    /// 0 enables PWM input; otherwise duty cycle D_C = 7 + (n * 3)%
    pub duty_cycle_control: u8,
    pub restart_after_loss_of_sync: bool,
    pub brake_when_inactive: bool,
    pub direction: Direction,
    pub enable: bool,
}

impl Default for RunRegister {
    fn default() -> Self {
        Self {
            motor_control_mode: MotorControlMode::IndirectDutyCycle,
            stop_on_fail: false,
            duty_cycle_control: 0,
            restart_after_loss_of_sync: true,
            brake_when_inactive: false,
            direction: Direction::Forward,
            enable: true,
        }
    }
}

impl A4963Register for RunRegister {
    const ADDRESS: u16 = 0b1110_0000_0000_0000;

    fn encode(self) -> u16 {
        assert!(self.duty_cycle_control <= 0b11111);

        let mut result = 0;

        result |= (self.motor_control_mode as u16) << 10;
        result |= if self.stop_on_fail { 0b0000_0010_0000_0000 } else { 0 };
        result |= (self.duty_cycle_control as u16) << 4;
        result |= if self.restart_after_loss_of_sync { 0b0000_0000_0000_1000 } else { 0 };
        result |= if self.brake_when_inactive { 0b0000_0000_0000_0100 } else { 0 };
        result |= (self.direction as u16) << 1;
        result |= if self.enable { 0b0000_0000_0000_0001 } else { 0 };

        result
    }

    fn decode(value: u16) -> Self {
        Self {
            motor_control_mode: MotorControlMode::from_repr(((value >> 10) & 0b11) as u8)
                // Safety: the enum covers all permutations
                .unwrap_or_else(|| unsafe { unreachable_unchecked() }),
            stop_on_fail: (value & 0b0000_0010_0000_0000) != 0,
            duty_cycle_control: ((value >> 4) & 0b11111) as u8,
            restart_after_loss_of_sync: (value & 0b0000_0000_0000_1000) != 0,
            brake_when_inactive: (value & 0b0000_0000_0000_0100) != 0,
            direction: Direction::from_repr(((value >> 1) & 0b1) as u8)
                // Safety: the enum covers all permutations
                .unwrap_or_else(|| unsafe { unreachable_unchecked() }),
            enable: (value & 0b0000_0000_0000_0001) != 0,
        }
    }
}

#[derive(Clone, Debug, Error, PartialEq, Eq)]
enum DiagnosticRegisterError {
    #[error("Unknown Fault")]
    UnknownFault,
    #[error("Power On Reset")]
    PowerOnReset,
    #[error("Serial Transfer Error")]
    SerialTransferError,
    #[error("High Temperature Warning")]
    HighTemperatureWarning,
    #[error("Overtemperature Fault")]
    Overtemperature,
    #[error("Loss of Synchronization Fault")]
    LossOfSynchronization,
    #[error("Undervoltage Fault")]
    Undervoltage,
    #[error("Phase A High Side Fault")]
    PhaseAHighSide,
    #[error("Phase A Low Side Fault")]
    PhaseALowSide,
    #[error("Phase B High Side Fault")]
    PhaseBHighSide,
    #[error("Phase B Low Side Fault")]
    PhaseBLowSide,
    #[error("Phase C High Side Fault")]
    PhaseCHighSide,
    #[error("Phase C Low Side Fault")]
    PhaseCLowSide,
}

impl A4963Diagnostic {
    pub(super) const fn decode(value: u16) -> Self {
        Self {
            fault_flag: (value & 0b1000_0000_0000_0000) != 0,
            power_on_reset: (value & 0b0100_0000_0000_0000) != 0,
            serial_transfer_error: (value & 0b0010_0000_0000_0000) != 0,

            high_temperature_warning: (value & 0b0000_1000_0000_0000) != 0,
            overtemperature_shutdown: (value & 0b0000_0100_0000_0000) != 0,
            loss_of_synchronization: (value & 0b0000_0010_0000_0000) != 0,
            undervoltage: (value & 0b0000_0000_1000_0000) != 0,
            phase_a_high_side_fault: (value & 0b0000_0000_0010_0000) != 0,
            phase_a_low_side_fault: (value & 0b0000_0000_0001_0000) != 0,
            phase_b_high_side_fault: (value & 0b0000_0000_0000_1000) != 0,
            phase_b_low_side_fault: (value & 0b0000_0000_0000_0100) != 0,
            phase_c_high_side_fault: (value & 0b0000_0000_0000_0010) != 0,
            phase_c_low_side_fault: (value & 0b0000_0000_0000_0001) != 0,
        }
    }

    pub fn assert_consistency(&self) -> Result<()> {
        let measured_fault = self.power_on_reset ||
            self.serial_transfer_error ||
            self.high_temperature_warning ||
            self.overtemperature_shutdown ||
            self.loss_of_synchronization ||
            self.undervoltage ||
            self.phase_a_high_side_fault ||
            self.phase_a_low_side_fault ||
            self.phase_b_high_side_fault ||
            self.phase_b_low_side_fault ||
            self.phase_c_high_side_fault ||
            self.phase_c_low_side_fault;

        ensure!(self.fault_flag == measured_fault, "Measured Diagnostic {self:?} is Inconsistent with Fault Flag");

        Ok(())
    }

    pub fn assert_healthy(self) -> Result<()> {
        if self.power_on_reset {
            Err(DiagnosticRegisterError::PowerOnReset.into())
        } else if self.serial_transfer_error {
            Err(DiagnosticRegisterError::SerialTransferError.into())
        } else if self.high_temperature_warning {
            Err(DiagnosticRegisterError::HighTemperatureWarning.into())
        } else if self.overtemperature_shutdown {
            Err(DiagnosticRegisterError::Overtemperature.into())
        } else if self.loss_of_synchronization {
            Err(DiagnosticRegisterError::LossOfSynchronization.into())
        } else if self.undervoltage {
            Err(DiagnosticRegisterError::Undervoltage.into())
        } else if self.phase_a_high_side_fault {
            Err(DiagnosticRegisterError::PhaseAHighSide.into())
        } else if self.phase_a_low_side_fault {
            Err(DiagnosticRegisterError::PhaseALowSide.into())
        } else if self.phase_b_high_side_fault {
            Err(DiagnosticRegisterError::PhaseBHighSide.into())
        } else if self.phase_b_low_side_fault {
            Err(DiagnosticRegisterError::PhaseBLowSide.into())
        } else if self.phase_c_high_side_fault {
            Err(DiagnosticRegisterError::PhaseCHighSide.into())
        } else if self.phase_c_low_side_fault {
            Err(DiagnosticRegisterError::PhaseCLowSide.into())
        } else if self.fault_flag {
            // This shouldn't be possible
            Err(DiagnosticRegisterError::UnknownFault.into())
        } else {
            Ok(())
        }
    }
}
