"""Alle gemessenen Koordinaten der Quelle und der Sonne haben den Ursprung in der linken unteren Ecke des Clusters in einem rechtshaendigen flachen System.
"""

import math
import objects.motor as motor
import numpy as np

# Einheitsvektoren
unit_x = np.array([1, 0, 0])
unit_y = np.array([0, 1, 0])
unit_z = np.array([0, 0, 1])

def get_axis(axis):
    "Axis are numbered from 1 to 3 from x to z."
    match axis:
        case 1:
            ax = unit_x
        case 2:
            ax = unit_y
        case 3:
            ax = unit_z
        case _:
            ax = unit_x
    return ax

def proj(vec, axis: int =1):
    """Simple vector projection onto an axis."""    
    ax = get_axis(axis)
    return np.dot(vec, ax) * ax

def abs_custom(vec):
    l = 0
    for i in range(3):
        l += vec[i] ** 2
    return np.sqrt(l)

def rotate(v, angle=90, axis=1):
    "Rotate a vector with an angle around a axis with the right hand rule."
    angle = angle/180 * np.pi
    k = get_axis(axis)

    return (
        v * np.cos(angle)
        + np.cross(k, v) * np.sin(angle)
        + k * np.dot(k, v) * (1 - np.cos(angle))
    )
        
def agl(a, b):
    "Get the angle between two vectors. This is always between 0 and 180 degree."
    return np.round(np.acos(np.dot(a, b)/(abs_custom(a) * abs_custom(b)))/(2 * np.pi) * 360)

def normalize(vec):
    l = abs_custom(vec)
    return vec/l

def get_angles(source, target):
    """Main function to get the phi and theta angles for a source and a target vector. Both vectors must lie on the front half sphere.
       Phi is from 0 to 180 where 0 means left when you look at the mirrors. The hardware is bounded between 45 and 135 degree. Thus the here provided angle needs to be subtracted by 45 and then doubled.
       Theta is from 0 to 90 where 0 means up."""
    source_planar = source - proj(source, 3)
    target_planar = target - proj(target, 3)

    source_phi = agl(source_planar, unit_x)
    target_phi = agl(target_planar, unit_x)

    source_theta = agl(rotate(source, 90 - source_phi, 3), unit_z)
    target_theta = agl(rotate(target, 90 - target_phi, 3), unit_z)

    phi = None
    theta = None

    theta_diff = None
    phi_diff = agl(source_planar, target_planar)
    if source_phi < target_phi:
        rota = rotate(source_planar, phi_diff, 3)
        theta_diff = agl(rota, target)
        phi = source_phi + phi_diff/2
    else:
        rota = rotate(target_planar, phi_diff, 3)
        theta_diff = agl(rota, source)
        phi = target_phi + phi_diff/2

    if source_theta < target_theta:
        theta = target_theta + theta_diff/2
    else:
        theta = source_theta + theta_diff/2

    return (phi, theta)

class MovingEntity:
    """Embedded entity in the world with a position."""

    def __init__(self, world):
        self.world = world
        self.pos = (0.0, 0.0, 0.0)  # (x, y, z) in local untilted coordinates

    def get_pos_rotated(self):
        """Return position rotated by world's tilt around y-axis."""
        return self.world.rotate_point_y(self.pos)

    def move(self, dx=0, dy=0, dz=0):
        self.pos = (self.pos[0] + dx, self.pos[1] + dy, self.pos[2] + dz)

class Target(MovingEntity):
    def __init__(self, world, pos=(0.0, 0.0, 0.0)):
        super().__init__(world)
        self.pos = pos

class Source(MovingEntity):
    def __init__(self, world, pos=(10.0, 10.0, 10.0)):
        super().__init__(world)
        self.pos = pos

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
        self.phi = motor.Motor(self.world.board)
        self.theta = motor.Motor(self.world.board)

        # Position in un-tilted coordinate system
        self.pos = (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 = np.array([self.get_pos_rotated()])
        rel_source = np.array([source.pos]) - rot_pos
        rel_target = np.array([target.pos]) - rot_pos

        phi, theta = get_angles(rel_source, rel_target)

        # Update the angles based on the normals in rotated positions
        self.phi.set_angle(phi)
        self.theta.set_angle(theta)

    def get_angles(self):
        return self.phi.angle, self.theta.angle

class World:
    def __init__(self, board, tilt_deg=0.0):
        self.board = board

        self.grid_size = 10  # In cm
        self.tilt_deg = tilt_deg  # Tilt of the grid system around y-axis
        self.mirrors = []

    def add_mirror(self, mirror):
        self.mirrors.append(mirror)

    def update_mirrors_from_source_target(self, source: Source, target: Target):
        for mirror in self.mirrors:
            mirror.set_angle_from_source_target(source, target)

    def rotate_point_y(self, point):
        """Rotate a point around the y-axis by the world's tilt angle."""
        x, y, z = point
        theta = math.radians(self.tilt_deg)
        cos_t = math.cos(theta)
        sin_t = math.sin(theta)
        x_rot = x * cos_t + z * sin_t
        y_rot = y
        z_rot = -x * sin_t + z * cos_t
        return (x_rot, y_rot, z_rot)