/************************ 
Ordinal Three Body Orbits
************************/

function ode45(outputState, inputState, f, options) {
    // Code of this function is from [https://observablehq.com/@rreusser/periodic-planar-three-body-orbits]
    // Copyright 2021 Ricky Reusser
    // Released under the MIT license
    const EPSILON = 2.220446049250313e-16;
    function minMag(a, b) {
        return (a > 0 ? Math.min : Math.max)(a, b);
    }
    function maxMag(a, b) {
        return (a > 0 ? Math.max : Math.min)(a, b);
    }
    var scratch = new Float64Array(1024);
    var k1tmp, k2tmp, k3tmp, k4tmp, k5tmp, k6tmp, w;
    if (typeof f !== 'function') {
        options = f;
        f = inputState;
        inputState = outputState;
    }
    options = options || {};
    var out = outputState || {};
    var tolerance = options.tolerance === undefined ? 1e-8 : +options.tolerance;
    let tolerance2 = tolerance * tolerance;
    var maxIncreaseFactor =
        options.maxIncreaseFactor === undefined ? 10 : options.maxIncreaseFactor;
    var maxDecreaseFactor =
        options.maxDecreaseFactor === undefined ? 10 : options.maxDecreaseFactor;
    var tLimit =
        options.tLimit === undefined
            ? inputState.dt > 0.0
                ? Infinity
                : -Infinity
            : options.tLimit;
    var safetyFactor = 0.9;
    var y = inputState.u;
    var n = y.length;
    var dt = inputState.dt === undefined ? 1.0 : +inputState.dt;
    var t = inputState.t === undefined ? 0.0 : +inputState.t;
    if (out.y === undefined) {
        out.y = new Float64Array(n);
    }
    var yOut = out.y;
    var inPlace = yOut === y;
    if (n * 7 > scratch.length) {
        scratch = new Float64Array(nextPow2(n * 7));
    }
    if (!w || w.length !== n) {
        w = scratch.subarray(0, n);
        k1tmp = scratch.subarray(1 * n, 2 * n);
        k2tmp = scratch.subarray(2 * n, 3 * n);
        k3tmp = scratch.subarray(3 * n, 4 * n);
        k4tmp = scratch.subarray(4 * n, 5 * n);
        k5tmp = scratch.subarray(5 * n, 6 * n);
        k6tmp = scratch.subarray(6 * n, 7 * n);
    }
    var w = inPlace ? w : out;
    tmp = f(k1tmp, y, t);
    var returnsOutput = tmp !== undefined && tmp.length >= n;
    k1 = returnsOutput ? tmp : k1tmp;
    var limitReached = false;
    var trialStep = 0;
    var thisDt = dt;
    while (true && trialStep++ < 1000) {
        thisDt = minMag(thisDt, tLimit - t);
        for (i = 0; i < n; i++) {
            w[i] = y[i] + thisDt * (0.2 * k1[i]);
        }
        tmp = f(k2tmp, w, t + dt * 0.2);
        k2 = returnsOutput ? tmp : k2tmp;
        for (i = 0; i < n; i++) {
            w[i] = y[i] + thisDt * (0.075 * k1[i] + 0.225 * k2[i]);
        }
        tmp = f(k3tmp, w, t + thisDt * 0.3);
        k3 = returnsOutput ? tmp : k3tmp;
        for (i = 0; i < n; i++) {
            w[i] = y[i] + thisDt * (0.3 * k1[i] + -0.9 * k2[i] + 1.2 * k3[i]);
        }
        tmp = f(k4tmp, w, t + thisDt * 0.6);
        k4 = returnsOutput ? tmp : k4tmp;
        for (i = 0; i < n; i++) {
            w[i] =
                y[i] +
                thisDt *
                (-0.203703703703703703 * k1[i] +
                    2.5 * k2[i] +
                    -2.592592592592592592 * k3[i] +
                    1.296296296296296296 * k4[i]);
        }
        tmp = f(k5tmp, w, t + thisDt);
        var k5 = returnsOutput ? tmp : k5tmp;
        for (i = 0; i < n; i++) {
            w[i] =
                y[i] +
                thisDt *
                (0.029495804398148148 * k1[i] +
                    0.341796875 * k2[i] +
                    0.041594328703703703 * k3[i] +
                    0.400345413773148148 * k4[i] +
                    0.061767578125 * k5[i]);
        }
        tmp = f(k6tmp, w, t + thisDt * 0.875);
        var k6 = returnsOutput ? tmp : k6tmp;
        var error2 = 0;
        for (var i = 0; i < n; i++) {
            let d =
                thisDt *
                (0.004293774801587301 * k1[i] +
                    -0.018668586093857832 * k3[i] +
                    0.034155026830808080 * k4[i] +
                    0.019321986607142857 * k5[i] +
                    -0.039102202145680406 * k6[i]);
            error2 += d * d;
        }
        if (error2 < tolerance2 || thisDt === 0.0) {
            break;
        }
        var nextDt = safetyFactor * thisDt * Math.pow(tolerance2 / error2, 0.1);
        thisDt = maxMag(thisDt / maxDecreaseFactor, nextDt);
    }
    for (var i = 0; i < n; i++) {
        y[i] +=
            thisDt *
            (0.097883597883597883 * k1[i] +
                0.402576489533011272 * k3[i] +
                0.210437710437710437 * k4[i] +
                0.289102202145680406 * k6[i]);
    }
    var previousDt = thisDt;
    out.t += thisDt;
    nextDt = safetyFactor * thisDt * Math.pow(tolerance2 / error2, 0.125);
    out.dt = maxMag(
        thisDt / maxDecreaseFactor,
        minMag(thisDt * maxIncreaseFactor, nextDt)
    );
    out.dtPrevious = thisDt;
    out.limitReached =
        isFinite(tLimit) &&
        Math.abs((out.t - options.tLimit) / previousDt) < EPSILON;

    return out;
}


