use crate::hardware::error::WrappingError;
use crate::hardware::mct8316a::Mct8316a;
use crate::hardware::mct8316a::closed_loop::{
    BemfThreshold, ClosedLoop1, ClosedLoop2, ClosedLoop3, ClosedLoop4, ClosedLoopDecelerationMode,
    ClosedLoopRate, CommutationMethod, CommutationMode, DegaussLowerBound, DegaussSamples,
    DegaussUpperBound, DegaussWindow, DutyCycleThreshold, FastBrakeDelta, IntegrationBemfThreshold,
    IntegrationCycleHighThreshold, IntegrationCycleLowThreshold, IntegrationDutyCycleThreshold,
    LeadAnglePolarity, LowerPercentLimit, MotorStopBrakeTime, MotorStopMode, PwmFrequency, PwmMode,
    PwmModulation, SpeedFeedbackConfig, SpeedFeedbackDivision, SpeedFeedbackMode,
    UpperPercentLimit,
};
use crate::hardware::mct8316a::constant_power::{ConstantPower, PowerHysteresis, PowerMode};
use crate::hardware::mct8316a::constant_speed::{ClosedLoopMode, ConstantSpeed};
use crate::hardware::mct8316a::device_config::{
    ClockSource, DeviceConfig, DeviceMode, ExternalClockFrequency, PwmRangeSelect,
};
use crate::hardware::mct8316a::eeprom::Mct8316AVEeprom;
use crate::hardware::mct8316a::fault_config::{
    AbnormalSpeedLock, AbnormalSpeedLockThreshold, AutomaticRetries, CycleByCycleCurrentLimit,
    FaultConfig1, FaultConfig2, LockDetectionCurrentLimit, LockDetectionCurrentLimitDeglitchTime,
    LockMinSpeed, LockMode, LockRetryTime, LossSyncTimes, MaxMotorVoltage, MinMotorVoltage,
    MotorVoltageMode, NoMotorDetectDeglitchTime, NoMotorThreshold, ZeroDutyThreshold,
};
use crate::hardware::mct8316a::gate_driver_config::{
    BuckCurrentLimit, BuckSlewRate, BuckVoltageSelection, CurrentSenseAmplifier,
    DemagComparatorThreshold, GateDriverConfig1, GateDriverConfig2, OvercurrentFaultMode,
    OvercurrentProtectionDeglitchTime, OvercurrentProtectionLevel, OvercurrentProtectionRetryTime,
    OvervoltageProtectionLevel, SlewRate, TargetDelay,
};
use crate::hardware::mct8316a::isd_config::{
    BrakeConfig, BrakeMode, IsdConfig, IsdConfigTimeValue, ResyncMinimumThreshold,
    StartupBreakTime, Threshold,
};
use crate::hardware::mct8316a::motor_startup::{
    AlignRampRate, AlignTime, DutyCycle, FirstCycleFrequencySelect, FullCurrentThreshold,
    IpdAdvanceAngle, IpdClockFrequency, IpdCurrentThreshold, IpdReleaseMode, IpdRepeat,
    MinimumDutyCycle, MotorStartup1, MotorStartup2, MotorStartupMethod, OpenClosedHandoffThreshold,
    OpenLoopAcceleration1, OpenLoopAcceleration2, OpenLoopCurrentLimitMode,
    SlowFirstCycleFrequency,
};
use crate::hardware::mct8316a::phase_profile::{
    ProfileSetting, ThreePhase150DegreeProfile, ThreePhaseLeadAngle, TwoPhase150DegreeProfile,
};
use crate::hardware::mct8316a::pin_config::{
    BrakeInputConfig, DirectionInputConfig, ExternalWatchdogFaultMode, ExternalWatchdogFrequency,
    ExternalWatchdogSource, Pin36Config, Pin37_38Config, PinConfig1, PinConfig2, SleepTime,
    SpeedInputConfig,
};
use crate::hardware::mct8316a::trap_config::{
    AVSLimitHysteresis, AVSNegativeCurrentLimit, FastStartupDivFactor, IsdBemfThreshold,
    IsdCycleThreshold, OpenLoopHandoffCycles, OpenLoopZcDetectionThreshold, TrapConfig1,
    TrapConfig2,
};
use anyhow::{Result, bail, ensure};
use embedded_hal::i2c::{I2c, Operation};
use log::trace;
use std::any::type_name;
use std::fmt::{Debug, Display, Formatter};
use std::sync::Mutex;
use crate::CRC_8_CCITT;

