DECLARE PARAMETER compassDegree IS 90. // Straight east is 90

CLEARSCREEN.
set filename to Ship:Shipname.
set filename to filename + "-log.csv".
SET ClambsReleased TO FALSE.


//Stage when ship thrust is 0.
WHEN SHIP:MAXTHRUST = 0 AND ClambsReleased THEN {
    PRINT "" + TIME +" Staging ".
    STAGE.
	WAIT UNTIL SHIP:MAXTHRUST > 0. 
    PRESERVE.
}

WHEN true THEN {
	PRINT "                                                                     " AT (0,33).
	PRINT "SHIP:MAXTHRUST " + ROUND(SHIP:MAXTHRUST)  AT (0,33).
	PRESERVE.
}

//THRUSTERS AT MAXIMUM!
LOCK THROTTLE TO 1.0.   

//Set Steering
SET desiredAngle TO 90.
SET deltaAngle TO 1.05.  //1 for TWR = 3. Perhaps set dynamically. The higher value indicates tipping faster
SET AdjustmentAltitude TO 1000.
SET deltaAltitude TO 300.
SET deltaGrowth TO 30.
SET MYSTEER TO HEADING(compassDegree,90).
LOCK STEERING TO MYSTEER. 

//COUNTDOWN
PRINT "Counting down:".
FROM {local countdown is 10.} UNTIL countdown = 0 STEP {SET countdown to countdown - 1.} DO {
    PRINT "..." + countdown.
    WAIT 0.3. 
}

//LAUNCH!
STAGE.
WAIT 4.
PRINT "Release docking clams".
STAGE.
WAIT 1.
SET ClambsReleased TO TRUE.


UNTIL SHIP:APOAPSIS > 240000 { //Remember, all altitudes will be in meters

	DumpFinishedBoostersIfAny().

	IF SHIP:ALTITUDE >= AdjustmentAltitude {
		SET desiredAngle TO desiredAngle - deltaAngle.
		IF (desiredAngle <= 0){
			SET desiredAngle TO 0.
		}
		SET AdjustmentAltitude TO AdjustmentAltitude + deltaAltitude.
		SET deltaAltitude TO deltaAltitude + deltaGrowth. 
		SET MYSTEER TO HEADING(compassDegree,desiredAngle).
		
		PRINT "Pitching down to " + ROUND(desiredAngle,1) + " degrees                   " AT(0,15).
	}
}.

CLEARSCREEN.
PRINT "250km apoapsis reached, starting to level off for a vertical path".
SET desiredAngle TO 0.
SET MYSTEER TO HEADING(compassDegree,desiredAngle).
SET apoapsisReached TO FALSE.

