import bpy
import math
import bmesh
import re
from mathutils.bvhtree import BVHTree
from mathutils import Vector, Euler, Matrix
from typing import List, Tuple

import time

print("\n\n\n\n\n\n\n\n", time.strftime("%Y-%m-%d %H:%M:%S", time.localtime()), "Starting script execution")

class Airport:
    name: str
    longitude: float
    latitude: float
    x: float
    y: float
    z: float
    object: bpy.types.Object | None

    def __init__(self, name: str, lon_lat: str, bvh: BVHTree, origin: Vector, radius: float, shader: bpy.types.Material | None):
        self.name = name
        lat, lon = parse_coordinates(lon_lat)
        print(f"Parsed coordinates: {lat}, {lon}")
        self.latitude = lat
        self.longitude = lon 
        # print(self.longitude, self.latitude)
        direction = direction_from_lat_lon(self.latitude, self.longitude)
        # print("direction:", direction)
        distance = distance_to_surface_from_latlon(bvh, origin, self.latitude, self.longitude)
        # print("distance:", distance)
        if distance is None:
            raise ValueError(f"No surface found at latitude {self.latitude} and longitude {self.longitude}")
        pos_vector = vector_to_coordinates(direction, distance, origin)
        # print("position vector:", pos_vector)
        # pos_vector = vector_to_coordinates(direction, distance, origin)
    
        self.x = pos_vector.x
        self.y = pos_vector.y
        self.z = pos_vector.z
        try:
            sphere = bpy.data.objects.get(name)
            if not sphere is None:
                # print(f"Sphere with name '{name}' already exists.")
                if not pos_vector == sphere.location:
                    print("Sphere already exists, but location is different. Updating location.")
                    sphere.location.x = pos_vector.x
                    sphere.location.y = pos_vector.y
                    sphere.location.z = pos_vector.z
            else:
                print(f"Sphere with name '{name}' does not exist, creating a new one.")
                sphere = create_sphere(pos_vector, radius=radius, name=name, shader=shader)
        except:
            print("Creating new sphere object.")
            sphere = create_sphere(pos_vector, radius=radius, name=name, shader=shader)
        self.object = sphere

    def __str__(self):
        return f"Airport(name={self.name}, longitude={self.longitude}, latitude={self.latitude}, x={self.x}, y={self.y}, z={self.z})"

    def get_coordinates(self):
        return Vector((self.x, self.y, self.z))

    def get_location(self):
        return Vector((self.phi, self.theta, self.height))

### World Building Functions

def parse_coordinates(coord_str: str) -> Tuple[float, float]:
    """
    Parses a coordinate string in the format '15° 52′ 16″ S, 47° 55′ 7″ W'
    and returns (latitude, longitude) in decimal degrees.
 
    Args:
        coord_str (str): Coordinate string.

    Returns:
        tuple: (latitude, longitude) in decimal degrees.
    """
    # Regular expression to match DMS components and direction
    pattern = r"(\d+)[°º]\s*(\d+)[′']\s*(\d+)[″\"]?\s*([NSEOW])"
    matches = re.findall(pattern, coord_str)

    if len(matches) != 2:
        raise ValueError("Invalid coordinate format. Expecting two sets of DMS with direction.")

    def dms_to_decimal(degree, minute, second, direction):
        degree = int(degree)
        minute = int(minute)
        second = int(second)
        decimal = degree + minute / 60 + second / 3600
        dir = direction.upper()

        # Normalize 'O' (German "Ost") to 'E' (East)
        if dir == 'O':
            dir = 'E'

        if dir in ('S', 'W'):
            return -decimal
        return decimal


    lat = dms_to_decimal(*matches[0])
    lon = dms_to_decimal(*matches[1])

    return lat, lon

def direction_from_lat_lon(latitude_deg: float, longitude_deg: float) -> Vector:
    """Convert latitude and longitude (in degrees) to a unit direction vector."""
    lat = math.radians(latitude_deg)
    lon = math.radians(longitude_deg)
    
    x = math.cos(lat) * math.cos(lon)
    y = math.cos(lat) * math.sin(lon)
    z = math.sin(lat)
    
    return Vector((x, y, z))

