#region File Description

//-----------------------------------------------------------------------------

// KeplerOrbit.cs

// Partial Transcript of the Kepler Orbit Simulation Toolkit (KOST) by 

// C. J. Plooy and Tim Blaxland

//

// Orbiter Navigator 

// Alexander Blaß - 2010

//-----------------------------------------------------------------------------

#endregion



using System;

using System.Collections.Generic;

//using System.Linq;

using System.Text;

using Orbiter_Navigator.VectorMath;



namespace Orbiter_Navigator.Model

{

    /// <summary>

    /// The KeplerOrbiter class encapsulates data needed to describe a Kepler Orbit ellipse

    /// and methods for all kinds of calculations around Kepler Orbits. 

    /// The code of this class is a partial transcript of the Kepler Orbit Simulation Toolkit (KOST) by 

    /// C. J. Plooy and Tim Blaxland. 

    /// </summary>

    public class KeplerOrbit

    {

        /// <summary>

        /// Semi-major axis

        /// </summary>

        public double a; 



        /// <summary>

        /// Eccentricity

        /// </summary>

        public double e; 



        /// <summary>

        /// Inclination

        /// </summary>

        public double i; 

        

        /// <summary>

        /// Longitude of ascending node

        /// </summary>

        public double theta;



        /// <summary>

        /// Longitude of periapsis

        /// </summary>

        public double omegab;



        /// <summary>

        /// Mean longitude at epoch

        /// </summary>

        public double L;





        /// <summary>

        /// Default constructor.

        /// </summary>

        public KeplerOrbit()

        {

            this.a = 1.0;

            this.e = 0;

            this.i = 0;

            this.theta = 0;

            this.omegab = 0;

            this.L = 0;

        }



        /// <summary>

        /// Constructs a KeplerOrbit object from the 6 oskulating elements. 

        /// </summary>

        /// <param name="a">Semi major axis.</param>

        /// <param name="e">Eccentricity</param>

        /// <param name="i">Inclination</param>

        /// <param name="theta">Longitude of ascending node.</param>

        /// <param name="omegab">Longitude of periapsis.</param>

        /// <param name="L">Mean longitude at epoch.</param>

        public KeplerOrbit(double a, double e, double i, double theta, double omegab, double L)

        {

            this.a = a;

            this.e = e;

            this.i = i;

            this.theta = theta;

            this.omegab = omegab;

            this.L = L;

        }





        /// <summary>

        /// This method samples the Keplerian orbit and returns an array of vectors depicting the orbit path or shape. 

        /// The for most interesting points of the an orbit periapsis, apoapsis, ascending node and descending node are

        /// calculated and returned in out vectors. 

        /// </summary>

        /// <param name="numPoints">The number of points used to sample the orbit. This is the size of the returned array.</param>

        /// <param name="pe">A vector receiving the periapsis position of the orbit.</param>

        /// <param name="ap">A vector receiving the apoapsis position. </param>

        /// <param name="an">Vector receiving the ascending node position.</param>

        /// <param name="dn">Vector receiving the descending node position.</param>

        /// <returns>An array of vectors depicting the orbits path.</returns>

        public Vector3d[] GetShape(int numPoints, out Vector3d pe, out Vector3d ap, out Vector3d an, out Vector3d dn)

