"""
============================================================================
圆柱水槽控制系统 — 精简演示脚本
============================================================================

本脚本演示三个驱动模块的基本用法，适合第一次接触本项目时快速了解全貌。

运行方式：
    python3 demo.py

注意事项：
    ⚠️  所有涉及真实硬件的操作都会打印 [需要硬件] 标记。
        如果你没有连接实际硬件，可以阅读代码和打印输出来了解流程。
============================================================================
"""

# ============================================================
# 第一步：导入依赖
# ============================================================
# 导入时间模块，用于延时和计时
import time
# 导入数学模块，用于数学计算（如正弦函数）
import math

# 导入三个驱动模块（请确保这些 .py 文件与本脚本在同一目录下）
# 编码器电机驱动 — 控制圆柱旋转
from drv_encodermotor import EncoderMotorDriver, SPEED_MODE
# 步进电机驱动 — 控制圆柱平移
from drv_stepmotor import StepMotorDriver
# ADC 驱动 — 采集应变片电压信号
from drv_adc import ADS124S08


# ============================================================
# 第二步：EncoderMotor（编码器电机）演示 — 控制圆柱旋转
# ============================================================
def demo_encodermotor():
    """
    演示编码器电机的速度控制流程。
    通过 I2C 总线与 M5Stack 4EncoderMotor 模块通信。
    """
    print("=" * 60)
    print("【EncoderMotor】编码器电机演示 — 控制圆柱旋转")
    print("=" * 60)

    # 使用 with 语句创建驱动实例，退出后自动释放 I2C 资源
    # bus=1 表示使用树莓派的 I2C 总线 1（/dev/i2c-1）
    with EncoderMotorDriver(bus=1) as driver:

        # 1. 设置通道 0 为速度模式（SPEED_MODE）
        #    速度模式下，电机会自动维持设定的目标转速
        print("  [需要硬件] 设置通道 0 为速度模式...")
        driver.set_mode(0, SPEED_MODE)

        # 2. 配置速度 PID 参数
        #    kp（比例）、ki（积分）、kd（微分）配合使速度控制更精确
        print("  [需要硬件] 配置 PID 参数 (kp=1, ki=100, kd=1)...")
        driver.set_speed_pid(0, kp=1, ki=100, kd=1)

        # 3. 启用软启停功能，让电机加减速更平滑
        print("  [需要硬件] 启用软启停...")
        driver.set_soft_start_and_stop(0, True)

        # 4. 设置目标线速度（单位：米/秒）
        #    正数 = 正转，负数 = 反转
        print("  [需要硬件] 设定目标速度：0.02 m/s")
        driver.set_linear_speed_m_s(0, 0.02)

        # 等待 1 秒让电机跑起来
        time.sleep(1)

        # 5. 读取编码器数值（电机转过的脉冲数）
        print("  [需要硬件] 读取编码器数值...")
        encoder_val = driver.get_encoder_value(0)
        print(f"    编码器数值：{encoder_val}")

        # 6. 读取电机总电流
        print("  [需要硬件] 读取电机电流...")
        current = driver.get_motor_current()
        print(f"    总电流：{current:.4f} A")

        # 7. 速度归零，停止电机
        print("  [需要硬件] 停止电机...")
        driver.set_linear_speed_m_s(0, 0.0)
        time.sleep(1)

    # 退出 with 语句块，I2C 连接自动关闭
    print("  [完成] EncoderMotor 演示结束，资源已释放\n")


# ============================================================
# 第三步：StepMotor（步进电机）演示 — 控制圆柱平移
# ============================================================
def demo_stepmotor():
    """
    演示步进电机的控制流程。
    通过 I2C + 硬件 PWM 与 M5Stack StepMotor Driver 模块通信。
    三个步进电机共享同一路 PWM 脉冲，但方向可独立控制。
    """
    print("=" * 60)
    print("【StepMotor】步进电机演示 — 控制圆柱平移")
    print("=" * 60)

    # 创建驱动实例（这里演示用 try/finally 手动释放资源）
    drv = StepMotorDriver()
    try:
        # 1. 使能电机 — 让电机驱动芯片开始接收脉冲
        print("  [需要硬件] 使能电机...")
        drv.enable_motor()

        # 2. 复位所有 3 个电机通道（恢复到初始状态）
        print("  [需要硬件] 复位所有电机通道...")
        for i in range(3):
            drv.reset_motor(i, True)

        # 3. 设置运动方向（True=正向，False=反向）
        print("  [需要硬件] 设置方向为正向...")
        drv.set_dir(True)

        # 4. S 曲线加速启动（速度 0.02 m/s，用 0.1 秒加速到目标）
        #    加速曲线用了余弦函数，非常平滑
        drv.ramp_time = 0.1  # 为了演示，把加减速时间设短
        print("  [需要硬件] S 曲线加速启动 (0.02 m/s)...")
        drv.start(0.02)
        time.sleep(1)

        # 5. 在运行中切换速度（0.04 m/s）
        print("  [需要硬件] 切换速度 (0.04 m/s)...")
        drv.change_speed(0.04)
        time.sleep(1)

        # 6. S 曲线减速停止
        print("  [需要硬件] S 曲线减速停止...")
        drv.stop()

        print("  [完成] StepMotor 演示结束\n")

    finally:
        # 释放资源（停止电机、关闭 I2C 和 PWM）
        drv.close()