def distance_to_surface_from_latlon(
        bvh: BVHTree,
        origin: Vector,
        latitude_deg: float,
        longitude_deg: float,
        max_dist=100.0
) -> float | None:
    """Cast a ray from origin in a direction defined by lat/lon and return the hit distance."""
    direction = direction_from_lat_lon(latitude_deg, longitude_deg)

    hit = bvh.ray_cast(origin, direction, max_dist)

    if hit[0]:  # hit[0] is hit location
        hit_point = hit[0]
        return (hit_point - origin).length
    else:
        return None  # No hit found

def vector_to_coordinates(direction: Vector, length: float, origin: Vector = Vector((0, 0, 0))) -> Vector:
    """
    Returns a 3D coordinate in space by extending a direction vector from an origin point.

    Args:
        direction (Vector): The direction vector (will be normalized).
        length (float): Distance from the origin along the direction.
        origin (Vector): Starting point (default is the origin (0, 0, 0)).

    Returns:
        Vector: The resulting 3D point.
    """
    direction = direction.normalized()
    return origin + direction * length

def create_sphere(location: Vector, radius: float, name: str, shader: bpy.types.Material | None) -> bpy.types.Object: # Create a red UV sphere at the specified location
    """
    Creates a red UV sphere at the given location in the scene.

    Args:
        location (Vector): The location to place the sphere.
        radius (float): Radius of the sphere.
        name (str): Name of the sphere object and its mesh data.
    """
    # Create the UV sphere
    bpy.ops.mesh.primitive_uv_sphere_add(radius=radius, location=location)
    sphere = bpy.context.active_object
    sphere.name = name
    sphere.data.name = f"{name}_Mesh"

    # Create a red material if it doesn't exist
    if shader == None:
        mat_name = "RedMaterial"
        if mat_name in bpy.data.materials:
            shader = bpy.data.materials[mat_name]
        else:
            shader = bpy.data.materials.new(name=mat_name)
            shader.diffuse_color = (1.0, 0.0, 0.0, 1.0)  # RGBA red
            shader.use_nodes = False

    # Assign the material
    if len(sphere.data.materials) == 0:
        sphere.data.materials.append(shader)
    else:
        sphere.data.materials[0] = shader
    return sphere

def create_sphere_from_coordinates(
    lat_long_str: str,
    name: str,
    bvh: BVHTree,
    origin: Vector,
    radius: float,
    shader: bpy.types.Material | None
) -> Tuple[Vector, bpy.types.Object]:
    """
    Creates a sphere at the specified latitude and longitude on the surface of the sphere.

    Args:
        lat_long_str (str): Latitude and longitude in the format '15° 52′ 16″ S, 47° 55′ 7″ W'.
        name (str): Name of the sphere object.
        bvh (BVHTree): The BVH tree for ray casting.
        origin (Vector): The origin point from which to calculate the sphere's position.
        radius (float): Radius of the sphere.
        shader (str | None): The shader to apply to the sphere, if any.
    Returns:
        bpy.types.Object: The created sphere object.
    """
    lat_deg, lon_deg = parse_coordinates(lat_long_str)
    print(lat_deg, lon_deg)
    direction = direction_from_lat_lon(lat_deg, lon_deg)
    print("direction:", direction)
    distance = distance_to_surface_from_latlon(bvh, origin, lat_deg, lon_deg)
    print("distance:", distance)

    if distance is None:
        raise ValueError(f"No surface found at latitude {lat_deg} and longitude {lon_deg}")
    point = vector_to_coordinates(direction, distance, origin)
    print("point:", point)
    sphere = None
    try:
        sphere = bpy.data.objects.get(name)
        if not sphere is None:
            # print(f"Sphere with name '{name}' already exists.")
            if not point == sphere.location:
                print("Sphere already exists, but location is different. Updating location.")
                sphere.location.x = point.x
                sphere.location.y = point.y
                sphere.location.z = point.z
        else:
            print(f"Sphere with name '{name}' does not exist, creating a new one.")
            sphere = create_sphere(point, radius=radius, name=name, shader=shader)
    except:
        print("Creating new sphere object.")
        sphere = create_sphere(point, radius=radius, name=name, shader=shader)
    return point, sphere

