"""
Stereo 4D Radar 绝对速度估计原理

单雷达限制：
- 多普勒只测量径向速度 v_r = v · cos(θ)
- 横向速度无法直接获取
- 需要假设目标运动方向才能估计全速度

双雷达方案：
- 左右雷达观测角度不同：θ_L 和 θ_R
- 各自获得径向速度：v_L = v · cos(θ_L), v_R = v · cos(θ_R)
- 两个方程两个未知数 → 解出 v_x, v_y（绝对速度向量）
"""

import numpy as np

class StereoRadarVelocityEstimator:
    """
    双 4D 雷达绝对速度估计器
    
    左右雷达测量同一目标的不同径向速度
    通过几何关系解算绝对速度
    """
    
    def __init__(self, baseline: float = 1.5):
        """
        Args:
            baseline: 双雷达间距（米），典型 1.2-1.8m
        """
        self.baseline = baseline  # 雷达间距
    
    def estimate_velocity(self, 
                          target_pos: np.ndarray,  # (3,) 目标 3D 位置
                          doppler_left: float,     # 左雷达多普勒速度
                          doppler_right: float,     # 右雷达多普勒速度
                          radar_left_pos: np.ndarray = None,
                          radar_right_pos: np.ndarray = None
                          ) -> np.ndarray:
        """
        从双雷达多普勒测量估计目标绝对速度
        
        Args:
            target_pos: (3,) 目标位置 [x, y, z] 相对车辆中心
            doppler_left: 左雷达径向速度 (m/s)
            doppler_right: 右雷达径向速度 (m/s)
        
        Returns:
            velocity: (2,) 目标绝对速度 [vx, vy]
        """
        if radar_left_pos is None:
            radar_left_pos = np.array([-self.baseline/2, 0, 0])
        if radar_right_pos is None:
            radar_right_pos = np.array([self.baseline/2, 0, 0])
        
        # 计算方向向量
        dir_left = target_pos - radar_left_pos
        dir_left = dir_left / np.linalg.norm(dir_left)
        
        dir_right = target_pos - radar_right_pos
        dir_right = dir_right / np.linalg.norm(dir_right)
        
        # 构建方程组: [dir_left_x, dir_left_y] @ [vx, vy] = doppler_left
        #            [dir_right_x, dir_right_y] @ [vx, vy] = doppler_right
        A = np.array([
            [dir_left[0], dir_left[1]],
            [dir_right[0], dir_right[1]]
        ])
        b = np.array([doppler_left, doppler_right])
        
        # 解线性方程组
        try:
            velocity = np.linalg.solve(A, b)
        except np.linalg.LinAlgError:
            # 矩阵奇异时（目标在正前方），退化到单雷达
            velocity = np.array([doppler_left, 0])
        
        return velocity


# ==================== 测试代码 ====================
if __name__ == "__main__":
    estimator = StereoRadarVelocityEstimator(baseline=1.5)
    
    # 场景1：目标正前方 20m，横向移动 3m/s
    target_pos = np.array([20, 5, 0])  # 前方 20m，偏右 5m
    true_vel = np.array([5, 3])  # 前进 5m/s，横向 3m/s
    
    # 计算各雷达径向速度
    r_left = np.array([-0.75, 0, 0])
    r_right = np.array([0.75, 0, 0])
    
    dir_l = target_pos - r_left
    dir_l = dir_l / np.linalg.norm(dir_l)
    dir_r = target_pos - r_right
    dir_r = dir_r / np.linalg.norm(dir_r)
    
    doppler_l = np.dot(true_vel, dir_l[:2])
    doppler_r = np.dot(true_vel, dir_r[:2])
    
    est_vel = estimator.estimate_velocity(target_pos, doppler_l, doppler_r)
    
    print("=" * 60)
    print("Stereo 4D Radar 绝对速度估计测试")
    print("=" * 60)
    print(f"目标位置: {target_pos} m")
    print(f"真实速度: {true_vel} m/s")
    print(f"左雷达多普勒: {doppler_l:.3f} m/s")
    print(f"右雷达多普勒: {doppler_r:.3f} m/s")
    print(f"估计速度: {est_vel} m/s")
    print(f"误差: {np.linalg.norm(est_vel - true_vel):.4f} m/s")
    
    # 场景2：座舱内场景（近距离 CPD）
    print("\n--- 座舱 CPD 场景 ---")
    cabin_target = np.array([1.0, 0.3, 0.5])  # 1m 处
    child_vel = np.array([0, 0.05])  # 呼吸级横向运动
    
    dir_l2 = cabin_target - r_left
    dir_l2 = dir_l2 / np.linalg.norm(dir_l2)
    dir_r2 = cabin_target - r_right
    dir_r2 = dir_r2 / np.linalg.norm(dir_r2)
    
    dop_l2 = np.dot(child_vel, dir_l2[:2])
    dop_r2 = np.dot(child_vel, dir_r2[:2])
    
    est_v2 = estimator.estimate_velocity(cabin_target, dop_l2, dop_r2)
    print(f"儿童位置: {cabin_target} m")
    print(f"呼吸速度: {child_vel} m/s")
    print(f"左多普勒: {dop_l2:.6f} m/s")
    print(f"右多普勒: {dop_r2:.6f} m/s")
    print(f"估计速度: {est_v2} m/s")
    print("💡 近距离双雷达可检测呼吸级运动")