import time
from dataclasses import dataclass

from drivers.modbus_device import ModbusDevice, ModbusDeviceError


@dataclass
class Position:
    x: int
    y: int
    z: int

    def __str__(self):
        return f"(x={self.x}, y={self.y}, z={self.z})mm"


class JigMovementError(ModbusDeviceError):
    pass


class Jig(ModbusDevice):
    def __init__(self, port, registers, slave_id=0x10):
        super().__init__(port, "Jig", registers, slave_id=slave_id)

    def move_to_position(self, position, wait_time=5):
        try:
            self.write_holding_register(self._addr("X POSITION"), position.x)
            self.write_holding_register(self._addr("Y POSITION"), position.y)
            self.write_holding_register(self._addr("Z POSITION"), position.z)
            self.write_coil(self._addr("MOVE TO POSITION"), True, verify=False)
            if wait_time > 0:
                time.sleep(wait_time)
        except ModbusDeviceError as exc:
            raise JigMovementError(f"failed to move to {position}") from exc

    def get_current_position(self):
        x = self.read_holding_register(self._addr("X POSITION"))
        y = self.read_holding_register(self._addr("Y POSITION"))
        z = self.read_holding_register(self._addr("Z POSITION"))
        return Position(x, y, z)