"""
works with all the transformations and calculation associated to position, speed, acceleration
"""

import numpy as np


def position_to_xyz(position: [float]) -> [float]:
    """
    allows to get the 3D xyz coordinates from a polar representation
    :param position: array (3,) with rho in meter, theta in rad, zed in meter
    :return: float array (3,) with x, y, z in meter
    """
    pos = position[0] * np.exp(1j * position[1])
    return [np.real(pos), np.imag(pos), position[2]]


"""
def _test_position_to_norm():
    assert position_to_norm([param.PERIMETER, 0, 100]) == [1, 0, 1]
    assert position_to_norm([0, -np.pi / 2, 0]) == [0, 0.75, 0]
    assert position_to_norm([0, np.pi / 2, 0]) == [0, 0.25, 0]
"""


def is_in_the_cone(position1: [float], position2: [float], vector2: [float], angle: float) -> bool:
    """
    checks if the point @ position 2 is in the cone from position 1 with an angle of angle
    :param position1: in x, y, z
    :param position2: in x, y, z
    :param vector2: in x, y, z
    :param angle: in rad
    :return:
    """
    vector1 = np.array(position2, dtype=float) - np.array(position1)
    vector1 /= np.linalg.norm(vector1)
    vector2 = np.array(vector2, dtype=float)
    vector2 /= np.linalg.norm(vector2)
    cos_theta = np.dot(vector1, vector2)
    if 0 < cos_theta:
        theta = np.arcsin(np.sqrt(1 - cos_theta ** 2))
        return theta < angle
    return False


def _test_is_in_the_cone():
    assert is_in_the_cone([0, 0, 0], [1, 0.1, 0], [1, 0, 0], np.pi / 5)
    assert is_in_the_cone([0, 0, 0], [1, 0.1, 0], [0, 1, 0], np.pi / 5)
    pass


def rhotheta_to_latlon(rho: float, theta: float, lat_tg: float, lon_tg: float) -> [float, float]:
    """
    transforms polar coordinates into lat, lon
    :param rho:
    :param theta:
    :param lat_tg: latitude de la target (0,0)
    :param lon_tg: longitude de la target (0,0)
    :return:
    """
    z = rho * np.exp(1j * theta)
    lat = np.imag(z) * 360 / (40075 * 1000) + lat_tg
    lon = np.real(z) * 360 / (40075 * 1000 * np.cos(np.pi / 180 * lat)) + lon_tg
    return lat, lon