// use embedded_hal_async::delay::DelayNs;
use esp_hal::{
    delay::{self, Delay},
    gpio::Level,
    rmt::{Channel, PulseCode, SingleShotTxTransaction},
};
use core::{cell::{self, Cell, Ref, RefCell}, ops::Index};
use num_traits::float::FloatCore;
use rtt_target::{rprint, rprintln};

fn calculate_crc(frame: u16) -> u16 {
    (0xffff ^ (frame ^ (frame >> 4) ^ (frame >> 8))) & 0xF
}

/*https://de.aliexpress.com/item/1005009484033753.html?spm=a2g0o.productlist.main.2.6389651cV8lVw8&algo_pvid=9cfe024b-8aff-49ea-981b-824f392de976&algo_exp_id=9cfe024b-8aff-49ea-981b-824f392de976-1&pdp_ext_f=%7B%22order%22%3A%2232%22%2C%22eval%22%3A%221%22%2C%22fromPage%22%3A%22search%22%7D&pdp_npi=6%40dis%21EUR%2111.99%2111.99%21%21%2113.55%2113.55%21%40211b813b17620408595223963e91a7%2112000049243823801%21sea%21DE%212739924950%21X%211%210%21n_tag%3A-29919%3Bd%3Abce7cd6%3Bm03_new_user%3A-29895&curPageLogUid=GNUFzsTU8oDr&utparam-url=scene%3Asearch%7Cquery_from%3A%7Cx_object_id%3A1005009484033753%7C_p_origin_prod%3A
IFlight XING-E Pro 2207 2750KV 
*/
const XING_EPRO_22_POLES: u32 = 4; 

#[allow(dead_code)]
#[allow(non_camel_case_types)]
#[derive(Debug)]
pub enum DSHOT_TELEMETRY_CMD {
    MOTOR_STOP = 0x0,
    BEEP1 = 0x01,
    BEEP2 = 0x02,
    BEEP3 = 0x03,
    BEEP4 = 0x04,
    BEEP5 = 0x05,
    ESC_INFO = 0x06,
    SPIN_DIRECTION_1 = 0x07,
    SPIN_DIRECTION_2 = 0x08,
    MODE_3D_OFF = 0x09,
    MODE_3D_ON = 0x0A,
    SETTINGS_REQUEST = 0x0B,
    SAVE_SETTINGS = 0x0C,
    EXTENDED_TELEMETRY_ENABLE = 0x0D,
    EXTENDED_TELEMETRY_DISABLE = 0x0E,
}

#[derive(Debug, Clone, Copy)]
pub struct BitTicks {
    t0_h: u16,
    t0_l: u16,
    t1_h: u16,
    t1_l: u16,
}

impl BitTicks {
    pub fn new(t1_h: u16, t0_h: u16) -> Self {
        Self {
            t0_h,
            t1_h,
            t0_l: t1_h,
            t1_l: t0_h,
        }
    }

    pub fn from_clk(
        clk_speed: u32,
        clk_divider: u8,
        speed: DShotSpeed,
    ) -> Self {
        let tick_len = (1. / clk_speed as f32) * (clk_divider as f32) * 1_000_000.;
        let bit_period_us = speed.bit_period_ns() as f32 / 1000.0;
        let bit_ticks = (bit_period_us / tick_len).round() as u16;
        let t1_h = (speed.bit_times().t1_h / tick_len).round() as u16;
        let t0_h = (speed.bit_times().t0_h / tick_len).round() as u16;
        let mut bittick = Self::new(t1_h, t0_h);
        bittick.t0_l = bit_ticks - t0_h;
        bittick.t1_l = bit_ticks - t1_h;
        bittick
    }
}

/// High and Low Times in µs
#[derive(Debug, Clone, Copy)]
pub struct BitTimes {
    t0_h: f32,
    t1_h: f32,
}

impl BitTimes {
    pub fn new(t1_h: f32, t0_h: f32) -> Self {
        Self { t0_h, t1_h }
    }
}

#[derive(Debug, Clone, Copy)]
#[allow(dead_code)]
pub enum DShotSpeed {
    DShot150,
    DShot300,
    DShot600,
    DShot1200,
}

impl DShotSpeed {
    pub fn bit_period_ns(&self) -> u32 {
        match self {
            // 6.67µs per bit
            Self::DShot150 => 6667,
            // 3.33µs per bit
            Self::DShot300 => 3333,
            // 1.67µs per bit
            Self::DShot600 => 1667,
            // 0.83µs per bit
            Self::DShot1200 => 833,
        }
    }

