use std::collections::VecDeque;
use std::time::Duration;

use kcan::protocol::{AK60_6_MIT_LIMITS, erpm_to_rad_s, float_to_uint};
use kcan::{
    CanBus, CanFrame, CanId, DirectCommand, HelperPolicy, MitHelperCommand, MotorConfig,
    MotorController, MotorManager, MotorModel, OriginMode, Result,
};

#[derive(Debug, Default)]
struct RecordingBus {
    sent: Vec<CanFrame>,
    received: VecDeque<CanFrame>,
}

impl RecordingBus {
    fn with_received(frames: impl IntoIterator<Item = CanFrame>) -> Self {
        Self {
            sent: Vec::new(),
            received: frames.into_iter().collect(),
        }
    }
}

impl CanBus for RecordingBus {
    fn send(&mut self, frame: &CanFrame) -> Result<()> {
        self.sent.push(frame.clone());
        Ok(())
    }

    fn receive(&mut self, _timeout: Duration) -> Result<Option<CanFrame>> {
        Ok(self.received.pop_front())
    }
}

fn strict_config() -> MotorConfig {
    MotorConfig {
        helper_policy: HelperPolicy::Strict,
        ..MotorConfig::default()
    }
}

#[test]
fn set_velocity_sends_enable_then_velocity_command() {
    let bus = RecordingBus::default();
    let mut motor = MotorController::new(bus, strict_config()).unwrap();

    motor.set_velocity_erpm(1_000).unwrap();
    let sent = &motor.bus().sent;

    assert_eq!(sent.len(), 2);
    assert_eq!(sent[0].id(), CanId::Extended(0x0803));
    assert_eq!(sent[1].id(), CanId::Extended(0x0803));
    assert!(motor.mit_enabled());

    let active = motor.active_command().unwrap();
    assert_eq!(active.kp, 0.0);
    assert_eq!(active.kd, MotorModel::Ak60_6.spec().mit_velocity_kd);
    assert_eq!(
        active.velocity_rad_s,
        erpm_to_rad_s(1_000, MotorModel::Ak60_6.spec())
    );
}

#[test]
fn set_velocity_clamps_to_motor_spec() {
    let bus = RecordingBus::default();
    let mut motor = MotorController::new(bus, strict_config()).unwrap();

    motor.set_velocity_erpm(100_000).unwrap();

    let active = motor.active_command().unwrap();
    assert_eq!(
        active.velocity_rad_s,
        erpm_to_rad_s(
            MotorModel::Ak60_6.spec().max_velocity_erpm,
            MotorModel::Ak60_6.spec()
        )
    );
}

#[test]
fn zero_position_sends_helper_frame() {
    let bus = RecordingBus::default();
    let mut motor = MotorController::new(bus, strict_config()).unwrap();

    motor.zero_position().unwrap();

    assert_eq!(motor.bus().sent.len(), 1);
    assert_eq!(motor.bus().sent[0].id(), CanId::Extended(0x03));
    assert_eq!(
        motor.bus().sent[0].data(),
        MitHelperCommand::ZeroPosition.data()
    );
}

#[test]
fn receive_feedback_filters_and_decodes_frames() {
    let unrelated = CanFrame::new_padded(CanId::Extended(0x9999), &[0; 8]).unwrap();
    let feedback_frame = CanFrame::new_padded(
        CanId::Extended(0x2903),
        &[0x00, 0x64, 0x00, 0x0A, 0xFF, 0x9C, 0x19, 0x00],
    )
    .unwrap();
    let bus = RecordingBus::with_received([unrelated, feedback_frame]);
    let mut motor = MotorController::new(bus, strict_config()).unwrap();

    let feedback = motor
        .receive_feedback(Duration::from_millis(10))
        .unwrap()
        .unwrap();

    assert_eq!(feedback.position_degrees, 10.0);
    assert_eq!(feedback.speed_erpm, 100);
    assert_eq!(feedback.current_amps, -1.0);
    assert!(motor.last_feedback().is_some());
}

#[test]
fn refresh_once_resends_active_command_and_captures_feedback() {
    let feedback_frame = CanFrame::new_padded(
        CanId::Extended(0x2903),
        &[0x00, 0x64, 0x00, 0x0A, 0xFF, 0x9C, 0x19, 0x00],
    )
    .unwrap();
    let bus = RecordingBus::with_received([feedback_frame]);
    let mut motor = MotorController::new(bus, strict_config()).unwrap();

    motor.set_velocity_erpm(1_000).unwrap();
    motor.bus_mut().sent.clear();

    let feedback = motor
        .refresh_once(Duration::from_millis(1))
        .unwrap()
        .unwrap();

    assert_eq!(motor.bus().sent.len(), 1);
    assert_eq!(motor.bus().sent[0].id(), CanId::Extended(0x0803));
    assert_eq!(feedback.speed_erpm, 100);
}