def create_bvh_from_object(Object: bpy.types.Object) -> BVHTree:
    """Creates a new BVH from the main mesh.
    Args:
        Object (bpy.types.Object): The Object to create the BVH from.
    Returns:
        BVHTree: The created BVH object.
    """
    main_mesh = Object.data
    bm = bmesh.new()
    bm.from_mesh(main_mesh)
    bm.transform(Object.matrix_world)
    bm.normal_update()

    # Create the BVH tree from the bmesh
    bvh = BVHTree.FromBMesh(bm)

    # Free the bmesh to avoid memory leaks
    bm.free()
    return bvh

def reset_object(object: bpy.types.Object):
    """Resets the location and rotation of the main objects in the scene."""
    if object is not None:
        object.location = Vector((0, 0, 0))
        object.rotation_euler = Euler((0, 0, 0), 'XYZ')
    else:
        print(f"Object {object} is None, cannot reset location and rotation.")

### Animation Functions
def align_vectors_via_xz_rotation(source: Vector, target: Vector) -> Tuple[float, float]:
    """
    Aligns 'source' vector to 'target' vector using rotation around X and Z axes only.
    
    Args:
        source (Vector): The source direction vector (should be normalized).
        target (Vector): The target direction vector (should be normalized).
        
    Returns:
        Tuple[float, float]: (rot_x, rot_z) in radians to align source to target.
    """
    # Ensure both vectors are normalized
    source = source.normalized()
    target = target.normalized()
    
    # Rotate source vector around X-axis to match target's Z component
    # Compute angle between projected vectors in YZ plane
    source_yz = Vector((source.y, source.z))
    target_yz = Vector((target.y, target.z))
    angle_x = source_yz.angle_signed(target_yz)

    # Apply X rotation to source vector
    temp_vec = source.copy()
    temp_vec.rotate(Euler((angle_x, 0, 0), 'XYZ'))

    # Rotate adjusted vector around Z-axis to match target in XY plane
    temp_xy = Vector((temp_vec.x, temp_vec.y))
    target_xy = Vector((target.x, target.y))
    angle_z = temp_xy.angle_signed(target_xy)

    return angle_x, angle_z

def apply_rotation_to_vector(
    vector: Vector,
    rot_x: float,
    rot_z: float
) -> Vector:
    """
    Applies rotation around X and Z axes to a vector.

    Args:
        vector (Vector): The original vector.
        rot_x (float): Rotation angle around X axis in radians.
        rot_z (float): Rotation angle around Z axis in radians.

    Returns:
        Vector: The rotated vector.
    """
    vector = Vector(vector)
    # Copy to avoid modifying original vector
    rotated_vector = vector.copy()
    
    # Apply rotations in XYZ order: first X, then Z
    rotation = Euler((rot_x, 0.0, rot_z), 'XYZ')
    rotated_vector.rotate(rotation)
    
    return rotated_vector

def local_z_rotation_to_align(vec1: Vector, vec2: Vector, degrees=False) -> float:
    """
    Calculates the rotation angle around the local Z axis of vec1 to align it with vec2.

    Args:
        vec1 (Vector): The source vector (must be normalized).
        vec2 (Vector): The target vector (must be normalized).
        degrees (bool): If True, return angle in degrees. Otherwise, radians.

    Returns:
        float: Angle to rotate vec1 around its local Z axis to align best with vec2.
    """
    # Normalize input
    v1 = vec1.normalized()
    v2 = vec2.normalized()

    # Project both vectors onto the XY plane (Z rotation only affects XY)
    v1_xy = Vector((v1.x, v1.y)).normalized()
    v2_xy = Vector((v2.x, v2.y)).normalized()

    # Compute angle between projections
    dot = max(-1.0, min(1.0, v1_xy.dot(v2_xy)))  # Clamp for safety
    angle = math.acos(dot)

    # Determine rotation direction (sign) using 2D cross product
    cross = v1_xy.x * v2_xy.y - v1_xy.y * v2_xy.x
    if cross < 0:
        angle = -angle

    return math.degrees(angle) if degrees else angle

