planeLandingStuff.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 600.
set drawVecs to true.
set landLoop to true.

set finalEnd to places["ksc 09"].
set finalStart to ship:body:geopositionof(finalEnd:position + (heading(-90, 0):forevector * 1500)).
set vecCircleCenter to finalStart:position + V(0, circRadius, 0).
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:position, red, "Circle", 1, true, 0.5, true).
//set arrCircleCenterDir to vecdraw(V(0,0,0), vecCircleCenter, yellow, "Circle", 1, true, 0.5, true).

set angNorthToCraft to (geoCircleCenter:heading - 180).
set angCraftToTangent to arccos(circRadius/vecCircleCenter: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.
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/vecCircleCenter:mag).
        set angNorthToTangent to angNorthToCraft - angCraftToTangent.

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


        set targetHeading to geoCraftToTangent:heading.
        print targetHeading.

        if vang(ship:facing:forevector, geoCraftToTangent:position) < 5 {
            set mode to "locked".
            print "mode locked".
        }
    }
    if mode = "locked" {
        set vecCircleCenterToTangent to heading(angNorthtoTangent, 0, 0):forevector * circRadius.
        set vecCraftToTangent to vecCircleCenter + vecCircleCenterToTangent.

        set targetHeading to geoCraftToTangent:heading.
        //print targetHeading + " " + geoCraftToTangent:distance.

        // Close or gone past it.
        if vecCraftToTangent:mag < 20 or vang(ship:facing:forevector, geoCraftToTangent:position) > 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 + 5 {
            if geoCircleCenter:distance > circRadius {
                set targetHeading to (geoCircleCenter:heading + 90) - 0.30.
            }
            set lastupdate to time:seconds.
            print geoCircleCenter:distance  + " >? " + circRadius + " " + targetHeading + " heading " + geoCircleCenter:heading + " => " + (geoCircleCenter:heading + 90).
        }
        //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).
        }
    }
    if mode = "final" {
        set targetHeading to finalEnd:heading.

        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.
        }
    }

    return landLoop.
}

//wait until false.