SET State_Initialize to 0.
SET State_Burn_To_Suborbital to 1.
SET State_Cruise_To_Deaccel TO 2.
SET State_Deaccelleration TO 3.
SET State_VerticalDescent TO 4.
SET State_Landed TO 5.

SET turnOnEngine TO 1.0.
SET turnOffEngine TO 0.0.
SET turnReverseEngine TO -1.0.
SET suicideBurnDone TO FALSE.
SET currentState TO State_Initialize.



CLEARSCREEN.
PRINT "Initiating landing code".

RCS ON.
SAS OFF.
LOCK STEERING TO mysteer. 


//Determine stage
IF (SHIP:PERIAPSIS < 0){
	PRINT "Suborbital trajectory detected. Changing state".
	SET currentState TO State_Deaccelleration.
}ELSE{
	PRINT "Setting state - Burn to Suborbital ".
	SET currentState TO State_Burn_To_Suborbital.
}

IF (currentState = State_Burn_To_Suborbital){
	PRINT "Preparing to establish sub-orbital path".
	
	SET mysteer TO ship:retrograde.
	WAIT 30.
	
	PRINT "Ignite engines".
	SET SHIP:CONTROL:FORE TO turnOnEngine.
	WAIT 4.
	LOCK THROTTLE TO turnOnEngine.  
	SET SHIP:CONTROL:FORE TO turnOffEngine. 

	UNTIL SHIP:PERIAPSIS < -100000 {
		WAIT 0.1.
	}

	LOCK THROTTLE TO turnOffEngine. 
	
	PRINT "Vessel on impact trajectory. Changing state".
	SET currentState TO State_Cruise_To_Deaccel.
}

IF (currentState = State_Cruise_To_Deaccel){

	PRINT "Prepare for deaccelleration burn".	
	PRINT "Waiting until burn time".
	WAIT 10.
	SET WARP TO 3.		
	LOCK height to ALT:RADAR.	
	WAIT UNTIL ((getDistanceToImpact < StoppingDistance*4)OR (height < 50000)).
	SET WARP TO 2.	
	WAIT UNTIL ((getDistanceToImpact < StoppingDistance*3) OR (height < 45000)).
	SET WARP TO 0.	
	SET mysteer TO ship:retrograde.
	
	PRINT "Vessel near deacceleration. Changing state".
	SET currentState TO State_Deaccelleration.
}

IF (currentState = State_Deaccelleration){

	LEGS ON.
	LOCK height to ALT:RADAR.	
	UNTIL height < 10 {
	
		SET mysteer TO ship:retrograde.
		//SET mysteer TO HEADING(SHIP:Retrograde
		PRINT mysteer AT(0,15). 
		
	
		SET distanceToImpact TO getDistanceToImpact.
	
		PRINT "Height:             " + ROUND(height,2) AT(0,18). 
		PRINT "Distance to impact: " + ROUND(distanceToImpact,2) AT(0,19). 
		//PRINT "Altitude to start:  " + ROUND(altitude_to_start_suicide_burn,2) AT(0,21). 
		PRINT "StoppingDistance    " + ROUND(StoppingDistance,2) AT(0,22).
	
		IF ((distanceToImpact < (StoppingDistance +8000)) AND suicideBurnDone = FALSE) {
			PRINT "Status: Starting deacceleration burn". 
			SET SHIP:CONTROL:FORE TO turnOnEngine.
			WAIT 2.
			LOCK THROTTLE TO turnOnEngine. 
			SET SHIP:CONTROL:FORE TO turnOffEngine.
			SET suicideBurnDone TO true.
		}	
		
		IF ((suicideBurnDone = TRUE) AND (StoppingDistance > distanceToImpact)){
			SET SHIP:CONTROL:FORE TO turnOnEngine.
		}
		
		IF ((suicideBurnDone = TRUE) AND (StoppingDistance < distanceToImpact)){
			SET SHIP:CONTROL:FORE TO turnOffEngine.
		}
		
		IF  (pythagoran < 10 AND suicideBurnDone = TRUE){
			//SET SHIP:CONTROL:FORE TO turnOffEngine.
			PRINT "Status: Ending deacceleration burn". 
			LOCK THROTTLE TO turnOffEngine. 
			BREAK.
		}
	}

	PRINT "Vessel deaccelerated. Changing state".
	SET currentState TO State_VerticalDescent.	
}