    /// High and Low Times in µs
    pub fn bit_times(&self) -> BitTimes {
        match &self {
            Self::DShot150 => BitTimes::new(5.00, 2.50),
            Self::DShot300 => BitTimes::new(2.50, 1.25),
            Self::DShot600 => BitTimes::new(1.25, 0.625),
            Self::DShot1200 => BitTimes::new(0.625, 0.313),
        }
    }
}

fn decode_bidi_telemetry(pulses: &[PulseCode]) -> Option<u16> {
    // Convert to simple list of edges.
    let mut edges = [(false, 0u32); 64];
    let mut nedges = 0usize;
    let mut time = 0u32;
    let mut plength = 0u32;
    for (i, p) in pulses.iter().enumerate() {
        if p.length1() == 0 {
            break;
        } else {
            nedges += 1;
            edges[i*2] = (p.level1() == Level::High, time + plength);
            time += plength;
            plength = p.length1() as u32;
        }
        if p.length2() == 0 {
            break;
        } else {
            nedges += 1;
            edges[i*2+1] = (p.level2() == Level::High, time + plength);
            time += plength;
            plength = p.length2() as u32;
        }
    }
    edges[nedges] = (!edges[nedges-1].0, time + plength);
    nedges += 1;

    // Process edges.
    // TODO: strip until we find the first negative edge.
    // assert_eq!(edges[0].0, false);
    let mut idx = 0;
    for i in 00..edges.len() {
        if edges[i].0 == false{

            idx = i;
            break
        }
    }
    // Approximate. Should be calculated instead, in practice these pulses are
    // around 1.34us.
    let period = 104u32;

    // Grad middle of each period, find latest applying period. Shitty
    // algorithm.
    let mut res = [false; 22];
    for i in idx..res.len() {
        let pos = (i as u32)*period + period/2;
        let mut level = edges[0].0;
        for edge in edges[..nedges].iter() {
            if edge.1 > pos {
                break;
            }
            level = edge.0;
        }
        res[i] = level;
    }

    let mut gcr_diff = 0u32;
    for i in 0..21 {
        gcr_diff |= (res[20-i] as u32) << i;
    }

    let gcr = (gcr_diff>>1) ^ gcr_diff;

    // Shitty LUT decode for GCR;
    let lut: [u8; 16] = [
        0x19, 0x1b, 0x12, 0x13, 0x1d, 0x15, 0x16, 0x17, 0x1a, 0x09, 0x0a, 0x0b,
        0x1e, 0x0d, 0x0e, 0x0f,
    ];

    let mut res = 0u16;
    for i in 0..4 {
        let fibble = ((gcr >> (i*5)) & 0b11111) as u8;
        if let Some(nibble) = lut.iter().position(|&v| v == fibble) {
            res |= (nibble as u16) << (i*4);
        } else {
            return None;
        }
    }
    Some(res)
}

#[allow(dead_code)]
pub struct DShot<'a> {
    rx_channel: RefCell<&'a mut Channel<'static, esp_hal::Blocking, esp_hal::rmt::Rx>>,
    speed: DShotSpeed,
    bit_ticks: BitTicks,
    rpm: Cell<Option<u32>>,
    pulses: RefCell<[u32; 17]>,
    requested_throttle: RefCell<u16>,
    current_tx: RefCell<Option<SingleShotTxTransaction<'a, 'a, u32>>>,
    current_tx_done_at: RefCell<Option<esp_hal::time::Instant>>,
}

impl<'a> DShot<'a> {
    pub fn new(
        rx_channel: &'a mut Channel<'static, esp_hal::Blocking, esp_hal::rmt::Rx>,
        tx_channel: &'a mut Channel<'static, esp_hal::Blocking, esp_hal::rmt::Tx>,

        speed: DShotSpeed,
        clk_speed: Option<u32>,
        clk_divider: Option<u8>,
    ) -> Self {
        let clk_speed = clk_speed.unwrap_or(80_000_000);
        let clk_divider = clk_divider.unwrap_or(1);
        let bit_ticks = BitTicks::from_clk(clk_speed, clk_divider, speed);
        let cell = Cell::new(None);
        let txc = tx_channel.reborrow().transmit(&[0u32]).unwrap();
        Self {
            rx_channel: RefCell::new(rx_channel),
            speed,
            bit_ticks,
            rpm: cell,
            pulses: RefCell::new([0u32; 17]),
            requested_throttle: RefCell::new(0),
            current_tx: RefCell::new(Some(txc)),
            current_tx_done_at: RefCell::new(None),
        }
    }
    fn set_rpm(&self,new_rpm:u32){
        self.rpm.set(Some(new_rpm));
    }
    pub fn get_rpm(&self) -> Option<u32> {
        self.rpm.get()
    }
    pub fn create_frame(value: u16, telemetry: bool) -> u16 {
        // Mask to 11 bits (0-2047 range) and set telemetry bit
        let frame = ((value & 0x07FF) << 1) | telemetry as u16;

        let crc = calculate_crc(frame);

        (frame << 4) | crc
    }

