import pydobot
from serial.tools import list_ports


class DobotMeasurer:
    def __init__(self, verbose=False):
        self.Z_UP = 0
        self.Z_DOWN = -25
        port = self.find_port()
        self.device = pydobot.Dobot(port=port, verbose=verbose)
        if self.device is None:
            raise RuntimeError("Failed to initialize Dobot.")
        print(f"Dobot found on {port}")

    def __del__(self):
        self.close()

    def init(self):
        if self.device is not None:
            self.close()
        self.device = pydobot.Dobot(port=self.port, verbose=self.verbose)
        if self.device is None:
            raise RuntimeError("Failed to initialize Dobot.")
        # Set initial position to Z_UP
        (x, y, z, r, j1, j2, j3, j4) = self.device.pose()
        if z < 0:
            self.device.move_to(x + 3, y, self.Z_UP, 0, wait=True)

    def find_port(self):
        available_ports = list_ports.comports()
        dobot_port = [x for x in available_ports if "CH340" in x.description]
        if not dobot_port:
            raise RuntimeError(
                "No Dobot port found. Please connect the Dobot and try again."
            )
        return dobot_port[0].device

    def set_speed(self, velocity=100.0, acceleration=100.0):
        if self.device is None:
            raise RuntimeError("Dobot not initialized.")
        self.device.speed(velocity, acceleration)

    def up(self, wait=True):
        if self.device is None:
            raise RuntimeError("Dobot not initialized.")
        (x, y, z, r, j1, j2, j3, j4) = self.device.pose()
        self.device.move_to(x, y, self.Z_UP, 0, wait)

    def down(self, wait=True):
        if self.device is None:
            raise RuntimeError("Dobot not initialized.")
        (x, y, z, r, j1, j2, j3, j4) = self.device.pose()
        self.device.move_to(x, y, self.Z_DOWN, 0, wait)

    def move(self, x, y, wait=True):
        if self.device is None:
            raise RuntimeError("Dobot not initialized.")
        self.device.move_to(x, y, self.Z_UP, 0, wait=wait)

    def wait(self, ms):
        if self.device is None:
            raise RuntimeError("Dobot not initialized.")
        self.device.wait(ms)

    def close(self):
        if self.device is not None:
            self.device.close()
            self.device = None


if __name__ == "__main__":
    from config import CC_XY as xy

    dobot = DobotMeasurer()
    dobot.up()
    num_cc = len(xy)
    for i in range(num_cc):
        print(f"Moving to point {i + 1}: {xy[i]}")
        dobot.move(xy[i][0], xy[i][1])
        dobot.down()
        dobot.wait(1000)  # Wait for 1 second
        dobot.up()