// Tuning parameters
SET pGain TO 1/800.				// proportional gain
SET iGain TO 1/200000000.	// integral gain
SET dGain TO 1/50.				// differential gain

SET maxPitch TO 67.5.			// Hard limit for maximum pitch angle (degrees)
SET asInfl TO 900.				// Determines how much airspeed influences maximum pitch angle
SET divePar TO 2.				  // Determines steepness of dive. Lower number = steeper
SET launchPitch TO 20.		// Initial pitch at launch (degrees)

// Cruising altitude calculation
IF TARGET:DISTANCE < 190000 {
	SET cruisingAlt TO 1000 + TARGET:DISTANCE / 10.
} ELSE {
	// Cruising altitude is capped at 20 km
	SET cruisingAlt TO 20000.
}

CLEARSCREEN.
print "Initial distance to target : " + ROUND(TARGET:DISTANCE).
print "Cruising altitude: " + ROUND(cruisingAlt).

WAIT 2.0.
LOCK THROTTLE TO 1.0.
TOGGLE AG1.

// Launch
STAGE.
LOCK STEERING TO HEADING(TARGET:HEADING, launchPitch).

WAIT UNTIL STAGE:SOLIDFUEL < 0.001.
STAGE.

SET anArrow TO VECDRAW().
SET anArrow:SHOW TO true.
SET anArrow:START TO V(0,0,0).
SET anArrow:COLOR TO RGB(1,0,0).
SET anArrow:SCALE TO 5.0.

WHEN true THEN {
	SET anArrow:VEC TO target:position.
	SET anArrow:LABEL TO TARGET:DISTANCE.
	PRINT "Distance to target: " + ROUND(TARGET:DISTANCE) AT(0,3).
	
	PRESERVE.
}

SET ticks TO 0.															      // For logging

// For integration
SET t0 TO TIME:SECONDS.
SET iError TO 0.

UNTIL ((TARGET:DISTANCE)^2-(ALTITUDE^2))^0.5 < ALTITUDE*divePar {
	SET dt TO TIME:SECONDS - t0.										// Time elapsed since previous iteration
	
	SET pError TO cruisingAlt - ALTITUDE.					  // Error in height
	SET iError TO iError + pError * dt.							// Integrated (cumulative) error in height
	SET dError TO -1*VERTICALSPEED.									// Error in vertical speed
	
	SET angleCor TO 1/(AIRSPEED/asInfl + 1).				// Correction on maximum pitch angle influenced by airspeed
	SET pitch TO ARCTAN(pError*pGain + iError*iGain + dError*dGain) / 90 * maxPitch * angleCor.
	LOCK STEERING TO HEADING(TARGET:HEADING, pitch).
	
	// Diplay live information
	PRINT "pError: " + ROUND(pError, 3) AT(0,5).
	PRINT "iError: " + ROUND(iError, 3) AT(0,6).
	PRINT "dError: " + ROUND(dError, 3) AT(0,7).
	PRINT "pTerm: " + ROUND(pError*pGain, 3) AT(20,5).
	PRINT "iTerm: " + ROUND(iError*iGain, 3) AT(20,6).
	PRINT "dTerm: " + ROUND(dError*dGain, 3) AT(20,7).
	PRINT "Pitch: " + ROUND(pitch, 3) AT(0,8).
	PRINT "Max pitch: " + ROUND(angleCor*90, 3) AT(20,8).
	
	// logging
	SET ticks TO ticks + 1.
	SET line TO ticks + "," + ALTITUDE + "," + pitch + "," + AIRSPEED.
	LOG line TO output.
}

PRINT "Target in range. Diving..." AT(0,10).

LOCK STEERING TO TARGET:POSITION.
WAIT UNTIL false.