planeLandingStuffPlane.ks

set places to readjson("0:/kerbin.json").

clearVecDraws().

set mode to "initial".
print "mode initial".

// Hack for ground operations, I think.
set craftHeight to 0.

set circRadius to 2500.
set drawVecs to true.
set landLoop to true.

set finalEnd to places["runway west"].
set finalStart to ship:body:geopositionof(finalEnd:altitudeposition(altitude) + (heading(-90, 0):forevector * 3600)).
set vecCircleCenter to finalstart:altitudeposition(altitude) + heading(0, 0):forevector * circRadius.
set geoCircleCenter to ship:body:geopositionof(vecCircleCenter).
//set vecCircleCenter:x to vecCircleCenter:x + craftHeight.
set vecNorth to V(0,1,0).

//set arrCircleCenterDir to vecdraw(V(0,0,0), geoCircleCenter:altitudeposition(altitude), red, "Circle", 1, true, 0.5, true).
//set arrCircleCenterDir to vecdraw(V(0,0,0), vecCircleCenter, yellow, "Circle", 1, true, 0.5, true).
// set arrFinal to vecdraw(finalStart:position, finalEnd:position - finalStart:position, blue, "Final", 1, true, 0.5, true).

set angNorthToCraft to (geoCircleCenter:heading - 180).
set angCraftToTangent to arccos(circRadius/geoCircleCenter:altitudeposition(altitude):mag).
set angNorthToTangent to angNorthToCraft - angCraftToTangent.

set vecCircleCenterToTangent to heading(angNorthtoTangent, 0, 0):forevector * circRadius.
set vecCraftToTangent to vecCircleCenter + vecCircleCenterToTangent.
//set vecCraftToTangent:x to craftHeight.

//when drawVecs then {

set vecCraftToTangent to geoCircleCenter:position + vecCircleCenterToTangent.
//set vecCraftToTangent:x to craftHeight.
set geoCraftToTangent to ship:body:geopositionof(vecCraftToTangent).

//wait 0.

//return false.

//}

print "distance to circle: " + vecCircleCenter:mag.
print "distance to circle (alt): " + geoCircleCenter:distance.
print "circle radius: " + circRadius.

print "angNorthToCraft " + angNorthToCraft.
print "angNorthToCraft (alt) " + (geoCircleCenter:heading - 180).
print "angCraftToTangent " + angCraftToTangent.
print "angNorthToTangent " + angNorthToTangent.

print "vecCircleCenterToTangent " + vecCircleCenterToTangent.
print "vecCraftToTangent " +  vecCraftToTangent.
print "geoCraftToTangent " +  geoCraftToTangent.
print "distance to tangent " + geoCraftToTangent:distance.
print "angle diff " +  (geoCircleCenter:heading - geoCraftToTangent:heading).

set targetHeading to geoCraftToTangent:heading.
set hdg to targetHeading.
//lock wheelsteering to targetHeading.
//lock wheelthrottle to 0.20.
//brakes off.

print "before vecs".
//set arrTargetDir to vecdraw(V(0,0,0), { return geoCraftToTangent:position. }, red, "Heading", 1, true, 0.5, true).
//set arrTangent to vecdraw({ return geoCircleCenter:position. }, { return vecCircleCenterToTangent. }, yellow, "Tangent", 1, true, 0.5, true).
print "after vecs".

