#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
# @Time    : 2026/2/26 14:07
# @Author  : reenrr
# @File    : servo_control.py
# @Desc    : 舵机对外控制接口
"""
import time

from servo_test import ServoController

# -------------参数配置--------------
BAUDRATE = 115200                        # 舵机的波特率
PORT = 'COM4'
SERVO_IDS = [1, 2, 3, 4, 5]              # 舵机们的 ID 号
POS_START = 2047
POS_END = 0
SPEED = 1500
ACC = 0

# 舵机参数换算常量（核心：0度=0，360度=4095）
DEGREE_TO_RAW_RATIO = 4095 / 360  # 1度对应的原始值
RAW_TO_DEGREE_RATIO = 360 / 4095  # 1个原始值对应的角度

custom_config = {
    'port': PORT,
    'baudrate': BAUDRATE,
    'servo_ids': SERVO_IDS,
    'pos_start': POS_START,
    'pos_end': POS_END,
    'speed': SPEED,
    'acc': ACC,
}


class ServoInterface:
    def __init__(self, config=None):
        self.servo_controller = ServoController(config)

        try:
            # 初始化串口
            self.servo_controller.setup_port()
            self.servo_controller.enable_all_servos()
            print("舵机控制器初始化成功（串口/舵机启用完成）")

        except Exception as e:
            raise RuntimeError(f"舵机初始化失败:{str(e)}") from e


# 全局变量
_Servo = ServoInterface(custom_config)


def _degree_to_raw(degree: int) -> int:
    """
    角度转原始值
    :param degree: 角度值(0-360)
    :return: 原始值(0-4095)
    """
    if not (0 <= degree <= 360):
        raise ValueError(f"角度值必须在0-360度之间，当前输入:{degree}")
    raw_value = int(degree * DEGREE_TO_RAW_RATIO)

    return max(0, min(raw_value, 4095))


def _raw_to_degree(raw: int) -> float:
    """
    原始值转角度
    :param raw: 原始值(0-4095)
    :return: 角度值(0-360)
    """
    return round(raw * RAW_TO_DEGREE_RATIO, 2)


def set_servo_speed(value: int):
    """
    设置舵机速度
    :param value: 速度值
    """
    global SPEED

    if 0 <= value <= 3400:
        SPEED = value
        print(f"舵机速度已设置为:{SPEED}")
    else:
        raise ValueError("速度值超出限定范围")


def set_servo_acceleration(value: int):
    """
    设置舵机加速度
    :param value: 加速度值
    """
    global ACC
    if 0 <= value <= 254:
        ACC = value
        print(f"舵机加速度已设置为:{ACC}")
    else:
        raise ValueError("加速度值超出限定范围")


def set_servo_ori_position(ori_position: int):
    """
    设置舵机原点位
    :param ori_position: 舵机原点位
    """
    global POS_START
    POS_START = _degree_to_raw(ori_position)
    print(f"舵机原点位置已设置为：{ori_position}度（对应原始值：{POS_START}）")


def set_servo_rot_position(rot_position: int):
    """
    设置舵机翻转位置
    :param rot_position: 舵机翻转位
    """
    global POS_END
    POS_END = _degree_to_raw(rot_position)
    print(f"舵机翻转位置已设置为：{rot_position}度（对应原始值：{POS_END}）")


def move_to_rot_position():
    """舵机旋转到翻转位置"""
    try:
        _Servo.servo_controller.write_position(POS_END, SPEED, ACC)
        time.sleep(2)
        target_degree = _raw_to_degree(POS_END)
        print(f"舵机已移动到翻转位置:{target_degree}度（原始值:{POS_END}）")

    except Exception as e:
        raise RuntimeError(f"舵机移动到翻转位置失败:{str(e)}") from e


def move_to_ori_position():
    """舵机旋转到原点"""
    try:
        _Servo.servo_controller.write_position(POS_START, SPEED, ACC)
        time.sleep(2)
        target_degree = _raw_to_degree(POS_START)
        print(f"舵机已移动到原点位置:{target_degree}度（原始值:{POS_START}）")

    except Exception as e:
        raise RuntimeError(f"舵机移动到原点位置失败:{str(e)}") from e


# ----------调试接口----------
if __name__ == '__main__':
    set_servo_speed(1500)
    set_servo_acceleration(0)
    set_servo_ori_position(0)
    set_servo_rot_position(180)

    move_to_rot_position()      # 旋转180度
    time.sleep(1)
    # move_to_ori_position()      # 旋转0度
    # time.sleep(1)
    
    while True:
        time.sleep(1)