use embassy_stm32::adc::Adc;
use embassy_stm32::gpio::{Level, Output, Speed};
use embassy_stm32::peripherals;
use embassy_time::{with_timeout, Delay, Duration, Timer};
use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
use embassy_util::channel::mpmc::{Receiver, Sender};
use embassy_util::{select, Either};
use futures_util::future::join;
use heapless::Vec;

use crate::usb::Gs232Cmd;
use crate::{AzElPair, RotorState};

// ADC reading for azimuth 0°
const AZ_MIN_READING: f32 = 0.0;
// ADC reading for azimuth 360°
const AZ_MAX_READING: f32 = 4096.0;
// Range of motion for azimuth (0 to AZ_RANGE)
const AZ_RANGE: f32 = 360.0;
// Tolerance for the azimuth setpoint
const AZ_SLOP : i16 = 1;

// ADC reading for elevation 0°
const EL_MIN_READING: f32 = 0.0;
// ADC reading for elevation 360°
const EL_MAX_READING: f32 = 4096.0;
// Range of motion for elevantion (0 to EL_RANGE)
const EL_RANGE: f32 = 180.0;
// Tolerance for the elevation setpoint
const EL_SLOP : i16 = 1;

#[embassy_executor::task]
pub async fn movement_task(
    adc1: peripherals::ADC1,
    mut az_pin: peripherals::PA0,
    mut el_pin: peripherals::PA1,
    cw_pin: peripherals::PA2,
    ccw_pin: peripherals::PA3,
    up_pin: peripherals::PA4,
    down_pin: peripherals::PA5,
    cmd_receiver: Receiver<'static, ThreadModeRawMutex, Gs232Cmd, 1>,
    pos_sender: Sender<'static, ThreadModeRawMutex, AzElPair, 1>,
    state_sender: Sender<'static, ThreadModeRawMutex, RotorState, 1>,
) {
    // Initialize the rotor state
    let mut rotor_state = RotorState {
        actual_pos: AzElPair { az: 0, el: 0 },
        setpoint_pos: AzElPair { az: 0, el: 0 },
        stopped: true,
    };

    // Setup output pins for moving the rotor
    let mut cw_pin = Output::new(cw_pin, Level::Low, Speed::Low);
    let mut ccw_pin = Output::new(ccw_pin, Level::Low, Speed::Low);
    let mut up_pin = Output::new(up_pin, Level::Low, Speed::Low);
    let mut down_pin = Output::new(down_pin, Level::Low, Speed::Low);

    // Setup the ADC for reading the rotor positions
    let mut adc = Adc::new(adc1, &mut Delay);

    // Do an initial ADC reading to initialize the averages
    let az_reading = adc.read(&mut az_pin) as f32;
    let el_reading = adc.read(&mut el_pin) as f32;
    let mut az_average = Average::new(az_reading);
    let mut el_average = Average::new(el_reading);

    loop {
        // Wait until either a new command has been received or 100ms have elapsed
        match select(
            cmd_receiver.recv(),
            Timer::after(Duration::from_millis(100)),
        )
        .await
        {
            // A new command has been received. This task only cares about MoveTo and Stop.
            Either::First(cmd) => match cmd {
                // Move to command. Update the setpoint pair in the rotor state
                Gs232Cmd::MoveTo(pair) => {
                    rotor_state.setpoint_pos = pair;
                    rotor_state.stopped = false;
                }
                // Stop command. Set the stopped flag.
                Gs232Cmd::Stop => {
                    rotor_state.stopped = true;
                }
                // Everthing elese is an noop.
                _ => {}
            },

            // Second case of the select statement. Timer has elapsed.
            Either::Second(_) => {
                // First read the current rotor position
                let az_reading = adc.read(&mut az_pin) as f32;
                let el_reading = adc.read(&mut el_pin) as f32;

                // Apply the averaging filters
                az_average.add(az_reading);
                el_average.add(el_reading);

                // Calculate the position in degreee
                let az_actual = (az_average.average() - AZ_MIN_READING)
                    / (AZ_MAX_READING - AZ_MIN_READING)
                    * AZ_RANGE;
                let el_actual = (el_average.average() - EL_MIN_READING)
                    / (EL_MAX_READING - EL_MIN_READING)
                    * EL_RANGE;

                // Update the rotor state
                rotor_state.actual_pos.az = az_actual as u16;
                rotor_state.actual_pos.el = el_actual as u16;

                let delta_az =
                    rotor_state.setpoint_pos.az as i16 - rotor_state.actual_pos.az as i16;
                let delta_el =
                    rotor_state.setpoint_pos.el as i16 - rotor_state.actual_pos.el as i16;

                if !rotor_state.stopped && delta_az > AZ_SLOP {
                    // Azimuth needs to move clockwise
                    cw_pin.set_high();
                    ccw_pin.set_low();
                } else if !rotor_state.stopped && delta_az < -AZ_SLOP {
                    // Azimuth needs to move counter clockwise
                    cw_pin.set_low();
                    ccw_pin.set_high();
                } else {
                    // Either azimuth is on the setpoint or the rotor has beend stopped.
                    cw_pin.set_low();
                    ccw_pin.set_low();
                }

                if !rotor_state.stopped && delta_el > EL_SLOP {
                    // Elevation needs to move up
                    up_pin.set_high();
                    down_pin.set_low();
                } else if !rotor_state.stopped && delta_el < -EL_SLOP {
                    // Elevation needs to move down
                    up_pin.set_low();
                    down_pin.set_high();
                } else {
                    // Either elevation is on the setpoint or the rotor has beend stopped.
                    up_pin.set_low();
                    down_pin.set_low();
                }

                // Send the state to the display task and the position usb.
                // Use timeouts to prevent blocking if display or usb task are unresponsive.
                join(
                    with_timeout(
                        Duration::from_millis(100),
                        pos_sender.send(rotor_state.actual_pos),
                    ),
                    with_timeout(Duration::from_millis(100), state_sender.send(rotor_state)),
                )
                .await;
            }
        };
    }
}

// Simple sliding average filter
struct Average {
    pos: usize,
    data: Vec<f32, 5>,
}

impl Average {
    // Create a new filter and prefill the state using an initial value
    fn new(initial: f32) -> Average {
        let mut data: Vec<f32, 5> = Vec::new();
        data.resize(5, initial).unwrap();
        Average { pos: 0, data }
    }

    // Adds a new value to internal state
    fn add(&mut self, sample: f32) {
        self.data[self.pos] = sample;
        self.pos = (self.pos + 1) % self.data.len();
    }

    // Calculate the average value from the internal state
    fn average(&self) -> f32 {
        let mut sum = 0.0;
        for sample in &self.data {
            sum += sample;
        }

        sum / self.data.len() as f32
    }
}