when landLoop = true then {
    if mode = "initial" {
        set angNorthToCraft to (geoCircleCenter:heading - 180).
        set angCraftToTangent to arccos(circRadius/geoCircleCenter:altitudeposition(altitude):mag).
        set angNorthToTangent to angNorthToCraft - angCraftToTangent.

        set vecCircleCenterToTangent to heading(angNorthtoTangent, 0, 0):forevector * circRadius.
        set vecCraftToTangent to geoCircleCenter:altitudeposition(altitude) + vecCircleCenterToTangent.
        set geoCraftToTangent to ship:body:geopositionof(vecCraftToTangent).


        set targetHeading to geoCraftToTangent:heading.
        set hdg to targetHeading.
print round(targetHeading, 2) + " " + round(abs(geoCraftToTangent:bearing), 2).

        if abs(geoCraftToTangent:bearing) < 5 {
            set mode to "locked".
            print "mode locked".
        }
    }
    if mode = "locked" {
        set vecCircleCenterToTangent to heading(angNorthtoTangent, 0, 0):forevector * circRadius.
        set vecCraftToTangent to geoCircleCenter:altitudeposition(altitude) + vecCircleCenterToTangent.

        set targetHeading to geoCraftToTangent:heading.
        set hdg to targetHeading.
//print round(vecCraftToTangent:mag) + " " + round(geoCraftToTangent:distance) + " " + round(geoCraftToTangent:bearing, 2).

        // Close or gone past it.
        if vecCraftToTangent:mag < 20 or abs(geoCraftToTangent:bearing) > 45 {
            set mode to "turning".
            print "mode turning".
            set lastupdate to time:seconds.
            //set arrTargetDir:show to false.

            //set arrTangent to vecdraw({ return geoCircleCenter:position. }, { return heading(geoCircleCenter:heading - 180, 0, 0):forevector * circRadius. }, yellow, "Tangent", 1, true, 0.5, true, false).

            //SET Kp TO 0.5.
            //SET Ki TO 0.6.
            //SET Kd TO 0.6.
            //SET PID TO PIDLOOP(Kp, Ki, Kd).
            //SET PID:SETPOINT TO circRadius.
            //set targetHeading TO geoCrafttoTangent:heading.
        }
    }
    if mode = "turning" {
        if lastUpdate < time:seconds + 3 {
            if geoCircleCenter:distance > circRadius {
                set targetHeading to (geoCircleCenter:heading + 90) - 15.
set hdg to targetHeading.

            }
if geoCircleCenter:distance > (circRadius*1.05) {
                set targetHeading to (geoCircleCenter:heading + 90) - 18.
set hdg to targetHeading.

            }
if geoCircleCenter:distance > (circRadius * 1.25) {
                set targetHeading to (geoCircleCenter:heading + 90) - 25.
set hdg to targetHeading.

            }
if geoCircleCenter:distance > (circRadius * 1.5) {
                set targetHeading to (geoCircleCenter:heading + 90) - 35.
set hdg to targetHeading.

            }
            set lastupdate to time:seconds.
            print round(geoCircleCenter:distance, 2)  + " >? " + round(circRadius, 2) + " " + round(targetHeading, 2) + " hdg " + round(geoCircleCenter:heading, 2) + " => " + round(geoCircleCenter:heading + 90, 2).
        }
        //SET targetHeading TO targetHeading + PID:UPDATE(TIME:SECONDS, geoCircleCenter:position:mag).

        if finalStart:distance < 10 or (finalStart:distance < (circRadius / 4) and abs(finalStart:BEARING) > 120) {
            set mode to "final".
            print "mode final".
            print finalstart:distance.
            print finalstart:bearing.
            //set arrTangent:show to false.
            //set arrTargetDir to vecdraw(V(0,0,0), { return finalEnd:position. }, red, "Heading", 1, true, 0.5, true, false).
when alt:radar < 15 then { hs(0). } when alt:radar < 10 then { hv(10). } hs(80). hv(-6). on ship:status { brakes on. hs(0). for p in ship:partsnamedpattern("wheel") { p:getModule("ModuleWheelBrakes"):SetField("brakes", 200). } }
        }
    }
    if mode = "final" {
        set targetHeading to finalEnd:heading.
set hdg to targetHeading.

        if finalEnd:distance < 10 or (finalEnd:distance < (circRadius / 4) and abs(finalEnd:BEARING) > 120) {
            set mode to "landing".
            print "mode landing".
            print finalend:distance.
            print finalend:bearing.
            brakes on.
        }
    }
   if mode = "landing" {
set hdg to places["runway west"]:heading.

}

    return landLoop.
}

//wait until false.