//! vcan 集成测试：在真实 Linux 内核 CAN 栈上验证桥接。
//!
//! 仅 Linux 下编译；默认 `#[ignore]`，需要 vcan0（见 justfile `vcan-test`）。

#![cfg(target_os = "linux")]
#![allow(clippy::unwrap_used, clippy::expect_used)]

use std::time::Duration;

use can::{CanFrame, CanLink};
use motor::{MotorFeedback, VelocityDrive};
use motor_control::{MotorPolicy, VelocityGains};
use socketcan_rs::tokio::CanSocket;
use socketcan_rs::{CanDataFrame, CanFrame as ScFrame, EmbeddedFrame as _, Frame as _, StandardId};
use tokio::time::timeout;

const IFACE: &str = "vcan0";

fn sc_data_frame(id: u16, data: [u8; 8]) -> ScFrame {
    ScFrame::Data(CanDataFrame::new(StandardId::new(id).unwrap(), &data).unwrap())
}

#[tokio::test]
#[ignore = "requires vcan0 (see `just vcan-test`)"]
async fn bridge_relays_command_to_bus() {
    let link = socketcan::open_link(IFACE).unwrap();
    let observer = CanSocket::open(IFACE).unwrap();

    let command = CanFrame::new(0x200, [0x04, 0xD2, 0, 0, 0, 0, 0, 0]);
    link.outgoing_tx.send(command).await.unwrap();

    let got = timeout(Duration::from_secs(2), observer.read_frame())
        .await
        .expect("timeout waiting for command frame")
        .unwrap();
    match got {
        ScFrame::Data(df) => {
            assert_eq!(df.raw_id(), 0x200);
            assert_eq!(df.data(), &[0x04, 0xD2, 0, 0, 0, 0, 0, 0]);
        }
        other => panic!("expected data frame, got {other:?}"),
    }
}

#[tokio::test]
#[ignore = "requires vcan0 (see `just vcan-test`)"]
async fn bridge_relays_feedback_from_bus() {
    let CanLink { outgoing_tx: _, mut incoming_rx } = socketcan::open_link(IFACE).unwrap();
    let simulator = CanSocket::open(IFACE).unwrap();

    // 电调 1 反馈：角度 8191、-364rpm、电流 123、45℃。
    let feedback = sc_data_frame(0x201, [0x1F, 0xFF, 0xFE, 0x94, 0x00, 0x7B, 45, 0]);
    simulator.write_frame(feedback).await.unwrap();

    let got = timeout(Duration::from_secs(2), incoming_rx.recv())
        .await
        .expect("timeout waiting for feedback frame")
        .unwrap();
    assert_eq!(got.id, 0x201);
    assert_eq!(got.data, [0x1F, 0xFF, 0xFE, 0x94, 0x00, 0x7B, 45, 0]);
}

#[tokio::test]
#[ignore = "requires vcan0 (see `just vcan-test`)"]
async fn esc_bus_end_to_end_over_vcan() {
    // EscBus ↔ 真实内核 CAN 栈 ↔ 模拟电调（第二个 socket）。
    let link = socketcan::open_link(IFACE).unwrap();
    let mut esc = EscSim::new(IFACE, &[1]);

    let config = esc_bus::EscConfig::gm3508("m1", 1, 1.0);
    let mut policy = MotorPolicy::new();
    policy.configure(1, config.current_limit, VelocityGains::new(8.0, 2.0, 0.0, 16384.0 / 9254.0));
    let bus =
        esc_bus::EscBus::spawn(link, vec![config], esc_bus::BusOptions::default(), policy).unwrap();
    bus.motor(1).unwrap().set_velocity(2000.0);

    // 仿真 2s：模拟电调按命令回反馈，速度环应收敛。
    let motor = bus.motor(1).unwrap().clone();
    let deadline = tokio::time::Instant::now() + Duration::from_secs(2);
    loop {
        esc.step_once().await;
        if motor.state().rotor_speed_rpm > 1900.0 {
            break;
        }
        assert!(
            tokio::time::Instant::now() < deadline,
            "not converged: {}",
            motor.state().rotor_speed_rpm
        );
        tokio::time::sleep(Duration::from_millis(5)).await;
    }
    bus.shutdown().await;
}

/// 最小电调模拟：收命令帧 → 更新转速 → 回反馈帧。
struct EscSim {
    socket: CanSocket,
    speed_rpm: f32,
    angle: u16,
}

impl EscSim {
    fn new(iface: &str, _esc_ids: &[u8]) -> Self {
        Self { socket: CanSocket::open(iface).unwrap(), speed_rpm: 0.0, angle: 0 }
    }

    /// 排空当前命令帧并推进模型一步（dt = 5ms）。
    async fn step_once(&mut self) {
        const DT: f32 = 0.005;
        let mut current = 0i16;
        while let Ok(frame) = self.socket.try_read_frame() {
            if let ScFrame::Data(df) = frame {
                if df.raw_id() == 0x200 {
                    current = i16::from_be_bytes([df.data()[0], df.data()[1]]);
                }
            }
        }
        let target = f32::from(current) * 9254.0 / 16384.0;
        self.speed_rpm += (target - self.speed_rpm) * (1.0 - (-DT / 0.12).exp());
        self.angle =
            (u32::from(self.angle) + (self.speed_rpm / 60.0 * 8192.0 * DT) as u32) as u16 % 8192;
        let data = {
            let mut d = [0u8; 8];
            d[0..2].copy_from_slice(&self.angle.to_be_bytes());
            d[2..4].copy_from_slice(&(self.speed_rpm as i16).to_be_bytes());
            d[4..6].copy_from_slice(&current.to_be_bytes());
            d[6] = 30;
            d
        };
        self.socket.write_frame(sc_data_frame(0x201, data)).await.unwrap();
    }
}