"""
When theta motor is at 0 then mirror is up and when at 180 then mirror is front.
Phi 0 equals right and phi 180 equals left by 45 degrees.
"""

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 # TODO: Fix this cyclic reference
        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)

        # 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