// Doesn't work, just learning to use github
 /// <summary>
 /// Find the Intersections of two orbits
 /// </summary>
 /// <returns>
 /// 0 for no intersections found, 1 for 1, 2 for 2
 /// True anomaly of intersections in out intersect1 and intersect2
 /// </returns>
 /// <param name='orbit'>
 /// Orbit.
 /// </param>
 /// <param name='tgtorbit'>
 /// Tgtorbit.
 /// </param>
 /// <param name='intersect1'>
 /// Intsect1.
 /// </param>
 /// <param name='intersect2'>
 /// Intsect2.
 /// </param>
 public static int FindIntersectSimple(this Orbit orbit, Orbit tgtorbit, out double intersect1, out double intersect2)
 {
     double anomaly = 0;
     intersect1 = 361;
     intersect2 = 361;
     int div = 50;
     double range = 360;
     double lowest = 1e10;
     int count = 0;
     while (lowest>100) {
         for (int i=0; i<div; i++) {
             double j;
             anomaly += (i / div) * range;
             j = Math.Abs (orbit.f (tgtorbit, anomaly));
             if (i == 0) {
                 lowest = j;
                 intersect1 = anomaly;
             }
             if (j < lowest) {
                 lowest = j;
                 intersect1 = anomaly;
             }
         }
         range = range / 2;
         anomaly = intersect1 - range / 2;
         count++;
         if (count > 30)
             return 2;
     }
     return 0;
 }

  /// <summary>
  /// Calculates position of the orbiting body for given time. It also updates <see cref="CurrentPosition"/> property.
  /// </summary>
  /// <param name="time">time from the creation of the start system (time 0)</param>
  /// <returns>Position as spherical coordinates</returns>
  public static SphericalCoordinates CalculatePosition(float time, Orbit orbitData)
  {
    time = Mathf.Repeat(time, orbitData.Period);
    //mean anomaly
    float n = 2f * Mathf.PI / orbitData.Period;
    float M = n * time;

    //eccentric anomaly
    float deltaMax = Mathf.Pow(10, anomalyPrecision * -1);
    float E = M;
    float error = E - orbitData.Eccentricity * Mathf.Sin(E) - M;
    int i = 0;
    while (Mathf.Abs(error) > deltaMax && i < 5)
    {
      E = E - error / (1f - orbitData.Eccentricity * Mathf.Cos(E));
      error = E - orbitData.Eccentricity * Mathf.Sin(E) - M;
      i++;
    }

    //true anomaly
    var S = Mathf.Sin(E);
    var C = Mathf.Cos(E);
    var fak = Mathf.Sqrt(1f - orbitData.Eccentricity * orbitData.Eccentricity);
    float phi = Mathf.Atan2(fak * S, C - orbitData.Eccentricity);

    //distance
    // float r = MeanDistance * (1 - Eccentricity * Eccentricity) / (1 + Eccentricity * Mathf.Cos(phi));
    float r = orbitData.MeanDistance * (1 - orbitData.Eccentricity * Mathf.Cos(E));
    phi = phi + orbitData.PeriapsisArgumentRadians;
    float elevation = orbitData.InclinationRadians * Mathf.Cos(phi);
    return new SphericalCoordinates(r, phi, elevation);
  }

		protected TransferCalculator(
			Orbit o, Orbit target,
			double min_departure_time,
			double max_departure_time,
			double min_transfer_time,
			double max_transfer_time,
			int width,
			int height)
		{
			originOrbit = o;
			destinationOrbit = target;

			origin = new Orbit();
			origin.UpdateFromOrbitAtUT(o, min_departure_time, o.referenceBody);
			destination = new Orbit();
			destination.UpdateFromOrbitAtUT(target, min_departure_time, target.referenceBody);
			maxDurationSamples = height;
			dateSamples = width;
			nextDateIndex = dateSamples;
			this.minDepartureTime = min_departure_time;
			this.maxDepartureTime = max_departure_time;
			this.minTransferTime = min_transfer_time;
			this.maxTransferTime = max_transfer_time;
			computed = new ManeuverParameters[dateSamples, maxDurationSamples];
			pendingJobs = 0;

#if DEBUG
			log = new string[dateSamples, maxDurationSamples];
#endif
		}

        public static ManeuverNode PlaceManeuverNode(this Vessel vessel, Orbit patch, Vector3d dV, double UT)
        {
            //placing a maneuver node with bad dV values can really mess up the game, so try to protect against that
            //and log an exception if we get a bad dV vector:
            for (int i = 0; i < 3; i++)
            {
                if (double.IsNaN(dV[i]) || double.IsInfinity(dV[i]))
                {
                    throw new Exception("VesselExtensions.PlaceManeuverNode: bad dV: " + dV);
                }
            }

            if (double.IsNaN(UT) || double.IsInfinity(UT))
            {
                throw new Exception("VesselExtensions.PlaceManeuverNode: bad UT: " + UT);
            }

            //It seems that sometimes the game can freak out if you place a maneuver node in the past, so this
            //protects against that.
            UT = Math.Max(UT, Planetarium.GetUniversalTime());

            //convert a dV in world coordinates into the coordinate system of the maneuver node,
            //which uses (x, y, z) = (radial+, normal-, prograde)
            Vector3d nodeDV = patch.DeltaVToManeuverNodeCoordinates(UT, dV);
            ManeuverNode mn = vessel.patchedConicSolver.AddManeuverNode(UT);
            mn.OnGizmoUpdated(nodeDV, UT);
            return mn;
        }

        public override ManeuverParameters MakeNodeImpl(Orbit o, double universalTime, MechJebModuleTargetController target)
        {
            double UT = timeSelector.ComputeManeuverTime(o, universalTime, target);
            var dV = OrbitalManeuverCalculator.DeltaVToResonantOrbit(o, UT, (double)resonanceNumerator.val / resonanceDenominator.val);

            return new ManeuverParameters(dV, UT);
        }

        static void WarpShip(Vessel vessel, Orbit newOrbit)
        {
            if (newOrbit.getRelativePositionAtUT (Planetarium.GetUniversalTime ()).magnitude > newOrbit.referenceBody.sphereOfInfluence)
                throw new ArgumentException ("Destination position was above the sphere of influence");

            vessel.Landed = false;
            vessel.Splashed = false;
            vessel.landedAt = string.Empty;
            var parts = vessel.parts;
            if (parts != null) {
                var clamps = parts.Where (p => p.Modules != null && p.Modules.OfType<LaunchClamp> ().Any ()).ToList ();
                foreach (var clamp in clamps)
                    clamp.Die ();
            }

            try {
                OrbitPhysicsManager.HoldVesselUnpack (60);
            } catch (NullReferenceException) {
            }

            foreach (var v in (FlightGlobals.fetch == null ? (IEnumerable<Vessel>)new[] { vessel } : FlightGlobals.Vessels).Where(v => !v.packed))
                v.GoOnRails ();

            HardsetOrbit (vessel.orbit, newOrbit);

            vessel.orbitDriver.pos = vessel.orbit.pos.xzy;
            vessel.orbitDriver.vel = vessel.orbit.vel;
        }

    /// <summary>
    /// Initializes the script.
    /// </summary>
    void Start()
    {
        // Store the central object original position
        originalCentralBodyPosition = CentralObject.transform.position;

        // Calculate the up vector if not entered
        if (Up == Vector3.zero)
            Up = CentralObject.transform.up;

        orbits = new Orbit[OrbitingObjects.Length];

        // for each orbiting object, calculate the normal vector to its orbital plane
        for (int i=0; i<OrbitingObjects.Length; i++) {

            // Get the normalized direction vector between the two objects
            Vector3 direction = (OrbitingObjects[i].orbitingObject.transform.position - CentralObject.transform.position).normalized;

            // Calculate a perpendicular vector to the orbital plane
            Vector3 normal = Vector3.Cross (direction, Up);

            normal = Vector3.Cross (normal, direction);

            // Add a new Orbit containing all the information needed for the orbit to the array of orbits
            orbits[i] = new Orbit (OrbitingObjects[i].orbitingObject, OrbitingObjects[i].revolutionsPerMinute, normal);
        }
    }

        /// <summary>
        /// Orbit foo, this finds the nodes of two orbits
        /// </summary>
        /// <returns>
        /// The true anomaly of the ascending node(descing node is 180degrees off)
        /// </returns>
        /// <param name='orbit'>
        /// Orbit.
        /// </param>
        /// <param name='tgtorbit'>
        /// Target Orbit
        /// </param>
        public static double FindAN(Orbit orbit, Orbit tgtorbit)
        {
            double rad = Math.PI/180;
            double Lan1 = orbit.LAN;
            double inc1 = orbit.inclination;
            double Lan2 = tgtorbit.LAN;
            double inc2 = tgtorbit.inclination;

            //see braeunig.us/space... cross product of two orbital planes gives the node location
            var a = new Vector3d(Math.Sin(inc1*rad)*Math.Cos(Lan1*rad), Math.Sin(inc1*rad)*Math.Sin(Lan1*rad),
                                 Math.Cos(inc1*rad));
            var b = new Vector3d(Math.Sin(inc2*rad)*Math.Cos(Lan2*rad), Math.Sin(inc2*rad)*Math.Sin(Lan2*rad),
                                 Math.Cos(inc2*rad));
            var c = new Vector3d(0, 0, 0);
            c = Vector3d.Cross(a, b);
            var coord = new double[] {0, 0};
            coord = LatLonofVector(c); //get the coordinates lat/lon of the ascending node
            double lat = coord[0];
            double lon = coord[1];

            //go look at the diagrams at braeunig.us/space
            double α = lon - Lan1; //its all greek to me
            if (α < 0) α += 360;
            double λ = Math.Atan(Math.Tan(α*rad)/Math.Cos(inc1*rad))/rad;
            double x = 180 + (λ - orbit.argumentOfPeriapsis);
            if (x > 360) return 360 - x;
            else return x;
        }

		private static void DrawOrbit(Orbit o, CelestialBody referenceBody, Matrix4x4 screenTransform, int numSegments)
		{
			if (!o.activePatch) {
				return;
			}

			double startTA;
			double endTA;
			double now = Planetarium.GetUniversalTime();
			if (o.patchEndTransition != Orbit.PatchTransitionType.FINAL) {
				startTA = o.TrueAnomalyAtUT(o.StartUT);
				endTA = o.TrueAnomalyAtUT(o.EndUT);
				if (endTA < startTA) {
					endTA += 2.0 * Math.PI;
				}
			} else {
				startTA = o.GetUTforTrueAnomaly(0.0, now);
				endTA = startTA + 2.0 * Math.PI;
			}
			double dTheta = (endTA - startTA) / (double)numSegments;
			double theta = startTA;
			double timeAtTA = o.GetUTforTrueAnomaly(theta, now);
			Vector3 lastVertex = screenTransform.MultiplyPoint3x4(o.getRelativePositionFromTrueAnomaly(theta).xzy + (o.referenceBody.getTruePositionAtUT(timeAtTA)) - (referenceBody.getTruePositionAtUT(timeAtTA)));
			for (int i = 0; i < numSegments; ++i) {
				GL.Vertex3(lastVertex.x, lastVertex.y, 0.0f);
				theta += dTheta;
				timeAtTA = o.GetUTforTrueAnomaly(theta, now);

				Vector3 newVertex = screenTransform.MultiplyPoint3x4(o.getRelativePositionFromTrueAnomaly(theta).xzy + (o.referenceBody.getTruePositionAtUT(timeAtTA)) - (referenceBody.getTruePositionAtUT(timeAtTA)));
				GL.Vertex3(newVertex.x, newVertex.y, 0.0f);

				lastVertex = newVertex;
			}
		}

 public OrbitInfo(Orbitable orb, SharedObjects sharedObj)
     : this()
 {
     orbit = orb.Orbit;
     Shared = sharedObj;
     name = orb.GetName();
 }

        public override ManeuverParameters MakeNodeImpl(Orbit o, double UT, MechJebModuleTargetController target)
        {
            if (!target.NormalTargetExists)
            {
                throw new OperationException("must select a target for the Hohmann transfer.");
            }
            else if (o.referenceBody != target.TargetOrbit.referenceBody)
            {
                throw new OperationException("target for Hohmann transfer must be in the same sphere of influence.");
            }
            else if (o.eccentricity > 1)
            {
                throw new OperationException("starting orbit for Hohmann transfer must not be hyperbolic.");
            }
            else if (target.TargetOrbit.eccentricity > 1)
            {
                throw new OperationException("target orbit for Hohmann transfer must not be hyperbolic.");
            }
            else if (o.RelativeInclination(target.TargetOrbit) > 30 && o.RelativeInclination(target.TargetOrbit) < 150)
            {
                errorMessage = "Warning: target's orbital plane is at a " + o.RelativeInclination(target.TargetOrbit).ToString("F0") + "º angle to starting orbit's plane (recommend at most 30º). Planned transfer may not intercept target properly.";
            }
            else if (o.eccentricity > 0.2)
            {
                errorMessage = "Warning: Recommend starting Hohmann transfers from a near-circular orbit (eccentricity < 0.2). Planned transfer is starting from an orbit with eccentricity " + o.eccentricity.ToString("F2") + " and so may not intercept target properly.";
            }

            Vector3d dV = OrbitalManeuverCalculator.DeltaVAndTimeForHohmannTransfer(o, target.TargetOrbit, UT, out UT);

            return new ManeuverParameters(dV, UT);
        }

		public void EllipticSector()
		{
			var o = new Orbit { SemiMajorAxis = 2, SemiMinorAxis = 1, TransformAngle = 0 };
			AssertAreClose(o.S(Math.PI), 0);
			AssertAreClose(o.S(0), Math.PI);
			AssertAreClose(o.S(1, -Math.PI), 2 * Math.PI);
		}

        public override ManeuverParameters MakeNodeImpl(Orbit o, double universalTime, MechJebModuleTargetController target)
        {
            double UT = timeSelector.ComputeManeuverTime(o, universalTime, target);
            var dV = OrbitalManeuverCalculator.DeltaVToShiftNodeLongitude(o, UT, target.targetLongitude);

            return new ManeuverParameters(dV, UT);
        }

        public static Vector3d DeltaVAndTimeForCheapestCourseCorrection(Orbit o, double UT, Orbit target, CelestialBody targetBody, double finalPeR, out double burnUT)
        {
            Vector3d collisionDV = DeltaVAndTimeForCheapestCourseCorrection(o, UT, target, out burnUT);
            Orbit collisionOrbit = o.PerturbedOrbit(burnUT, collisionDV);
            double collisionUT = collisionOrbit.NextClosestApproachTime(target, burnUT);
            Vector3d collisionPosition = target.SwappedAbsolutePositionAtUT(collisionUT);
            Vector3d collisionRelVel = collisionOrbit.SwappedOrbitalVelocityAtUT(collisionUT) - target.SwappedOrbitalVelocityAtUT(collisionUT);

            double soiEnterUT = collisionUT - targetBody.sphereOfInfluence / collisionRelVel.magnitude;
            Vector3d soiEnterRelVel = collisionOrbit.SwappedOrbitalVelocityAtUT(soiEnterUT) - target.SwappedOrbitalVelocityAtUT(soiEnterUT);

            double E = 0.5 * soiEnterRelVel.sqrMagnitude - targetBody.gravParameter / targetBody.sphereOfInfluence; //total orbital energy on SoI enter
            double finalPeSpeed = Math.Sqrt(2 * (E + targetBody.gravParameter / finalPeR)); //conservation of energy gives the orbital speed at finalPeR.
            double desiredImpactParameter = finalPeR * finalPeSpeed / soiEnterRelVel.magnitude; //conservation of angular momentum gives the required impact parameter

            Vector3d displacementDir = Vector3d.Cross(collisionRelVel, o.SwappedOrbitNormal()).normalized;
            Vector3d interceptTarget = collisionPosition + desiredImpactParameter * displacementDir;

            Vector3d velAfterBurn;
            Vector3d arrivalVel;
            LambertSolver.Solve(o.SwappedRelativePositionAtUT(burnUT), interceptTarget - o.referenceBody.position, collisionUT - burnUT, o.referenceBody, true, out velAfterBurn, out arrivalVel);

            Vector3d deltaV = velAfterBurn - o.SwappedOrbitalVelocityAtUT(burnUT);
            return deltaV;
        }

        public override ManeuverParameters MakeNodeImpl(Orbit o, double universalTime, MechJebModuleTargetController target)
        {
            double UT = timeSelector.ComputeManeuverTime(o, universalTime, target);

            if (!target.NormalTargetExists)
            {
                throw new OperationException("must select a target to match planes with.");
            }
            else if (o.referenceBody != target.TargetOrbit.referenceBody)
            {
                throw new OperationException("can only match planes with an object in the same sphere of influence.");
            }
            else if (timeSelector.timeReference == TimeReference.REL_ASCENDING)
            {
                if (!o.AscendingNodeExists(target.TargetOrbit))
                {
                    throw new OperationException("ascending node with target doesn't exist.");
                }
            }
            else
            {
                if (!o.DescendingNodeExists(target.TargetOrbit))
                {
                    throw new OperationException("descending node with target doesn't exist.");
                }
            }

            Vector3d dV = (timeSelector.timeReference == TimeReference.REL_ASCENDING) ?
                OrbitalManeuverCalculator.DeltaVAndTimeToMatchPlanesAscending(o, target.TargetOrbit, UT, out UT):
                OrbitalManeuverCalculator.DeltaVAndTimeToMatchPlanesDescending(o, target.TargetOrbit, UT, out UT);

            return new ManeuverParameters(dV, UT);
        }

        double tauP; //normalized parabolic transfer time

        #endregion Fields

        #region Constructors

        /// <summary>
        /// Initializes a new instance of the <see cref="ThrottleControlledAvionics.LambertSolver"/> class.
        /// </summary>
        /// <param name="orb">Starting orbit.</param>
        /// <param name="destination">Destination radius-vector.</param>
        /// <param name="UT">Starting UT.</param>
        public LambertSolver(Orbit orb, Vector3d destination, double UT)
        {
            orbit = orb;
            body = orbit.referenceBody;
            StartUT = UT;
            mu = body.gravParameter;

            r1 = orb.getRelativePositionAtUT(UT);
            var h  = Vector3d.Cross(r1, destination);
            if(h.sqrMagnitude < 0.01) h = orb.GetOrbitNormal();
            c  = destination-r1;

            cm  = c.magnitude;
            var r1m = r1.magnitude;
            var r2m = destination.magnitude;
            var rrm = r1m+r2m;
            m  = rrm+cm;
            n  = rrm-cm;
            m3 = m*m*m;

            var transfer_angle = Vector3d.Angle(r1, destination)*Mathf.Deg2Rad;
            if(h.z < 0) transfer_angle = Utils.TwoPI-transfer_angle;
            sigma = Math.Sqrt(n/m);
            if(transfer_angle > Math.PI) sigma = -sigma;
            sigma2 = sigma*sigma;
            sigma3 = sigma2*sigma;
            sigma5 = sigma2*sigma3;

            tauP = 2/3.0*(1-sigma3);
            tauME = Math.Acos(sigma)+sigma*Math.Sqrt(1-sigma2);
        }

        //Computes the time until the phase angle between the launchpad and the target equals the given angle.
        //The convention used is that phase angle is the angle measured starting at the target and going east until
        //you get to the launchpad.
        //The time returned will not be exactly accurate unless the target is in an exactly circular orbit. However,
        //the time returned will go to exactly zero when the desired phase angle is reached.
        public static double TimeToPhaseAngle(double phaseAngle, CelestialBody launchBody, double launchLongitude, Orbit target)
        {
            double launchpadAngularRate = 360 / launchBody.rotationPeriod;
            double targetAngularRate = 360.0 / target.period;
            if (Vector3d.Dot(target.SwappedOrbitNormal(), launchBody.angularVelocity) < 0) targetAngularRate *= -1; //retrograde target

            Vector3d currentLaunchpadDirection = launchBody.GetSurfaceNVector(0, launchLongitude);
            Vector3d currentTargetDirection = target.SwappedRelativePositionAtUT(Planetarium.GetUniversalTime());
            currentTargetDirection = Vector3d.Exclude(launchBody.angularVelocity, currentTargetDirection);

            double currentPhaseAngle = Math.Abs(Vector3d.Angle(currentLaunchpadDirection, currentTargetDirection));
            if (Vector3d.Dot(Vector3d.Cross(currentTargetDirection, currentLaunchpadDirection), launchBody.angularVelocity) < 0)
            {
                currentPhaseAngle = 360 - currentPhaseAngle;
            }

            double phaseAngleRate = launchpadAngularRate - targetAngularRate;

            double phaseAngleDifference = MuUtils.ClampDegrees360(phaseAngle - currentPhaseAngle);

            if (phaseAngleRate < 0)
            {
                phaseAngleRate *= -1;
                phaseAngleDifference = 360 - phaseAngleDifference;
            }

            return phaseAngleDifference / phaseAngleRate;
        }

        /// <summary>
        /// Find the point on the orbit that includes the initial time where the ejection angle is closest to the suplied one
        /// </summary>
        /// <param name="oObject">Orbit of the vessel/body</param>
        /// <param name="timeInitial">The UT you want to search around for the angle - will search 1/2 an orbit back and 1/2 forward</param>
        /// <param name="numDivisions">Higher thisnumber the more precise the answer - and the longer it will take</param>
        /// <param name="closestAngle">The output of the closest angle the method could find</param>
        /// <param name="targetAngle">The ejection angle we are looking for</param>
        /// <returns></returns>
        internal static double timeOfEjectionAngle(Orbit oObject, double timeInitial, double targetAngle, double numDivisions, out double closestAngle)
        {
            double timeStart = timeInitial - oObject.period/2;
            double periodtoscan = oObject.period;
            
            double closestAngleTime = timeStart;
            double closestAngleValue = Double.MaxValue;
            double minTime = timeStart;
            double maxTime = timeStart + periodtoscan;

            //work out iterations for precision - we only really need to within a second - so how many iterations do we actually need
            //Each iteration gets us 1/10th of the period to scan

            for (int iter = 0; iter < 8; iter++) {
                double dt = (maxTime - minTime) / numDivisions;
                for (int i = 0; i < numDivisions; i++) {
                    double t = minTime + i * dt;
                    double angle = oObject.getEjectionAngleAtUT(t);
                    if (Math.Abs(angle - targetAngle) < closestAngleValue) {
                        closestAngleValue = Math.Abs(angle - targetAngle);
                        closestAngleTime = t;
                    }
                }
                minTime = (closestAngleTime - dt).Clamp(timeStart, timeStart + periodtoscan);
                maxTime = (closestAngleTime + dt).Clamp(timeStart, timeStart + periodtoscan); 
            }

            closestAngle = closestAngleValue + targetAngle;
            return closestAngleTime;
        }

 public OrbitInfo( Orbit orb, SharedObjects sharedObj )
 {
     shared = sharedObj;
     orbit = orb;
     name = "<unnamed>";
     InitializeSuffixes();
 }

        public override ManeuverParameters MakeNodeImpl(Orbit o, double UT, MechJebModuleTargetController target)
        {
            if (!target.NormalTargetExists)
                throw new OperationException("must select a target for the course correction.");

            Orbit correctionPatch = o;
            while (correctionPatch != null)
            {
                if (correctionPatch.referenceBody == target.TargetOrbit.referenceBody)
                {
                    o = correctionPatch;
                    UT = correctionPatch.StartUT;
                    break;
                }
                correctionPatch = target.core.vessel.GetNextPatch(correctionPatch);
            }

            if (correctionPatch == null || correctionPatch.referenceBody != target.TargetOrbit.referenceBody)
                throw new OperationException("target for course correction must be in the same sphere of influence");

            if (o.NextClosestApproachTime(target.TargetOrbit, UT) < UT + 1 ||
                    o.NextClosestApproachDistance(target.TargetOrbit, UT) > target.TargetOrbit.semiMajorAxis * 0.2)
            {
                errorMessage = "Warning: orbit before course correction doesn't seem to approach target very closely. Planned course correction may be extreme. Recommend plotting an approximate intercept orbit and then plotting a course correction.";
            }

            CelestialBody targetBody = target.Target as CelestialBody;
            Vector3d dV = targetBody != null ?
                OrbitalManeuverCalculator.DeltaVAndTimeForCheapestCourseCorrection(o, UT, target.TargetOrbit, targetBody, targetBody.Radius + courseCorrectFinalPeA, out UT):
                OrbitalManeuverCalculator.DeltaVAndTimeForCheapestCourseCorrection(o, UT, target.TargetOrbit, interceptDistance, out UT);


            return new ManeuverParameters(dV, UT);
        }