"""
NXP NCJ29D6 UWB CPD Detection Algorithm Pipeline
基于 NXP 白皮书与 IEEE 802.15.4z 标准
"""

import numpy as np
from scipy import signal as sig
from typing import Tuple

class UWBCPD_Detector:
    """
    UWB 儿童存在检测器
    
    核心原理：
    1. 发射 UWB 脉冲（~2ns 宽度），接收反射信号
    2. 对反射信号做距离-多普勒分析
    3. 提取微动信号（呼吸频率 0.1-0.5 Hz, 心跳 0.8-2.0 Hz）
    4. 与环境基线对比，判定是否有人/动物
    
    参考：NXP White Paper "UWB Radar for Child Presence Detection"
    """
    
    def __init__(self, config: dict):
        # UWB 参数
        self.fc = config.get('center_freq', 7.0e9)  # 7 GHz 中心频率
        self.bw = config.get('bandwidth', 500e6)     # 500 MHz 带宽
        self.pulse_width = 1 / self.bw               # ~2 ns
        self.sample_rate = config.get('sample_rate', 1e9)  # 1 GS/s
        self.range_resolution = 3e8 / (2 * self.bw)   # ~0.3 m
        
        # 检测参数
        self.breath_range = (0.1, 0.5)   # 呼吸频率 Hz
        self.heart_range = (0.8, 2.0)    # 心跳频率 Hz
        self.detection_threshold = config.get('threshold', 0.35)
        self.history_frames = config.get('history', 300)  # 30s @ 10fps
        
    def generate_uwb_pulse(self, n_samples: int = 256) -> np.ndarray:
        """生成 UWB 发射脉冲"""
        t = np.arange(n_samples) / self.sample_rate
        # Gaussian pulse (5th derivative, 符合 FCC 频谱模板)
        pulse = np.zeros(n_samples)
        sigma = self.pulse_width / (2 * np.sqrt(2 * np.log(2)))
        center = n_samples // 2
        x = (t - t[center]) / sigma
        pulse = x * (4 * x**4 - 12 * x**2 + 3) * np.exp(-x**2)
        return pulse / np.max(np.abs(pulse))
    
    def range_profile(self, rx_signal: np.ndarray, tx_pulse: np.ndarray) -> np.ndarray:
        """
        距离剖面估计（匹配滤波）
        
        Args:
            rx_signal: 接收信号, shape=(N,)
            tx_pulse: 发射脉冲, shape=(M,)
            
        Returns:
            range_profile: 距离剖面, shape=(N-M+1,)
        """
        # 匹配滤波（互相关）
        range_prof = np.correlate(rx_signal, tx_pulse, mode='full')
        range_prof = range_prof[len(tx_pulse)-1:]
        
        # 取包络
        analytic = sig.hilbert(range_prof)
        envelope = np.abs(analytic)
        
        return envelope
    
    def extract_micro_motion(self, range_profiles: np.ndarray, 
                              target_range_bin: int) -> np.ndarray:
        """
        从距离剖面序列中提取微动信号
        
        Args:
            range_profiles: 距离剖面序列, shape=(T, N_range)
            target_range_bin: 目标距离单元索引
            
        Returns:
            micro_motion: 微动信号时序, shape=(T,)
        """
        # 提取目标距离单元的相位变化
        phase_series = np.angle(range_profiles[:, target_range_bin])
        
        # 相位差分（消除静态杂波）
        phase_diff = np.diff(phase_series)
        # 处理相位跳变
        phase_diff = np.mod(phase_diff + np.pi, 2 * np.pi) - np.pi
        
        # 带通滤波：提取呼吸和心跳频段
        nyquist = 0.5 * 10  # 假设 10 Hz 帧率
        bp_breath = sig.butter(2, 
                               [self.breath_range[0]/nyquist, 
                                self.breath_range[1]/nyquist], 
                               btype='band')
        breath_signal = sig.filtfilt(*bp_breath, phase_diff)
        
        return breath_signal
    
    def detect_presence(self, micro_motion: np.ndarray, 
                        baseline_noise: float) -> Tuple[bool, float]:
        """
        判定座舱内是否存在生命体
        
        Args:
            micro_motion: 微动信号时序
            baseline_noise: 环境基线噪声水平
            
        Returns:
            is_present: 是否检测到存在
            confidence: 置信度 0-1
        """
        # 频谱分析
        freqs = np.fft.rfftfreq(len(micro_motion), d=0.1)  # 10 Hz 帧率
        spectrum = np.abs(np.fft.rfft(micro_motion))
        
        # 在呼吸频段寻找峰值
        breath_mask = (freqs >= self.breath_range[0]) & (freqs <= self.breath_range[1])
        breath_power = np.sum(spectrum[breath_mask])
        
        # 在心跳频段寻找峰值
        heart_mask = (freqs >= self.heart_range[0]) & (freqs <= self.heart_range[1])
        heart_power = np.sum(spectrum[heart_mask])
        
        # 总微动能量
        total_power = breath_power + 0.5 * heart_power
        
        # 信噪比判定
        snr = total_power / (baseline_noise + 1e-10)
        is_present = snr > self.detection_threshold
        confidence = min(1.0, snr / (self.detection_threshold * 2))
        
        return is_present, confidence


