"""Turnpoint geometry and optimization utilities for XCTrack task calculations.

This module provides:
- The `TaskTurnpoint` class: immutable turnpoint model for paragliding/hang gliding tasks
- Distance and geometry utilities for cylinders and goal lines
- Perimeter and optimal point calculations for distance optimization
- Support for both cylinder and goal line turnpoint types
- All calculations use WGS84 ellipsoid and geodesic math

Intended for use in parsing, generating, and optimizing XCTrack tasks.
"""

from geopy.distance import geodesic
from pyproj import Geod
from scipy.optimize import fminbound

from .optimization_config import DEFAULT_ANGLE_STEP

# Initialize WGS84 ellipsoid
geod = Geod(ellps="WGS84")


def _calculate_distance_through_point(
    start_point: tuple[float, float],
    point: tuple[float, float],
    end_point: tuple[float, float],
) -> float:
    """Calculate total distance from start through point to end."""
    return geodesic(start_point, point).meters + geodesic(point, end_point).meters


def _find_optimal_cylinder_point(
    cylinder_center: tuple[float, float],
    cylinder_radius: float,
    start_point: tuple[float, float],
    end_point: tuple[float, float],
) -> tuple[float, float]:
    """Find optimal point on cylinder using continuous optimization.

    Uses scipy.optimize for precise optimization.

    Args:
        cylinder_center: (lat, lon) of cylinder center
        cylinder_radius: Radius in meters
        start_point: (lat, lon) of starting point
        end_point: (lat, lon) of ending point

    Returns:
        (lat, lon) of optimal point on cylinder perimeter
    """
    # Edge case: if start and end points are identical (or very close),
    # return the cylinder center to avoid meaningless optimization
    start_to_end_distance = geodesic(start_point, end_point).meters
    if start_to_end_distance < 1.0:  # Less than 1 meter difference
        return cylinder_center

    cylinder_lon, cylinder_lat = cylinder_center[1], cylinder_center[0]

    # Use continuous optimization
    def objective(azimuth):
        lon, lat, _ = geod.fwd(cylinder_lon, cylinder_lat, azimuth, cylinder_radius)
        point = (lat, lon)
        return _calculate_distance_through_point(start_point, point, end_point)

    optimal_azimuth = fminbound(objective, 0, 360, xtol=0.01)
    lon, lat, _ = geod.fwd(cylinder_lon, cylinder_lat, optimal_azimuth, cylinder_radius)
    return (lat, lon)


def _get_optimized_perimeter_points(
    turnpoint: "TaskTurnpoint",
    prev_point: tuple[float, float],
    next_point: tuple[float, float],
    angle_step: int = DEFAULT_ANGLE_STEP,
) -> list[tuple[float, float]]:
    """Get optimized perimeter points for a turnpoint.

    Finds the optimal entry/exit points using scipy optimization.

    Args:
        turnpoint: TaskTurnpoint object
        prev_point: Previous point in route (for optimization)
        next_point: Next point in route (for optimization)
        angle_step: Angle step for uniform sampling fallback (if needed)

    Returns:
        List of (lat, lon) points on perimeter
    """
    # Handle goal lines
    if turnpoint.goal_type == "LINE":
        if prev_point:
            # For goal lines, we need the previous point to determine orientation
            optimal_point = turnpoint._find_optimal_goal_line_point(
                prev_point, next_point
            )
            return [optimal_point]
        else:
            # If no previous point available, return center
            return [turnpoint.center]

    if turnpoint.radius == 0:
        return [turnpoint.center]

    if prev_point and next_point:
        # Use optimized single point
        optimal_point = _find_optimal_cylinder_point(
            turnpoint.center, turnpoint.radius, prev_point, next_point
        )
        return [optimal_point]
    else:
        # Fall back to uniform sampling
        return turnpoint.perimeter_points(angle_step)


