////////////////////////////////////////////////////////////////////
// Copyright © 2009 Carl Johansen
// 
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published
// by the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more
// details (<http://www.gnu.org/licenses/>).
////////////////////////////////////////////////////////////////////

using System;
using System.Windows.Media.Media3D;

namespace GravityDemo
{
    public class EllipticalOrbit
    {
        public Body CentralBody { get; set; }
        public double SemiMajorAxis { get; set; }
        public double Eccentricity { get; set; }
        public double Inclination { get; set; }
        public double AscendingNodeLongitude { get; set; }
        public double ArgumentOfPeriapsis { get; set; }
        public bool AnglesInDegrees { get; set; }
        public bool DistancesInKilometres { get; set; }
        public EllipticalOrbit()
        {
            AnglesInDegrees = true;
            DistancesInKilometres = false; //Default assumes distances are in metres.
        }

        /// <summary>
        /// The time that the orbiting body will take to complete one orbit (in seconds).
        /// </summary>
        public double Period
        {
            get
            {
                //Thanks to http://galileo.phys.virginia.edu/classes/152.mf1i.spring02/EllipticOrbits.htm.
                double aCubed = SemiMajorAxis * SemiMajorAxis * SemiMajorAxis;
                if (DistancesInKilometres)
                    aCubed *= 1000000000; //convert cubic km to cubic metres.
                return Math.Sqrt((4 * Math.PI * Math.PI * aCubed) / CentralBody.GM);
            }
        }

        public double GetEccentricAnomaly(double meanAnomaly)
        {
            double ma = meanAnomaly;
            if (AnglesInDegrees)
            {
                double deg2Rad = Math.PI / 180;
                ma *= deg2Rad;
            }

            double eca = ma + Eccentricity / 2.0;
            double epsilon = .000001;
            double eNew;

            do
            {
                eNew = eca - (eca - Eccentricity * Math.Sin(eca) - ma) / (1 - Eccentricity * Math.Cos(eca));
                eca = eNew;
            } while (Math.Abs(eNew - eca) > epsilon);

            return eca;
        }

        public OrbitStateVectorPair GetStateVectors(double meanAnomaly)
        {
            //Based on code from http://www.geocities.com/SiliconValley/2902/ele2vec.htm.

            double a = SemiMajorAxis;
            double ec = Eccentricity;
            double i = Inclination;
            double w0 = ArgumentOfPeriapsis;
            double o0 = AscendingNodeLongitude;
            double m0 = meanAnomaly;
            if (AnglesInDegrees)
            {
                double deg2Rad = Math.PI / 180;
                i *= deg2Rad;
                o0 *= deg2Rad;
                w0 *= deg2Rad;
                m0 *= deg2Rad;
            }

            double eca = GetEccentricAnomaly(meanAnomaly);

            double ceca = Math.Cos(eca);
            double seca = Math.Sin(eca);
            double e1 = a * Math.Sqrt(1 - ec * ec);

            double xw = a * (ceca - ec);
            double yw = e1 * seca;
            double gm = CentralBody.GM;
            if (DistancesInKilometres)
                gm /= 1000000000; //Convert cubic km to cubic metres.
            double edot = Math.Sqrt(gm / a) / (a * (1 - ec * ceca)); 
            double xdw = -a * edot * seca;
            double ydw = e1 * edot * ceca;
            double cw = Math.Cos(w0);
            double sw = Math.Sin(w0);
            double co = Math.Cos(o0);
            double so = Math.Sin(o0);
            double ci = Math.Cos(i);
            double si = Math.Sin(i);
            double swci = sw * ci;
            double cwci = cw * ci;
            double px = cw * co - so * swci;
            double py = cw * so + co * swci;
            double pz = sw * si;
            double qx = -sw * co - so * cwci;
            double qy = -sw * so + co * cwci;
            double qz = cw * si;
            double x = xw * px + yw * qx;
            double y = xw * py + yw * qy;
            double z = xw * pz + yw * qz;
            double xd = xdw * px + ydw * qx;
            double yd = xdw * py + ydw * qy;
            double zd = xdw * pz + ydw * qz;

            //Two conversions to note:
            // 1. From km to metres.
            // 2. My system uses the convention of y-axis = up/down, so I swap the y/z
            //    values returned by this calculation.
            double unitConversionFactor = (DistancesInKilometres ? 1000 : 1);
            return new OrbitStateVectorPair
            {
                Position = Vector3D.Multiply(unitConversionFactor, new Vector3D(x, z, y)),
                Velocity = Vector3D.Multiply(unitConversionFactor, new Vector3D(xd, zd, yd))
            };
        }
    }
}