//KEEP IN MIND MY NOTES MAY BE INCORRECT AS IT HAS BEEN A YEAR SINCE THIS CODE HAS BEEN TOUCHED, *MY COMMENTS MAY BE WRONG*
CLEARSCREEN.
CLEARVECDRAWS().

//VARIABLES
SET looping TO TRUE.
SET L TO 0.
SET sm TO "". //sm just means steering mode
SET lastClear TO TIME:SECONDS.
SET LZ1 TO Kerbin:GEOPOSITIONLATLNG(-0.0973726146087016,-74.534842938848).
SET LAUNCHPAD TO Kerbin:GEOPOSITIONLATLNG(-0.0971980934745649,-74.5576639199546).
SET LZ TO LZ1.


//FUNCTIONS YOULL NEED
FUNCTION YASS_PID_LOOP {

	PARAMETER kP.
	PARAMETER kI.
	PARAMETER kD.

	PARAMETER input.
	PARAMETER wanted.

	IF DEFINED lastTime {
	}ELSE{
		SET P TO 0.
		SET I TO 0.
		SET D TO 0.

		SET lastP TO 0.
		SET lastI TO 0.
		SET lastTime TO TIME:SECONDS.
	}

	LOCAL thisTime IS TIME:SECONDS.
	LOCAL deltaTime IS (thisTime-lastTime).

	SET P TO (wanted-input).

	IF deltaTime > 0 {
		SET I TO (lastI)    +  (((P+lastP)/2) * deltaTime).
		SET D TO (P-lastP)  /  (deltaTime).
	}

	SET outputValue TO (P*kP) + (I*kI) + (D*kD).

	RETURN outputValue.

	SET lastTime TO thisTime.
	SET lastP TO P.
	SET lastI TO I.

	//IF(TIME:SECONDS-now > 0.05) {
		//LOG input TO TESTHOVER0.csv.
		//LOG (TIME:SECONDS-takeOffTime) TO TESTHOVER1.csv.
		//SET now TO TIME:SECONDS.
	//}
	//PRINT "P :        " + round(P,2)     	    + "               " AT (0,1).
	//PRINT "I :        " + round(I,2)     	    + "               " AT (0,2).
	//PRINT "D :        " + round(D,2)     	    + "               " AT (0,3).
	//
	//PRINT "output :   " + round(outputValue,2)  + "               " AT (0,5).
	//WAIT 0.
}


FUNCTION EQ { // d1=(yB-yA) --- d2=(xB-xA) --- Termine noto=((d2*yA)+(d1*-xA)) --- Result =((d1*currentInput)+termineNoto)/d2
	PARAMETER xA. //Starting input
	PARAMETER yA. //Starting output
	PARAMETER xB. //Final input
	PARAMETER yB. //Final output
	PARAMETER currentInput. //Example eqCalc(0,90,65000,0,SHIP:APOAPSIS)
	PARAMETER allowGrowing IS FALSE.

	LOCAL returner IS (((yB-yA)*currentInput)+(((xB-xA)*yA)+((yB-yA)*-xA)))/(xB-xA).

	IF allowGrowing = TRUE {
		RETURN returner.
	}ELSE IF allowGrowing = FALSE {
		RETURN minmax(yb,ya,returner).
	}
}

FUNCTION constantTWR { //mantain a constant twr
	PARAMETER wantedTWR.

	LOCAL output IS (wantedTWR*SHIP:MASS*gValue())/((SHIP:AVAILABLETHRUST)+1).
	RETURN MIN(1, MAX(0,output)).
}

FUNCTION minmax { //clamp between minv and maxv
	DECLARE PARAMETER minv.
	DECLARE PARAMETER maxv.
	DECLARE PARAMETER input.

	IF maxv > minv {
		RETURN MIN(maxv, MAX(minv,input)).
	}ELSE IF maxv < minv {
		RETURN MIN(minv, MAX(maxv,input)).
	}
}

FUNCTION dToRad { //degrees to radians
	PARAMETER d.
    RETURN d * (CONSTANT:PI()/180).
}