        {

            Vector3d[] points = new Vector3d[numPoints];



            // utility values

            double multiplier = this.a * (1.0 - this.e * this.e);

            double AgP = this.omegab - this.theta;



            pe = new Vector3d(this.a * (1.0 - this.e), 0.0, 0.0);

            ap = new Vector3d(-this.a * (1.0 + this.e), 0.0, 0.0);



            double TrA;

            double absr;

            Vector3d direction;

            if (numPoints == 1)

            {

                points[0] = pe;

            }

            else if (numPoints > 1)

            {

                double maxTrA, dTrA;



                maxTrA = Math.PI;

                if (this.e >= 1.0)

                {

                    maxTrA = Math.Acos(-1.0 / this.e);



                    // Make it a bit smaller to avoid division by zero

                    maxTrA *= (((double)numPoints) / (numPoints + 1));

                }



                dTrA = (2 * maxTrA) / (numPoints - 1);



                TrA = -maxTrA;

                

                for (int i = 0; i < numPoints; i++)

                {

                    absr = Math.Abs(multiplier / (1.0 + this.e * Math.Cos(TrA)));



                    direction = new Vector3d(Math.Cos(TrA), Math.Sin(TrA), 0.0);

                    points[i] = direction * absr;



                    TrA += dTrA;

                }

            }



            // ascending node

            TrA = -AgP;

            absr = multiplier / (1.0 + this.e * Math.Cos(TrA));



            if (absr <= 0.0)

            {

                an = new Vector3d(0.0, 0.0, 0.0);

            }

            else

            {

                direction = new Vector3d(Math.Cos(TrA), Math.Sin(TrA), 0.0);

                an = direction * absr;

            }



            // descending node

            TrA = Math.PI - AgP;

            absr = multiplier / (1.0 + this.e * Math.Cos(TrA));



            if (absr <= 0.0)

            {

                dn = new Vector3d(0.0, 0.0, 0.0);

            }

            else

            {

                direction = new Vector3d(Math.Cos(TrA), Math.Sin(TrA), 0.0);

                dn = direction * absr;

            }



            Matrixd AgPMat, LANMat, IncMat, transform;



            AgPMat = Matrixd.CreateRotationZ(AgP);

            IncMat = Matrixd.CreateRotationX(this.i);

            LANMat = Matrixd.CreateRotationZ(this.theta);



            transform = LANMat * IncMat * AgPMat;



            pe = Vector3d.Transform(pe, transform);

            ap = Vector3d.Transform(ap, transform);

            an = Vector3d.Transform(an, transform);

            dn = Vector3d.Transform(dn, transform);



            if (numPoints != 0)

                for (int i = 0; i < numPoints; i++)

                    points[i] = Vector3d.Transform(points[i], transform);



            return points;

        }



        /// <summary>

        /// Calculates the mean anomaly at a given time. 

        /// </summary>

        /// <param name="mu">The gravitational parameter.</param>

        /// <param name="timeSinceEpoch">The time that has passed since the orbits epoch in seconds.</param>

        /// <returns>The mean anomaly at the given time.</returns>

        public double getMeanAnomalyAtTime(double mu, double timeSinceEpoch)

        {

            // calculate mean motion

            double meanMotion = Math.Sqrt(mu/Math.Pow(Math.Abs(this.a), 3.0));



            // calculate change in mean anomaly

            double deltaMeanAnomaly = timeSinceEpoch * meanMotion;



            // calculate mean anomaly

            double meanAnomaly = (this.L - this.omegab + deltaMeanAnomaly) % (2.0 * Math.PI);



            if (meanAnomaly < 0)

                meanAnomaly += (2.0 * Math.PI);



            return meanAnomaly;

        }



        /// <summary>

        /// Calculates the eccentric anomaly for the orbit at a given mean anomaly. 

        /// The value is estimated using Newtons method. 

        /// </summary>

        /// <param name="meanAnomaly">The mean anomaly</param>

        /// <param name="eccentricAnomalyEstimate">A starting value for estimating the eccentric anomaly, the closer this is to the 

        /// eccentric anomaly the faster the method can calculate the eccentric anomaly using Newtons method.

        /// If you don't have a good estimate, the mean anomaly can be used.</param>

        /// <param name="maxRelativeError">Maximum relative error between two successive runs of Newtons method. If the values of two successive runs

        /// deviate by less than this value, the calculation is ended and the estimate returned.</param>

        /// <param name="maxIterations">Maximum iterations for the calculation using Newtons method. This can be used

        /// to limit the calculation time of the method. The calculation can be split over severeal calls this way.</param>

        /// <returns>The estimated value for the mean anomaly.</returns>

        public double getEccentricAnomaly(double meanAnomaly, double eccentricAnomalyEstimate, double maxRelativeError, int maxIterations)

