/*
 *  2022-4-8
 *
 *  Originally written by Nerdu#7492
 *
 *  2D orbital mechanics 
 *  solves for both Keplerian elements and orbital state vectors
 *  Supports hyperbolic orbits
 *
 */

using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class spaceshipScript : MonoBehaviour
{
    private Camera cam;
    private LineRenderer orbitLR;
    private LineRenderer shipToPlanetLR;
    private LineRenderer velocityLR;
    private LineRenderer perpRLR;

    public float mouseScrollMultiplier;
    public AnimationCurve curveForMouseZoom;
    public float maxMouseZoomMultiplier;

    public float minZoom;

    public float maxZoom;

    public Transform orbitedPlanet;
    public celestialBodyScript orbitedPlanetScript;
    

    private Rigidbody2D rb;

    public Vector2 initVelocity;
    public OrbitalParameters currentOrbitalParams;

    private orbitSimulationManager simManager;

    public float orbitDrawnMaxWidth;
    public AnimationCurve orbitDrawnWidthCurve;

    public float thrustVelocity;

    public float rotationSpeed;

    //km
    public float physicsDistance;
    // Start is called before the first frame update
    void Start()
    {
        orbitLR = transform.GetChild(0).GetComponent<LineRenderer>();
        shipToPlanetLR = transform.GetChild(1).GetComponent<LineRenderer>();
        velocityLR = transform.GetChild(2).GetComponent<LineRenderer>();
        perpRLR = transform.GetChild(3).GetComponent<LineRenderer>();
        simManager = GameObject.FindGameObjectWithTag("simManager").GetComponent<orbitSimulationManager>();
        cam = Camera.main;
        rb = GetComponent<Rigidbody2D>();
        rb.velocity = initVelocity / simManager.scale;
        Vector2 posFromPlanet = (Vector2)transform.position  - (Vector2) orbitedPlanet.position;
        posFromPlanet *= simManager.scale;
        CalculateOrbitalParams(new Vector2d((double)initVelocity.x, (double)initVelocity.y), new Vector2d(posFromPlanet.x, posFromPlanet.y), orbitedPlanetScript.mass, true);
    }

    // Update is called once per frame
    void Update()
    {
        if (Input.GetKey(KeyCode.A))
        {
            transform.Rotate(0f,0f,rotationSpeed * Time.deltaTime * simManager.timeScale);
        }
        if (Input.GetKey(KeyCode.D))
        {
            transform.Rotate(0f,0f,-rotationSpeed * Time.deltaTime * simManager.timeScale);
        }
        ZoomOrthoCamera();
        float orbitWidthVal = orbitDrawnMaxWidth * orbitDrawnWidthCurve.Evaluate(cam.orthographicSize / maxZoom);
        orbitLR.startWidth = orbitWidthVal;
        orbitLR.endWidth = orbitWidthVal;
        shipToPlanetLR.startWidth = orbitWidthVal;
        shipToPlanetLR.endWidth = orbitWidthVal;
        velocityLR.startWidth = orbitWidthVal;
        velocityLR.endWidth = orbitWidthVal;
        perpRLR.startWidth = orbitWidthVal;
        perpRLR.endWidth = orbitWidthVal;
    }

    private void FixedUpdate()
    {
        Vector2d dir = new Vector2d(Mathd.Cos((double)transform.rotation.eulerAngles.z * Mathd.Deg2Rad), Mathd.Sin((double)transform.rotation.eulerAngles.z * Mathd.Deg2Rad));
        if (Input.GetKey(KeyCode.Space))
        {
            if (Vector2.Distance(transform.position, orbitedPlanet.position) > (orbitedPlanetScript.radius + physicsDistance)/simManager.scale)
            {
                Vector2 posFromPlanet = (Vector2)transform.position  - (Vector2) orbitedPlanet.position;
                if (simManager.timeScale > 8)
                {
                    simManager.timeScale = 8;
                }
                posFromPlanet *= simManager.scale;
                Vector2d currentVel = new Vector2d(currentOrbitalParams.v * Mathd.Cos(currentOrbitalParams.theta), currentOrbitalParams.v * Mathd.Sin(currentOrbitalParams.theta));
                currentVel += thrustVelocity * dir * Time.fixedDeltaTime * simManager.timeScale;
                CalculateOrbitalParams(currentVel, new Vector2d(posFromPlanet.x, posFromPlanet.y), orbitedPlanetScript.mass, false);
                rb.velocity = new Vector2((float)currentVel.x, (float)currentVel.y) / simManager.scale;
            }
            else
            {
                //print((new Vector2((float)dir.x, (float)dir.y) * (thrustVelocity * simManager.timeScale * Time.fixedDeltaTime)));
                rb.AddForce(new Vector2((float)dir.x, (float)dir.y).normalized * (50f * Time.deltaTime));
                //rb.velocity = rb.velocity + (new Vector2((float)dir.x, (float)dir.y) * (thrustVelocity * simManager.timeScale * Time.fixedDeltaTime))/simManager.scale;
                Vector2d currentVel = new Vector2d(rb.velocity.x, rb.velocity.y) * simManager.scale;
                Vector2 posFromPlanet = (Vector2)transform.position  - (Vector2) orbitedPlanet.position;
                posFromPlanet *= simManager.scale;
                CalculateOrbitalParams(currentVel, new Vector2d(posFromPlanet.x, posFromPlanet.y), orbitedPlanetScript.mass, false);
                
            }
            
        }

        if (Vector2.Distance(transform.position, orbitedPlanet.position) > (orbitedPlanetScript.radius + physicsDistance)/simManager.scale)
        {
            rb.isKinematic = true;
            Vector2d pos = currentOrbitalParams.RecalculateAtTime(simManager.time);
            Vector3 relative = new Vector3((float) pos.x / simManager.scale, (float) pos.y / simManager.scale, 0f);
            shipToPlanetLR.positionCount = 2;
            perpRLR.positionCount = 2;
            shipToPlanetLR.SetPositions(new Vector3[]{new Vector3(0f,0f,0f), relative});
            perpRLR.SetPositions(new Vector3[]{relative, relative + new Vector3((float)dir.x, (float)dir.y) * 500f});
            float velAngle = (float) currentOrbitalParams.theta;
            float vel = (float) currentOrbitalParams.v;
            Vector3 velocityVector = new Vector3(vel * Mathf.Cos(velAngle), vel * Mathf.Sin(velAngle));
            velocityLR.SetPositions(new Vector3[]{relative, velocityVector + relative});
            transform.position = orbitedPlanet.position + relative;
            Vector2d currentVel = new Vector2d(currentOrbitalParams.v * Mathd.Cos(currentOrbitalParams.theta), currentOrbitalParams.v * Mathd.Sin(currentOrbitalParams.theta));
            rb.velocity = new Vector2((float)currentVel.x, (float)currentVel.y) / simManager.scale;
        }
        else
        {
            if (simManager.timeScale > 4)
            {
                simManager.timeScale = 4;
            }
            rb.isKinematic = false;
            double gravVal = currentOrbitalParams.G *
                             (orbitedPlanetScript.mass /
                              Mathf.Pow(Vector2.Distance(orbitedPlanet.position, transform.position) * simManager.scale,
                                  2));
            print(gravVal);
            print((float)gravVal * Time.fixedDeltaTime * (10/simManager.scale));
            rb.velocity = rb.velocity + (float) gravVal * (1 / simManager.scale) * Time.fixedDeltaTime * simManager.timeScale *
                           (Vector2)(orbitedPlanet.position - transform.position).normalized;
            //rb.AddForce(((float)gravVal * rb.mass * (10/simManager.scale) * Time.fixedDeltaTime * simManager.timeScale * (orbitedPlanet.position - transform.position).normalized) );
            currentOrbitalParams.v = ((double) rb.velocity.magnitude * Mathd.Sign((double) rb.velocity.magnitude)) *
                                     simManager.scale;
            currentOrbitalParams.theta = Mathd.Atan2(rb.velocity.normalized.y, rb.velocity.normalized.x);
        }
       

    }

    private void LateUpdate()
    {
        orbitLR.transform.position = orbitedPlanet.position;
        orbitLR.transform.rotation = Quaternion.identity;
        perpRLR.transform.position = orbitedPlanet.position;
        perpRLR.transform.rotation = Quaternion.identity;
        velocityLR.transform.position = orbitedPlanet.position;
        velocityLR.transform.rotation = Quaternion.identity;
        shipToPlanetLR.transform.position = orbitedPlanet.position + new Vector3(0f, 0f, 1f);
        shipToPlanetLR.transform.rotation = Quaternion.identity;
        Camera.main.transform.rotation = Quaternion.identity;
        //RotateCam();
    }

    void CalculateOrbitalParams(Vector2d initialVel, Vector2d positionFromPlanet, double planetMass, bool doPrints)
    {
        currentOrbitalParams = new OrbitalParameters();
        currentOrbitalParams.CalculateAllInitialParams(initialVel, positionFromPlanet, planetMass, simManager.time - Time.fixedDeltaTime * simManager.timeScale, doPrints);
        DrawOrbit(100);
    }

    void DrawOrbit(int posCount)
    {
        orbitLR.positionCount = posCount;
        Vector3[] positions = new Vector3[posCount];
        for (var i = 0; i < posCount; i++)
        {
            double value;
            if (currentOrbitalParams.e < 1)
            {
                value = ((currentOrbitalParams.T / posCount) * i) + currentOrbitalParams.t0;;
            }
            else
            {
                value = ((2 * Mathd.PI / posCount) * i) + currentOrbitalParams.w;
            }
            
            //double dist = currentOrbitalParams.EvaluateEllipticalOrbit(currentOrbitalParams.a, currentOrbitalParams.e,
            //    currentOrbitalParams.w, angle);
            //Vector2d pos = new Vector2d(dist * Mathd.Cos(angle), dist * Mathd.Sin(angle));
            Vector2d pos = currentOrbitalParams.GetPositionAtTime(value);
            Vector2 adjustedPos = new Vector2((float)pos.x/simManager.scale, (float)pos.y/simManager.scale);
            positions[i] = adjustedPos;
        }
        orbitLR.SetPositions(positions);
    }

    void ZoomOrthoCamera()
    {
        // Calculate how much we will have to move towards the zoomTowards position

        mouseScrollMultiplier = curveForMouseZoom.Evaluate(cam.orthographicSize/maxZoom) * maxMouseZoomMultiplier;
        // Zoom camera
        cam.orthographicSize -= Input.mouseScrollDelta.y * mouseScrollMultiplier;

        // Limit zoom
        cam.orthographicSize = Mathf.Clamp(cam.orthographicSize, minZoom, maxZoom);
    }

    void RotateCam()
    {
        Vector2 dir = transform.position - orbitedPlanet.position;
        float angle = Mathf.Atan2(dir.y, dir.x) * Mathf.Rad2Deg;
        angle -= 90f;
        cam.transform.rotation = Quaternion.Euler(0f, 0f, angle);
    }
    
}

