Refactor all and add some tests for the calculations

objects/mirror.py

@@ -0,0 +1,42 @@
+import numpy as np
+
+from objects.generic import Source, Target
+from objects.motor import Motor
+
+from calculator import get_angles
+
+class Mirror:
+    def __init__(self, world, cluster_x=0, cluster_y=0):
+        self.world = world
+        self.cluster_x = cluster_x
+        self.cluster_y = cluster_y
+
+        # Store the motors
+        # Need to get first the theta because
+        # of the ordeing of the cables on the board
+        self.motor_theta: Motor = Motor(self.world.board)
+        self.motor_phi: Motor = Motor(self.world.board)
+
+        # Position in un-tilted coordinate system
+        self.pos = np.array(
+            [cluster_x * self.world.grid_size, cluster_y * self.world.grid_size, 0.0]
+        )
+
+    def get_pos_rotated(self):
+        return self.world.rotate_point_y(self.pos)
+
+    def set_angle_from_source_target(self, source: Source, target: Target):
+        "Set the angles of a mirror from global source and target vectors."
+
+        rot_pos = self.get_pos_rotated()
+        rel_source = source.pos - rot_pos
+        rel_target = target.pos - rot_pos
+
+        phi, theta = get_angles(rel_source, rel_target)  # ty:ignore[unresolved-reference]
+
+        # Update the angles based on the normals in rotated positions
+        self.motor_phi.set_angle(phi)
+        self.motor_theta.set_angle(theta)
+
+    def get_angles(self):
+        return self.motor_phi.angle, self.motor_theta.angle