use std::any::type_name;
use std::fmt::{Debug, Formatter};
use std::sync::mpsc::Receiver;
use std::time::Instant;
use log::{error, info, trace, warn};
use crate::drive::Drive;
use crate::hardware::a4963::A4963;
use crate::hardware::pwm::PwmOutput;
use crate::scheduler::{CyclicTask, TaskHandle};

const OFF_THROTTLE: f64 = 0.0;
const THRESHOLD_THROTTLE: f64 = 0.2;
const MINIMUM_THROTTLE: f64 = 0.3;
const STARTUP_THROTTLE: f64 = 0.5;
const MAXIMUM_THROTTLE: f64 = 1.0;

const STARTUP_TIME: u16 = 5; // 5 cycles = 0.5 second

#[derive(Clone, Debug)]
pub enum DriveMessage {
    SetThrust(f64),
}

impl<D: Debug> Drive for TaskHandle<DriveMessage, D> {
    fn set_throttle(&self, throttle: f64, valid_until: Instant, priority: u8) {
        trace!(
            "TaskHandle<DriveMessage, D>::set_pin(self: {self:?}, throttle: {throttle}, valid_until: {valid_until:?}, priority: {priority})"
        );
        // This can only fail if the other side is disconnected which we want to ignore
        let _ = self.sender.send(DriveMessage::SetThrust(throttle));
    }
}

pub struct DriveTask<MotorController, Pwm> {
    motor_controller: MotorController,
    pwm: Pwm,
    thrust_setpoint: f64,
    state: State,
}

impl<MotorController, Pwm> Debug for DriveTask<MotorController, Pwm> {
    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
        write!(
            f,
            "DriveTask<MotorController={}, Pwm={}> {{ thrust_setpoint: {}, state: {:?} }}",
            type_name::<MotorController>(),
            type_name::<Pwm>(),
            self.thrust_setpoint,
            self.state,
        )
    }
}

#[derive(Clone, Debug)]
enum State {
    Off,
    Configure,
    Startup {
        timer: u16,
    },
    Operational,
    Error,
}

impl<MotorController, Pwm> DriveTask<MotorController, Pwm>
where
    MotorController: A4963,
    Pwm: PwmOutput
{
    pub fn new(motor_controller: MotorController, pwm: Pwm) -> Self {
        Self {
            motor_controller,
            pwm,
            thrust_setpoint: OFF_THROTTLE,
            state: State::Off,
        }
    }

    fn step_state(&mut self, state: State) -> State {
        trace!("DriveTask::step_state(self: {self:?}, state: {state:?})");
        match state {
            State::Error => {
                // A generic error state which cleans up
                // any internals
                self.thrust_setpoint = OFF_THROTTLE;

                State::Off
            },
            State::Off => {
                if let Err(err) = self.pwm.set_duty_cycle(OFF_THROTTLE) {
                    error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Pwm Error: {err}");
                    return state;
                }

                if self.thrust_setpoint >= THRESHOLD_THROTTLE {
                    // Immediately start performing the startup
                    return self.step_state(State::Configure);
                }

                state
            }
            State::Configure => {
                let diagnostic = match self.motor_controller.check_health() {
                    Ok(diagnostic) => diagnostic,
                    Err(err) => {
                        error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Failed to check device health: {err}");
                        return self.step_state(State::Error);
                    }
                };
                if let Err(err) = diagnostic.assert_consistency() {
                    error!("DriveTask::step_state(self: {self:?}, state: {state:?} - {err}")
                }
                if diagnostic.power_on_reset {
                    // We just had a power on or a reset event
                    // That's perfectly Ok as long as we only see it once
                    info!("Encountered a power on/reset");
                    return state;
                }

                match self.motor_controller.write_configuration() {
                    Ok(()) => State::Startup {
                        timer: 0,
                    },
                    Err(err) => {
                        error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Failed to write motor configuration: {err}");
                        self.step_state(State::Error)
                    },
                }
            }
            State::Startup { timer } => {
                if self.thrust_setpoint < THRESHOLD_THROTTLE {
                    return self.step_state(State::Off);
                }
                if let Err(err) = self.pwm.set_duty_cycle(STARTUP_THROTTLE) {
                    error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Pwm Error: {err}");
                    return self.step_state(State::Error);
                }

                // remain in the current state
                if timer < STARTUP_TIME {
                    State::Startup {
                        timer: timer.saturating_add(1)
                    }
                } else {
                    State::Operational
                }
            }
            State::Operational => {
                if self.thrust_setpoint < THRESHOLD_THROTTLE {
                    return self.step_state(State::Off);
                }

                let diagnostic = match self.motor_controller.check_health() {
                    Ok(diagnostic) => diagnostic,
                    Err(err) => {
                        error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Failed to check device health: {err}");
                        return self.step_state(State::Error);
                    }
                };
                if let Err(err) = diagnostic.assert_consistency() {
                    error!("DriveTask::step_state(self: {self:?}, state: {state:?} - {err}")
                }
                if diagnostic.fault_flag {
                    if diagnostic.loss_of_synchronization {
                        if let Err(err) = self.pwm.set_duty_cycle(OFF_THROTTLE) {
                            error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Pwm Error: {err}");
                            return State::Error;
                        }
                        warn!("Loss of sync");
                        return State::Startup { timer: 0 };
                    }
                    error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Diagnostic: {diagnostic:?}");
                    return State::Error;
                }

                let new_thrust = self.thrust_setpoint.clamp(MINIMUM_THROTTLE, MAXIMUM_THROTTLE);
                if let Err(err) = self.pwm.set_duty_cycle(new_thrust) {
                    error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Pwm Error: {err}");
                    return self.step_state(State::Error);
                }

                State::Operational
            }
        }
    }
}

impl<MotorController, Pwm> CyclicTask for DriveTask<MotorController, Pwm>
where
    MotorController: A4963,
    Pwm: PwmOutput
{
    type Message = DriveMessage;
    type Data = ();

    fn get_data(&self) -> Self::Data {
        ()
    }

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

        while let Ok(message) = receiver.try_recv() {
            match message {
                DriveMessage::SetThrust(new_thrust) => self.thrust_setpoint = new_thrust,
            }
        }

        self.state = self.step_state(self.state.clone());
    }
}
