from threading import RLock

from dirigo import units
from dirigo.hw_interfaces.scanner import ObjectiveZScanner

from zaber_motion import Library, Units
from zaber_motion.ascii import Connection



class ZaberObjectiveZActuator(ObjectiveZScanner):
    def __init__(
        self,
        serial_port: str,
        device_number: int = 1,
        axis_number: int = 1,
        axis: str = "z",
        **kwargs
    ) -> None:
        
        # Check arguments
        if not serial_port:
            raise ValueError("`serial_port` must be a non-empty string.")
        if device_number < 1:
            raise ValueError("`device_number` must be >= 1.")
        if axis_number < 1:
            raise ValueError("`axis_number` must be >= 1.")

        super().__init__(axis=axis, **kwargs)

        self._lock = RLock()
        self._serial_port = serial_port
        self._device_number = device_number
        self._axis_number = axis_number

        self._connection = Connection.open_serial_port(serial_port)

        device_list = self._connection.detect_devices()
        if len(device_list) < 1:
            raise RuntimeError("No devices found on the connection")
        if len(device_list) > 1:
            raise RuntimeError("More than 1 device found on the connection")
        self._device = device_list[0]
        
        self._axis = self._device.get_axis(axis_number)

    @property
    def device_info(self) -> None:
        # TODO
        pass


    def close(self) -> None:
        with self._lock:
            self._connection.close()

    @property
    def position_limits(self) -> units.PositionRange:
        with self._lock:
            limit_min_mm = float(self._axis.settings.get("limit.min", Units.LENGTH_MILLIMETRES))
            limit_max_mm = float(self._axis.settings.get("limit.max", Units.LENGTH_MILLIMETRES))

        return units.PositionRange(
            min=f"{limit_min_mm} mm",
            max=f"{limit_max_mm} mm",
        )

    @property
    def position(self) -> units.Position:
        with self._lock:
            position_mm = float(self._axis.get_position(Units.LENGTH_MILLIMETRES))
        return units.Position(f"{position_mm} mm")

    @property
    def moving(self) -> bool:
        with self._lock:
            return bool(self._axis.is_busy())

    def move_to(self, position: units.Position, blocking: bool = False) -> None:
        if not isinstance(position, units.Position):
            raise ValueError("`position` must be a Position object.")
        
        position_mm = float(position) * 1e3
        with self._lock:
            self._axis.move_absolute(
                position_mm,
                Units.LENGTH_MILLIMETRES,
                wait_until_idle=blocking,
            )

    def move_relative(self, move_distance: units.Position, blocking: bool = False) -> None:
        if not isinstance(move_distance, units.Position):
            raise ValueError("`move_distance` must be a Position object.")
        
        distance_mm = float(move_distance) * 1e3
        with self._lock:
            self._axis.move_relative(
                distance_mm,
                Units.LENGTH_MILLIMETRES,
                wait_until_idle=blocking,
            )

    def move_velocity(self, velocity: units.Velocity) -> None:
        if not isinstance(velocity, units.Velocity):
            raise ValueError("`velocity` must be a Velocity object.")
        
        velocity_mm_s = float(velocity) * 1e3
        with self._lock:
            self._axis.move_velocity(
                velocity_mm_s,
                Units.VELOCITY_MILLIMETRES_PER_SECOND,
            )

    def stop(self) -> None:
        with self._lock:
            self._axis.stop()

    def home(self, blocking: bool = False) -> None:
        with self._lock:
            self._axis.home(wait_until_idle=blocking)

    @property
    def homed(self) -> bool:
        with self._lock:
            return bool(self._axis.is_homed())

    @property
    def max_velocity(self) -> units.Velocity:
        with self._lock:
            max_velocity_mm_s = float(
                self._axis.settings.get(
                    "maxspeed",
                    Units.VELOCITY_MILLIMETRES_PER_SECOND,
                )
            )
        return units.Velocity(f"{max_velocity_mm_s} mm/s")

    @max_velocity.setter
    def max_velocity(self, value: units.Velocity) -> None:
        if not isinstance(value, units.Velocity):
            raise ValueError("`max_velocity` must be set with a Velocity object.")
        
        max_velocity_mm_s = float(value) * 1e3
        with self._lock:
            self._axis.settings.set(
                "maxspeed",
                max_velocity_mm_s,
                Units.VELOCITY_MILLIMETRES_PER_SECOND,
            )

    @property
    def acceleration(self) -> units.Acceleration:
        with self._lock:
            acceleration_mm_s2 = float(
                self._axis.settings.get(
                    "accel",
                    Units.ACCELERATION_MILLIMETRES_PER_SECOND_SQUARED,
                )
            )
        return units.Acceleration(f"{acceleration_mm_s2} mm/s^2")

    @acceleration.setter
    def acceleration(self, value: units.Acceleration) -> None:
        if not isinstance(value, units.Acceleration):
            raise ValueError("`acceleration` must be set with an Acceleration object.")
        
        acceleration_mm_s2 = float(value) * 1e3
        with self._lock:
            self._axis.settings.set(
                "accel",
                acceleration_mm_s2,
                Units.ACCELERATION_MILLIMETRES_PER_SECOND_SQUARED,
            )
