using System;
using System.Collections.Generic;
using System.Net;
using System.Timers;
using KRPC.Client;
using KRPC.Client.Services.SpaceCenter;

class Program
{
    public static void Main ()
    {
        //var conn = new Connection ("Ares-Raptor");
        var conn = new Connection(
                   name: "Ares-Raptor",
                   address: IPAddress.Parse("127.0.0.1"),
                   rpcPort: 5000,
                   streamPort: 5001);
        var vessel = conn.SpaceCenter ().ActiveVessel;

        float turnStartAltitude = 500;
        float turnEndAltitude = 200000;
        float targetAltitude = 650000;
        float targetazimuth = 51;

        // Set up streams for telemetry
        var ut = conn.AddStream (() => conn.SpaceCenter ().UT);
        var flight = vessel.Flight ();
        var altitude = conn.AddStream (() => flight.MeanAltitude);
        var apoapsis = conn.AddStream (() => vessel.Orbit.ApoapsisAltitude);
        var stage1Resources =
            vessel.ResourcesInDecoupleStage (stage: -1, cumulative: false);
        var boosterfuel  = conn.AddStream(() => stage1Resources.Amount("Kerosene"));
        // Pre-launch setup
        vessel.Control.SAS = true;
        vessel.Control.RCS = false;
        vessel.Control.Throttle = 1;

        // Countdown...
        Console.WriteLine ("3...");
        System.Threading.Thread.Sleep (1000);
        Console.WriteLine ("2...");
        System.Threading.Thread.Sleep (1000);
        Console.WriteLine ("1...");
        System.Threading.Thread.Sleep (1000);
        Console.WriteLine ("Launch!");
                // Activate the first stage

        vessel.Control.ActivateNextStage ();
        System.Threading.Thread.Sleep (5500);
        vessel.Control.ActivateNextStage ();
        vessel.AutoPilot.Engage ();
        vessel.AutoPilot.TargetPitchAndHeading (90, 90);

        double turnAngle = 0;
        bool stage1sep = false;

        while (true) {

            // Gravity turn
            if (altitude.Get () > turnStartAltitude && altitude.Get () < turnEndAltitude) {
                double frac = (altitude.Get () - turnStartAltitude) / (turnEndAltitude - turnStartAltitude);
                double newpitchangle = frac * 90.0;
                if (Math.Abs (newpitchangle - turnAngle) > 0.5) {
                    turnAngle = newpitchangle;
                    vessel.AutoPilot.TargetPitchAndHeading (
                        (float)(90 - turnAngle), targetazimuth);
                }
            }

                        // Separate SRBs when finished
            if (!stage1sep) {
         //       Console.WriteLine(boosterfuel.Get());
                if (boosterfuel.Get () < 10) {
                    vessel.Control.ActivateNextStage ();
                    stage1sep = true;
                    Console.WriteLine ("Stage 1 separated");
                    System.Threading.Thread.Sleep (1000);
                    Console.WriteLine ("Stage 2 Ignition");
                }
            }

            // Decrease throttle when approaching target apoapsis
            if (altitude.Get() > 240000) {

                Console.WriteLine ("Approaching target apoapsis");
                vessel.Control.ToggleActionGroup(1);
                vessel.AutoPilot.TargetPitchAndHeading (10, targetazimuth);
                // Disable engines when target apoapsis is reached
                vessel.Control.Throttle = 1;
                while (apoapsis.Get () < targetAltitude) {
                }
                Console.WriteLine ("Target apoapsis reached");
                vessel.Control.Throttle = 0;
                break;
            }
        }



        // Wait until out of atmosphere
        Console.WriteLine ("Coasting out of atmosphere");
        while (altitude.Get () < 70500) {
        }
                // Plan circularization burn (using vis-viva equation)
        Console.WriteLine ("Planning circularization burn");
        double mu = vessel.Orbit.Body.GravitationalParameter;
        double r = vessel.Orbit.Apoapsis;
        double a1 = vessel.Orbit.SemiMajorAxis;
        double a2 = r;
        double v1 = Math.Sqrt (mu * ((2.0 / r) - (1.0 / a1)));
        double v2 = Math.Sqrt (mu * ((2.0 / r) - (1.0 / a2)));
        double deltaV = v2 - v1;
        var node = vessel.Control.AddNode (
            ut.Get () + vessel.Orbit.TimeToApoapsis, prograde: (float)deltaV);

        // Calculate burn time (using rocket equation)
        double F = vessel.AvailableThrust;
        double Isp = vessel.SpecificImpulse * 9.82;
        double m0 = vessel.Mass;
        double m1 = m0 / Math.Exp (deltaV / Isp);
        double flowRate = F / Isp;
        double burnTime = (m0 - m1) / flowRate;
                // Orientate ship
        Console.WriteLine ("Orientating ship for circularization burn");
        vessel.AutoPilot.ReferenceFrame = node.ReferenceFrame;
        vessel.AutoPilot.TargetDirection = Tuple.Create (0.0, 1.0, 0.0);
        vessel.AutoPilot.Wait ();

        // Wait until burn
        Console.WriteLine ("Waiting until circularization burn");
        double burnUT = ut.Get () + vessel.Orbit.TimeToApoapsis - (burnTime / 2.0);
        double leadTime = 5;
        conn.SpaceCenter ().WarpTo (burnUT - leadTime);
                // Execute burn
        Console.WriteLine ("Ready to execute burn");
        var timeToApoapsis = conn.AddStream (() => vessel.Orbit.TimeToApoapsis);
        while (timeToApoapsis.Get () - (burnTime / 2.0) > 0) {
        }
        Console.WriteLine ("Executing burn");
        vessel.Control.Throttle = 1;
        System.Threading.Thread.Sleep ((int)((burnTime - 0.1) * 1000));
        Console.WriteLine ("Fine tuning");
        vessel.Control.Throttle = 0.05f;
        var remainingBurn = conn.AddStream (
            () => node.RemainingBurnVector (node.ReferenceFrame));
        while (remainingBurn.Get ().Item1 > 0) {
        }
        vessel.Control.Throttle = 0;
        node.Remove ();

        Console.WriteLine ("Launch complete");
        conn.Dispose();
    }
}