use stm32f0xx_hal as hal;
use hal::prelude::*;
use hal::spi::{Spi, Mode, Phase, Polarity, EightBit};
use hal::gpio::{Output, PushPull, Alternate, AF0, AF5};
use embedded_hal::blocking::spi::Transfer;
use defmt::*;

pub const PEDAL_A_MIN: u8 = 71;
pub const PEDAL_A_MAX: u8 = 248;
pub const PEDAL_B_MIN: u8 = 83;
pub const PEDAL_B_MAX: u8 = 250;

const IIR_ALPHA: u16 = 51;

pub struct Tlc0832 {
    spi: Spi<hal::stm32::SPI2, 
        hal::gpio::gpiob::PB10<Alternate<AF5>>,
        hal::gpio::gpiob::PB14<Alternate<AF0>>,
        hal::gpio::gpiob::PB15<Alternate<AF0>>,
        EightBit>,
    cs: hal::gpio::gpiob::PB12<Output<PushPull>>,
    pedal_a_filtered: u16,
    pedal_b_filtered: u16,
    last_midi_a: u8,
    last_midi_b: u8,
}

impl Tlc0832 {
    pub fn new(
        spi2: hal::stm32::SPI2,
        sck: hal::gpio::gpiob::PB10<hal::gpio::Input<hal::gpio::Floating>>,
        miso: hal::gpio::gpiob::PB14<hal::gpio::Input<hal::gpio::Floating>>,
        mosi: hal::gpio::gpiob::PB15<hal::gpio::Input<hal::gpio::Floating>>,
        cs: hal::gpio::gpiob::PB12<hal::gpio::Input<hal::gpio::Floating>>,
        rcc: &mut hal::rcc::Rcc,
    ) -> Self {
        cortex_m::interrupt::free(|critical_section| {
            let sck = sck.into_alternate_af5(critical_section);
            let miso = miso.into_alternate_af0(critical_section);
            let mosi = mosi.into_alternate_af0(critical_section);
            let mut cs = cs.into_push_pull_output(critical_section);
            cs.set_high().ok();

            let spi_mode = Mode {
                polarity: Polarity::IdleLow,
                phase: Phase::CaptureOnFirstTransition,
            };

            let spi = Spi::spi2(
                spi2,
                (sck, miso, mosi),
                spi_mode,
                100.khz(),
                rcc,
            );

            Self {
                spi,
                cs,
                pedal_a_filtered: 128 << 8,
                pedal_b_filtered: 128 << 8,
                last_midi_a: 64,
                last_midi_b: 64,
            }
        })
    }

    pub fn read_channel(&mut self, channel: u8) -> Result<u8, &'static str> {
        if channel > 1 {
            return Err("Invalid channel");
        }
        
        self.cs.set_low().ok();
        
        // Small delay after CS low
        for _ in 0..100 {
            cortex_m::asm::nop();
        }

        // TLC0832 protocol:
        // Send 3 bits: Start(1) + SGL/DIF(1 for single-ended) + ODD/EVEN(channel)
        // Then 1 clock cycle of MUX settling (DO goes low)
        // Then 8 clock cycles of MSB-first data
        
        // For single-ended mode:
        // Channel 0: 110 (Start=1, SGL/DIF=1, ODD/EVEN=0)
        // Channel 1: 111 (Start=1, SGL/DIF=1, ODD/EVEN=1)
        
        let command_bits = 0x06 | channel; // 110 or 111 in the top 3 bits
        let command_byte = command_bits << 5; // Shift to MSB position: 11000000 or 11100000
        
        // Send command byte + 2 dummy bytes to clock out the full response
        // The TLC0832 needs 11 total clock cycles:
        // 3 for command + 1 for MUX settling + 8 for data = 12 bits total
        let mut data = [command_byte, 0x00];
        
        let response = match self.spi.transfer(&mut data) {
            Ok(resp) => resp,
            Err(_) => {
                self.cs.set_high().ok();
                error!("SPI transfer error");
                return Err("SPI transfer error");
            }
        };

        // The result spans across the response bytes
        // After the 3 command bits + 1 MUX settling bit (4 bits total),
        // the next 8 bits are the conversion result
        // So we need to extract bits from both response bytes
        
        // response[0] contains the first 8 bits (3 command + 1 settling + 4 data MSBs)
        // response[1] contains the remaining 4 data LSBs
        
        let result = ((response[0] & 0x0F) << 4) | ((response[1] & 0xF0) >> 4);
        
        // Small delay before CS high
        for _ in 0..100 {
            cortex_m::asm::nop();
        }
        
        self.cs.set_high().ok();
        
        Ok(result)
    }

    fn apply_iir_filter(&mut self, channel: u8, new_value: u8) {
        let new_value_scaled = (new_value as u32) << 8;
        
        let filtered_value = match channel {
            0 => &mut self.pedal_a_filtered,
            1 => &mut self.pedal_b_filtered,
            _ => return,
        };

        let old_filtered_val = *filtered_value as u32;
        let alpha = IIR_ALPHA as u32;

        let new_filtered_val = (old_filtered_val * (256 - alpha) + new_value_scaled * alpha) / 256;
        
        *filtered_value = new_filtered_val as u16;
    }

    fn adc_to_midi(&self, adc_value: u8, min_adc: u8, max_adc: u8) -> u8 {
        if adc_value <= min_adc {
            return 0;
        }
        if adc_value >= max_adc {
            return 127;
        }
        
        let range = max_adc - min_adc;
        let scaled = ((adc_value - min_adc) as u16 * 127) / range as u16;
        scaled.min(127) as u8
    }

    pub fn update_and_get_midi_changes(&mut self) -> (Option<u8>, Option<u8>) {
        let mut midi_a_change = None;
        let mut midi_b_change = None;

        // Read channel 0 (pedal A)
        match self.read_channel(0) {
            Ok(raw_a) => {
                self.apply_iir_filter(0, raw_a);
                let filtered_a = (self.pedal_a_filtered >> 8) as u8;
                let midi_a = self.adc_to_midi(filtered_a, PEDAL_A_MIN, PEDAL_A_MAX);
                
                if midi_a != self.last_midi_a {
                    info!("Channel 0 MIDI change: {} -> {}", self.last_midi_a, midi_a);
                    self.last_midi_a = midi_a;
                    midi_a_change = Some(midi_a);
                }
            }
            Err(e) => {
                error!("Error reading channel 0: {}", e);
            }
        }

        // Read channel 1 (pedal B)
        match self.read_channel(1) {
            Ok(raw_b) => {
                self.apply_iir_filter(1, raw_b);
                let filtered_b = (self.pedal_b_filtered >> 8) as u8;
                let midi_b = self.adc_to_midi(filtered_b, PEDAL_B_MIN, PEDAL_B_MAX);
                
                if midi_b != self.last_midi_b {
                    info!("Channel 1 MIDI change: {} -> {}", self.last_midi_b, midi_b);
                    self.last_midi_b = midi_b;
                    midi_b_change = Some(midi_b);
                }
            }
            Err(e) => {
                error!("Error reading channel 1: {}", e);
            }
        }

        (midi_a_change, midi_b_change)
    }
}