UNTIL SHIP:PERIAPSIS > 200000{
	
	IF SHIP:ALTITUDE > 170000 OR apoapsisReached {
		//If the rocket is raising above 250km apoapsis
		IF SHIP:VERTICALSPEED > 0 AND SHIP:APOAPSIS > 250000 {
			SET desiredAngle TO desiredAngle -0.07. 
			SET MYSTEER TO HEADING(compassDegree,desiredAngle).
			
			IF (desiredAngle <= -20){
				SET desiredAngle TO -20.
				SET MYSTEER TO HEADING(compassDegree,desiredAngle).
				PRINT "desiredAngle <= 0 " + ROUND(desiredAngle,1) + " degrees                 " AT(0,16).
			}
			
			PRINT "Pitching down to " + ROUND(desiredAngle,1) + " degrees                   " AT(0,15).
		
			WAIT 0.5. // Only slowly tip downwards.
		}
		
		//if vertical speed is decreasing, increase angle. The rocket is falling downwards.
		IF SHIP:VERTICALSPEED < 0 AND SHIP:APOAPSIS < 250000 {
			SET desiredAngle TO desiredAngle +0.1.
			SET MYSTEER TO HEADING(compassDegree,desiredAngle).
			
			PRINT "Vessel is falling downards                 			 		" AT (0,14).				
			PRINT "Pitching up to " + ROUND(desiredAngle,1) + " degrees         " AT(0,15).
			
			WAIT 0.5.
		}
	}
	
	//If Apoapsis point is reached
	IF SHIP:ALTITUDE > SHIP:APOAPSIS-1000 AND SHIP:ALTITUDE < SHIP:APOAPSIS+1000 {
		
		SET desiredAngle TO -1.
		SET MYSTEER TO HEADING(compassDegree,desiredAngle).
		SET apoapsisReached TO TRUE.
		
		PRINT "Apoapsis point reached                         			 		" AT (0,14).			
		PRINT "Pitching up to " + ROUND(desiredAngle,1) + " degrees				" AT(0,15).
	}
	
	// If ship has passed the Apoapsis point, then turn upwards to maintain height
	IF apoapsisReached AND SHIP:ALTITUDE < SHIP:APOAPSIS-2000 {
			
		SET desiredAngle TO 7.
		SET MYSTEER TO HEADING(compassDegree,desiredAngle).
		SET apoapsisReached TO TRUE.

		PRINT "Vessel drifting past Apoapsis point                           	" AT (0,14).			
		PRINT "Pitching up to " + ROUND(desiredAngle,1) + " degrees.            " AT(0,15).					
	}

	//If the Apoapsis points is running away, before being reacehed
	IF SHIP:APOAPSIS > 300000 AND NOT apoapsisReached {
	
		SET desiredAngle TO -10.
		SET MYSTEER TO HEADING(compassDegree,desiredAngle).
			
		PRINT "Apoapsis points in running away from vessel                    " AT (0,15).	
	}	
	
	//If the Apoapsis points is running away, after being reacehed
	IF SHIP:APOAPSIS > 580000 AND apoapsisReached {
			
		PRINT "Vessel is increasing apoapsis too high. Shutting down engines. " AT (0,15).	
		
		SET desiredAngle TO 0.
		SET MYSTEER TO HEADING(compassDegree,desiredAngle).
		LOCK THROTTLE TO 0.
		BREAK.
	}	
	
}		

PRINT "Program end condition reached. Termination program. Enjoy your orbit." AT (0,21).		

LOCK THROTTLE TO 0.
SET SHIP:CONTROL:PILOTMAINTHROTTLE TO 0.
PANELS ON.
DeployAntenna.


//If the ship is under thrust, and there are flamed out engines, they must be boosters, so Stage.
function DumpFinishedBoostersIfAny{

	LIST ENGINES IN elist.
	SET flameoutFound TO FALSE.
	FOR e IN elist {
		IF (e:FLAMEOUT and e:IGNITION AND flameoutFound = FALSE AND SHIP:MAXTHRUST > 0) {
			SET flameoutFound TO TRUE.
		}
	}

	IF (flameoutFound){				
			PRINT "Staging boosters".
			STAGE.
			SET flameoutFound TO FALSE.
	}
}

function DeployAntenna{
	SET listOfParts TO SHIP:PARTSDUBBED("Communotron 32").
	FOR item IN listOfParts {
		IF (item:GETMODULE("ModuleRTAntenna"):HASEVENT("ACTIVATE")){
			item:GETMODULE("ModuleRTAntenna"):DOEVENT("ACTIVATE").
		}
	}	
	
	PRINT "Antenna deployed".	
}	

function checkEngines {
    set engActive to false.
    set engOut to false.
    list engines in engineList.
	
    for eng in engineList {
        set engActive to engActive or eng:ignition.
        set engOut to engOut or (eng:ignition and eng:flameout).
    }
    if engOut or not engActive {
        stage.
        wait 0.2.
    }
}

function test  {
	LIST ENGINES IN englist. 
	FOR eng IN englist {
		IF ENG:FLAMEOUT OR MAXTHRUST = 0 {
			STAGE. 
			BREAK.
		}
	}
}





///IDEAS
//Once reached desired height, just eliminate negative vertical speed.