race track

// vtol code: !runscript https://pastebin.com/xeNy4xmv
// self:      !runscript https://pastebin.com/3gF9PnXd
clearvecdraws().
local tracks is lexicon("temp", 1).
global gates is list().
set tracks to readjson("0:/json/tracks.json").

set defaultGateWidth to 25.
set defaultSL to 150.

//cache
set gateI to 0.
set gate to lexicon().
set nextGate to lexicon().
set gateGeo to latlng(0,0).
set gatePos to v(0,0,0).
set gateFacing to facing.
set gateRadius to 0.
set vdList to list().
set vdScale to 50.
set terSlope to 0.
for i in range(4) {
    vdList:add(vecdraw(v(0,0,0), v(0,0,0), rgb(0.8,0.8,0.8), "", 1, true, 1, false)).
    set vdList[i]:wiping to false.
}
set vdList[0]:color to rgb(1,0.25,0).
set vdList[2]:color to rgb(0.1,0.35,1).

function createGate {
    parameter geo, hdg, p, alt2, gatesl is defaultSL, width is defaultGateWidth.
    return lexicon("geo", geo, "hdg", hdg, "p", p, "alt", alt2, "sl", gatesl, "width", width).
}
function gateHere {
    parameter hdg is 0, ptch is 0, gatesl is defaultSL.
    return createGate(ship:geoposition, hdg, ptch, round(altitude,1), gatesl).
}
function addGateHereFacing {
    parameter pitch is 0, gatesl is defaultSL.
    gates:add(gateHere(body:geopositionof(facing:topvector * -1000):heading,pitch, gatesl)).
    next().
}
function updateGate {
    set gates[(gateI - 1)] to gate.
}
function clearTrack {
    set gateI to 0. set gates to list().
}
function randomGate {
    set gpos to body:geopositionof(heading(random() * 360,0):vector * 1000 * random()).
    return createGate(gpos, random() * 360, 0, gpos:terrainheight + 20).
}

function randomTrack {
    parameter cnt is 10.
    set gates to list().
    //createGate(geo, hdg, pitch,  alt, (sl), (width) )
    for i in range(cnt) gates:add(randomGate()).

}

function loadTrack {
    parameter str is "default".
    set tracks to readjson("0:/json/tracks.json").
    set gates to tracks[str].
    set gateI to 0. next().
}
function saveTrack {
    parameter str.
    local tracks is readjson("0:/json/tracks.json").
    if tracks:haskey(str) set tracks[str] to gates.
    else tracks:add(str, gates).

    writejson(tracks, "0:/json/tracks.json").
}

function next {

    set gate to gates[gateI].
    print "next gate: " + gateI + ", SL: " + gate:sl + "m/s".


    set gateI to gateI + 1.
    if gateI >= gates:length set gateI to 0.

    set nextGate to gates[gateI].
}

//loadtrack("ozin").

set cam to addons:camera:flightcamera.
set race to false.
when race then {
    set gateFacing to heading(gate["hdg"], gate["p"]).
    set gateRadius to gate["width"] / 2.
    set gatePos to gate["geo"]:altitudeposition(gate:alt).

    //draw gate
    set vdList[0]:vec to gateFacing:topvector * (gateRadius * 2).
    set vdList[0]:start to gatePos + gateFacing:starvector * -gateRadius + gateFacing:topvector * -gateRadius.
    set vdList[0]:width to vdScale / (gatePos - cam:position):mag.
    for i in range(1, 4) {
        set rot to angleaxis(90 * i, gateFacing:vector).
        set vdList[i]:vec to rot * vdList[0]:vec.
        set vdList[i]:start to (rot * (vdList[0]:start-gatePos)) + gatePos.
        set vdList[i]:width to vdList[0]:width.
    }

    //detect
    set frontDist to vdot(gateFacing:vector, gatePos).
    if abs(vdot(gatePos, gateFacing:topvector)) < gateRadius and abs(vdot(gatePos, gateFacing:starvector)) < gateRadius and frontDist >= 0 and frontDist-vdot(gateFacing:vector, velocity:surface / 25) < 0 {
        //ship is in front of gate, within the star and top distance bounds, and is a about to pass through it within next two physics ticks based on current velocity, so consider completed
        next().
    }


    return race.
}


function go {
    race on.
    lock taltoverride to gate:alt.
    lock sl to min(100, gate:sl).
    set tersteps to 6.
    set alignterrainheight to -100.
    set vsMin to -3.
    set tltpid to pidloop(20, 2, 2, -50, 50).
    set tltStarpid to pidloop(40, 2, 2, -40, 40).
    when true then {
        set sideDist to abs(vdot(gateFacing:starvector, gatePos)).
        set frontDist to vdot(gateFacing:vector, gatePos).

        set aimPoint to vxcl(up:vector, gatePos + gateFacing:vector * max(-300, -1 * (sideDist^1.1) / 3 + (choose 5 if frontDist >= 0 else -10))):normalized.
        set ang to min(90, vang(vxcl(up:vector,gatePos + gateFacing:vector * 10), vxcl(gateFacing:vector, gatePos))).
        set tv to aimPoint * min(sl, sqrt(2 * 3 * max(10, sideDist - max(0,vdot(aimpoint,velocity:surface)) * 0.25)) / cos(ang)).

        set terSlope to min(90 - vang(up:vector, gatePos), terSlope).
        return race.
    }
}

//needs to be run when spawned on the launchpad without moving!!
function convertTrackFromKerbinToGael {
    set kerbinRW to LATLNG(-0.0485129700103269,-74.7243272069546).
    set gp to latlng(8.6870184757, -168.3459248301).

    set heightDiff to ship:geoposition:terrainheight - 69.1217.
    set latDiff to gp:lat - kerbinRW:lat.
    set lngDiff to gp:lng - kerbinRW:lng.
    for g in gates {
        set g:geo to latlng(g:geo:lat + latDiff, g:geo:lng + lngDiff).
        set g:alt to g:alt + heightDiff.
    }
    set gateI to 0. next().
}