import time
import logging
from typing import override

from unitree_actuator_sdk import *

from utils import timeit

from motor_instance import MotorInstance

logger = logging.getLogger(__file__.split("/")[-1])


class A1Motor(MotorInstance):
    motor_name: str
    serial_path: str
    id: int
    motor_mode: MotorMode
    motor_type: MotorType
    serial: SerialPort

    motor_cmd: MotorCmd = MotorCmd()
    motor_data: MotorData = MotorData()

    # motor init value
    _tau: float = 0
    _dq: float = 0
    _q: float = 0
    _kp: float = 0
    _kd: float = 0

    def __init__(
        self,
        serial_path: str,
        id: int,
        motor_name: str = None,
        mode: MotorMode = MotorMode.CALIBRATE,
        motor_type: MotorType = MotorType.A1,
        tau: float = 0,
        dq: float = 0,
        q: float = 0,
        kp: float = 0,
        kd: float = 0,
    ):
        self.serial_path = serial_path
        self.motor_id = id
        self.motor_mode = mode
        self.motor_type = motor_type
        self.motor_name = "-".join([self.serial_path, str(id), str(int(time.time()))]) if motor_name is None else motor_name
        self.serial = SerialPort(self.serial_path)
        self.reduction_ratio = queryGearRatio(self.motor_type)
        self._tau = tau
        self._dq = dq
        self._q = q
        self._kp = kp
        self._kd = kd
        self.init_motor_cmd()
        self.init_motor_data()

    def init_motor_cmd(self):
        motor_cmd = MotorCmd()
        motor_cmd.motorType = self.motor_type
        motor_cmd.mode = queryMotorMode(self.motor_type, self.motor_mode)
        motor_cmd.id = self.motor_id
        motor_cmd.tau = self._tau
        motor_cmd.dq = self._dq
        motor_cmd.q = self._q
        motor_cmd.kp = self._kp
        motor_cmd.kd = self._kd
        self.motor_cmd = motor_cmd

    def init_motor_data(self):
        motor_data = MotorData()
        motor_data.motorType = self.motor_type
        motor_data.mode = queryMotorMode(self.motor_type, self.motor_mode)
        motor_data.motor_id = self.motor_id
        motor_data.tau = 0
        motor_data.dq = 0
        motor_data.q = 0
        motor_data.temp = 0
        self.motor_data = motor_data

    @property
    def tau(self):
        return self.motor_data.tau

    @tau.setter
    def tau(self, value):
        if abs(value) < 128:
            self.motor_cmd.tau = value
        else:
            raise Exception(f"tau value invalid: {value}")

    @property
    def dq(self):
        return self.motor_data.dq

    @dq.setter
    def dq(self, value):
        if abs(value) < 256:
            self.motor_cmd.dq = value * self.reduction_ratio
        else:
            raise Exception(f"dq value invalid: {value}")

    @property
    def q(self):
        return self.motor_data.q

    @q.setter
    def q(self, value):
        if abs(value) < 823549:
            self.motor_cmd.q = value * self.reduction_ratio
        else:
            raise Exception(f"q value invalid: {value}")

    @property
    def kp(self):
        return self.motor_cmd.kp

    @kp.setter
    def kp(self, value):
        if 0 <= value and value < 16:
            self.motor_cmd.kp = value
        else:
            raise Exception(f"kp value invalid: {value}")

    @property
    def kd(self):
        return self.motor_cmd.kd

    @kd.setter
    def kd(self, value):
        if 0 <= value and value < 32:
            self.motor_cmd.kd = value
        else:
            raise Exception(f"kd value invalid: {value}")

    @property
    def temp(self):
        return self.motor_data.temp

    @override
    @timeit
    def reset(self):
        self.init_motor_cmd()
        self.sendrecv(self.motor_cmd)

    @timeit
    def send_pingpong(self):
        pass

    @override
    def get_motor_name(self):
        return self.motor_name

    @override
    @timeit
    def sendrecv(self, cmd: MotorCmd) -> MotorData:
        data: MotorData = MotorData()
        self.serial.sendRecv(cmd, data)
        self.motor_data = data
        return data