const CRC: crc::Crc<u8> = crc::Crc::<u8>::new(&CRC_8_CCITT);

#[allow(dead_code)]
#[derive(Copy, Clone, Eq, PartialEq, Debug)]
pub(super) enum Mct8316AVData {
    Two(u16),
    Four(u32),
    Eight(u64),
}

impl Display for Mct8316AVData {
    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
        match self {
            Mct8316AVData::Two(val) => write!(f, "{val:04x}"),
            Mct8316AVData::Four(val) => write!(f, "{val:08x}"),
            Mct8316AVData::Eight(val) => write!(f, "{val:016x}"),
        }
    }
}

#[derive(Debug, Copy, Clone)]
enum OperationRW {
    Read,
    Write,
}

fn control_word(
    operation_rw: OperationRW,
    crc: bool,
    data: Mct8316AVData,
    address: u32,
) -> [u8; 3] {
    trace!(
        "control_word(operation_rw: {operation_rw:?}, crc: {crc}, data: {data}, address: {address:06x})"
    );
    let mut control_word = [0u8; _];

    control_word[0] |= match operation_rw {
        OperationRW::Read => 0x80,
        OperationRW::Write => 0x00,
    };
    control_word[0] |= if crc { 0x40 } else { 0x00 };
    control_word[0] |= match data {
        Mct8316AVData::Two(_) => 0x00,
        Mct8316AVData::Four(_) => 0x10,
        Mct8316AVData::Eight(_) => 0x20,
    };
    control_word[0] |= ((address >> 16) & 0x0F) as u8;
    control_word[1] |= ((address >> 8) & 0xFF) as u8;
    control_word[2] |= ((address) & 0xFF) as u8;

    control_word
}

pub struct Mct8316AVDriver<I2C>
where
    I2C: I2c + Send + Sync,
    I2C::Error: Send,
    I2C::Error: Sync,
    I2C::Error: 'static,
{
    i2c: Mutex<I2C>,
    address: u8,
}

impl<I2C> Debug for Mct8316AVDriver<I2C>
where
    I2C: I2c + Send + Sync,
    I2C::Error: Send,
    I2C::Error: Sync,
    I2C::Error: 'static,
{
    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
        write!(
            f,
            "Mct8316AVDriver<I2C={}> {{ address: {} }}",
            type_name::<I2C>(),
            self.address
        )
    }
}

