using System;
using System.Collections.Generic;
using System.Text;

using Motion6D;
using Vector3D;
using DataPerformer;

namespace CelestialMechanics
{
    public abstract class Orbit : IPosition, IDynamical
    {
        #region Fields
        protected double[] elements = new double[6];

        protected double p;
        protected double e;

        protected RigidReferenceFrame frame = new RigidReferenceFrame();

        protected object paremeters;
        /*      protected double inclination;
              protected double ascendingNode;
              protected double argOfPeriapsis;
              protected double tau;
              protected double[,] matrix = new double[3, 3];
              protected double[] q = new double[4];*/
        protected double mu;
        protected double[] x = new double[] { 0, 0, 0 };
        protected double[] y = new double[] { 0, 0, 0 };

        protected IPosition position;

        protected IReferenceFrame parent;

        protected double L;

        protected double E;

        #endregion

        public abstract void GetPlanar(double time, out double x, out double y);



        /*       protected Orbit(double p, double e, double inclination, double ascendingNode, double argOfPeriapsis, double tau, double mu)
               {
                   this.p = p;
                   this.e = e;
                   this.inclination = inclination;
                   this.ascendingNode = ascendingNode;
                   this.argOfPeriapsis = argOfPeriapsis;
                   this.tau = tau;
                   double[] q1 = new double[4];
                   V3DOperations.Rotate(ascendingNode, 3, q1);
                   double[] q2 = new double[4];
                   V3DOperations.Rotate(inclination, 1, q2);
                   double[] q3 = new double[4];
                   V3DOperations.QuaternionMultiply(q1, q2, q3);
                   double[] q4 = new double[4];
                   V3DOperations.Rotate(argOfPeriapsis, 3, q4);
                   V3DOperations.QuaternionMultiply(q3, q4, q);
                   double[,] qq = new double[4,4];
                   V3DOperations.CalculateMatrix(q, matrix, qq);
                   this.mu = mu;
               }

        */

        /*     #region IOrientation Members

             double[] IOrientation.Quaternion
             {
                 get { return q; }
             }

             double[,] IOrientation.Matrix
             {
                 get { return matrix; }
             }
             #endregion*/

        //     #region Specific Members
        /*
                public static void Create(double[] x6D, double[,] matrix)
                {
                    double a = 0;
                    for (int i = 0; i < 3; i++)
                    {
                        a += x6D[i] * x6D[i];
                    }
                    a = 1 / Math.Sqrt(a);
                    double[] x = new double[3];
                    for (int i = 0; i < x.Length; i++)
                    {
                        double xx = x6D[i] * a;
                        x[i] = xx;
                        matrix[0, i] = xx;
                    }
                    double[] v = new double[3];
                    for (int i = 0; i < 3; i++)
                    {
                        v[i] = x6D[i + 3];
                    }
                    double[] rv = V3DOperations.VectorPoduct(x, v);
                    V3DOperations.Normalize(rv);
                    for (int i = 0; i < 3; i++)
                    {
                        matrix[2, i] = rv[i];
                    }
                    double[] y = V3DOperations.VectorPoduct(rv, x);
                    for (int i = 0; i < 3; i++)
                    {
                        matrix[1, i] = y[i];
                    }
                }

                public void GetPosition(IPosition p, double time)
                {
                    double x;
                    double y;
                    GetPlanar(time, out x, out y);
                    double[] pos = p.Position;
                    pos[0] = matrix[0, 0] * x + matrix[0, 1] * y;
                    pos[1] = matrix[1, 0] * x + matrix[1, 1] * y;
                    pos[2] = matrix[2, 0] * x + matrix[2, 1] * y;
                }

                public static Orbit Create(double p, double e, double inclination, double ascendingNode, double argOfPeriapsis, double tau, double mu)
                {
                    if (e < 1)
                    {
                        return new EllipticalOrbit(p, e, inclination, ascendingNode, argOfPeriapsis, tau, mu);
                    }
                    return null;
                }
        */

        public static Orbit Create(double[] x, double t, double mu)
        {
            double[] el = EL(x, mu);
            double E = el[0];
            double L = el[1];
            double R = el[2];
            double mul = mu / L;
            double discr = mul * mul - E;
            if (discr > 0)
            {
                return null;
            }
            return null;
            
        }
               


        #region IPosition Members

        double[] IPosition.Position
        {
            get { return position.Position; }
        }

        IReferenceFrame IPosition.Parent
        {
            get
            {
                return frame.Parent;
            }
            set
            {
                frame.Parent = value;
            }
        }

        object IPosition.Parameters
        {
            get
            {
                return paremeters;
            }
            set
            {
                paremeters = value;
            }
        }

        void IPosition.Update()
        {
            position.Update();
        }

        #endregion

        #region IDynamical Members

        public virtual double Time
        {
            set
            {
                GetPlanar(value, out x[0], out x[1]);
               // frame.
            }
        }
        #endregion

        #region Specific Members

        protected void PrepareFrame(double[] x)
        {
            position = new Position(y);
            double[] v = new double[3];
            double[] r = new double[3];
            for (int i = 0; i < 3; i++)
            {
                r[i] = x[i];
                v[i] = x[i + 3];
            }
            position.Parent = frame;
            frame.RelativeMatrix = V3DOperations.SquareReper(x);
          //  double L = V3DOperations.ScalarNorm(V3DOperations.VectorPoduct(r, v));
          //  double R = V3DOperations.ScalarNorm(r);
          //  double E = -mu / R * R;

           
        }


        static protected double[] EL(double[] x, double mu)
        {
            double[] v = new double[3];
            double[] r = new double[3];
            for (int i = 0; i < 3; i++)
            {
                r[i] = x[i];
                v[i] = x[i + 3];
            }
            double L = V3DOperations.ScalarNorm(V3DOperations.VectorPoduct(r, v));
            double R = V3DOperations.ScalarNorm(r);
            double V = V3DOperations.ScalarNorm(v);
            double E = 0.5 * V * V - mu / (R * R);
            return new double[] { E, L, R, V };
        }

        #endregion
    }
}