#[test]
fn refresh_for_reports_iterations_and_missed_feedback() {
    let mut config = strict_config();
    config.control_rate_hz = 1_000.0;
    let bus = RecordingBus::default();
    let mut motor = MotorController::new(bus, config).unwrap();

    let stats = motor
        .refresh_for(Duration::from_millis(2), Duration::ZERO)
        .unwrap();

    assert!(stats.iterations >= 1);
    assert_eq!(stats.feedback_frames, 0);
    assert_eq!(stats.missed_feedback, stats.iterations);
    assert!(stats.effective_hz().is_some());
    assert_eq!(stats.feedback_ratio(), Some(0.0));
}

#[test]
fn position_command_uses_profile_gains() {
    let bus = RecordingBus::default();
    let mut motor = MotorController::new(bus, strict_config()).unwrap();

    motor.set_position_degrees(90.0).unwrap();

    let active = motor.active_command().unwrap();
    assert_eq!(active.kp, MotorModel::Ak60_6.spec().mit_position_kp);
    assert_eq!(active.kd, MotorModel::Ak60_6.spec().mit_position_kd);
    assert!((active.position_rad - 90_f64.to_radians()).abs() < 1e-12);
}

#[test]
fn torque_command_is_saturated_by_packer() {
    let bus = RecordingBus::default();
    let mut motor = MotorController::new(bus, strict_config()).unwrap();

    motor.set_torque_nm(999.0).unwrap();
    let payload = motor.bus().sent.last().unwrap().data();
    let torque_raw = (u32::from(payload[6] & 0x0F) << 8) | u32::from(payload[7]);

    assert_eq!(
        torque_raw,
        float_to_uint(
            999.0,
            AK60_6_MIT_LIMITS.torque_min_nm,
            AK60_6_MIT_LIMITS.torque_max_nm,
            12
        )
        .unwrap()
    );
}

#[test]
fn direct_current_method_sends_current_loop_frame() {
    let bus = RecordingBus::default();
    let mut motor = MotorController::new(bus, strict_config()).unwrap();

    motor.set_current_amps(3.0).unwrap();

    assert_eq!(motor.bus().sent.len(), 1);
    assert_eq!(motor.bus().sent[0].id(), CanId::Extended(0x0103));
    assert_eq!(motor.bus().sent[0].len(), 4);
    assert_eq!(&motor.bus().sent[0].data()[..4], &3_000_i32.to_be_bytes());
}

#[test]
fn direct_origin_method_sends_origin_frame() {
    let bus = RecordingBus::default();
    let mut motor = MotorController::new(bus, strict_config()).unwrap();

    motor.set_origin(OriginMode::Temporary).unwrap();

    assert_eq!(motor.bus().sent[0].id(), CanId::Extended(0x0503));
    assert_eq!(motor.bus().sent[0].len(), 1);
    assert_eq!(motor.bus().sent[0].data()[0], 0x00);
}

#[test]
fn manager_rejects_duplicate_ids() {
    let result = MotorManager::new(
        RecordingBus::default(),
        [MotorConfig::default(), MotorConfig::default()],
    );

    assert!(result.is_err());
}

#[test]
fn manager_routes_keepalive_to_all_motors() {
    let mut right = MotorConfig::default();
    right.motor_id = 0x04;
    let mut manager = MotorManager::new(RecordingBus::default(), [MotorConfig::default(), right])
        .unwrap()
        .with_labels([("left".to_owned(), 0x03), ("right".to_owned(), 0x04)])
        .unwrap();

    manager.send_keepalive_all().unwrap();

    assert_eq!(manager.id_for_label("left").unwrap(), 0x03);
    assert_eq!(manager.bus().sent.len(), 2);
    assert_eq!(manager.bus().sent[0].id(), CanId::Extended(0x0803));
    assert_eq!(manager.bus().sent[1].id(), CanId::Extended(0x0804));
}

#[test]
fn manager_routes_direct_commands_by_motor_id() {
    let mut right = MotorConfig::default();
    right.motor_id = 0x04;
    let mut manager =
        MotorManager::new(RecordingBus::default(), [MotorConfig::default(), right]).unwrap();

    manager
        .send_direct_command(0x04, DirectCommand::Velocity(1_234))
        .unwrap();

    assert_eq!(manager.bus().sent.len(), 1);
    assert_eq!(manager.bus().sent[0].id(), CanId::Extended(0x0304));
    assert_eq!(&manager.bus().sent[0].data()[..4], &1_234_i32.to_be_bytes());
}