        {

            double e, meanAnomalyEstimate, relativeError;



            if (this.e == 0)

                return meanAnomaly;



            if (this.e == 1.0) // parabolic orbit - approximate to hyperbolic

                e = this.e + 1e-6;

            else

                e = this.e;



            int i = 0;

            do

            {

                if (this.e < 1.0) // eliptical orbit

                {

                    // calulate next estimate of the root of Keplers equation using Newtons method

                    eccentricAnomalyEstimate = eccentricAnomalyEstimate - (eccentricAnomalyEstimate - e * Math.Sin(eccentricAnomalyEstimate) - meanAnomaly) / (1 - e * Math.Cos(eccentricAnomalyEstimate));

                    /* calculate estimate of mean anomaly from estimate of eccentric anomaly */

                    meanAnomalyEstimate = eccentricAnomalyEstimate - e * Math.Sin(eccentricAnomalyEstimate);



                }

                else // hyperbolic orbit

                {

                    /* sinh is not continuous over the range 0 to 2*pi, so it must be brought into the range -pi to pi */

                    if (eccentricAnomalyEstimate > Math.PI)

                        eccentricAnomalyEstimate -= (2.0 * Math.PI);

                    /* calculate next estimate of the root of Kepler's equation using Newton's method */

                    eccentricAnomalyEstimate = eccentricAnomalyEstimate - (e * Math.Sinh(eccentricAnomalyEstimate) - eccentricAnomalyEstimate - meanAnomaly) / (e * Math.Cosh(eccentricAnomalyEstimate) - 1);

                    /* calculate estimate of mean anomaly from estimate of eccentric anomaly */

                    meanAnomalyEstimate = e * Math.Sinh(eccentricAnomalyEstimate) - eccentricAnomalyEstimate;

                }

                /* calculate relativeError */

                relativeError = 1 - meanAnomalyEstimate / meanAnomaly;

                i++;





            }

            while ((i < maxIterations) && (Math.Abs(relativeError) > Math.Abs(maxRelativeError)));



            /* check range is in 0 to 2*pi */

            eccentricAnomalyEstimate = eccentricAnomalyEstimate % (2.0 * Math.PI);

            if (eccentricAnomalyEstimate < 0)

                eccentricAnomalyEstimate += (2.0 * Math.PI);



            return eccentricAnomalyEstimate;

        }



        /// <summary>

        /// Calculates the true anomaly for the orbit and a given eccentric anomaly.

        /// </summary>

        /// <param name="mu">The gravitational parameter.</param>

        /// <param name="eccentricAnomaly">The eccentric anomaly.</param>

        /// <returns>The true anomaly.</returns>

        public double getTrueAnomaly(double mu, double eccentricAnomaly)

        {

            if (this.e < 1) // elliptical orbit

                return (2 * Math.Atan(Math.Sqrt((1 + this.e) / (1 - this.e)) * Math.Tan(eccentricAnomaly / 2)));

            else // hyperbolic orbit

                return (Math.Acos((Math.Cosh(eccentricAnomaly) - this.e) / (1 - this.e * Math.Cosh(eccentricAnomaly))));

        }



        /// <summary>

        /// Calculates the position and velocity vectors for the orbit at a given true anomaly and gravitational parameter.

        /// </summary>

        /// <param name="mu">The gravitational parameter</param>

        /// <param name="trueAnomaly">The true anomaly.</param>

        /// <param name="position">Receives the calculated position.</param>

        /// <param name="velocity">Receives the calculated velocity.</param>

        public void getStateVector(double mu, double trueAnomaly, out Vector3d position, out Vector3d velocity)