[System.Serializable]
public class OrbitalParameters
{
    [Header("Semi-major axis")] public double a;
    [Header("Semi-minor axis")] public double b;
    [Header("Eccentricity")] public double e;
    [Header("True Anomaly")] public double f;
    [Header("Angle Of Perigee")] public double w;
    [Header("Eccentric Anomaly")] public double E;
    [Header("Initial Mean Anomaly")] public double M0;
    [Header("Initial Time")] public double t0;
    [Header("Current Mean Anomaly")] public double M;
    [Header("Orbital Period")] public double T;
    [Header("Mean Motion")] public double n;
    [Header("Flight Path Angle")] public double phi;
    [Header("Velocity")] public double v;
    [Header("VelAngle")] public double theta;

    private Vector2d eVec;
    [HideInInspector]
    public double h;
    private double u;

    [HideInInspector]
    public double G = 6.67408 * Mathd.Pow(10, -11);

    public void CalculateAllInitialParams(Vector2d velocity, Vector2d positionFromPlanet, double planetMass, double _t0, bool doPrints)
    {
        u = G * planetMass;
        t0 = _t0;
        if (doPrints)
        {
            Debug.Log("circleVel: " + Mathd.Sqrt(u/positionFromPlanet.magnitude) + ", escape Vel: " +  Mathd.Sqrt(2 * u/positionFromPlanet.magnitude));
        }
        //circular vel
        //velocity = new Vector2d(-Mathd.Sqrt(u/positionFromPlanet.magnitude), 0);
        //parabolic vel - escape vel
        //velocity = new Vector2d(Mathd.Sqrt(2 * u/positionFromPlanet.magnitude), 0);
        a = CalculateSemiMajorAxis(velocity.magnitude, positionFromPlanet.magnitude, u);
        h = Vector2d.CrossProduct(positionFromPlanet, velocity);
        eVec = CalculateEccentricityVector(velocity, positionFromPlanet, u);
        e = eVec.magnitude;
        T = CalculateOrbitalPeriod(u, a, e);
        n = CalculateMeanMotion(T, h, e, u, a);
        f = TrueAnomalyFromStateVectors(eVec, positionFromPlanet, velocity);
        w = AngleOfPerigee(positionFromPlanet, f);

        if (doPrints)
        {
            Debug.Log("(" + eVec.x + ", " + eVec.y + ")");
        }
        if (e < 1)
        {
            E = CalculateEccentricAnomalyFromTrueAnomaly(f, e);
            M0 = MeanAnomalyFromEccentricAnomaly(E, e);
            M = M0;
            if (doPrints)
            {
                Debug.Log(f + " " + E);
            }
        }
        else if(e >= 1)
        {
            E = CalculateHyperbolicAnomalyFromTrueAnomaly(f, e);
            M0 = MeanAnomalyFromHyperbolicAnomaly(E, e);
            M = M0;
            if (doPrints)
            {
                Debug.Log(f + " " + E + " " + (e * Mathd.Sinh(E) - E));
            }
        }
        else
        {
            
        }

        v = CalculateVelocityVisViva(a, u, positionFromPlanet.magnitude);
        phi = CalculateFlightPathAngle(e, f);
        theta = GetCurrentDirectionOfVelocity(positionFromPlanet, phi, h);

        //E = CalculateEccentricAnomalyFromMeanAnomaly(e, M, 5);
    }

