From f4ca0d837058b8320c3ef0a747d6cab2f7667b9c Mon Sep 17 00:00:00 2001
From: Sindre Hansen <sindrehansen92@gmail.com>
Date: Thu, 17 Oct 2024 10:18:21 +0200
Subject: [PATCH] Add Laser peripheral

---
 blueye/sdk/drone.py       |  3 +++
 blueye/sdk/guestport.py   | 32 ++++++++++++++++++++++++++++++++
 tests/test_peripherals.py | 22 ++++++++++++++++++++++
 3 files changed, 57 insertions(+)

diff --git a/blueye/sdk/drone.py b/blueye/sdk/drone.py
index 410a5f03..a1d5e138 100755
--- a/blueye/sdk/drone.py
+++ b/blueye/sdk/drone.py
@@ -20,6 +20,7 @@
     Gripper,
     GuestPortCamera,
     GuestPortLight,
+    Laser,
     Peripheral,
     device_to_peripheral,
 )
@@ -269,6 +270,8 @@ def _create_peripherals_from_drone_info(self, gp_info: blueye.protocol.GuestPort
                     self.external_camera = peripheral
                 elif isinstance(peripheral, Gripper):
                     self.gripper = peripheral
+                elif isinstance(peripheral, Laser):
+                    self.laser = peripheral
 
     def connect(
         self,
diff --git a/blueye/sdk/guestport.py b/blueye/sdk/guestport.py
index ef507d61..293aaf6e 100644
--- a/blueye/sdk/guestport.py
+++ b/blueye/sdk/guestport.py
@@ -130,6 +130,33 @@ def rotation_velocity(self, value: float):
         )
 
 
+class Laser(Peripheral):
+    def __init__(
+        self, parent_drone: "Drone", port_number: bp.GuestPortNumber, device: bp.GuestPortDevice
+    ):
+        Peripheral.__init__(self, parent_drone, port_number, device)
+
+    def set_intensity(self, intensity: float):
+        """
+        Sets the intensity of the laser
+
+        If the laser does not support dimming but only on and off, a
+        value of 0 turns the laser off, and any value above 0 turns the
+        laser on.
+        """
+        if intensity < 0 or intensity > 1:
+            raise ValueError("Laser intensity must be between 0 and 1.")
+        self.parent_drone._ctrl_client.set_laser_intensity(intensity)
+
+    def get_intensity(self) -> Optional[float]:
+        """Returns the current intensity of the laser (0..1)"""
+        telemetry_msg = self.parent_drone.telemetry.get(bp.LaserTel)
+        if telemetry_msg is None:
+            return None
+        else:
+            return telemetry_msg.laser.value
+
+
 def device_to_peripheral(
     parent_drone: "Drone", port_number: bp.GuestPortNumber, device: bp.GuestPortDevice
 ) -> Peripheral:
@@ -149,6 +176,11 @@ def device_to_peripheral(
         or device.device_id == bp.GuestPortDeviceID.GUEST_PORT_DEVICE_ID_BLUEPRINT_LAB_REACH_ALPHA
     ):
         peripheral = Gripper(parent_drone, port_number, device)
+    elif (
+        device.device_id == bp.GuestPortDeviceID.GUEST_PORT_DEVICE_ID_LASER_TOOLS_SEA_BEAM
+        or device.device_id == bp.GuestPortDeviceID.GUEST_PORT_DEVICE_ID_SPOT_X_LASER_SCALERS
+    ):
+        peripheral = Laser(parent_drone, port_number, device)
     else:
         peripheral = Peripheral(parent_drone, port_number, device)
     return peripheral
diff --git a/tests/test_peripherals.py b/tests/test_peripherals.py
index 8b4ef026..379cc59d 100644
--- a/tests/test_peripherals.py
+++ b/tests/test_peripherals.py
@@ -1,4 +1,5 @@
 import blueye.protocol as bp
+import pytest
 
 import blueye.sdk
 
@@ -19,3 +20,24 @@ def test_peripheral_attribute_setter(mocked_drone):
     mocked_drone._create_peripherals_from_drone_info(gp_info)
     assert isinstance(mocked_drone.external_camera, blueye.sdk.guestport.GuestPortCamera)
     assert isinstance(mocked_drone.external_light, blueye.sdk.guestport.GuestPortLight)
+
+
+def test_laser_peripheral(mocked_drone):
+    spotx_laser = bp.GuestPortDevice(
+        {"device_id": bp.GuestPortDeviceID.GUEST_PORT_DEVICE_ID_SPOT_X_LASER_SCALERS}
+    )
+    gp_info = bp.GuestPortInfo()
+    gp_info.gp1 = bp.GuestPortConnectorInfo()
+    gp_info.gp1.device_list = bp.GuestPortDeviceList()
+    gp_info.gp1.device_list.devices.append(spotx_laser)
+
+    mocked_drone._create_peripherals_from_drone_info(gp_info)
+    assert isinstance(mocked_drone.laser, blueye.sdk.guestport.Laser)
+    mocked_drone.laser.set_intensity(0.5)
+    mocked_drone._ctrl_client.set_laser_intensity.assert_called_once_with(0.5)
+    laser_tel = bp.LaserTel(laser=bp.Laser(value=1))
+    laser_tel_msg = bp.LaserTel.serialize(laser_tel)
+    mocked_drone._telemetry_watcher._state[bp.LaserTel] = laser_tel_msg
+    assert mocked_drone.laser.get_intensity() == 1
+    with pytest.raises(ValueError):
+        mocked_drone.laser.set_intensity(2)