import math
import numpy as np
from markov.log_handler.deepracer_exceptions import GenericRolloutException
from markov.track_geom.utils import euler_to_quaternion, apply_orientation, inverse_quaternion


def get_angle_between_two_points_2d_rad(pt1, pt2):
        dx = pt2.x - pt1.x
        dy = pt2.y - pt1.y
        return math.pi if dx == 0 else math.atan2(dy, dx)


def lerp(current, target, fraction):
    fraction = np.clip(fraction, 0.0, 1.0)
    return current + fraction * (target - current)


def lerp_angle_rad(current, target, fraction):
    while current > target + np.pi:
        target += np.pi * 2.0
    while target > current + np.pi:
        target -= np.pi * 2.0
    return lerp(current, target, fraction)


def normalize(vector):
    vector = np.array(vector)
    norm = np.linalg.norm(vector)
    if norm == 0:
        return vector
    return vector / norm


def project_to_2d(point_on_plane, plane_center, plane_width, plane_height, plane_quaternion):
    if not isinstance(point_on_plane, list) and not isinstance(point_on_plane, tuple) \
            and not isinstance(point_on_plane, np.ndarray):
        raise GenericRolloutException("point_on_plane must be a type of list, tuple, or numpy.ndarray")
    if not isinstance(plane_center, list) and not isinstance(plane_center, tuple) \
            and not isinstance(plane_center, np.ndarray):
        raise GenericRolloutException("plane_center must be a type of list, tuple, or numpy.ndarray")
    if not isinstance(plane_quaternion, list) and not isinstance(plane_quaternion, tuple) \
            and not isinstance(plane_quaternion, np.ndarray):
        raise GenericRolloutException("plane_quaternion must be a type of list, tuple, or numpy.ndarray")

    point_on_plane = np.array(point_on_plane)
    plane_center = np.array(plane_center)
    plane_quaternion = np.array(plane_quaternion)

    # Transpose the center back to origin
    point_on_plane_from_origin = point_on_plane - plane_center

    # Reverse the rotation so plane can align back to y-axis
    inverse_cam_quaternion = inverse_quaternion(plane_quaternion)
    point_on_y_axis = apply_orientation(inverse_cam_quaternion, point_on_plane_from_origin)

    # Rotate pitch 90 degree and yaw 90 degree, so plane will align to x and y axis
    # Remember rotation order is roll, pitch, yaw in euler_to_quaternion method
    project_2d_quaternion = euler_to_quaternion(pitch=np.pi / 2.0, yaw=np.pi / 2.0)
    point_on_2d_plane = apply_orientation(np.array(project_2d_quaternion), point_on_y_axis)

    # Align plane to origin at x, y = (0, 0)
    point_on_2d_plane = point_on_2d_plane + np.array([plane_width / 2.0, plane_height / 2.0, 0.0])

    # Re-scale x and y space between 0 and 1
    return (point_on_2d_plane[0] / plane_width), (point_on_2d_plane[1] / plane_height)


def ray_plane_intersect(ray_origin, ray_dir, plane_normal, plane_offset, threshold=0.0001):
    ray_dir = normalize(ray_dir) if np.dot(ray_dir, ray_dir) - 1.0 > threshold else ray_dir
    plane_normal = normalize(plane_normal) if np.dot(plane_normal, plane_normal) - 1.0 > threshold else plane_normal
    point_on_plane = None
    denominator = np.dot(plane_normal, ray_dir)
    if np.abs(denominator) >= threshold:
        t = (-plane_offset - np.dot(ray_origin, plane_normal)) / denominator
        # Point on near plane that intersects the ray and the plane
        point_on_plane = ray_origin + t * ray_dir
    return point_on_plane