    //not context based
    public Vector2d GetPositionAtTime(double t)
    {
        if (e < 1)
        {
            double _M = M0 + n * (t - t0);
            double _E = CalculateEccentricAnomalyFromMeanAnomaly(e, _M, 7);
            double _f = CalculateTrueAnomalyFromEccentricAnomaly(_E, e, 7);
            double angle = _f + w;
            double r = EvaluateOrbit(a, e, w, angle);
            return new Vector2d(r * Mathd.Cos(angle),r * Mathd.Sin(angle));
        }
        else
        {
            /*double _M = M0 + n * (t - t0);
            double _E = CalculateHyperbolicAnomalyFromMeanAnomaly(e, _M, 7);
            double _f = CalculateTrueAnomalyFromHyperbolicAnomaly(_E, e);
            double angle = _f + w;
            double r = EvaluateOrbit(a, e, w, angle);
            return new Vector2d(r * Mathd.Cos(angle),r * Mathd.Sin(angle));*/
            double r = EvaluateOrbit(a, e, w, t);
            return new Vector2d(r * Mathd.Cos(t), r * Mathd.Sin(t));
        }
        
    }

    //context based
    public Vector2d RecalculateAtTime(double t)
    {
        if (e < 1)
        {
            M = M0 + n * (t - t0);
            E = CalculateEccentricAnomalyFromMeanAnomaly(e, M, 7);
            f = CalculateTrueAnomalyFromEccentricAnomaly(E, e, 7);
            double angle = f + w;
            double r = EvaluateOrbit(a, e, w, angle);
            Vector2d pos = new Vector2d(r * Mathd.Cos(angle),r * Mathd.Sin(angle));
            v = CalculateVelocityVisViva(a, u, pos.magnitude);
            phi = CalculateFlightPathAngle(e, f);
            theta = GetCurrentDirectionOfVelocity(pos, phi,h);
            return pos;
        }
        else
        {
            M = M0 + n * (t - t0);
            E = CalculateHyperbolicAnomalyFromMeanAnomaly(e, M, 7);
            f = CalculateTrueAnomalyFromHyperbolicAnomaly(E, e);
            double angle = f + w;
            double r = EvaluateOrbit(a, e, w, angle);
            Vector2d pos = new Vector2d(r * Mathd.Cos(angle),r * Mathd.Sin(angle));
            v = CalculateVelocityVisViva(a, u, pos.magnitude);
            phi = CalculateFlightPathAngle(e, f);
            theta = GetCurrentDirectionOfVelocity(pos, phi, h);
            return pos;
        }
    }