        {

            /* Pseudocode

             *

             * calc nodal vector, n

             * calc angular momentum vector, h

             * calc position vector

             *  calc argument of position

             *  calc direction of position vector

             *  calc length of position vector

             * calc velocity vector

             *  calculate magnitude of velocity vector

             *  calc components of velocity vector perpendicular and parallel to radius vector

             *  add to get velocity vector */



            Vector3d n; /* unit vector in direction of ascending node */

            Vector3d h; /* angular momentum vector */

            Vector3d north; /* unit vector pointing ecliptic north */

            Vector3d vPro, vO; /* prograde and outward components of velocity vector */

            Vector3d tmp;

            double argPos; /* argument of position, measured from the longitude of ascending node */

            double rPe, vPe; /* radius and velocity at periapsis */

            double v2; /* magnitude squared of velocity vector */

            double e; /* eccentricity */



            e = this.e;

            if (e == 1.0) /* parabolic orbit - approximate to hyperbolic orbit */

                e += 1e-6;



            /* calc nodal vector */

            n = new Vector3d(Math.Cos(this.theta), Math.Sin(this.theta), 0.0);



            /* ecliptic north vector */

            north = new Vector3d(0.0, 0.0, 1.0);



            /* calc angular momentum vector, h */

            /* projection of h in ecliptic (xy) plane */



            h = Vector3d.Cross(n, north);

            h *= Math.Sin(this.i);



            /* elevation of h */

            h.z = Math.Cos(this.i);

            h = Vector3d.Normalize(h);



            /* calc magnitude of h */

            /* calc radius and velocity at periapsis */

            if (e < 1.0) /* elliptical orbit */

            {

                rPe = this.a * (1.0 - e * e) / (1.0 + e);

                vPe = Math.Sqrt(mu * (2 / rPe - 1 / this.a));

            }

            else /* hyperbolic orbit */

            {

                rPe = Math.Abs(this.a) * (e * e - 1.0) / (1.0 + e);

                vPe = Math.Sqrt(mu * (2 / rPe + 1 / Math.Abs(this.a)));

            }

            /* calc h */

            h *= (rPe * vPe);



            /* calc position vector */

            /* calc argument of position */

            argPos = this.omegab - this.theta + trueAnomaly;



            /* calc direction of position vector */

            tmp = n * Math.Cos(argPos);

            position = Vector3d.Normalize(h);

            position = Vector3d.Cross(position, n);

            position *= Math.Sin(argPos);

            position += tmp;



            /* calc length of position vector */

            if (e < 1.0) /* elliptical orbit */

                position *= (this.a * (1.0 - e * e) / (1.0 + e * Math.Cos(trueAnomaly)));

            else /* hyperbolic orbit */

                position *= (Math.Abs(this.a) * (e * e - 1.0) / (1.0 + e * Math.Cos(trueAnomaly)));



            /* calc velocity vector */

            /*  calculate magnitude of velocity vector */

            if (e < 1.0) /* elliptical orbit */

                v2 = mu * (2 / position.Length() - 1 / this.a);

            else /* hyperbolic orbit */

                v2 = mu * (2 / position.Length() + 1 / Math.Abs(this.a));



            /* calc components of velocity vector perpendicular and parallel to radius vector:

            vPro = (|h|/|pos|) * normal(h x pos)



            vO = sqrt(v^2 - |vPro|^2) * normal(pos);

            */

            vPro = Vector3d.Cross(h, position);

            vPro = Vector3d.Normalize(vPro);

            vPro *= (h.Length() / position.Length());



            vO = Vector3d.Normalize(position);

            vO *= (Math.Sqrt(v2 - vPro.LengthSquared()) * Math.Sin(trueAnomaly) / Math.Abs(Math.Sin(trueAnomaly)));



            /* add to get velocity vector */

            velocity = vPro + vO;

        }



        /// <summary>

        /// Calculates the position and velocity of the body whos orbit is described by this KeplerOrbit object at a given time. 

        /// </summary>

        /// <param name="mu">The gravitational parameter.</param>

        /// <param name="timeSinceEpoch">The time in seconds that has passed since the orbits epoch.</param>

        /// <param name="position">A vector receiving the relative position of the orbiting body at the provided time.</param>

        /// <param name="velocity">A vector receiving the relative velocity of the orbiting body at the provided time.</param>

        public void getStateVectorAtTime(double mu, double timeSinceEpoch, out Vector3d position, out Vector3d velocity)

        {

            // calculate mean anomaly

            double meanAnomaly = getMeanAnomalyAtTime(mu, timeSinceEpoch);



            // calculate eccentric anomaly 

            // to get an idea of the practical implications of the maximum relative error passed 

            // to getEccentricAnomaly:

            // neptune radius = ~25000 km

            // neptune orbit path length for one revolution = ~28e9 km

            // so with 2 * PI * 25000 / 28e9 we get ~0.00000561 which means the maximum error 

            // would be one neptune radius for neptunes orbit with this value

            // thus the value of 0.0000001 used here should calculate neptunes position with a deviation of

            // 1/56th of the neptune radius or ~446km

            double eccentricAnomaly = getEccentricAnomaly( meanAnomaly, meanAnomaly, 0.0000001, 10); 



            // calculate true anomaly

            double trueAnomaly = getTrueAnomaly(mu, eccentricAnomaly);



            // calculate vectors

            getStateVector(mu, trueAnomaly, out position, out velocity);

        }



        public delegate void changedHandler();

        public event changedHandler changed;



        /// <summary>

        /// Updates this Orbit object by copying the values of another one. 

        /// </summary>

        /// <param name="keplerOrbit"></param>

        internal void updateFromOrbit(KeplerOrbit keplerOrbit)

        {

            this.a = keplerOrbit.a;

            this.e = keplerOrbit.e;

            this.i = keplerOrbit.i;

            this.L = keplerOrbit.L;

            this.omegab = keplerOrbit.omegab;

            this.theta = keplerOrbit.theta;



            if (this.changed != null)

                this.changed();

        }

    }

}