use embassy_stm32::adc::Adc;
use embassy_stm32::gpio::{Level, Output, Speed};
use embassy_stm32::peripherals;
use embassy_time::{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};

const AZ_MIN_READING: f32 = 0.0;
const AZ_MAX_READING: f32 = 4096.0;
const AZ_RANGE: f32 = 360.0;

const EL_MIN_READING: f32 = 0.0;
const EL_MAX_READING: f32 = 4096.0;
const EL_RANGE: f32 = 90.0;

#[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>,
) {
    let mut rotor_state = RotorState {
        actual_pos: AzElPair { az: 0, el: 0 },
        setpoint_pos: AzElPair { az: 0, el: 0 },
        stopped: true,
    };

    let mut adc = Adc::new(adc1, &mut Delay);

    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);

    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 {
        match select(
            cmd_receiver.recv(),
            Timer::after(Duration::from_millis(100)),
        )
        .await
        {
            Either::First(cmd) => match cmd {
                Gs232Cmd::MoveTo(pair) => {
                    rotor_state.setpoint_pos = pair;
                    rotor_state.stopped = false;
                }
                Gs232Cmd::Stop => {
                    rotor_state.stopped = true;
                }
                _ => {}
            },
            Either::Second(_) => {
                let az_reading = adc.read(&mut az_pin) as f32;
                let el_reading = adc.read(&mut el_pin) as f32;

                az_average.add(az_reading);
                el_average.add(el_reading);

                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;

                rotor_state.actual_pos.az = az_actual as u16;
                rotor_state.actual_pos.el = el_actual as u16;

                if !rotor_state.stopped && rotor_state.actual_pos.az < rotor_state.setpoint_pos.az {
                    cw_pin.set_high();
                    ccw_pin.set_low();
                } else if !rotor_state.stopped
                    && rotor_state.actual_pos.az > rotor_state.setpoint_pos.az
                {
                    cw_pin.set_low();
                    ccw_pin.set_high();
                } else {
                    cw_pin.set_low();
                    ccw_pin.set_low();
                }

                if !rotor_state.stopped && rotor_state.actual_pos.el < rotor_state.setpoint_pos.el {
                    up_pin.set_high();
                    down_pin.set_low();
                } else if !rotor_state.stopped
                    && rotor_state.actual_pos.el > rotor_state.setpoint_pos.el
                {
                    up_pin.set_low();
                    down_pin.set_high();
                } else {
                    up_pin.set_low();
                    down_pin.set_low();
                }

                join(
                    pos_sender.send(rotor_state.actual_pos),
                    state_sender.send(rotor_state),
                )
                .await;
            }
        };
    }
}

struct Average {
    pos: usize,
    data: Vec<f32, 10>,
}

impl Average {
    fn new(initial: f32) -> Average {
        let mut data: Vec<f32, 10> = Vec::new();
        data.resize(10, initial).unwrap();
        Average { pos: 0, data }
    }

    fn add(&mut self, sample: f32) {
        self.data[self.pos] = sample;
        self.pos = (self.pos + 1) % self.data.len();
    }

    fn average(&self) -> f32 {
        let mut sum = 0.0;
        for sample in &self.data {
            sum += sample;
        }

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