    public double EvaluateOrbit(double _a, double _e, double _w, double angle)
    {
        double top = _a * (1 - Mathd.Pow(_e, 2));
        double bot = 1 + e * Mathd.Cos(angle - _w);
        return top / bot;
    }

    public double CalculateVelocityVisViva(double _a, double _u, double _r)
    {
        return Mathd.Sqrt(_u * ((2.0 / _r) - (1.0 / _a)));
    }

    public double CalculateFlightPathAngle(double _e, double _f)
    {
        return Mathd.Atan2(_e * Mathd.Sin(_f), 1 + _e * Mathd.Cos(_f));
    }

    public double GetCurrentDirectionOfVelocity(Vector2d pos, double _phi, double _h)
    {
        Vector2d perp = new Vector2d(pos.y, -pos.x) * 0.5f;
        Vector2d perp2 = new Vector2d(-pos.y, pos.x) * 0.5f;
        double perpAngle = Mathd.Atan2(perp2.y - perp.y, perp2.x - perp.x);
        double velAngle;
        if (_h < 0)
        {
            velAngle = perpAngle - _phi + Mathd.PI;
        }
        else
        {
            velAngle = perpAngle - _phi;
        }
        
        return velAngle;
    }

    public double AngleOfPerigee(Vector2d r, double _f)
    {
        return Mathd.Atan2(r.y, r.x) - _f;
    }

