import * as math from "../math";
import { Mat3 } from "../math/Mat3";

type Func = (x: number) => number;

export function getEccentricAnomaly(M: number, ecc: number): number {
    if (ecc === 0.0) {
        // Circular orbit
        return M;
    } else if (ecc < 0.2) {
        // Low eccentricity, so use the standard iteration technique
        return math.solve_iteration_fixed(solveKeplerFunc1(ecc, M), M, 5);
    } else if (ecc < 0.9) {
        // Higher eccentricity elliptical orbit; use a more complex but
        // much faster converging iteration.
        return math.solve_iteration_fixed(solveKeplerFunc2(ecc, M), M, 6);
    } else if (ecc < 1.0) {
        // Extremely stable Laguerre-Conway method for solving Kepler's
        // equation.  Only use this for high-eccentricity orbits, as it
        // requires more calculation.
        let E = M + 0.85 * ecc * Math.sign(Math.sin(M));
        return math.solve_iteration_fixed(solveKeplerLaguerreConway(ecc, M), E, 8);
    } else if (ecc === 1.0) {
        // TODO: Parabolic orbit
        return M;
    } else {
        // Laguerre-Conway method for hyperbolic (ecc > 1) orbits.
        let E = Math.log((2 * M) / ecc + 1.85);
        return math.solve_iteration_fixed(solveKeplerLaguerreConwayHyp(ecc, M), E, 30);
    }
}

// Standard iteration for solving Kepler's Equation
function solveKeplerFunc1(ecc: number, M: number): Func {
    return function (x: number) {
        return M + ecc * Math.sin(x);
    };
}

// Faster converging iteration for Kepler's Equation; more efficient
// than above for orbits with eccentricities greater than 0.3.  This
// is from Jean Meeus's _Astronomical Algorithms_ (2nd ed), p. 199
function solveKeplerFunc2(ecc: number, M: number): Func {
    return function (x: number) {
        return x + (M + ecc * Math.sin(x) - x) / (1 - ecc * Math.cos(x));
    };
}

function solveKeplerLaguerreConway(ecc: number, M: number): Func {
    return function (x: number) {
        const s = ecc * Math.sin(x);
        const c = ecc * Math.cos(x);
        const f = x - s - M;
        const f1 = 1 - c;
        x += (-5 * f) / (f1 + Math.sign(f1) * Math.sqrt(Math.abs(16 * f1 * f1 - 20 * f * s)));
        return x;
    };
}

function solveKeplerLaguerreConwayHyp(ecc: number, M: number): Func {
    return function (x: number) {
        const s = ecc * Math.sinh(x);
        const c = ecc * Math.cosh(x);
        const f = s - x - M;
        const f1 = c - 1;
        x += (-5 * f) / (f1 + Math.sign(f1) * Math.sqrt(Math.abs(16 * f1 * f1 - 20 * f * s)));
        return x;
    };
}

export function getEllipticalEccentricAnomaly(meanAnomaly: number, eccentricity: number): number {
    const tol = 0.00000001745;
    let iterations = 20;
    let e = meanAnomaly - 2.0 * Math.PI * ((meanAnomaly / (2.0 * Math.PI)) | 0);
    let err = 1;
    while (Math.abs(err) > tol && iterations > 0) {
        err = e - eccentricity * Math.sin(e) - meanAnomaly;
        let delta = err / (1 - eccentricity * Math.cos(e));
        e -= delta;
        iterations--;
    }
    return e;
}

export function getTrueAnomaly(eccentricAnomaly: number, eccentricity: number): number {
    const revs = Math.floor(eccentricAnomaly / math.TWO_PI);
    eccentricAnomaly -= revs * math.TWO_PI;
    let trueAnomaly = Math.atan2(
        Math.sin(eccentricAnomaly) * Math.sqrt(1 - eccentricity * eccentricity),
        Math.cos(eccentricAnomaly) - eccentricity
    );
    trueAnomaly = math.zeroTwoPI(trueAnomaly);
    if (eccentricAnomaly < 0) {
        trueAnomaly -= math.TWO_PI;
    }
    return trueAnomaly + revs * math.TWO_PI;
}

export function getPerifocalToCartesianMatrix(
    argumentOfPeriapsis: number,
    inclination: number,
    rightAscension: number
): Mat3 {
    let res = new Mat3();
    let cosap = Math.cos(argumentOfPeriapsis),
        sinap = Math.sin(argumentOfPeriapsis),
        cosi = Math.cos(inclination),
        sini = Math.sin(inclination),
        cosraan = Math.cos(rightAscension),
        sinraan = Math.sin(rightAscension);
    res._m[0] = cosraan * cosap - sinraan * sinap * cosi;
    res._m[1] = sinraan * cosap + cosraan * sinap * cosi;
    res._m[2] = sinap * sini;
    res._m[3] = -cosraan * sinap - sinraan * cosap * cosi;
    res._m[4] = -sinraan * sinap + cosraan * cosap * cosi;
    res._m[5] = cosap * sini;
    res._m[6] = sinraan * sini;
    res._m[7] = -cosraan * sini;
    res._m[8] = cosi;
    return res;
}