impl<I2C> Mct8316AVDriver<I2C>
where
    I2C: I2c + Send + Sync,
    I2C::Error: Send,
    I2C::Error: Sync,
    I2C::Error: 'static,
{
    pub fn new(i2c: I2C, address: u8) -> Self {
        trace!(
            "Mct8316AVDriver<I2C={}>::new(i2c, address: 0x{address:02x})",
            type_name::<I2C>()
        );
        Self {
            i2c: i2c.into(),
            address,
        }
    }

    #[allow(clippy::identity_op)]
    pub(super) fn write(&self, address: u32, data: Mct8316AVData) -> Result<()> {
        trace!("Mct8316AVDriver::write(self: {self:?}, address: {address:06x}, data: {data})");

        // 1 for target byte (for crc check)
        // 3 for control word
        // 8 for data
        // 1 for crc
        let mut write_data = [0u8; 1 + 3 + 8 + 1];
        write_data[0] = (self.address << 1) | 0b0;
        write_data[1..4].copy_from_slice(&control_word(OperationRW::Write, true, data, address));
        let data_length = match data {
            Mct8316AVData::Two(val) => {
                write_data[4..6].copy_from_slice(&val.to_le_bytes());
                2
            }
            Mct8316AVData::Four(val) => {
                write_data[4..8].copy_from_slice(&val.to_le_bytes());
                4
            }
            Mct8316AVData::Eight(val) => {
                let first_half = ((val >> 32) & u32::MAX as u64) as u32;
                let second_half = (val & u32::MAX as u64) as u32;
                write_data[4..8].copy_from_slice(&first_half.to_le_bytes());
                write_data[8..12].copy_from_slice(&second_half.to_le_bytes());
                8
            }
        };
        let crc_result = CRC.checksum(&write_data[0..(4 + data_length)]);
        write_data[4 + data_length] = crc_result;

        match self.i2c.lock() {
            Ok(mut lock) => lock
                .write(self.address, &write_data[1..=(4 + data_length)])
                .map_err(WrappingError)?,
            Err(_) => bail!("Lock was poisoned"),
        }

        Ok(())
    }

    #[allow(clippy::identity_op)]
    pub(super) fn read(&self, address: u32, data: &mut Mct8316AVData) -> Result<()> {
        trace!("Mct8316AVDriver::read(self: {self:?}, address: {address:06x}, data: {data})");
        // 1 target id
        // 3 control word
        // 1 target id
        // 8 data bytes
        // 1 crc
        let mut read_data = [0u8; 1 + 3 + 1 + 8 + 1];
        // Least significant bit should be 0
        read_data[0] = (self.address << 1) | 0b0;
        read_data[1..4].copy_from_slice(&control_word(OperationRW::Read, true, *data, address));
        read_data[4] = (self.address << 1) | 0b1;
        let data_length = match data {
            Mct8316AVData::Two(_) => 2,
            Mct8316AVData::Four(_) => 4,
            Mct8316AVData::Eight(_) => 8,
        };

        let (left, right) = read_data.split_at_mut(5);

        let mut i2c_ops = [
            Operation::Write(&left[1..4]),
            Operation::Read(&mut right[..=data_length]),
        ];

        match self.i2c.lock() {
            Ok(mut lock) => lock
                .transaction(self.address, &mut i2c_ops)
                .map_err(WrappingError)?,
            Err(_) => bail!("Lock was poisoned"),
        }

        let expected_crc = CRC.checksum(&read_data[0..(5 + data_length)]);

        ensure!(
            expected_crc == read_data[5 + data_length],
            "CRC Mismatch. {expected_crc} expected. {}",
            read_data[5 + data_length]
        );

        match data {
            Mct8316AVData::Two(val) => {
                *val = u16::from_le_bytes([read_data[5], read_data[6]]);
            }
            Mct8316AVData::Four(val) => {
                *val = u32::from_le_bytes([read_data[5], read_data[6], read_data[7], read_data[8]]);
            }
            Mct8316AVData::Eight(val) => {
                *val = u64::from_le_bytes([
                    read_data[9],
                    read_data[10],
                    read_data[11],
                    read_data[12],
                    read_data[5],
                    read_data[6],
                    read_data[7],
                    read_data[8],
                ]);
            }
        }

        Ok(())
    }
}

