//hellolaunch

//First, we'll clear the terminal screen to make it look nice
CLEARSCREEN.

if ADDONS:TR:AVAILABLE {
    if ADDONS:TR:HASIMPACT {
        PRINT ADDONS:TR:IMPACTPOS.
    } else {
        PRINT "Impact position is not available".
    }
} else {
    PRINT "Trajectories is not available.".
}

SET TR1 TO LATLNG(5.360278, -52.951944).
PRINT TR1:DISTANCE / 1000.            // Print distance from vessel to x
PRINT TR1:HEADING.             // Print the heading to the point
PRINT TR1:BEARING.

WAIT 0.5.

//Next, we'll lock our throttle to 100%.
LOCK THROTTLE TO 1.0.   // 1.0 is the max, 0.0 is idle.



SET MYSTEER TO HEADING(90,90). //90 degrees east and pitched up 90 degrees (straight up)
LOCK STEERING TO MYSTEER. // from now on we'll be able to change steering by just assigning a new value to MYSTEER

STAGE.
WAIT 0.5.
UNTIL SHIP:VERTICALSPEED > 140


		{SET MYSTEER TO HEADING(90,90).}

set imptrghdg  to 0.

SET Kp TO 0.01.
SET Ki TO 0.006.
SET Kd TO 0.006.
SET PID TO PIDLOOP(Kp, Ki, Kd).
SET PID:SETPOINT TO 0.

// setting pitch and yaw to an aproximate heading and a designated pitch until 20km
UNTIL SHIP:ALTITUDE > 20000 {
	SET MYSTEER TO HEADING(TR1:HEADING - PID:UPDATE(TIME:SECONDS, tr1:bearing),60).          //ship:srfprograde:pitch
               // Print the heading to the point
	PRINT TR1:BEARING.
    // pid:update() is given the input time and input and returns the output. gforce is the input.
    WAIT 0.3.
}

// loop with PID to maintain heading that is parallel to the heading from the estimated impact location to the target
UNTIL SHIP:ALTITUDE > 400000 {
	set deltaLong to (tr1:LNG - ADDONS:TR:IMPACTPOS:LNG).

	// calculating heading from predicted impact location to the target
	set imptrghdg to arctan2(sin(deltaLong) * cos(tr1:lat), cos(ADDONS:TR:IMPACTPOS:lat) * sin(tr1:lat) - sin(ADDONS:TR:IMPACTPOS:lat) * cos(tr1:lat) * cos(deltaLong)).

	//calcuating the difference between heading from ship to target and heading from impact to target
	SET DIFFDIR TO (tr1:HEADING - imptrghdg).


	//PID loop to lower the difference above to zero
	SET MYSTEER TO HEADING((142 -  PID:UPDATE(TIME:SECONDS, DIFFDIR)),45).          //ship:srfprograde:pitch
               // Print the heading to the point
	//PRINT  TR1:BEARING.


	//printing values to monitor during flight
	PRINT "AAAAAA".
	print  tr1:heading.




	print  imptrghdg.
	PRINT (DIFFDIR).

	//PRINT ADDONS:TR:IMPACTPOS.
    // pid:update() is given the input time and input and returns the output. gforce is the input.
    WAIT 0.01.
}







WAIT UNTIL SHIP:ALTITUDE > 70000.

// NOTE that it is vital to not just let the script end right away
// here.  Once a kOS script just ends, it releases all the controls
// back to manual piloting so that you can fly the ship by hand again.
// If the program just ended here, then that would cause the throttle
// to turn back off again right away and nothing would happen.