    public double CalculateSemiMajorAxis(double v, double r, double u)
    {
        return (u * r) / (2 * u - (Mathd.Pow(v, 2) * r));
    }

    public double CalculateOrbitalPeriod(double u, double _a, double _e)
    {
        if (_e <= 1)
        {
            return 2 * Mathd.PI * Mathd.Sqrt(Mathd.Pow(_a, 3) / u);
        }
        else
        {
            return -1;
        }
    }

    //WRONG GIVES MEAN MOTION U IDIOT MF.
    public double CalculateMeanMotion(double _T, double _h, double _e, double _u, double _a)
    {
        if (_e < 1)
        {
            return (2 * Mathd.PI) / _T * Mathd.Sign(_h);
        }
        else if (_e > 1)
        {
            return Mathd.Sqrt(_u / -Mathd.Pow(_a, 3)) * Mathd.Sign(_h);
        }
        else
        {
            return Mathd.Sqrt(_u / Mathd.Pow(_a, 3)) * Mathd.Sign(_h);
        }
        
    }

    public Vector2d CalculateEccentricityVector(Vector2d v, Vector2d r, double u)
    {
        double _h = Vector2d.CrossProduct(r, v);
        return ((Mathd.Pow(v.magnitude, 2) - (u / r.magnitude)) * r - Vector2d.Dot(r, v) * v) / u;
        //return new Vector2d(((v.y * _h)/u) - (r.x/r.magnitude), ((v.x * _h)/u) - (r.y/r.magnitude));
    }