FUNCTION haversine {  //haversine is just a math formula used to calculate distance between two points on a sphere
	DECLARE LOCAL PARAMETER pointA.
	DECLARE LOCAL PARAMETER pointB.
	DECLARE LOCAL PARAMETER zeroer IS "".

	SET aLAT TO dToRad(pointA:LAT).
	SET aLNG TO dToRad(pointA:LNG).
	SET bLAT TO dToRad(pointB:LAT).
	SET bLNG TO dToRad(pointB:LNG).
	SET deltaLAT TO ABS(aLAT-bLAT).
	SET deltaLNG TO ABS(aLNG-bLNG).
	IF zeroer = "LNG" {
		SET deltaLAT TO 0.
	}ELSE IF zeroer = "LAT" {
		SET deltaLNG TO 0.
	}
	LOCAL a IS (SIN(deltaLAT/2))^2+COS(aLAT)*COS(bLAT)*(SIN(deltaLNG/2))^2.
	RETURN 2*ARCSIN(SQRT(a))*BODY:RADIUS.
}


FUNCTION line { //used for display output
	DECLARE PARAMETER adder.
	SET L TO L+adder.
}


FUNCTION gValue {
	DECLARE LOCAL PARAMETER altit IS ALTITUDE.
	RETURN (CONSTANT:G*BODY:MASS)/((BODY:RADIUS+altit)^2).
}







//LANDING VARIABLES
SET radarOffset TO 50. //Account for legs (add about +3m more)

RCS ON. SAS OFF. BRAKES OFF.
LOCK THROTTLE TO t.
LOCK STEERING TO LOOKDIRUP(xy,NORTH:VECTOR*-1).

//VECDRAWS
WHEN showVec = 1 THEN {
	VECDRAW({RETURN SHIP:POSITION.},{RETURN IP:POSITION.},YELLOW,"SHIP-IP",1,TRUE,.1). //VECTOR TO VESSEL IMPACTPOINT
	VECDRAW({RETURN SHIP:POSITION.},{RETURN LZ:POSITION.},WHITE,"SHIP-LZ",1,TRUE,.1). //VECTOR TO OUR LANDING ZONE (OR HOVER TARGET)
	VECDRAW({RETURN SHIP:POSITION.},{RETURN HVvec:NORMALIZED.},RGB(153,0,153),"Horizontal",35,TRUE,.005). //VECTOR OF OUR HORIZONTAL VELOCITY
	VECDRAW({RETURN SHIP:POSITION.},{RETURN xy.},BLACK,"steering",40,TRUE,.005). //VECTOR OF OUR COMMANDED STEERING HEADING
	VECDRAW({RETURN SHIP:POSITION.},{RETURN VXCL(UP:VECTOR,xy):NORMALIZED.},BLACK,"steering",20,TRUE,.005). //VECOTR OF THE HORIZONTAL COMPONENT OF OUR COMMANDED STEERING HEADING
	VECDRAW({RETURN SHIP:POSITION.},{RETURN vecDiffHoriz.},WHITE,"steering",35,TRUE,.005). //HORIZONTAL VECTOR TO OUR LZ (basically the one which we will probe and will add our horizontal velocity to)
	VECDRAW({RETURN SHIP:POSITION.},{RETURN errorVec:NORMALIZED.},BLUE,"errorVec",35,TRUE,.005). //mirrored vector (See line 102)
	VECDRAW({RETURN SHIP:POSITION.},{RETURN SHIP:FACING:VECTOR.},GREEN,"Facing Vector",35,TRUE,.005). //Facing vector
}

//DISPLAY READOUTS //line(1).	PRINT "" AT (0,L).
FUNCTION readouts {
	line(1).	PRINT "STEERING" AT (0,L).
				PRINT "mode: " + sm AT (10,L).
	line(1).	PRINT "--------" AT (0,L).
	line(1).   	PRINT "TILTANGLE" AT (0,L).
				PRINT "± " + ROUND(tiltAngleRange,1) AT (10,L).
	line(1).   	PRINT "Real: " + ROUND(VANG(xy,UP:VECTOR),1) AT (0,L).
	line(1).   	PRINT "Raw: " + ROUND(tiltAngleRaw,1) AT (0,L).
			   	PRINT +"|" + ROUND(tiltAngle,1) AT (10,L).
			   	PRINT " comm" AT (16,L).
	line(1).   	PRINT "targetHVRaw: " + ROUND(targetHVRaw,1) AT (0,L).
			   	PRINT "|" + ROUND(targetHV,1) AT (17,L).
			   	PRINT " comm" AT (22,L).
	line(1).   	PRINT "vecDiffHorizHV: " + ROUND(vecDiffHorizHV,1) AT (0,L).
	line(1).   	PRINT "HVvec:MAG: " + ROUND(HVvec:MAG,1) AT (0,L).
	line(1).   	PRINT "VANG HVvec-steeringVec : " 	+ ROUND(VANG(HVvec,vecDiffHoriz),1) AT (0,L).

	line(3).	PRINT "THROTTLE" AT (0,L).
	line(1).	PRINT "--------" AT (0,L).
	line(1).   	PRINT "t: " + ROUND(t,1) AT (0,L).
				PRINT "|" + ROUND(SHIP:VERTICALSPEED,1) AT (15,L).

	line(1).   	PRINT "hs IPLZ : " + ROUND(distanceIPLZ,1) AT (0,L).
	line(1).   	PRINT "hs SHIPLZ : " + ROUND(haversine(SHIP:GEOPOSITION,LZ),1) AT (0,L).

	SET L TO 0.
}

