Orbital race

// !runscript xSjerGzW
// racetrack setup script: !runscript FWe0mbFH

set station to vessel("Space Race").
set racecourse to list("PStart","w1","w2","w3","w4","w5","w6","w7","w8", "w9", "w10","PEnd").
set gates to list().
set col0 to rgba(0,0,0,0).
for i in range(10) {
    set i2 to i + 1.
    set str to racecourse[i2].
    set g to station:partstagged(str)[0].
    gates:add(list(g, highlight(g, col0) )).
}
set hl to gates[0][1].
set i to 0.
set gate to ship:rootpart.
set gateT to 0.
set gateTMax to 1.5.
set rel_v to velocity:orbit - station:velocity:orbit.
set gateFA to v(1,0,0).
set gateVel to v(0,0,0).
set baseSpeed to 12.
set thrustMult to 1.5.

wait until not(brakes).
set lastIntersect to time:seconds.


set port to ship:rootpart.
set docking to false.
set t to time:seconds.
set th to 0. set st to facing:vector.

function findPort {
    parameter p is core:part.
    set pp to p:parent.
    if pp:typename = "dockingport" {
        set port to pp.
    } else {
        findPort(pp).
    }
}
findPort().

core:part:controlfrom().
port:undock().
wait 0.
set station to vessel("Space Race").
kuniverse:forceactive(ship).
port:getmodule("moduledockingnode"):setfield("docking acquire force", 0).
sas off.
set t to time:seconds.
set undockTime to time:seconds.
set ship:control:fore to -0.5.


list engines in engs.
for en in engs {
    en:activate().
}
for eng in ship:partsdubbedpattern("vernier") {
    set eng:deadband to 0.000.
    set eng:thrustlimit to 50.
}
for m in ship:modulesnamed("ModuleLight") {
    if m:hasevent("Lights on") m:doevent("Lights on").
}

set th to 0. set st to facing.
lock throttle to th.
lock steering to st.

function next {
    if i = 1 {
        if defined tOld set tNew to time:seconds - tOld.
        set tOld to time:seconds.
        if defined tNew print "<color>Round time: " + round(tNew,2) + "s</color>".
    }
    set gate to gates[i][0].
    set hl:color to col0.
    set hl to gates[i][1].

    set i to i + 1.
    if i = gates:length set i to 0.
    when not(docking) and abs(vdot(gateFA, gate:position)) < rel_v:mag/20 and vdot(rel_v:normalized, gate:position) > 0 and vxcl(gateFA,gate:position):mag < 2.6 then {
        print "passed gate " + gate:tag.
        set gateT to gateTMax.
        next().
    }
    set gateVel to rel_v.
    set lastIntersect to time:seconds.

    //set l to gate:children[0]:getmodule("modulelighteva").
    //set hl:color to rgba(l:getfield("light r"),l:getfield("light g"),l:getfield("light b"),0.3).
}
next().



function go {
    parameter spd is 12.
    set basespeed to spd.
    list targets in tars.  for t in tars if t:distance < 500 and t <> station t:connection:sendmessage(list("go", spd)).
}
function exit2 {
    set ship:control:neutralize to true. rcs off. unlock throttle. unlock steering.
}
function dock {
    set minAng to 180.
    for d in station:dockingports {
        if d:nodetype = port:nodetype and d:state = "ready" and vang(-d:portfacing:vector,d:position) < minAng and station:elements[0]:parts:contains(d) {
            set gate to d.
            set minAng to vang(-d:portfacing:vector,d:position).
            set docking to true.

        }
    }
    if docking {
        print "Docking".
        port:getmodule("moduledockingnode"):setfield("docking acquire force", 150).
        when (gate:nodeposition-port:nodeposition):mag < 0.5 then { exit2(). }
        on ship:parts:length {
            set TRANSFERALL("OXIDIZER", ship:elements[0], core:element):active to true.
            set TRANSFERALL("LIQUIDFUEL", ship:elements[0], core:element):active to true.
            set TRANSFERALL("ELECTRICCHARGE", ship:elements[0], core:element):active to true.
            global dockTime is time:seconds. when time:seconds > dockTime + 3 then { undockandgo(). }
        }


    }

}
when not(docking) and ship:resources[0]:amount / ship:resources[0]:capacity < 0.1 and time:seconds > 2 + lastIntersect and gate:position:mag > 10 then {
    dock(). return true.
}