    public double CalculateEccentricAnomalyFromMeanAnomaly(double _e, double _M, int dp)
    {
        //dp = number of decimal places

        int maxIter=30, i=0;

        double delta=Mathd.Pow(10,-dp);

        double _E, F;

        _E=_M;

        F = _E - _e*Mathd.Sin(_M) - _M;

        while ((Mathd.Abs(F)>delta) && (i<maxIter)) {

            _E = _E - F/(1.0-_e*Mathd.Cos(_E));
            F = _E - _e*Math.Sin(_E) - _M;

            i = i + 1;

        }

        return _E;
    }

    public double CalculateHyperbolicAnomalyFromMeanAnomaly(double _e, double _M, int dp)
    {
        //dp = number of decimal places

        int maxIter=30, i=0;

        double delta=Mathd.Pow(10,-dp);

        double _E, F;

        _E=_M;

        F = _e * Mathd.Sinh(_E) - _E - _M;

        while ((Mathd.Abs(F)>delta) && (i<maxIter)) {

            _E = _E + F/(1.0 - _e * Mathd.Cosh(_E));
            F = _e * Mathd.Sinh(_E) - _E - _M;

            i = i + 1;

        }

        return _E;
    }

    public double CalculateEccentricAnomalyFromTrueAnomaly(double _f, double _e)
    {
        return Mathd.Atan2(Mathd.Sqrt(1 - Mathd.Pow(_e, 2)) * Mathd.Sin(_f), _e + Mathd.Cos(_f));
    }

    public double CalculateHyperbolicAnomalyFromTrueAnomaly(double _f, double _e)
    {
        return 2 * Mathd.Atanh(Mathd.Sqrt((_e - 1)/(_e + 1)) * Mathd.Tan(_f/2));
    }

    public double MeanAnomalyFromEccentricAnomaly(double _E, double _e)
    {
        return _E - _e * Mathd.Sin(_E);
    }

    public double MeanAnomalyFromHyperbolicAnomaly(double _H, double _e)
    {
        return _e * Mathd.Sinh(_H) - _H;
    }

    public double CalculateTrueAnomalyFromEccentricAnomaly(double _E, double _e, int dp)
    {
        return Mathd.Atan2(Mathd.Sqrt(1 - Mathd.Pow(_e, 2)) * Mathd.Sin(_E), Mathd.Cos(_E) - _e);
    }

    public double CalculateTrueAnomalyFromHyperbolicAnomaly(double _H, double _e)
    {
        return 2 * Mathd.Atan2(Mathd.Sqrt(_e + 1) * Mathd.Sinh(_H/2), Mathd.Sqrt(_e - 1) * Mathd.Cosh(_H/2));
    }

    public double TrueAnomalyFromStateVectors(Vector2d _e, Vector2d r, Vector2d v)
    {
        if (_e.magnitude > 0)
        {
            double h = Vector2d.CrossProduct(r, v);
            double val = Mathd.Acos(Vector2d.Dot(_e, r) / (_e.magnitude * r.magnitude));
            if (!Mathd.Approximately(Mathd.Sign(Vector2d.Dot(r, v)), Mathd.Sign(h)))
            {
                return (2 * Mathd.PI) - val;
            }
            else
            {
                return val;
            }
        }
        else
        {
            return Mathd.Atan2(r.y, r.x);
        }
        
    }
    
}