//SCRIPT
UNTIL looping = FALSE {

	IF ADDONS:TR:HASIMPACT {
		SET IP TO ADDONS:TR:IMPACTPOS.
		SET distanceIPLZ TO haversine(IP,LZ).
	}ELSE{
		SET IP TO SHIP:GEOPOSITION. //IP is impact point
	}

	IF TIME:SECONDS - lastClear > .25 {
		CLEARSCREEN.
		SET lastClear TO TIME:SECONDS.
	}


	SET HVvec TO VXCL(UP:VECTOR,SHIP:VELOCITY:SURFACE). //HVvec is the horizontal velocicty vector

	SET vecDiff TO (LZ:POSITION-IP:POSITION). //vecDiff is the difference (subtraction) between the landing zone (target) and the impact point (where you are headed basically), and vecDiffHoriz just takes only the horizontal component of that vector, the one we need. This vector will be used for all calculations
	SET vecDiffHoriz TO VXCL(UP:VECTOR,vecDiff):NORMALIZED. //N=(0,y,0). E=(x,0,z). ErrorN is HVvec-N

	//i explain the need of this thing below at line 102
	SET inverseHVvec TO HVvec*-1. //https://math.stackexchange.com/questions/13261/how-to-get-a-reflection-vector
	SET errorVec TO inverseHVvec:NORMALIZED-2* VDOT(inverseHVvec:NORMALIZED,vecDiffHoriz:NORMALIZED) *vecDiffHoriz:NORMALIZED.

	//i dont 100% remember what this does but its needed so that we can input these vector into some kind of steering information
	SET vecDiffHorizOffset TO VCRS(UP:VECTOR,vecDiffHoriz).
	SET errorVecOffset TO VCRS(UP:VECTOR,errorVec).

	SET targetHVRaw TO EQ(30,distanceIPLZ/2,0,1+distanceIPLZ/2,distanceIPLZ). //target horizontal velcity we want towards the target only, which i clamp between a min and max of 40
	SET targetHV TO minmax(-40,40,targetHVRaw). //TODO: CHANGE targetHV limits so if need be it should move faster towards LZ

	SET vecDiffHorizHV TO VDOT(vecDiffHoriz,HVvec). //this here is to calculate how much horizontal velocity do we have towards the target
	SET tiltAngleRaw TO YASS_PID_LOOP(0.8,0,0.06,vecDiffHorizHV,targetHV). //pid loop to tilt our vessel which i again clamp between a range i calculate with a simple equation (if the distanceIPLZ is 0 the range is 10, if its 25 the range is 45)
	SET tiltAngleRange TO EQ(0,10,25,45,distanceIPLZ).
	SET tiltAngle TO minmax(tiltAngleRange*-1,tiltAngleRange,tiltAngleRaw).


	//STEERING
	//Sometimes the vessel just accumulates too much error and it starts to pick up horizontal velocity in a different direction and im just telling it if the angle between the horizontal v vector and the vector pointing at the LZ is greater than .1 then you are to steer not anymore towards the LZ but towards its mirrored vector (to basically cancel out the error, until it goes below .1 again). The upper limit of 15 was added because once youre really close to the target it goes way above 15 and you cant keep heading towards that errorvector or youll just fly way off
	IF VANG(HVvec,vecDiffHoriz) > .1 AND VANG(HVvec,vecDiffHoriz) < 15 {
		SET xy TO errorVec+ANGLEAXIS(tiltAngle-90,errorVecOffset).
		SET sm TO "CONVERGING".
	}ELSE{
		SET xy TO vecDiffHoriz+ANGLEAXIS(tiltAngle-90,vecDiffHorizOffset).
		SET sm TO "DIRECT".
	}

	//THROTTLE CONTROL
	SET t TO constantTWR(1)+YASS_PID_LOOP(.08,0.00005,.06,SHIP:VERTICALSPEED,0). //doesnt need to be this complex, it is beacuse this is originally a landing script (which originally was a hover from A to B script)

	SET showVec TO 1.
	readouts().
}