import time

from unitree_actuator_sdk import *


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

    motor_cmd: MotorCmd
    motor_data: MotorData

    def __init__(
        self,
        serial_path: str,
        id: int,
        motor_name: str = None,
        mode: MotorMode = MotorMode.CALIBRATE,
        motor_type: MotorType = MotorType.A1,
    ):
        self.serial_path = serial_path
        self.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)

    def reset(self):
        pass

    def send_pingpong(self):
        pass

    def get_motor_name(self):
        return self.motor_name

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