using static System.Math;
using Vim.Math3d;

public static class Kepler
{
    /// <summary>
    /// 2 * PI
    /// </summary>
    private static double TAU => PI * 2.0;
    /// <summary>
    /// Universal gravitational constant
    /// </summary>
    private static double G => 6.677e-11;

    /// <summary>
    /// Get the mean anomaly of an orbit given at a given time, given
    /// the period of the orbit.
    /// </summary>
    /// <param name="time">Time of mean anomaly</param>
    /// <param name="P">Orbital period</param>
    /// <returns></returns>
    public static double GetMeanAnomaly(double time, double P)
    {
        var M = ((time % P / P) * TAU) % TAU;
        return M % TAU;
    }

    /// <summary>
    /// 
    /// </summary>
    /// <param name="P">Orbital period</param>
    /// <returns></returns>
    public static double GetMeanAngularVelocity(double P) => TAU / P;

    /// <summary>
    /// 
    /// </summary>
    /// <param name="a">Semimajor axis in AU</param>
    /// <param name="U">Standard gravitational parameter</param>
    /// <returns></returns>
    public static double GetPeriod(double a, double U)
    {
        var a3 = a * a * a;
        var T = TAU * Sqrt(a3 / U);

        return T;
    }

    /// <summary>
    /// Get the standard gravitational parameter of a mass
    /// </summary>
    /// <param name="M">Mass in kilograms</param>
    /// <returns>Standard gravitational parameter</returns>
    public static double U(double M) => M * G;
    /// <summary>
    /// Get the standard gravitational parameter of a mass
    /// </summary>
    /// <param name="M">Mass in kilograms</param>
    /// <returns>Standard gravitational parameter</returns>
    public static double GetStandardGravitationalParameter(double M) => M * G;

    /// <summary>
    /// Get the eccentric anomaly for a given mean anomaly.
    /// </summary>
    /// <param name="e">Eccentricity</param>
    /// <param name="M">Mean anomaly</param>
    /// <returns>Eccentric anomaly</returns>
    public static double GetEccentricAnomaly(double e, double M) =>
        GetEccentricAnomalyBS(e, M);

    /// <summary>
    /// Get the eccentric anomaly for a given mean anomaly 
    /// using a binary search.
    /// </summary>
    /// <param name="e">Eccentricity</param>
    /// <param name="M">Mean anomaly</param>
    /// <returns>Eccentric anomaly</returns>
    private static double GetEccentricAnomalyBS(double e, double M)
    {
        // from Astronomical Algorithms, p. 206
        const int iterations = 33;
        double E0, D;

        double F = Sign(M);
        M = Abs(M) / TAU;
        M = (M - (int)M) * TAU * F;
        if (M < 0)
        {
            M = M + TAU;
        }
        F = 1.0;
        if (M > PI)
        {
            F = -1.0;
            M = TAU - M;
        }
        E0 = PI / 2;
        D = PI / 4;
        for (int J = 0; J < iterations; J++)
        {
            double M1 = E0 - e * Sin(E0);
            E0 = E0 + D * Sign(M - M1);
            D *= 0.5;
        }
        E0 *= F;

        return E0;
    }

    /// <summary>
    /// Get the Cartesian position on an orbit given its eccentricity, 
    /// its semi-major axis and a given mean anomaly.
    /// </summary>
    /// <param name="e">Eccentricity</param>
    /// <param name="a">Semi-major axis</param>
    /// <param name="M">Mean anomaly</param>
    /// <returns>Position on the orbit</returns>
    public static DVector2 GetEccentricPosition2D(double e, double a, double M)
    {
        var E = Kepler.GetEccentricAnomaly(e, M);

        var P = a * (Cos(E) - e);
        var Q = a * Sin(E) * Sqrt(1.0 - Pow(e, 2));

        return new DVector2(P, Q);
    }
}