    #[allow(clippy::needless_range_loop)]
    pub fn create_pulses(&self, throttle_value: u16, telemetry: bool) -> [u32; 17] {
        let frame = Self::create_frame(throttle_value, telemetry);
        let mut pulses = [0; 17];
        for i in 0..16 {
            let bit = (frame >> (15 - i)) & 1;

            pulses[i] = if bit == 1 {
                PulseCode::new(
                    Level::Low,
                    self.bit_ticks.t1_h,
                    Level::High,
                    self.bit_ticks.t1_l,
                )
                .into()
            } else {
                PulseCode::new(
                    Level::Low,
                    self.bit_ticks.t0_h,
                    Level::High,
                    self.bit_ticks.t0_l,
                )
                .into()
            };
        }

        pulses[16] = 0;
        pulses
    }

    pub fn process(&'a self) {
        let mut rc = self.current_tx.borrow_mut();
        if let Some(current) = rc.as_mut() {
            if current.poll() {
                // We're done - but we might still be waiting for a response over bidi.
                let mut ended = self.current_tx_done_at.borrow_mut();
                if ended.is_none() {
                    let start = esp_hal::time::Instant::now();
                    *ended = Some(start);
                    
                    // Try to receive a response for up to 100us.
                    let mut rx = self.rx_channel.borrow_mut();
                    let rx = rx.reborrow();
                    let mut buf = [PulseCode::default(); 32];
                    {
                        let mut rxt = rx.receive(&mut buf).unwrap();
                        while start.elapsed().as_micros() < 150 { 
                            // Got something within this time.
                            if rxt.poll() {
                                // Receive it fully.
                                let (len, _) = rxt.wait().unwrap();

                                // Parse out telemetry data.
                                if len != 0 {
                                    // Decode to a 16 bit value.
                                    if let Some(frame) = decode_bidi_telemetry(&buf[..len]) {
                                        // Check CRC.
                                        let mut data = frame >> 4;
                                        let crc = calculate_crc(data);
                                        if data != 0xfff && crc == (frame&0b1111) {
                                            // Fucking got it.

                                            let exp = data >> 9;

                                            let mantissa = data & 0b111111111;
                                            let period_ms: u32 = (mantissa << exp).into();
                                            let erpm = (60 * 1_000_000) / period_ms;

                                            let rpm = (((erpm))/((XING_EPRO_22_POLES)));
                                            self.rpm.set(Some(rpm))
                                        }
                                    }
                                }
                                break;
                            }
                        }
                    }
                }

                let ended_at = *ended.as_ref().unwrap();

                if ended_at.elapsed().as_millis() > 1 {
                    // Schedule new transaction
                    let tx_chann = rc.take().unwrap().wait().unwrap();
                    let mut pulses = self.pulses.borrow_mut();
                    let throttle = self.requested_throttle.borrow();
                    *pulses = self.create_pulses(*throttle, true);
                    let pulses = pulses.as_ref();
                    // SAFETY: eat shit, rust
                    let pulses = unsafe {
                        core::mem::transmute(pulses)
                    };
                    let m = tx_chann
                        .transmit(pulses)
                        .unwrap();

                    *rc = Some(m);
                    *ended = None;
                } else{
                    // Wait for bidi dshot data.
                }
            }
        } else {
            // Nothing to do, twiddle our thumbs.
        }
    }

    pub fn set_throttle(&self, v: u16) {
        *self.requested_throttle.borrow_mut() = v;
    }

    #[allow(dead_code)]
    pub fn arm(&'a self) -> Result<(), &'static str> {
        let start = esp_hal::time::Instant::now();

        rprintln!("ARM: Sending pre-throttle...");
        while start.elapsed().as_secs() < 2 {
            self.set_throttle(100);
            self.process();
        }
        rprintln!("ARM: Sending 0...");
        while start.elapsed().as_secs() < 4 {
            self.set_throttle(0);
            self.process();
        }
        rprintln!("ARM: Done.");

        Ok(())
    }
}