mod thruster_balancing;

use crate::commanded_state::CommandedState;
use crate::comms::command::InstantPriorityCommand;
use crate::hardware::channelization::{
    ChannelPin, PinoutChannel, RCS0, RCS1, RCS2, RCS3, RCS4, RCS5, RCS6, RCS7, RCS8, RCS9,
};
use crate::hardware::pin::{Pin, PinDevice};
use crate::rcs::thruster_balancing::BalancingConstants;
use crate::scheduler::{CyclicTask, TaskHandle};
use embedded_hal::digital::PinState;
use log::trace;
use nautilus_common::command::set_rcs::SetRcs;
use nautilus_common::command::valid_priority_command::ValidPriorityCommand;
use nautilus_common::math::Vector;
use std::fmt::Debug;
use std::iter::zip;
use std::sync::mpsc::Receiver;
use std::time::{Duration, Instant};

const PIN_PRIORITY: u8 = 0;

struct Thruster {
    channel: PinoutChannel,
    position: Vector,
    direction: Vector,
    force: f64,
}

impl Thruster {
    const fn new(channel: PinoutChannel, position: Vector, direction: Vector, force: f64) -> Self {
        Self {
            channel,
            position,
            direction,
            force,
        }
    }
}

const RCS_THRUSTERS: [Thruster; 10] = [
    Thruster::new(RCS0, Vector::ZERO, Vector::ZERO, 1.0),
    Thruster::new(RCS1, Vector::ZERO, Vector::ZERO, 1.0),
    Thruster::new(RCS2, Vector::ZERO, Vector::ZERO, 1.0),
    Thruster::new(RCS3, Vector::ZERO, Vector::ZERO, 1.0),
    Thruster::new(RCS4, Vector::ZERO, Vector::ZERO, 1.0),
    Thruster::new(RCS5, Vector::ZERO, Vector::ZERO, 1.0),
    Thruster::new(RCS6, Vector::ZERO, Vector::ZERO, 1.0),
    Thruster::new(RCS7, Vector::ZERO, Vector::ZERO, 1.0),
    Thruster::new(RCS8, Vector::ZERO, Vector::ZERO, 1.0),
    Thruster::new(RCS9, Vector::ZERO, Vector::ZERO, 1.0),
];

#[derive(Clone, Debug)]
pub enum RcsMessage {
    SetRcs(InstantPriorityCommand<SetRcs>),
}

impl<D: Debug> TaskHandle<RcsMessage, D> {
    pub fn set_rcs(&self, cmd: InstantPriorityCommand<SetRcs>) {
        trace!("TaskHandle<RcsMessage, D>::set_rcs(self: {self:?}, cmd: {cmd:?})");
        // This can only fail if the other end is disconnected - which we intentionally want to
        // ignore
        let _ = self.sender.send(RcsMessage::SetRcs(cmd));
    }

    pub fn new_set_rcs_callback<'a>(&self) -> impl Fn(ValidPriorityCommand<SetRcs>) + 'a
    where
        Self: Sized + Clone + 'a,
    {
        let this = self.clone();
        move |cmd| {
            this.set_rcs(cmd.into());
        }
    }
}

#[derive(Clone)]
pub struct RcsTask<P> {
    rcs_pins: [P; RCS_THRUSTERS.len()],
    command: CommandedState<(Vector, Vector)>,
}

impl<A, B> RcsTask<ChannelPin<A, B>>
where
    A: PinDevice + Debug + Clone,
    B: PinDevice + Debug + Clone,
{
    pub fn new(a: &A, b: &B) -> Self {
        let rcs_pins = RCS_THRUSTERS.map(|thruster| thruster.channel.get_pin(a.clone(), b.clone()));
        Self {
            rcs_pins,
            command: CommandedState::new((Vector::ZERO, Vector::ZERO)),
        }
    }
}

impl<P> CyclicTask for RcsTask<P>
where
    P: Pin,
{
    type Message = RcsMessage;
    type Data = ();

    fn get_data(&self) -> Self::Data {
        trace!("RcsTask::get_data(self)");
    }

    fn step(&mut self, receiver: &Receiver<Self::Message>, step_time: Instant) {
        trace!("RcsTask::step(self, receiver, step_time: {step_time:?})");

        self.command.evaluate(step_time);

        while let Ok(msg) = receiver.try_recv() {
            match msg {
                RcsMessage::SetRcs(InstantPriorityCommand {
                    inner:
                        SetRcs {
                            translation,
                            rotation,
                        },
                    valid_until,
                    priority,
                }) => {
                    self.command
                        .insert((translation, rotation), valid_until, priority);
                }
            }
        }

        let (translation, rotation) = *self.command;

        let throttle = thruster_balancing::balance_thrust(
            translation,
            rotation,
            &RCS_THRUSTERS,
            BalancingConstants::default(),
        );

        for (pin, throttle) in zip(&mut self.rcs_pins, throttle) {
            pin.set(
                if throttle >= 0.5 {
                    PinState::High
                } else {
                    PinState::Low
                },
                // This signal is valid for the next half second
                step_time + Duration::from_secs(1),
                PIN_PRIORITY,
            );
        }
    }
}
