use std::time::{Duration, Instant};

use crate::bus::{CanBus, CanFrame, CanId};
use crate::error::{Error, Result};
use crate::protocol::{
    DEFAULT_CONTROL_RATE_HZ, DEFAULT_MOTOR_ID, DirectCommand, FeedbackFilter, MitCommand,
    MitHelperCommand, MotorFeedback, MotorModel, OriginMode, clamp_erpm, degrees_to_radians,
    direct_command_frame, erpm_to_rad_s, mit_command_frame, mit_helper_frame,
};

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub enum HelperPolicy {
    Strict,
    FcFd,
    Legacy,
}

#[derive(Debug, Clone, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct MotorConfig {
    pub motor_id: u8,
    pub model: MotorModel,
    pub feedback_id: Option<CanId>,
    pub helper_policy: HelperPolicy,
    pub allow_legacy_feedback_ids: bool,
    pub control_rate_hz: f64,
    pub mit_velocity_kd: Option<f64>,
}

#[derive(Debug, Clone, PartialEq, Eq)]
pub struct RefreshStats {
    pub iterations: u64,
    pub feedback_frames: u64,
    pub missed_feedback: u64,
    pub expected_period: Duration,
    pub elapsed: Duration,
}

impl RefreshStats {
    pub const fn new(expected_period: Duration) -> Self {
        Self {
            iterations: 0,
            feedback_frames: 0,
            missed_feedback: 0,
            expected_period,
            elapsed: Duration::ZERO,
        }
    }

    pub fn effective_hz(&self) -> Option<f64> {
        if self.iterations == 0 || self.elapsed.is_zero() {
            return None;
        }
        Some(self.iterations as f64 / self.elapsed.as_secs_f64())
    }

    pub fn feedback_ratio(&self) -> Option<f64> {
        if self.iterations == 0 {
            return None;
        }
        Some(self.feedback_frames as f64 / self.iterations as f64)
    }
}

impl MotorConfig {
    pub fn feedback_filter(&self) -> FeedbackFilter {
        let mut filter = FeedbackFilter::for_motor(self.model, self.motor_id);
        if let Some(feedback_id) = self.feedback_id {
            filter = filter.with_explicit_id(feedback_id);
        }
        if self.allow_legacy_feedback_ids {
            filter = filter.with_legacy_ids(self.motor_id);
        }
        filter
    }

    pub fn refresh_period(&self) -> Result<Duration> {
        if self.control_rate_hz <= 0.0 || !self.control_rate_hz.is_finite() {
            return Err(Error::InvalidRefreshRate(self.control_rate_hz));
        }
        Ok(Duration::from_secs_f64(1.0 / self.control_rate_hz))
    }

    pub fn velocity_kd(&self) -> f64 {
        self.mit_velocity_kd
            .unwrap_or_else(|| self.model.spec().mit_velocity_kd)
    }
}

impl Default for MotorConfig {
    fn default() -> Self {
        Self {
            motor_id: DEFAULT_MOTOR_ID,
            model: MotorModel::Ak60_6,
            feedback_id: None,
            helper_policy: HelperPolicy::FcFd,
            allow_legacy_feedback_ids: false,
            control_rate_hz: DEFAULT_CONTROL_RATE_HZ,
            mit_velocity_kd: None,
        }
    }
}

#[derive(Debug)]
pub struct MotorController<B> {
    bus: B,
    config: MotorConfig,
    feedback_filter: FeedbackFilter,
    last_feedback: Option<MotorFeedback>,
    active_command: Option<MitCommand>,
    mit_enabled: bool,
}

impl<B: CanBus> MotorController<B> {
    pub fn new(bus: B, config: MotorConfig) -> Result<Self> {
        config.refresh_period()?;
        Ok(Self {
            feedback_filter: config.feedback_filter(),
            bus,
            config,
            last_feedback: None,
            active_command: None,
            mit_enabled: false,
        })
    }

    pub fn config(&self) -> &MotorConfig {
        &self.config
    }

