import numpy as np
import torch
import torch.nn as nn
from sklearn.ensemble import RandomForestClassifier
import xgboost as xgb

class VsESM:
    """
    Valid sEMG Selection Machine
    
    论文核心方法：从噪声sEMG数据中筛选有效信号
    
    问题：方向盘sEMG受机械振动、手部移动影响，大量噪声
    方案：PU Learning + XGBoost半监督筛选
    """
    
    def __init__(self, n_features=20):
        self.n_features = n_features
        self.feature_extractor = SEMGFeatureExtractor()
        self.pu_learner = XGBClassifier(use_label_encoder=False)
        
    def fit(self, raw_sEMG, valid_samples, unlabeled_samples):
        """
        Args:
            raw_sEMG: (N, T) 原始sEMG信号
            valid_samples: 确认有效的样本索引（P集）
            unlabeled_samples: 未标注样本（U集）
        """
        # 提取特征
        features = self.feature_extractor.transform(raw_sEMG)
        
        # PU Learning: 将U集中不似P的标记为"可能的噪声"
        P = features[valid_samples]
        U = features[unlabeled_samples]
        
        # 训练XGBoost区分P和U
        X = np.vstack([P, U])
        y = np.concatenate([np.ones(len(P)), np.zeros(len(U))])
        self.pu_learner.fit(X, y)
        
        # 从U中筛选出"似P"的样本
        probas = self.pu_learner.predict_proba(U)[:, 1]
        self.threshold = np.percentile(probas, 50)  # 中位数阈值
    
    def transform(self, raw_sEMG):
        """筛选有效sEMG段"""
        features = self.feature_extractor.transform(raw_sEMG)
        probas = self.pu_learner.predict_proba(features)[:, 1]
        valid_mask = probas > self.threshold
        return valid_mask


class SEMGFeatureExtractor:
    """sEMG特征提取：统计特征 + 二阶特征"""
    
    def transform(self, sEMG_signal):
        """
        Args:
            sEMG_signal: (N, T) 多通道sEMG
        Returns:
            features: (N, n_features)
        """
        features = []
        for ch in range(sEMG_signal.shape[1]):
            signal = sEMG_signal[:, ch]
            
            # 一阶统计特征
            mean = np.mean(signal)
            std = np.std(signal)
            rms = np.sqrt(np.mean(signal**2))
            max_val = np.max(np.abs(signal))
            
            # 二阶特征（本文创新）
            # Slope: 信号变化斜率
            slope = np.diff(signal).mean()
            # Absolute distance: 信号幅度面积变化
            sma = np.sum(np.abs(signal))
            abs_dist = np.abs(sma - np.mean(sma))
            
            # Lempel-Ziv复杂度斜率（关键疲劳指标）
            lz_complexity = self._lemel_ziv_complexity(signal)
            
            features.extend([mean, std, rms, max_val, 
                            slope, abs_dist, lz_complexity])
        
        return np.array(features)
    
    def _lemel_ziv_complexity(self, signal, threshold=None):
        """计算Lempel-Ziv复杂度"""
        if threshold is None:
            threshold = np.mean(signal)
        binary = (signal > threshold).astype(int)
        binary_str = ''.join(map(str, binary))
        
        # LZ复杂度计算
        n = len(binary_str)
        c = 1
        i = 0
        while i < n:
            j = 1
            while i + j <= n:
                if binary_str[i:i+j] in binary_str[:i+j-1]:
                    j += 1
                else:
                    break
            c += 1
            i += j - 1
        
        # 归一化
        import math
        return c * np.log2(n) / n if n > 0 else 0


# 疲劳检测分类器
class SteeringWheelFatigueDetector:
    """
    方向盘sEMG疲劳检测完整管道
    
    论文实现: sEMG → VsESM → 特征提取 → Random Forest → 疲劳/清醒
    """
    
    def __init__(self):
        self.vsesm = VsESM()
        self.extractor = SEMGFeatureExtractor()
        self.classifier = RandomForestClassifier(
            n_estimators=100, max_depth=10, random_state=42
        )
        
    def fit(self, raw_sEMG, labels, valid_indices, unlabeled_indices):
        """
        Args:
            raw_sEMG: (N, T, C) 原始信号
            labels: (N,) 疲劳标签（0=清醒, 1=疲劳）
            valid_indices: 已确认有效段
            unlabeled_indices: 未标注段
        """
        # 1. 训练VsESM
        self.vsesm.fit(raw_sEMG, valid_indices, unlabeled_indices)
        
        # 2. 筛选有效段
        valid_mask = self.vsesm.transform(raw_sEMG)
        
        # 3. 提取特征
        features = self.extractor.transform(raw_sEMG[valid_mask])
        valid_labels = labels[valid_mask]
        
        # 4. 训练分类器
        self.classifier.fit(features, valid_labels)
        
    def predict(self, raw_sEMG):
        """预测疲劳状态"""
        valid_mask = self.vsesm.transform(raw_sEMG)
        if not np.any(valid_mask):
            return 0  # 无有效信号，默认清醒
        features = self.extractor.transform(raw_sEMG[valid_mask])
        return self.classifier.predict(features)
    
    def predict_proba(self, raw_sEMG):
        """疲劳概率"""
        valid_mask = self.vsesm.transform(raw_sEMG)
        if not np.any(valid_mask):
            return 0.0
        features = self.extractor.transform(raw_sEMG[valid_mask])
        return self.classifier.predict_proba(features)[:, 1].mean()


# 测试
if __name__ == "__main__":
    detector = SteeringWheelFatigueDetector()
    
    # 模拟数据
    np.random.seed(42)
    # 清醒状态：高频小幅振动
    alert_sEMG = np.random.normal(0.5, 0.15, (1000, 100, 3))
    # 疲劳状态：低频大幅振动（肌肉代偿）
    fatigued_sEMG = np.random.normal(0.3, 0.25, (1000, 100, 3))
    
    X = np.vstack([alert_sEMG, fatigued_sEMG])
    y = np.concatenate([np.zeros(1000), np.ones(1000)])
    
    # 训练
    valid_idx = np.random.choice(2000, 500, replace=False)
    unlabeled_idx = np.setdiff1d(np.arange(2000), valid_idx)
    detector.fit(X, y, valid_idx, unlabeled_idx)
    
    # 测试
    test_sample = np.random.normal(0.35, 0.20, (10, 100, 3))  # 偏疲劳
    pred = detector.predict(test_sample)
    proba = detector.predict_proba(test_sample)
    
    print(f"预测: {'疲劳' if pred[0] == 1 else '清醒'}")
    print(f"疲劳概率: {proba:.2f}")
    print("✅ 方向盘sEMG疲劳检测管道验证完成")