class Physics {
    constructor() {
        this.initialConditions = {};
        if (typeof overwriteOdeCfg === 'undefined') {
            this.ode45Configs = {
                tolerance: 1e-13,
                tLimit: 9999999
            }
        }
        else {
            this.ode45Configs = overwriteOdeCfg;
        }
    }

    initState() {
        this.state = { u: new Array(this.initialConditions.bodies * 4).fill(0), t: 0, dt: null };
        this.moveToCenterOfMass()
    }

    calculateCenterOfMassVelocity() {
        var centerOfMassVelocity = { x: 0, y: 0 }
        var sumOfMasses = 0

        for (var iBody = 0; iBody < this.initialConditions.bodies; iBody++) {
            centerOfMassVelocity.x +=
                this.initialConditions.masses[iBody] * this.initialConditions.velocities[iBody].x
            centerOfMassVelocity.y +=
                this.initialConditions.masses[iBody] * this.initialConditions.velocities[iBody].y
            sumOfMasses += this.initialConditions.masses[iBody]
        }

        centerOfMassVelocity.x /= sumOfMasses
        centerOfMassVelocity.y /= sumOfMasses

        return centerOfMassVelocity
    }

    calculateCenterOfMass() {
        var centerOfMass = { x: 0, y: 0 }
        var sumOfMasses = 0

        for (var iBody = 0; iBody < this.initialConditions.bodies; iBody++) {
            centerOfMass.x +=
                this.initialConditions.masses[iBody] * this.initialConditions.positions[iBody].x
            centerOfMass.y +=
                this.initialConditions.masses[iBody] * this.initialConditions.positions[iBody].y
            sumOfMasses += this.initialConditions.masses[iBody]
        }

        centerOfMass.x /= sumOfMasses
        centerOfMass.y /= sumOfMasses

        return centerOfMass
    }
    moveToCenterOfMass() {
        var centerOfMassVelocity = this.calculateCenterOfMassVelocity()
        var centerOfMass = this.calculateCenterOfMass()

        for (let iBody = 0; iBody < this.initialConditions.bodies; iBody++) {
            this.initialConditions.positions[iBody].x -= centerOfMass.x
            this.initialConditions.positions[iBody].y -= centerOfMass.y
            this.initialConditions.velocities[iBody].x -= centerOfMassVelocity.x
            this.initialConditions.velocities[iBody].y -= centerOfMassVelocity.y
        }
    }
    resetStateToInitialConditions() {
        var iBody, bodyStart

        for (iBody = 0; iBody < this.initialConditions.bodies; iBody++) {
            bodyStart = iBody * 4

            var position = this.initialConditions.positions[iBody]
            this.state.u[bodyStart + 0] = position.x
            this.state.u[bodyStart + 1] = position.y

            var velocity = this.initialConditions.velocities[iBody]
            this.state.u[bodyStart + 2] = velocity.x
            this.state.u[bodyStart + 3] = velocity.y
        }

    }