def get_point_of_triangle(v1: Vector, v2: Vector, vector1: Vector) -> Vector:
    """
    Returns the point of intersection of the triangle formed by v1, v2, and vector with the XY plane.

    Args:
        v1 (Vector): First vertex of the triangle (with 90 degrees).
        v2 (Vector): Second vertex of the triangle.
        vector (Vector): Vector to 3rd vertex of the triangle.

    Returns:
        Vector: The point where the 3rd vertex would be.
    """
    direction = vector1.normalized()
    cos_alpha = v1.normalized().dot(direction)
    cos_alpha = max(min(cos_alpha, 1.0), -1.0)

    if abs(cos_alpha) < 1e-6:
        raise ValueError("Angle too close to 90°, cannot compute third point reliably.")

    v3_length = v1.length / cos_alpha
    v3 = direction * v3_length

    return v3

def clear_timeline(objects: List[bpy.types.Object]) -> None:
    """
    Clears keyframes for 'rotation_euler' and 'location.z' from the given objects,
    and resets those values to 0.0.

    Args:
        objects (List[bpy.types.Object]): List of objects to clear keyframes from.
    """
    for obj in objects:
        if obj.animation_data and obj.animation_data.action:
            action = obj.animation_data.action
            fcurves_to_remove = [
                fc for fc in action.fcurves
                if fc.data_path == "rotation_euler"
                or (fc.data_path == "location" and fc.array_index == 2)
            ]
            for fc in fcurves_to_remove:
                action.fcurves.remove(fc)

        # Reset rotation_euler (X, Y, Z) to 0
        obj.rotation_euler = (0.0, 0.0, 0.0)
        
        # Reset location.z to 0, keep x and y unchanged
        loc = obj.location
        obj.location = (loc.x, loc.y, 0.0)