impl<I2C> Mct8316a for Mct8316AVDriver<I2C>
where
    I2C: I2c + Send + Sync,
    I2C::Error: Send,
    I2C::Error: Sync,
    I2C::Error: 'static,
{
    #[allow(clippy::too_many_lines)]
    fn init(&mut self) -> Result<()> {
        trace!("Mct8316AVDriver::init(self: {self:?})");

        // Settings taken from Table 3-3
        // https://www.ti.com/lit/ug/sllu336a/sllu336a.pdf?ts=1759001083565
        Mct8316AVEeprom::load(self)?
            .set_isd_config(IsdConfig {
                enable_isd: true,
                enable_brake: true,
                enable_high_impedance: false,
                enable_reverse_drive: true,
                enable_resynchronization: true,
                enable_stationary_brake: true,
                stationary_detect_threshold: Threshold::MilliVolt20,
                brake_mode: BrakeMode::LowOn,
                brake_config: BrakeConfig::BrakeTimeExit,
                brake_current_threshold: Threshold::MilliVolt15,
                brake_time: IsdConfigTimeValue::Millisecond500,
                high_impedence_time: IsdConfigTimeValue::Millisecond10,
                startup_break_time: StartupBreakTime::Millisecond100,
                resync_minimum_threshold: ResyncMinimumThreshold::Computed,
            })?
            .set_motor_startup1(MotorStartup1 {
                motor_startup_method: MotorStartupMethod::DoubleAlign,
                align_ramp_rate: AlignRampRate::VoltsPerSecond10,
                align_time: AlignTime::Millisecond100,
                align_current_threshold: FullCurrentThreshold::Volts0_3,
                ipd_clock_frequency: IpdClockFrequency::Hertz50,
                ipd_current_threshold: IpdCurrentThreshold::Volts0_4,
                ipd_release_mode: IpdReleaseMode::Brake,
                ipd_advance_angle: IpdAdvanceAngle::Degrees90,
                ipd_repeat: IpdRepeat::Average3,
                slow_first_cycle_frequency: SlowFirstCycleFrequency::Hertz1,
            })?
            .set_motor_startup2(MotorStartup2 {
                open_loop_current_limit_mode: OpenLoopCurrentLimitMode::OpenLoopCurrentLimit,
                open_loop_duty_cycle: DutyCycle::Percent20,
                open_loop_current_limit: FullCurrentThreshold::Volts0_4,
                open_loop_acceleration1: OpenLoopAcceleration1::HertzPerSecond2_5,
                open_loop_acceleration2: OpenLoopAcceleration2::HertzPerSecondSecond2_5,
                open_closed_handoff_threshold: OpenClosedHandoffThreshold::Hertz70,
                auto_handoff_enable: true,
                first_cycle_frequency_select: FirstCycleFrequencySelect::SlowFirstCycle,
                minimum_duty_cycle: MinimumDutyCycle::Percent8,
            })?
            .set_closed_loop1(ClosedLoop1 {
                commutation_mode: CommutationMode::Degrees120,
                closed_loop_acceleration_rate: ClosedLoopRate::VoltsPerSecond12_5,
                closed_loop_deceleration_mode: ClosedLoopDecelerationMode::DecelerationRate,
                closed_loop_deceleration_rate: ClosedLoopRate::VoltsPerSecond12_5,
                pwm_frequency: PwmFrequency::Kilohertz25,
                pwm_modulation: PwmModulation::Mixed,
                pwm_mode: PwmMode::SingleEnded,
                lead_angle_polarity: LeadAnglePolarity::Positive,
                lead_angle: 0.0,
            })?
            .set_closed_loop2(ClosedLoop2 {
                speed_feedback_mode: SpeedFeedbackMode::Always,
                speed_feedback_division: SpeedFeedbackDivision::Pole2,
                speed_feedback_config: SpeedFeedbackConfig::AboveBEMFThreshold,
                bemf_threshold: BemfThreshold::MilliVolt5,
                motor_stop_mode: MotorStopMode::HighImpedance,
                motor_stop_brake_time: MotorStopBrakeTime::Milliseconds100,
                active_low_high_brake_threshold: DutyCycleThreshold::Percent10,
                brake_pin_threshold: DutyCycleThreshold::Percent10,
                avs_enable: true,
                cycle_current_limit: FullCurrentThreshold::Volts0_6,
            })?
            .set_closed_loop3(ClosedLoop3 {
                degauss_samples: DegaussSamples::Samples4,
                degauss_upper_bound: DegaussUpperBound::Volts0_12,
                degauss_lower_bound: DegaussLowerBound::Volts0_09,
                integration_cycle_low_threshold: IntegrationCycleLowThreshold::Samples4,
                integration_cycle_high_threshold: IntegrationCycleHighThreshold::Samples8,
                integration_duty_cycle_low_threshold: IntegrationDutyCycleThreshold::Percent15,
                integration_duty_cycle_high_threshold: IntegrationDutyCycleThreshold::Percent18,
                bemf_threshold2: IntegrationBemfThreshold::Value0,
                bemf_threshold1: IntegrationBemfThreshold::Value775,
                commutation_method: CommutationMethod::ZC,
                degauss_window: DegaussWindow::Degrees22_5,
                degauss_enable: false,
            })?
            .set_closed_loop4(ClosedLoop4 {
                wcomp_blanking_enable: true,
                fast_deceleration_duty_window: LowerPercentLimit::Percent10,
                fast_deceleration_duty_threshold: UpperPercentLimit::Percent65,
                dynamic_brake_current_lower_threshold: FullCurrentThreshold::Volts0_4,
                dynamic_braking_current_enable: true,
                fast_deceleration_enable: false,
                fast_deceleration_current_threshold: FullCurrentThreshold::Volts0_8,
                fast_brake_delta: FastBrakeDelta::Percent2_5,
            })?
            .set_constant_speed(ConstantSpeed {
                speed_power_kp: 0.001f64,
                speed_power_ki: 0.000_005_f64,
                speed_power_upper_limit: UpperPercentLimit::Percent100,
                speed_power_lower_limit: LowerPercentLimit::Percent10,
                mode: ClosedLoopMode::Disabled,
            })?
            .set_constant_power(ConstantPower {
                max_speed: 3000f64,
                dead_time_compensation_enable: true,
                max_power: 50f64,
                power_hysteresis: PowerHysteresis::Percent7_5,
                mode: PowerMode::Disabled,
            })?
            .set_two_phase_profile(TwoPhase150DegreeProfile {
                steps: [
                    ProfileSetting::Percent97_5,
                    ProfileSetting::Percent93_75,
                    ProfileSetting::Percent83_75,
                    ProfileSetting::Percent83_75,
                    ProfileSetting::Percent83_75,
                    ProfileSetting::Percent75,
                    ProfileSetting::Percent50,
                    ProfileSetting::Percent50,
                ],
            })?
            .set_three_phase_profile(ThreePhase150DegreeProfile {
                steps: [
                    ProfileSetting::Percent83_75,
                    ProfileSetting::Percent87_5,
                    ProfileSetting::Percent87_5,
                    ProfileSetting::Percent93_75,
                    ProfileSetting::Percent93_75,
                    ProfileSetting::Percent99,
                    ProfileSetting::Percent99,
                    ProfileSetting::Percent99,
                ],
                lead_angle: ThreePhaseLeadAngle::Degrees0,
            })?
            .set_trap_config1(TrapConfig1 {
                open_loop_handoff_cycles: OpenLoopHandoffCycles::Cycles6,
                avs_negative_current_limit: AVSNegativeCurrentLimit::Limit0,
                avs_limit_hysteresis: AVSLimitHysteresis::Limit10,
                isd_bemf_threshold: IsdBemfThreshold::Limit1600,
                isd_cycle_threshold: IsdCycleThreshold::Limit11,
                open_loop_zc_detection_threshold: OpenLoopZcDetectionThreshold::Degrees8,
                fast_startup_div_factor: FastStartupDivFactor::Value4,
            })?
            .set_trap_config2(TrapConfig2 {
                blanking_time_microseconds: 0b0111,
                comparator_deglitch_time_microseconds: 0b010,
                align_duty_cycle: DutyCycle::Percent15,
            })?
            .set_fault_config1(FaultConfig1 {
                no_motor_detect_deglitch_time: NoMotorDetectDeglitchTime::Milliseconds100,
                cycle_by_cycle_current_limit:
                    CycleByCycleCurrentLimit::RecoverNextPwmFaultInactiveRecirculation,
                lock_detection_current_limit: LockDetectionCurrentLimit::Volts1_4,
                lock_detection_current_limit_mode: LockMode::ReportOnly,
                lock_detection_current_limit_deglitch_time:
                    LockDetectionCurrentLimitDeglitchTime::Milliseconds75,
                cycle_by_cycle_pwm_limit: 0,
                motor_lock_mode: LockMode::RecoverRetryTristated,
                lock_retry_time: LockRetryTime::Milliseconds5000,
            })?
            .set_fault_config2(FaultConfig2 {
                lock_abnormal_speed_enable: true,
                lock_loss_of_sync_enable: true,
                lock_no_motor_enable: true,
                abnormal_speed_lock_threshold: AbnormalSpeedLockThreshold::Hertz4000,
                loss_sync_times: LossSyncTimes::Count6,
                no_motor_threshold: NoMotorThreshold::Volts0_0075,
                max_motor_voltage_mode: MotorVoltageMode::AutomaticClear,
                max_motor_voltage: MaxMotorVoltage::Volts50,
                min_motor_voltage_mode: MotorVoltageMode::AutomaticClear,
                min_motor_voltage: MinMotorVoltage::NoLimit,
                automatic_retries: AutomaticRetries::NoLimit,
                lock_min_speed: LockMinSpeed::Hertz0_5,
                abnormal_speed_lock: AbnormalSpeedLock::Ratio6,
                zero_duty_threshold: ZeroDutyThreshold::Percent1_5,
            })?
            .set_pin_config1(PinConfig1 {
                dacout1_address: 0x5ae,
                dacout2_address: 0x40c,
                brake_input_config: BrakeInputConfig::HardwarePinBrake,
                direction_input_config: DirectionInputConfig::HardwarePinDirection,
                // Modification from default
                speed_input_config: SpeedInputConfig::Pwm,
            })?
            .set_pin_config2(PinConfig2 {
                pin36config: Pin36Config::Dacout2,
                pin37_38config: Pin37_38Config::Pin37Dacout1Pin38Dacout2,
                sleep_time: SleepTime::Sleep50micros,
                enable_external_watchdog: false,
                external_watchdog_source: ExternalWatchdogSource::I2c,
                external_watchdog_fault_mode: ExternalWatchdogFaultMode::ReportOnly,
                external_watchdog_frequency: ExternalWatchdogFrequency::Hertz10,
            })?
            .set_device_config(DeviceConfig {
                max_frequency: 0x7FFF,
                stl_enable: false,
                ssm_enable: false,
                device_mode: DeviceMode::Standby,
                pwm_range_select: PwmRangeSelect::Hertz325To95kHz,
                clock_source: ClockSource::InternalOscillator,
                external_clock_enable: false,
                external_clock_frequency: ExternalClockFrequency::KHertz8,
            })?
            .set_gate_driver_config1(GateDriverConfig1 {
                slew_rate: SlewRate::VoltsPerMicro200,
                overvoltage_protection_level: OvervoltageProtectionLevel::Volts32,
                overvoltage_protection_enable: true,
                overtemperature_warning_enable: false,
                overcurrent_protection_deglitch_time:
                    OvercurrentProtectionDeglitchTime::Microsecond0_2,
                overcurrent_protection_retry_time: OvercurrentProtectionRetryTime::Millisecond5,
                overcurrent_protection_level: OvercurrentProtectionLevel::Amps16,
                overcurrent_fault_mode: OvercurrentFaultMode::Latch,
                low_demag_comparator_threshold: DemagComparatorThreshold::Milliamps100,
                high_demag_comparator_threshold: DemagComparatorThreshold::Milliamps100,
                synchronous_rectification_enable: false,
                asynchronous_rectification_enable: false,
                current_sense_amplifier: CurrentSenseAmplifier::VoltsPerAmp0_15,
            })?
            .set_gate_driver_config2(GateDriverConfig2 {
                driver_delay_compensation_enable: false,
                target_delay: TargetDelay::Automatic,
                buck_slew_rate: BuckSlewRate::VoltsPerMicrosecond1000,
                buck_power_sequencing_disable: false,
                buck_current_limit: BuckCurrentLimit::Milliamps600,
                buck_voltage_selection: BuckVoltageSelection::Volts3_3,
                buck_disable: false,
            })?
            .commit()?;

        Ok(())
    }
}