function vesselSetup {
    rcs on.
    set steeringmanager:maxstoppingtime to 8.
    set steeringmanager:pitchpid:kp to 4.
    set steeringmanager:yawpid:kp to 4.
    set steeringmanager:pitchpid:kd to 0.3.
    set steeringmanager:yawpid:kd to 0.3.
}
vesselSetup().
set tpid to pidloop(0.8,0,0.0,-1,1).
set spid to pidloop(tpid:kp,tpid:ki,tpid:kd,-1,1).
set fpid to pidloop(tpid:kp,tpid:ki,tpid:kd,-1,1).

set vd_ve to vecdraw(v(0,0,0), v(0,0,0), yellow, "", 1, true, 0.4).

set turn to true.
when true then {
    if (not (port:state:contains("docked")) and undockTime + 5 < time:seconds) {
        set rel_v to velocity:orbit - station:velocity:orbit.
        set gateFA to gate:facing:vector.

        set side to vxcl(gateFA, gate:position):mag.
        if docking {
            set fwdDist to vdot(gateFA, port:nodeposition - gate:nodeposition).
            set wantPos to gate:position + gateFA *  (min(3, max(fwdDist - 1, -0.1 + 8*side)) + min(15, side / 2)).
            set wanted_v to gateVel * gateT + (gateTMax-gateT)/gateTMax * (wantPos:normalized * (wantPos:mag^0.75)).
            set ve to wanted_v - rel_v.

            set st to choose -gateFA if side < 2 and gate:position:mag < 10 else (ve + (rel_v:normalized * (-5/20))).
        }
        else {
            if vdot(gate:position, gate:facing:vector) < 0 set gateFA to -gateFA.
            set sl to baseSpeed + (choose (gate:position:mag/10 - 1) if baseSpeed <= 0 else (max(0.001,side-3)^0.3)).
            set wanted_v to (gateVel * (gateT/gateTMax)) + ( (gateTMax-gateT)/gateTMax * ((gate:position + gateFA * max(-14, 2 - side^1.2)):normalized * sl) ).
            set ve to wanted_v - rel_v.
            set st to ve + (rel_v:normalized * (-baseSpeed/30)).

            set vd_ve:vec to ve.
        }

        set gateT to max(0,gateT- (basespeed / 300) ).
        if vang(facing:vector, ve) < 60
            set th to vdot(facing:vector,ve) * thrustMult.
        else set th to 0.

        set ship:control:translation to v(-spid:update(time:seconds, vdot(facing:starvector,ve)),-tpid:update(time:seconds,vdot(facing:topvector,ve)),-fpid:update(time:seconds,vdot(facing:vector,ve))).
    } else {
        set ship:control:translation to v(0,0,0).
        set th to 0.
    }
    return turn.
}

set camDist to 5.
set trackcam to true.
set cam to addons:camera:flightcamera.
set gPos to -facing:vector * 80.
set camMode to "gate".
if random() < 0.5 set camMode to "station".
when trackcam then {
    set gPos to gPos * 0.97 + (rel_v:normalized * 5 + gate:position):normalized * 0.03.

    if camMode = "gate" set cam:position to gPos:normalized * -camDist.
    else if camMode = "puke" set cam:position to facing:vector * -camDist.
    else if camMode = "chase" set cam:position to rel_v:normalized * -camDist.
    else set cam:position to station:position:normalized * -camDist + vxcl(station:position,gPos:normalized * -camDist/2).
    return true.
}

function undockAndGo {
    set minAng to 180.
    for j in range(gates:length) {
        if vang(-port:portfacing:vector, gates[j][0]:position) < minAng {
            set i to j.
            set minAng to vang(-port:portfacing:vector, gates[j][0]:position).
        }
    }
    set docking to false.
    core:part:controlfrom().
    port:undock().
    set undockTime to time:seconds.

    wait 0.
    set station to vessel("Space Race").
    port:getmodule("moduledockingnode"):setfield("docking acquire force", 0).
    sas off.
    set ship:control:fore to -0.5.

    set t to time:seconds.
    lock throttle to th.
    lock steering to st.

    vesselSetup().
    set gate to gates[i][0].
}