# 测试代码
if __name__ == "__main__":
    detector = UWBCPD_Detector({'threshold': 0.35, 'sample_rate': 1e9})
    
    # 模拟 UWB 脉冲
    tx = detector.generate_uwb_pulse(256)
    print(f"UWB Pulse: {len(tx)} samples, peak={np.max(tx):.3f}")
    print(f"Range Resolution: {detector.range_resolution*100:.1f} cm")
    
    # 模拟微动信号（呼吸 0.3 Hz + 心跳 1.2 Hz + 噪声）
    t = np.arange(300) * 0.1  # 30 秒
    breath = 0.5 * np.sin(2 * np.pi * 0.3 * t)
    heart = 0.2 * np.sin(2 * np.pi * 1.2 * t)
    noise = 0.1 * np.random.randn(300)
    micro_motion = breath + heart + noise
    
    # 检测
    is_present, conf = detector.detect_presence(micro_motion, baseline_noise=0.01)
    print(f"Presence Detected: {is_present}")
    print(f"Confidence: {conf:.2%}")
    
    # 空座模拟
    empty_motion = 0.05 * np.random.randn(300)
    is_present2, conf2 = detector.detect_presence(empty_motion, baseline_noise=0.01)
    print(f"Empty Cabin Detected: {is_present2} (should be False)")
    print(f"Empty Confidence: {conf2:.2%}")

"""
IMS CPD 模块集成接口
将 UWB/毫米波 CPD 检测结果接入 IMS 主控
"""

from dataclasses import dataclass, field
from enum import Enum
from typing import Optional, List
import time

class CPD_SensorType(Enum):
    UWB = "NXP_NCJ29D6"
    MMWAVE_60G = "TI_AWRL6432"
    MMWAVE_4D = "Infineon_BGT60"

class CPD_AlertLevel(Enum):
    NONE = 0
    INFO = 1      # 检测到可能存在
    WARNING = 2   # 确认存在，温度正常
    CRITICAL = 3  # 存在 + 高温风险

@dataclass
class CPD_DetectionResult:
    sensor_type: CPD_SensorType
    node_id: str                    # 节点标识（如 "roof", "rear_left"）
    is_present: bool
    confidence: float               # 0-1
    breath_rate: Optional[float]    # Hz, None if not detected
    heart_rate: Optional[float]     # Hz
    timestamp: float
    cabin_temp: Optional[float]     # 摄氏度
    alert_level: CPD_AlertLevel = CPD_AlertLevel.NONE

@dataclass
class CPD_FusionResult:
    is_present: bool
    confidence: float
    alert_level: CPD_AlertLevel
    source_results: List[CPD_DetectionResult] = field(default_factory=list)
    detection_latency_ms: int = 0

class CPD_FusionManager:
    """多节点 CPD 数据融合"""
    
    def __init__(self, nodes: List[str]):
        self.nodes = nodes
        self.results_buffer = {}  # node_id -> List[CPD_DetectionResult]
        
    def update(self, result: CPD_DetectionResult):
        """更新某节点检测结果"""
        if result.node_id not in self.results_buffer:
            self.results_buffer[result.node_id] = []
        self.results_buffer[result.node_id].append(result)
        
    def fuse(self) -> CPD_FusionResult:
        """
        融合多节点检测结果
        策略：任一节点检测到即触发，多节点一致提升置信度
        """
        positive_nodes = 0
        max_confidence = 0
        max_alert = CPD_AlertLevel.NONE
        
        for node_id, results in self.results_buffer.items():
            if not results:
                continue
            latest = results[-1]
            if latest.is_present:
                positive_nodes += 1
                max_confidence = max(max_confidence, latest.confidence)
                if latest.alert_level.value > max_alert.value:
                    max_alert = latest.alert_level
        
        # 多节点一致性加权
        total_nodes = len(self.results_buffer)
        consensus_weight = positive_nodes / max(total_nodes, 1)
        fused_confidence = max_confidence * (0.5 + 0.5 * consensus_weight)
        
        is_present = positive_nodes > 0 and fused_confidence > 0.5
        
        return CPD_FusionResult(
            is_present=is_present,
            confidence=fused_confidence,
            alert_level=max_alert if is_present else CPD_AlertLevel.NONE,
            detection_latency_ms=30,
        )


# 测试
if __name__ == "__main__":
    fusion = CPD_FusionManager(["roof", "rear_left", "rear_right"])
    
    # 模拟三个节点检测
    fusion.update(CPD_DetectionResult(
        sensor_type=CPD_SensorType.UWB,
        node_id="roof",
        is_present=True,
        confidence=0.85,
        breath_rate=0.3,
        heart_rate=1.2,
        timestamp=time.time(),
        cabin_temp=35.0,
        alert_level=CPD_AlertLevel.WARNING,
    ))
    
    fusion.update(CPD_DetectionResult(
        sensor_type=CPD_SensorType.UWB,
        node_id="rear_left",
        is_present=True,
        confidence=0.72,
        breath_rate=0.28,
        heart_rate=None,
        timestamp=time.time(),
        cabin_temp=35.0,
        alert_level=CPD_AlertLevel.WARNING,
    ))
    
    fusion.update(CPD_DetectionResult(
        sensor_type=CPD_SensorType.UWB,
        node_id="rear_right",
        is_present=False,
        confidence=0.15,
        breath_rate=None,
        heart_rate=None,
        timestamp=time.time(),
        cabin_temp=35.0,
        alert_level=CPD_AlertLevel.NONE,
    ))
    
    result = fusion.fuse()
    print(f"Present: {result.is_present}")
    print(f"Confidence: {result.confidence:.2%}")
    print(f"Alert Level: {result.alert_level.name}")