using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Coordinate conversion that keeps Frenet lateral sign relative to travel direction.</summary>
public static class FrenetTransform
{
    public static bool TryReconstruct(FrenetReferencePoint referencePoint, double lateralOffset, double lateralDerivative,
        double minimumFrenetDenominator, out Pose2D pose)
    {
        pose = null;
        if (referencePoint == null || !IsFinite(lateralOffset) || !IsFinite(lateralDerivative) ||
            !IsFinite(minimumFrenetDenominator) || minimumFrenetDenominator <= 0d)
            return false;

        double denominator = 1d - referencePoint.GeometricCurvature * lateralOffset;
        if (!IsFinite(denominator) || denominator < minimumFrenetDenominator)
            return false;

        double travelYaw = referencePoint.TravelYaw;
        double x = referencePoint.X - lateralOffset * Math.Sin(travelYaw);
        double y = referencePoint.Y + lateralOffset * Math.Cos(travelYaw);
        double optimizedTravelYaw = travelYaw + Math.Atan2(lateralDerivative, denominator);
        double vehicleYaw = referencePoint.Direction == TravelDirection.Forward
            ? AngleMath.NormalizeRadians(optimizedTravelYaw)
            : AngleMath.NormalizeRadians(optimizedTravelYaw + Math.PI);
        if (!IsFinite(x) || !IsFinite(y) || !IsFinite(vehicleYaw))
            return false;

        pose = new Pose2D(x, y, vehicleYaw);
        return true;
    }

    internal static double GetTravelYaw(double vehicleYaw, TravelDirection direction)
    {
        return direction == TravelDirection.Forward ? vehicleYaw : vehicleYaw + Math.PI;
    }

    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }
}