IF (currentState = State_VerticalDescent){

	PRINT "State_VerticalDescent".
	SET suicideBurnDone TO FALSE.
	LOCK height to ALT:RADAR.	

	UNTIL height < 10 {

		SET mysteer TO ship:retrograde.
	
		//SET distanceToImpact TO getDistanceToImpact.
	
		PRINT "Height:             " + ROUND(height,2) AT(0,18). 
		//PRINT "Distance to impact: " + ROUND(distanceToImpact,2) AT(0,19). 
		//PRINT "Altitude to start:  " + ROUND(altitude_to_start_suicide_burn,2) AT(0,21). 
		PRINT "StoppingDistance    " + ROUND(StoppingDistance,2) AT(0,22).
		PRINT "suicideBurnDone       " + suicideBurnDone AT(0,23).
	
		IF ((height < (StoppingDistance +50)) AND (suicideBurnDone = FALSE)) {
			PRINT "Status: Starting second deacceleration burn" AT(0,24). 
			SET SHIP:CONTROL:FORE TO turnOnEngine.
			WAIT 1.
			LOCK THROTTLE TO turnOnEngine. 
			SET SHIP:CONTROL:FORE TO turnOffEngine.
			SET suicideBurnDone TO true.
		}	
	
		IF  (SHIP:VERTICALSPEED > -5 AND suicideBurnDone = TRUE){
			PRINT "Status: Ending second deacceleration burn" AT(0,24). 
			LOCK THROTTLE TO turnOffEngine. 
			SET suicideBurnDone TO FALSE.
			WAIT 1.
		}
	
		IF (height < 500 AND SHIP:VERTICALSPEED < -5){
			SET SHIP:CONTROL:FORE TO turnOnEngine.
		}
		IF (height < 500 AND SHIP:VERTICALSPEED > -5){
			SET SHIP:CONTROL:FORE TO turnOffEngine.
		}
		IF (height < 50 AND SHIP:VERTICALSPEED < -2){
			SET SHIP:CONTROL:FORE TO turnOnEngine.
		}
		IF (height < 50 AND SHIP:VERTICALSPEED > -2){
			SET SHIP:CONTROL:FORE TO turnOffEngine.
		}
	}

	LOCK THROTTLE TO turnOffEngine. 
	SET SHIP:CONTROL:FORE TO turnOffEngine.
	RCS OFF.
	SAS OFF.
	
	PRINT "Vessel landed. Changing state".
	SET currentState TO State_Landed.	
}


IF (currentState = State_Landed){
	PRINT "Performing post landing operations".
	PRINT "Glory to The Illyrien Republic. Masters of Science!".
}





function StoppingDistance{
	declare local safety TO 1.10.

	declare local g to constant:g * body:mass / body:radius^2.		// Gravity (m/s^2)
	declare local maxDecel to (ship:maxthrust / ship:mass) - g.	// Maximum deceleration possible (m/s^2)
	declare local stopDist to pythagoran^2 / (2 * maxDecel).		// The distance the burn will require
	RETURN stopDist*safety.
}

function altitude_to_start_suicide_burn {
	//(altitude to start suicide burn) = [ (current altitude)(acceleration of gravity) + (1/2)(current velocity)2 ] / (acceleration of engines)
	  
	declare local goal to (getDistanceToImpact * gravity_calculator + (1/2) * pythagoran^2) / ship:maxthrust.
	return goal.
}.

function gravity_calculator {
	declare local GM to ship:body:mu.
	declare local R to (body:radius + ship:altitude).
	declare local g to GM/(R^2).
	return g.
}.

function pythagoran{
	declare local a TO SHIP:Groundspeed.
	declare local b TO SHIP:VERTICALSPEED.

	SET c TO SQRT(a^2 + b^2).
	
	PRINT "pythagoran    " + c AT(0,26).
	
	return c.
}

function getDistanceToImpact{
	SET distance TO ALT:RADAR / SIN(getAngleA).
	return distance.
}

function getAngleA{
	declare local a TO SHIP:VERTICALSPEED. 	
	declare local c TO pythagoran.
	
	PRINT "a2      " + a AT(0,27).
	PRINT "c2      " + c AT(0,28).
	PRINT "ABS(a/c)" + ABS(a/c) AT(0,29).
	
	SET A TO ARCSIN(ABS(a/c)).
	PRINT "Impact Angle:       " + ROUND(A,2) AT(0,20). 
	RETURN A.
}

//sumvector of speed.
function pythagoran{
	declare local a TO SHIP:Groundspeed.
	declare local b TO SHIP:VERTICALSPEED.

	SET c TO SQRT(a^2 + b^2).
	return c.
}