# ============================================================
# 第四步：ADC（模数转换器）演示 — 采集电压信号
# ============================================================
def demo_adc():
    """
    演示 ADC 的数据采集流程。
    通过 SPI 总线与 ADS124S08 芯片通信，读取应变片电压信号。
    """
    print("=" * 60)
    print("【ADC】模数转换器演示 — 采集电压信号")
    print("=" * 60)

    # 创建 ADC 实例（构造函数会自动完成上电和初始化）
    adc = ADS124S08()
    try:
        # 1. 配置 PGA（可编程增益放大器）
        #    pga_en=1 启用放大，gain=5 对应 ×32 倍
        print("  [需要硬件] 配置 PGA (×32 倍)...")
        adc.set_pga(pga_en=1, gain=5)

        # 2. 配置数据速率和转换模式
        #    dr=8 对应 200 SPS（每秒 200 次采样）
        #    mode=1 单次转换模式（每次触发转换一次）
        print("  [需要硬件] 配置数据速率 (200 SPS, 单次模式)...")
        adc.set_datarate(dr=8, mode=1)

        # 3. 读取三个通道（0, 1, 2）的数据
        #    返回 24 位有符号整数的列表
        print("  [需要硬件] 读取通道 0/1/2...")
        channels = [0, 1, 2]
        raw_values = adc.request_channels(channels=channels)
        print(f"    原始采样值：{raw_values}")

        # 4. 将 ADC 原始值转换为电压
        print("    转换为电压值：")
        for ch, raw in zip(channels, raw_values):
            voltage = adc.convert_to_voltage(raw)
            print(f"      通道 {ch}：{voltage:.6f} V")

        print("  [完成] ADC 演示结束\n")

    finally:
        # 关闭 SPI 总线，释放 GPIO 引脚
        adc.close()


# ============================================================
# 第五步：信号生成演示（纯数学计算，无需硬件）
# ============================================================
def demo_signal_generation():
    """
    演示如何根据频率分量合成周期运动信号。
    这是控制循环中的核心算法，不需要硬件即可运行。
    """
    print("=" * 60)
    print("【信号合成】周期运动信号生成演示（纯软件，无需硬件）")
    print("=" * 60)

    # 定义一组简化的信号特征（包含均值 + 两个正弦波分量）
    signal_feature = {
        "mean": 0.0,                      # 均值（直流分量）
        "components": [                    # 正弦波分量列表
            (0.27, 0.005, -1.5),          # (频率Hz, 幅值, 相位rad)
            (0.81, 0.001,  2.8),          # (频率Hz, 幅值, 相位rad)
        ]
    }

    # 在 3 秒内每隔 0.5 秒生成一个信号值
    print("  时间(s) |  信号值(m/s) ")
    print("  -------|-------------")
    t = 0.0
    while t <= 3.0:
        # 合成信号：均值 + 各正弦分量之和
        value = signal_feature["mean"]
        for freq, amp, phase in signal_feature["components"]:
            value += amp * math.cos(2 * math.pi * freq * t + phase)
        print(f"  {t:5.1f}   |  {value:+.6f}")
        t += 0.5

    print("  [完成] 信号合成演示结束\n")


# ============================================================
# 主程序入口
# ============================================================
if __name__ == "__main__":
    print()
    print("⏺  圆柱水槽控制系统 — 精简演示")
    print("⏺  本脚本将逐个演示三个驱动模块和信号生成功能")
    print()

    # 逐个执行演示（按顺序注释掉可以单独运行某个演示）
    demo_encodermotor()
    demo_stepmotor()
    demo_adc()
    demo_signal_generation()

    print("=" * 60)
    print("所有演示已完成。")
    print("提示：运行 test.ipynb 可以获得更详细的分步讲解。")
    print("=" * 60)
    print()