    pub fn bus(&self) -> &B {
        &self.bus
    }

    pub fn bus_mut(&mut self) -> &mut B {
        &mut self.bus
    }

    pub fn into_inner(self) -> B {
        self.bus
    }

    pub fn last_feedback(&self) -> Option<&MotorFeedback> {
        self.last_feedback.as_ref()
    }

    pub const fn mit_enabled(&self) -> bool {
        self.mit_enabled
    }

    pub fn active_command(&self) -> Option<MitCommand> {
        self.active_command
    }

    pub fn mit_frame(&self, command: MitCommand) -> Result<CanFrame> {
        mit_command_frame(self.config.model, self.config.motor_id, command)
    }

    pub fn helper_frame(&self, helper: MitHelperCommand) -> Result<CanFrame> {
        mit_helper_frame(self.config.model, self.config.motor_id, helper)
    }

    pub fn direct_frame(&self, command: DirectCommand) -> Result<CanFrame> {
        direct_command_frame(self.config.motor_id, command)
    }

    pub fn enable_mit_mode(&mut self) -> Result<()> {
        match self.config.helper_policy {
            HelperPolicy::Strict => {}
            HelperPolicy::FcFd => self.send_helper(MitHelperCommand::Enable)?,
            HelperPolicy::Legacy => {
                self.send_helper(MitHelperCommand::Enable)?;
                self.send_helper(MitHelperCommand::EnableLegacy)?;
            }
        }

        self.send_mit_frame(MitCommand::neutral())?;
        self.mit_enabled = true;
        Ok(())
    }

    pub fn disable_mit_mode(&mut self) -> Result<()> {
        for _ in 0..3 {
            self.send_mit_frame(MitCommand::neutral())?;
        }

        match self.config.helper_policy {
            HelperPolicy::Strict => {}
            HelperPolicy::FcFd => self.send_helper(MitHelperCommand::Disable)?,
            HelperPolicy::Legacy => {
                self.send_helper(MitHelperCommand::Disable)?;
                self.send_helper(MitHelperCommand::DisableLegacy)?;
            }
        }

        self.active_command = None;
        self.mit_enabled = false;
        Ok(())
    }

    pub fn zero_position(&mut self) -> Result<()> {
        self.send_helper(MitHelperCommand::ZeroPosition)?;
        self.last_feedback = None;
        Ok(())
    }

    pub fn set_mit_mode(&mut self, mut command: MitCommand) -> Result<()> {
        if !self.mit_enabled {
            self.enable_mit_mode()?;
        }

        let spec = self.config.model.spec();
        let requested_erpm = crate::protocol::rad_s_to_erpm(command.velocity_rad_s, spec);
        let clamped_erpm = clamp_erpm(requested_erpm, spec);
        command.velocity_rad_s = erpm_to_rad_s(clamped_erpm, spec);

        self.send_mit_frame(command)?;
        self.active_command = Some(command);
        Ok(())
    }

    pub fn set_velocity_erpm(&mut self, velocity_erpm: i32) -> Result<()> {
        if velocity_erpm == 0 {
            return self.stop();
        }

        let spec = self.config.model.spec();
        let clamped_erpm = clamp_erpm(velocity_erpm, spec);
        self.set_mit_mode(MitCommand {
            position_rad: 0.0,
            velocity_rad_s: erpm_to_rad_s(clamped_erpm, spec),
            kp: 0.0,
            kd: self.config.velocity_kd(),
            torque_nm: 0.0,
        })
    }

    pub fn set_position_degrees(&mut self, position_degrees: f64) -> Result<()> {
        let spec = self.config.model.spec();
        self.set_mit_mode(MitCommand {
            position_rad: degrees_to_radians(position_degrees),
            velocity_rad_s: 0.0,
            kp: spec.mit_position_kp,
            kd: spec.mit_position_kd,
            torque_nm: 0.0,
        })
    }

