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 = 10;
pub const PEDAL_A_MAX: u8 = 245;
pub const PEDAL_B_MIN: u8 = 10;
pub const PEDAL_B_MAX: u8 = 245;

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 cs = cs.into_push_pull_output(critical_section);

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

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

            let adc = Self {
                spi,
                cs,
                pedal_a_filtered: 128 << 8,
                pedal_b_filtered: 128 << 8,
                last_midi_a: 64,
                last_midi_b: 64,
            };
            
            // Small delay to ensure SPI peripheral is ready
            for _ in 0..1000 {
                cortex_m::asm::nop();
            }
            
            adc
        })
    }

    pub fn read_channel(&mut self, channel: u8) -> Result<u8, &'static str> {
        if channel > 1 {
            return Err("Invalid channel");
        }

        self.cs.set_high().ok();
        // Small delay before starting transaction
        for _ in 0..100 {
            cortex_m::asm::nop();
        }
        
        self.cs.set_low().ok();
        // Small delay after CS low
        for _ in 0..100 {
            cortex_m::asm::nop();
        }

        let start_bit = 1u8 << 7;
        let channel_bit = if channel == 0 { 0x00 } else { 1u8 << 6 };
        let command = start_bit | channel_bit;

        info!("Reading channel {} with command 0x{:02x}", channel, command);
        
        // Use HAL SPI transfer - much simpler and more reliable
        let mut data = [command, 0x00, 0x00];  // TLC0832 needs 3 bytes
        let result = match self.spi.transfer(&mut data) {
            Ok(response) => {
                info!("SPI transfer successful: [{:02x}, {:02x}, {:02x}]", 
                      response[0], response[1], response[2]);
                // TLC0832 returns data in the 3rd byte for 8-bit mode
                response[2]
            },
            Err(_) => {
                self.cs.set_high().ok();
                error!("SPI transfer error");
                return Err("SPI transfer error");
            }
        };

        // Small delay before CS high
        for _ in 0..100 {
            cortex_m::asm::nop();
        }
        self.cs.set_high().ok();
        info!("Read channel {} = {}", channel, result);
        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>) {
        info!("Updating and getting MIDI changes");
        let mut midi_a_change = None;
        let mut midi_b_change = None;

        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 {
                    self.last_midi_a = midi_a;
                    midi_a_change = Some(midi_a);
                }
            }
            Err(e) => {
                error!("Error reading channel 0: {}", e);
            }
        }

        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 {
                    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)
    }
}