using System;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;

/// <summary>满足两个二维端点二阶边界条件的参数五次 Hermite 曲线。</summary>
internal sealed class QuinticHermiteCurve2D
{
    private readonly double _x0;
    private readonly double _x1;
    private readonly double _x2;
    private readonly double _x3;
    private readonly double _x4;
    private readonly double _x5;
    private readonly double _y0;
    private readonly double _y1;
    private readonly double _y2;
    private readonly double _y3;
    private readonly double _y4;
    private readonly double _y5;

    private QuinticHermiteCurve2D(
        double x0, double x1, double x2, double x3, double x4, double x5,
        double y0, double y1, double y2, double y3, double y4, double y5)
    {
        _x0 = x0;
        _x1 = x1;
        _x2 = x2;
        _x3 = x3;
        _x4 = x4;
        _x5 = x5;
        _y0 = y0;
        _y1 = y1;
        _y2 = y2;
        _y3 = y3;
        _y4 = y4;
        _y5 = y5;
    }

    internal static bool TryCreate(
        double x0, double y0, double dx0, double dy0, double ddx0, double ddy0,
        double x1, double y1, double dx1, double dy1, double ddx1, double ddy1,
        out QuinticHermiteCurve2D curve,
        out string reason)
    {
        curve = null;
        reason = string.Empty;
        if (!AreFinite(x0, y0, dx0, dy0, ddx0, ddy0, x1, y1, dx1, dy1, ddx1, ddy1))
        {
            reason = "五次 Hermite 曲线需要有限的边界条件。";
            return false;
        }
        if (IsZeroVector(dx0, dy0) || IsZeroVector(dx1, dy1))
        {
            reason = "五次 Hermite 曲线端点一阶导数不能为零。";
            return false;
        }

        SolveCoordinate(x0, dx0, ddx0, x1, dx1, ddx1,
            out double ax0, out double ax1, out double ax2, out double ax3, out double ax4, out double ax5);
        SolveCoordinate(y0, dy0, ddy0, y1, dy1, ddy1,
            out double ay0, out double ay1, out double ay2, out double ay3, out double ay4, out double ay5);
        if (!AreFinite(ax0, ax1, ax2, ax3, ax4, ax5, ay0, ay1, ay2, ay3, ay4, ay5))
        {
            reason = "五次 Hermite 曲线系数溢出。";
            return false;
        }

        curve = new QuinticHermiteCurve2D(ax0, ax1, ax2, ax3, ax4, ax5, ay0, ay1, ay2, ay3, ay4, ay5);
        return true;
    }

    internal void Evaluate(
        double u,
        out double x, out double y,
        out double dx, out double dy,
        out double ddx, out double ddy)
    {
        if (!IsFinite(u) || u < 0d || u > 1d)
            throw new ArgumentOutOfRangeException(nameof(u), "The curve parameter must be finite and within [0, 1].");

        x = EvaluateValue(_x0, _x1, _x2, _x3, _x4, _x5, u);
        y = EvaluateValue(_y0, _y1, _y2, _y3, _y4, _y5, u);
        dx = EvaluateFirstDerivative(_x1, _x2, _x3, _x4, _x5, u);
        dy = EvaluateFirstDerivative(_y1, _y2, _y3, _y4, _y5, u);
        ddx = EvaluateSecondDerivative(_x2, _x3, _x4, _x5, u);
        ddy = EvaluateSecondDerivative(_y2, _y3, _y4, _y5, u);
    }

    private static void SolveCoordinate(double p0, double v0, double acceleration0, double p1, double v1, double acceleration1,
        out double a0, out double a1, out double a2, out double a3, out double a4, out double a5)
    {
        a0 = p0;
        a1 = v0;
        a2 = acceleration0 / 2d;
        double c0 = p1 - (a0 + a1 + a2);
        double c1 = v1 - (a1 + 2d * a2);
        double c2 = acceleration1 - 2d * a2;
        a3 = 10d * c0 - 4d * c1 + 0.5d * c2;
        a4 = -15d * c0 + 7d * c1 - c2;
        a5 = 6d * c0 - 3d * c1 + 0.5d * c2;
    }

    private static double EvaluateValue(double a0, double a1, double a2, double a3, double a4, double a5, double u) =>
        (((((a5 * u + a4) * u + a3) * u + a2) * u + a1) * u) + a0;

    private static double EvaluateFirstDerivative(double a1, double a2, double a3, double a4, double a5, double u) =>
        ((((5d * a5 * u + 4d * a4) * u + 3d * a3) * u + 2d * a2) * u) + a1;

    private static double EvaluateSecondDerivative(double a2, double a3, double a4, double a5, double u) =>
        (((20d * a5 * u + 12d * a4) * u + 6d * a3) * u) + 2d * a2;

    private static bool IsZeroVector(double x, double y) => x == 0d && y == 0d;

    private static bool AreFinite(params double[] values)
    {
        for (int index = 0; index < values.Length; index++)
        {
            if (!IsFinite(values[index])) return false;
        }
        return true;
    }

    private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
}