    pub fn set_torque_nm(&mut self, torque_nm: f64) -> Result<()> {
        self.set_mit_mode(MitCommand {
            position_rad: 0.0,
            velocity_rad_s: 0.0,
            kp: 0.0,
            kd: 0.0,
            torque_nm,
        })
    }

    pub fn send_direct_command(&mut self, command: DirectCommand) -> Result<()> {
        let frame = self.direct_frame(command)?;
        self.bus.send(&frame)
    }

    pub fn set_duty_cycle(&mut self, duty: f64) -> Result<()> {
        self.send_direct_command(DirectCommand::DutyCycle(duty))
    }

    pub fn set_current_amps(&mut self, current_amps: f64) -> Result<()> {
        self.send_direct_command(DirectCommand::Current(current_amps))
    }

    pub fn set_brake_current_amps(&mut self, current_amps: f64) -> Result<()> {
        self.send_direct_command(DirectCommand::BrakeCurrent(current_amps))
    }

    pub fn set_direct_velocity_erpm(&mut self, velocity_erpm: i32) -> Result<()> {
        self.send_direct_command(DirectCommand::Velocity(velocity_erpm))
    }

    pub fn set_direct_position_degrees(&mut self, position_degrees: f64) -> Result<()> {
        self.send_direct_command(DirectCommand::Position(position_degrees))
    }

    pub fn set_origin(&mut self, origin_mode: OriginMode) -> Result<()> {
        self.send_direct_command(DirectCommand::SetOrigin(origin_mode))
    }

    pub fn send_keepalive(&mut self) -> Result<()> {
        let command = self.active_command.unwrap_or_else(MitCommand::neutral);
        self.send_mit_frame(command)
    }

    pub fn refresh_once(&mut self, feedback_timeout: Duration) -> Result<Option<MotorFeedback>> {
        self.send_keepalive()?;
        self.receive_feedback(feedback_timeout)
    }

    pub fn refresh_for(
        &mut self,
        duration: Duration,
        feedback_timeout: Duration,
    ) -> Result<RefreshStats> {
        let period = self.config.refresh_period()?;
        let started = Instant::now();
        let mut stats = RefreshStats::new(period);

        while started.elapsed() < duration {
            let loop_started = Instant::now();
            match self.refresh_once(feedback_timeout)? {
                Some(_) => stats.feedback_frames += 1,
                None => stats.missed_feedback += 1,
            }
            stats.iterations += 1;

            let loop_elapsed = loop_started.elapsed();
            if loop_elapsed < period {
                std::thread::sleep(period - loop_elapsed);
            }
        }

        stats.elapsed = started.elapsed();
        Ok(stats)
    }

    pub fn receive_feedback(&mut self, timeout: Duration) -> Result<Option<MotorFeedback>> {
        let deadline = Instant::now() + timeout;

        loop {
            let now = Instant::now();
            if now >= deadline {
                return Ok(None);
            }

            let Some(frame) = self.bus.receive(deadline.saturating_duration_since(now))? else {
                return Ok(None);
            };

            if !self.feedback_filter.matches(&frame) {
                continue;
            }

            let feedback = self.config.model.parse_feedback(frame.data())?;
            self.last_feedback = Some(feedback.clone());
            return Ok(Some(feedback));
        }
    }

    pub fn check_communication(&mut self, timeout: Duration) -> Result<bool> {
        self.enable_mit_mode()?;
        self.send_keepalive()?;
        Ok(self.receive_feedback(timeout)?.is_some())
    }

    pub fn stop(&mut self) -> Result<()> {
        for _ in 0..4 {
            self.send_mit_frame(MitCommand::neutral())?;
        }

        self.disable_mit_mode()
    }

    fn send_mit_frame(&mut self, command: MitCommand) -> Result<()> {
        let frame = self.mit_frame(command)?;
        self.bus.send(&frame)
    }

    fn send_helper(&mut self, helper: MitHelperCommand) -> Result<()> {
        let frame = self.helper_frame(helper)?;
        self.bus.send(&frame)
    }
}
