# -190000 190000
# -180000 0   90  -55000 - -185000 = 130000
# -210000 0   90  -110000 - -240000 = 130000
import time
from src.ZDT_Servo import Emm42Driver

def test_three_motors():
    # 初始化三个电机（假设串口号为COM3-COM5）
    motors = [
        Emm42Driver(port="/dev/ttyAMA5", device_id=1),
        Emm42Driver(port="/dev/ttyAMA5", device_id=2), 
        Emm42Driver(port="/dev/ttyAMA5", device_id=3)
    ]
    
    try:
        # 能所有电机

        for motor in motors:
            motor.enable_motor(enable=False)

        
        # 初始化位置极值记录
        position_stats = [
            {"min": float('inf'), "max": float('-inf')},  # 电机1
            {"min": float('inf'), "max": float('-inf')},  # 电机2
            {"min": float('inf'), "max": float('-inf')}   # 电机3
        ]
        
        # 无限循环读取位置，按q退出
        print("Reading positions (press 'q' to quit)...")
        try:
            while True:
                for i, motor in enumerate(motors):
                    pos = motor.read_position()
                    if pos is not None:
                        position_stats[i]["min"] = min(position_stats[i]["min"], pos)
                        position_stats[i]["max"] = max(position_stats[i]["max"], pos)
                
                # 非阻塞检测键盘输入
                try:
                    import sys
                    import select
                    if sys.stdin in select.select([sys.stdin], [], [], 0)[0]:
                        key = sys.stdin.read(1)
                        if key.lower() == 'q':
                            break
                except:
                    pass
                
                time.sleep(0.1)
        
        except KeyboardInterrupt:
            pass
        
        # 打印结果
        print("\nPosition statistics:")
        for i, stats in enumerate(position_stats, 1):
            print(f"Motor {i}: min={stats['min']}, max={stats['max']}")
        
    finally:
        # 失能所有电机
        print("\nDisabling motors...")
        for motor in motors:
            motor.enable_motor(enable=False)
            time.sleep(0.1)

if __name__ == "__main__":
    test_three_motors()