class TaskTurnpoint:
    """Turnpoint class for distance calculations."""

    def __init__(
        self,
        lat: float,
        lon: float,
        radius: float = 0,
        goal_type: str | None = None,
        goal_line_length: float | None = None,
    ):
        """Initialize a task turnpoint.

        Args:
            lat (float): Latitude in degrees.
            lon (float): Longitude in degrees.
            radius (float): Cylinder radius in meters.
            goal_type (Optional[str]): Type of goal (None, "CYLINDER", or "LINE").
            goal_line_length (Optional[float]): Length of goal line in meters (None means calculate from radius).
        """
        self.center = (lat, lon)
        self.radius = radius
        self.goal_type = goal_type
        self.goal_line_length = goal_line_length

    def perimeter_points(
        self, angle_step: int = DEFAULT_ANGLE_STEP
    ) -> list[tuple[float, float]]:
        """Generate perimeter points around the turnpoint at given angle steps.

        Args:
            angle_step (int): Angle step in degrees.

        Returns:
            List[Tuple[float, float]]: List of (lat, lon) tuples representing points on the cylinder perimeter.
        """
        if self.goal_type == "LINE":
            # For goal lines, we need a previous point to determine orientation
            # This case is handled in goal_line_points() with a previous point
            return [self.center]

        if self.radius == 0:
            return [self.center]

        points = []
        for azimuth in range(0, 360, angle_step):
            lon, lat, _ = geod.fwd(self.center[1], self.center[0], azimuth, self.radius)
            points.append((lat, lon))
        return points

    def goal_line_points(
        self, prev_point: tuple[float, float], angle_step: int = DEFAULT_ANGLE_STEP
    ) -> list[tuple[float, float]]:
        """Generate points along a goal line.

        The goal line is perpendicular to the line from the previous point to the center,
        with the goal line center being in the middle of the line.

        Args:
            prev_point (Tuple[float, float]): Previous point in route (needed for goal line orientation).
            angle_step (int): Angle step in degrees for sampling points.

        Returns:
            List[Tuple[float, float]]: List of (lat, lon) tuples representing points on the goal line.
        """
        if self.goal_type != "LINE":
            return self.perimeter_points(angle_step)

        # If goal line length is not specified, use a reasonable default
        # Note: this should normally not happen as we should calculate from radius
        # in _task_to_turnpoints, but this is a fallback
        goal_line_length = self.goal_line_length
        if goal_line_length is None:
            # Default to a standard FAI-style goal line of 400m if no better information
            goal_line_length = 400.0

        # Calculate bearing from previous point to goal center
        forward_azimuth, _, _ = geod.inv(
            prev_point[1], prev_point[0], self.center[1], self.center[0]
        )

        # Goal line is perpendicular to the approach direction
        perpendicular_azimuth_1 = (forward_azimuth + 90) % 360
        perpendicular_azimuth_2 = (forward_azimuth - 90) % 360

        # Calculate the endpoints of the goal line
        half_length = goal_line_length / 2
        lon1, lat1, _ = geod.fwd(
            self.center[1], self.center[0], perpendicular_azimuth_1, half_length
        )
        lon2, lat2, _ = geod.fwd(
            self.center[1], self.center[0], perpendicular_azimuth_2, half_length
        )

        # Sample points along the goal line
        points = []
        total_steps = int(360 / angle_step)

        # Add the endpoints and center point
        points.append((lat1, lon1))
        points.append(self.center)
        points.append((lat2, lon2))

        # Add semi-circle points behind the goal line
        # The semi-circle has radius = half_length and is on the opposite side from the approach
        back_azimuth = (forward_azimuth + 180) % 360
        for i in range(total_steps):
            angle = angle_step * i
            if angle > 180:
                continue  # Only create semi-circle, not full circle

            # Calculate point on semi-circle behind goal line
            semi_azimuth = (back_azimuth - 90 + angle) % 360
            lon, lat, _ = geod.fwd(
                self.center[1], self.center[0], semi_azimuth, half_length
            )
            points.append((lat, lon))

        return points

    def optimal_point(
        self,
        prev_point: tuple[float, float],
        next_point: tuple[float, float],
    ) -> tuple[float, float]:
        """Find the optimal point on this turnpoint's cylinder or goal line.

        Uses scipy's optimization for precise results.

        Args:
            prev_point (Tuple[float, float]): (lat, lon) of previous point in route.
            next_point (Tuple[float, float]): (lat, lon) of next point in route.

        Returns:
            Tuple[float, float]: (lat, lon) of optimal point on cylinder perimeter or goal line.
        """
        if self.goal_type == "LINE":
            return self._find_optimal_goal_line_point(prev_point, next_point)

        if self.radius == 0:
            return self.center

        return _find_optimal_cylinder_point(
            self.center, self.radius, prev_point, next_point
        )

    def _find_optimal_goal_line_point(
        self, prev_point: tuple[float, float], next_point: tuple[float, float]
    ) -> tuple[float, float]:
        """Find the optimal point on the goal line.

        For a goal line, the optimal crossing point depends on:
          1. Direction of approach (from prev_point)
          2. The perpendicular line with the goal line center in the middle
          3. The semi-circle control zone behind the goal line

        Args:
            prev_point (Tuple[float, float]): (lat, lon) of previous point in route.
            next_point (Tuple[float, float]): (lat, lon) of next point in route (may not be used for goal line).

        Returns:
            Tuple[float, float]: (lat, lon) of optimal point on the goal line or semi-circle control zone.
        """
        # Calculate bearing from previous point to goal center
        forward_azimuth, _, distance_to_center = geod.inv(
            prev_point[1], prev_point[0], self.center[1], self.center[0]
        )

        # Goal line is perpendicular to the approach direction
        perpendicular_azimuth_1 = (forward_azimuth + 90) % 360
        perpendicular_azimuth_2 = (forward_azimuth - 90) % 360

        # If goal line length is not specified, use a reasonable default
        goal_line_length = self.goal_line_length
        if goal_line_length is None:
            # Default to a standard FAI-style goal line of 400m if no better information
            goal_line_length = 400.0

        # Calculate the endpoints of the goal line
        half_length = goal_line_length / 2
        lon1, lat1, _ = geod.fwd(
            self.center[1], self.center[0], perpendicular_azimuth_1, half_length
        )
        lon2, lat2, _ = geod.fwd(
            self.center[1], self.center[0], perpendicular_azimuth_2, half_length
        )

        # Calculate the points to test for optimization
        endpoint1 = (lat1, lon1)
        endpoint2 = (lat2, lon2)

        # For goal lines, the optimal crossing is typically the perpendicular projection
        # from the previous point onto the goal line

        # First, determine if the perpendicular projection falls on the goal line segment
        # Calculate vector from endpoint1 to endpoint2
        e1_to_e2_azimuth, _, e1_to_e2_distance = geod.inv(
            endpoint1[1], endpoint1[0], endpoint2[1], endpoint2[0]
        )

        # Calculate vector from endpoint1 to previous point
        e1_to_prev_azimuth, _, e1_to_prev_distance = geod.inv(
            endpoint1[1], endpoint1[0], prev_point[1], prev_point[0]
        )

        # Calculate angle difference to determine if projection falls on line segment
        angle_diff = abs((e1_to_e2_azimuth - e1_to_prev_azimuth + 180) % 360 - 180)
        if angle_diff > 90:
            # Projection falls outside line segment, use closest endpoint
            dist1 = geodesic(prev_point, endpoint1).meters
            dist2 = geodesic(prev_point, endpoint2).meters
            return endpoint1 if dist1 < dist2 else endpoint2

        # Calculate perpendicular projection onto goal line
        # We need to find the point on the goal line that forms a right angle with prev_point
        def objective(t):
            # Parametric representation of the line, t from 0 to 1
            # Calculate intermediate point on the goal line
            lon, lat, _ = geod.fwd(
                endpoint1[1], endpoint1[0], e1_to_e2_azimuth, t * e1_to_e2_distance
            )

            # Calculate azimuth from prev_point to this point
            azimuth_to_point, _, _ = geod.inv(prev_point[1], prev_point[0], lon, lat)

            # The angle between this azimuth and the goal line azimuth should be 90 degrees
            # for the optimal projection. We want to minimize the deviation from 90 degrees.
            angle_diff = abs((azimuth_to_point - forward_azimuth + 180) % 360 - 180)
            return abs(angle_diff - 90)

        # Find the parameter t that minimizes the objective function
        optimal_t = fminbound(objective, 0, 1, xtol=0.0001)
        lon, lat, _ = geod.fwd(
            endpoint1[1], endpoint1[0], e1_to_e2_azimuth, optimal_t * e1_to_e2_distance
        )
        optimal_point = (lat, lon)

        return optimal_point

    def optimized_perimeter_points(
        self,
        prev_point: tuple[float, float],
        next_point: tuple[float, float],
        angle_step: int = DEFAULT_ANGLE_STEP,
    ) -> list[tuple[float, float]]:
        """Get optimized perimeter points for this turnpoint.

        Args:
            prev_point (Tuple[float, float]): Previous point in route.
            next_point (Tuple[float, float]): Next point in route.
            angle_step (int): Angle step for fallback uniform sampling.

        Returns:
            List[Tuple[float, float]]: List of (lat, lon) points on perimeter.
        """
        if self.goal_type == "LINE":
            # For goal lines, we need both a previous point and a special handling
            if prev_point:
                # Find optimal point on goal line
                optimal_point = self._find_optimal_goal_line_point(
                    prev_point, next_point
                )
                return [optimal_point]
            else:
                # Cannot optimize without previous point, return center
                return [self.center]

        return _get_optimized_perimeter_points(self, prev_point, next_point, angle_step)


def distance_through_centers(turnpoints: list[TaskTurnpoint]) -> float:
    """Calculate distance through turnpoint centers.

    Args:
        turnpoints (List[TaskTurnpoint]): List of TaskTurnpoint objects.

    Returns:
        float: Distance through centers in meters.
    """
    if len(turnpoints) < 2:
        return 0.0

    total = 0.0
    for i in range(len(turnpoints) - 1):
        a = turnpoints[i].center
        b = turnpoints[i + 1].center
        total += geodesic(a, b).meters
    return total