    shiftConditions() {
        function dotProduct(a, b) {
            return a.x * b.x + a.y * b.y;
        }

        function norm(v) {
            return Math.sqrt(v.x * v.x + v.y * v.y);
        }

        var iBody, bodyStart, endx, endy, end_position, ini_position;
        let dotProductResult, cross_product, radian;
        let radianSum = 0;

        for (iBody = 0; iBody < this.initialConditions.bodies; iBody++) {
            bodyStart = iBody * 4;

            [endx, endy] = this.state.u.slice(bodyStart, bodyStart + 2);
            end_position = { x: endx, y: endy };
            ini_position = this.initialConditions.positions[iBody];

            dotProductResult = dotProduct(ini_position, end_position) / (norm(ini_position) * norm(end_position));
            dotProductResult = Math.max(-1, Math.min(1, dotProductResult));

            cross_product = ini_position.x * end_position.y - ini_position.y * end_position.x;
            radian = Math.acos(dotProductResult);

            if (cross_product < 0) {
                radian = -radian;
            }

            radianSum += radian;
        }

        radian = radianSum / this.initialConditions.bodies;

        let rotation_matrix = [
            [Math.cos(radian), -Math.sin(radian)],
            [Math.sin(radian), Math.cos(radian)]
        ];

        for (iBody = 0; iBody < this.initialConditions.bodies; iBody++) {
            bodyStart = iBody * 4

            let position = this.initialConditions.positions[iBody]
            let velocity = this.initialConditions.velocities[iBody]

            // Rotate position
            this.state.u[bodyStart + 0] = rotation_matrix[0][0] * position.x + rotation_matrix[0][1] * position.y;
            this.state.u[bodyStart + 1] = rotation_matrix[1][0] * position.x + rotation_matrix[1][1] * position.y;

            // Rotate velocity
            this.state.u[bodyStart + 2] = rotation_matrix[0][0] * velocity.x + rotation_matrix[0][1] * velocity.y;
            this.state.u[bodyStart + 3] = rotation_matrix[1][0] * velocity.x + rotation_matrix[1][1] * velocity.y;
        }
    }

    getPlanarNBodyDerivative = () => {
        const masses = this.initialConditions.masses;
        return (yp, y, t) => {
            var n = masses.length;
            for (var i = 0; i < n; i++) {
                var mi = masses[i];
                var xi = y[i * 4], yi = y[i * 4 + 1], vxi = y[i * 4 + 2], vyi = y[i * 4 + 3];

                // The first-order derivative of the position is the velocity
                yp[i * 4] = vxi;
                yp[i * 4 + 1] = vyi;

                // The first-order derivative of the velocity is the acceleration
                yp[i * 4 + 2] = 0;
                yp[i * 4 + 3] = 0;

                for (var j = 0; j < n; j++) {
                    if (i == j) continue;
                    var mj = masses[j];
                    var xj = y[j * 4], yj = y[j * 4 + 1];
                    var dx = xj - xi, dy = yj - yi;
                    var r3 = Math.pow(dx * dx + dy * dy, 1.5);

                    yp[i * 4 + 2] += mj * dx / r3;  // Accumulate acceleration in x direction
                    yp[i * 4 + 3] += mj * dy / r3;  // Accumulate acceleration in y direction
                }
            }
        }
    }


    updatePosition(maxStepTime) {
        this.state.dt = Math.min(this.state.dt, maxStepTime)
        let out = ode45(this.state, this.getPlanarNBodyDerivative(), this.ode45Configs)
        return out.dtPrevious
    }

    changeInitialConditions(conditions) {
        this.initialConditions.masses = conditions.masses.slice()
        this.initialConditions.bodies = conditions.masses.length
        this.initialConditions.positions = conditions.positions
        this.initialConditions.velocities = conditions.velocities
        this.initState()
    }
}