if __name__ == "__main__":

    ### Initialization

    main_sphere = bpy.data.objects["Sphere.002"]

    origin: Vector = main_sphere.matrix_world.translation

    bvh=create_bvh_from_object(main_sphere)

    ### Setting up world

    airport_shader: bpy.types.Material | None = None # Placeholder for shader, can be set to a specific material if needed

    # flughafen1_vec, flughafen1_obj = create_sphere_from_coordinates(
    #     lat_long_str="52° 21′ 44″ N, 13° 30′ 2″ O",
    #     name="BER1",
    #     bvh=bvh,
    #     origin=origin,
    #     radius=0.1,
    #     shader=airport_shader
    # )

    flughafen1 = Airport(
        name="BER2",
        lon_lat="52° 21′ 44″ N, 13° 30′ 2″ O",
        bvh=bvh,
        origin=origin,
        radius=0.1,
        shader=airport_shader
    )

    flughafen2 = Airport(
        name="HEL",
        lon_lat="60° 19′ 2″ N, 24° 57′ 48″ O",
        bvh=bvh,
        origin=origin,
        radius=0.1,
        shader=airport_shader
    )

    flughafen3 = Airport(
        name="NRT",
        lon_lat="35° 45′ 53″ N, 140° 23′ 11″ O",
        bvh=bvh,
        origin=origin,
        radius=0.1,
        shader=airport_shader
    )

    ### Initialize Animation

    airplane = bpy.data.objects["Airplane"]
    # airplane_vector = Vector((0, 1, 0))  # Setting the airplane default vector (x, y, z)
    # reset_object(airplane)  # Resetting the airplane location and rotation

    centerjoint = bpy.data.objects["Center_Joint"]
    # centerjoint_vectors = Vector((0, 0, 1))  # Setting the center joint default vector (x, y, z)
    # reset_object(centerjoint)  # Resetting the center joint location and rotation


    ### Animation Logic
    # Way between airport1 and airport2
    for airport in [flughafen1, flughafen2, flughafen3]:
        print("lat: ", airport.latitude, "lon: ", airport.longitude)
    lat=[
        math.radians(flughafen1.latitude - 90),
        math.radians(flughafen2.latitude - 90),
        math.radians(flughafen3.latitude - 90)
    ]
    print("lat:", lat)

    lon=[
        math.radians(flughafen1.longitude-90),
        math.radians(flughafen2.longitude-90),
        math.radians(flughafen3.longitude-90)
    ]
    print("lon:", lon)

    airplane_height = [
        flughafen1.get_coordinates().length + 0.1,
        flughafen2.get_coordinates().length + 0.1,
        flughafen3.get_coordinates().length + 0.1
    ]
    print("airplane_height:", airplane_height)

    travel_height = airplane_height[0] * 1.1
    print("travel_height:", travel_height)

    airplane_rotation = [
        math.radians(-35),
        math.radians(-42),
        math.radians(-111),
        math.radians(-102)
    ]

    frame_count = 720

    clear_timeline([airplane, centerjoint])

    airplane.rotation_euler.x = math.radians(90)

    ### Start BER
    bpy.context.scene.frame_set(0)
    airplane.location.z = airplane_height[0]
    airplane.keyframe_insert(data_path="location", index=2)
    airplane.rotation_euler.z = airplane_rotation[0]
    airplane.keyframe_insert(data_path="rotation_euler", index=2)

    centerjoint.rotation_euler.x = lat[0]
    centerjoint.rotation_euler.z = lon[0]
    centerjoint.keyframe_insert(data_path="rotation_euler", index=-1)

    ### flight height start
    bpy.context.scene.frame_set(32)
    airplane.location.z = travel_height
    airplane.keyframe_insert(data_path="location", index=2)

    ### End of high flight
    bpy.context.scene.frame_set(49)
    airplane.location.z = travel_height
    airplane.keyframe_insert(data_path="location", index=2)

    ### Landing HEL
    bpy.context.scene.frame_set(81)
    airplane.location.z = airplane_height[1]
    airplane.keyframe_insert(data_path="location", index=2)
    airplane.rotation_euler.z = airplane_rotation[1]
    airplane.keyframe_insert(data_path="rotation_euler", index=2)

    centerjoint.rotation_euler.x = lat[1]
    centerjoint.rotation_euler.z = lon[1]
    centerjoint.keyframe_insert(data_path="rotation_euler", index=-1)

    ### Start HEL
    bpy.context.scene.frame_set(127)
    airplane.location.z = airplane_height[1]
    airplane.keyframe_insert(data_path="location", index=2)
    airplane.rotation_euler.z = airplane_rotation[2]
    airplane.keyframe_insert(data_path="rotation_euler", index=2)

    centerjoint.rotation_euler.x = lat[1]
    centerjoint.rotation_euler.z = lon[1]
    centerjoint.keyframe_insert(data_path="rotation_euler", index=-1)

    ### flight height start
    bpy.context.scene.frame_set(255)
    airplane.location.z = travel_height
    airplane.keyframe_insert(data_path="location", index=2)

    ### End of high flight
    bpy.context.scene.frame_set(592)
    airplane.location.z = travel_height
    airplane.keyframe_insert(data_path="location", index=2)

    ### Landing NRT
    bpy.context.scene.frame_set(720)
    airplane.location.z = airplane_height[2]
    airplane.keyframe_insert(data_path="location", index=2)
    airplane.rotation_euler.z = airplane_rotation[3]
    airplane.keyframe_insert(data_path="rotation_euler", index=2)

    centerjoint.rotation_euler.x = lat[2]
    centerjoint.rotation_euler.z = lon[2]
    centerjoint.keyframe_insert(data_path="rotation_euler", index=-1)

    ### reset
    bpy.context.scene.frame_set(0)
        
    print("END")