# 确保你已安装 python3 和 pip
pip install numpy

# 预期输出：
# Successfully installed numpy-x.x.x

# src/data_analysis.py
import numpy as np

# dt: 传感器读数之间的时间差
dt = 0.01 

def get_accel_angle(ax, ay, az):
  """从加速度计数据计算倾斜角度。"""
  # 注意：这是针对单轴的简化计算。
  # 完整实现会使用 atan2(ay, az) 计算俯仰角等。
  return np.arctan2(ay, np.sqrt(ax**2 + az**2)) * 180 / np.pi

# 陀螺仪数据需要积分
gyro_angle_x = 0.0

def get_gyro_angle(gx, dt):
  """通过积分陀螺仪数据计算角度。"""
  global gyro_angle_x
  gyro_angle_x += gx * dt
  return gyro_angle_x

# --- 让我们模拟一些读数 ---
# 模拟二头弯举：角度从 -90 到 0 再回来
true_angle = -90
gyro_drift = 0.1 # 一个小的恒定漂移
accel_noise_stddev = 2.5 # 加速度计读数中的噪声

# 存储结果
simulated_gyro_angles = []
simulated_accel_angles = []

for _ in range(300): # 模拟 3 秒的数据
    # 模拟平滑的弯举动作
    true_angle += 0.6
    if true_angle > 0: true_angle = 0 # 顶部封顶
    
    # 生成有噪声的加速度计数据
    accel_reading_y = np.sin(np.deg2rad(true_angle)) + np.random.normal(0, accel_noise_stddev)
    accel_reading_z = np.cos(np.deg2rad(true_angle)) + np.random.normal(0, accel_noise_stddev)
    
    # 生成漂移的陀螺仪数据
    gyro_reading_x = 0.6 / dt # 理想角速度
    gyro_reading_x += gyro_drift # 添加漂移
    
    # 计算角度
    accel_angle = get_accel_angle(0, accel_reading_y, accel_reading_z)
    gyro_angle = get_gyro_angle(gyro_reading_x, dt)

    simulated_accel_angles.append(accel_angle)
    simulated_gyro_angles.append(gyro_angle)

print(f"最终陀螺仪角度（含漂移）: {simulated_gyro_angles[-1]:.2f}")
print(f"最终加速度计角度（应有噪声但围绕0）: {simulated_accel_angles[-1]:.2f}")

// src/fusion_filter.py
import numpy as np

class ComplementaryFilter:
    def __init__(self, alpha=0.98):
        self.alpha = alpha
        self.angle = 0.0

    def process(self, ax, ay, az, gx, dt):
        """
        处理新的传感器数据以更新融合角度。
        """
        # 从加速度计计算角度
        accel_angle = np.arctan2(ay, np.sqrt(ax**2 + az**2)) * 180 / np.pi
        
        # 积分陀螺仪数据获得角度变化
        gyro_angle_change = gx * dt
        
        # 应用互补滤波器
        # 信任陀螺仪的短期变化，用加速度计校正
        self.angle = self.alpha * (self.angle + gyro_angle_change) + (1 - self.alpha) * accel_angle
        
        return self.angle

# --- 让我们用滤波器重新运行模拟 ---
filter = ComplementaryFilter(alpha=0.98)
fused_angles = []

# 重置模拟变量
gyro_angle_x = 0.0 

for _ in range(300):
    # （复用步骤一相同的模拟逻辑）
    true_angle += 0.6
    if true_angle > 0: true_angle = 0
    accel_reading_y = np.sin(np.deg2rad(true_angle)) + np.random.normal(0, accel_noise_stddev)
    accel_reading_z = np.cos(np.deg2rad(true_angle)) + np.random.normal(0, accel_noise_stddev)
    gyro_reading_x = (0.6 / dt) + gyro_drift
    
    fused_angle = filter.process(0, accel_reading_y, accel_reading_z, gyro_reading_x, dt)
    fused_angles.append(fused_angle)

print(f"最终融合角度: {fused_angles[-1]:.2f}")

# src/rep_counter.py

class RepCounter:
    def __init__(self, entry_threshold, exit_threshold):
        self.count = 0
        self.state = "IDLE" # 可以是 IDLE, GOING_UP, GOING_DOWN
        self.entry_threshold = entry_threshold # 开始一个动作的角度
        self.exit_threshold = exit_threshold   # 完成一个动作的角度

    def process(self, angle):
        """
        处理新角度以更新动作计数。
        返回当前动作计数。
        """
        if self.state == "IDLE":
            if angle > self.entry_threshold:
                self.state = "GOING_UP"
        
        elif self.state == "GOING_UP":
            if angle < self.exit_threshold:
                self.state = "GOING_DOWN"
                
        elif self.state == "GOING_DOWN":
            if angle >= self.exit_threshold:
                self.count += 1
                print(f"检测到第 {self.count} 次动作！")
                self.state = "IDLE"
                
        return self.count

# --- 将所有内容组合 ---
# 对于二头弯举，一个动作可能在手臂通过 45 度时开始
# 在接近顶部时结束（例如 140 度）。
# 让我们定义融合角度空间的阈值。
# 假设融合角度起始约 0，峰值约 90。
rep_counter = RepCounter(entry_threshold=30.0, exit_threshold=75.0)

# 模拟两个完整动作
full_motion_fused_angles = fused_angles + list(reversed(fused_angles)) # 一个动作
full_motion_fused_angles += full_motion_fused_angles # 两个动作

for angle in full_motion_fused_angles:
    rep_counter.process(angle)

print(f"\n最终动作计数: {rep_counter.count}")