@LAZYGLOBAL off.

run once "lib/node".
run once "lib/target".
run once "lib/vectors".


function rcs_isp_sum {
    //
    // Return the sum of ISP for RCS thrusters.
    //
    local sum is 0.
    for thruster in SHIP:modulesnamed("ModuleRCSFX") {
        set sum to sum + thruster:getfield("rcs isp").
    }
    return sum.
}


function rcs_maxthrust {
    // There's no way to get thrust of RCS dynamically in kOS; gonna have to hard-code it :/
    // Source: https://wiki.kerbalspaceprogram.com/wiki/Reaction_Control_System#Thrusters
    local rcs_thrusts is LEXICON(  // in kN
        "RCSBlock", 1.0,  // RV-105 RCS Thruster Block
        "RCSBlock.v2", 1.0,  // RV-105 RCS Thruster Block (KSP v1.7+)
        "linearRcs", 2.0,  // Place-Anywhere 7 Linear RCS Port
        "vernierEngine", 12.0,  // Vernor Engine
        "mk1-3pod", 1.0  // Mk1-3 Command Pod
    ).

    local thrust_sum is 0.
    for module in SHIP:modulesnamed("ModuleRCSFX") {
        local part_name is module:part:name.
        if rcs_thrusts:haskey(part_name) {
            set thrust_sum to thrust_sum + rcs_thrusts[part_name].
        } else {  // if non-stock part
            print "WARNING: Unknown RCS Thruster '" + part_name + "': using default thrust of 1.0 kN".
            set thrust_sum to thrust_sum + 1.0.
        }
    }
    return thrust_sum.
}


function rcs_estimated_burn_duration {
    //
    // Calculate estimated burn duration of a vector or node using RCS thrusters instead of main engines.
    //
    parameter burn.
    return estimated_burn_duration(burn, rcs_isp_sum(), rcs_maxthrust()).
}


function rcs_translate {
    //
    // Translate the ship's position by the given vector using RCS thrusters.
    //
    parameter translation_vector_delegate.
    parameter max_speed.

    local lock translation_vector to translation_vector_delegate().
    // Find available RCS acceleration by Newton's second law: (F=mg). Note that the acceleration is conservatively divided
    // by 6 since thrusters might not be aligned with thrust vector (but assumed evenly distributed on the 6 axes).
    // Furthermore, acceleration is limited to 0.05m/s^2 since small ships with low mass are uncontrollable if allowed to accelerate wildly.
    local acceleration is min(0.05, (rcs_maxthrust() / SHIP:MASS) / 6).
    // Time t to travel distance d under constant acceleration a is t = sqrt(2d/a) (https://en.wikipedia.org/wiki/Equations_for_a_falling_body).
    // Multiply this by the available acceleration to get speed (s * m/s^2 = m/s) required to decelerate to 0 by the time d=0.
    local lock desired_speed to min(max_speed, sqrt(2*translation_vector:mag / acceleration) * acceleration).
    local lock desired_velocity to translation_vector:normalized * desired_speed.

    // The proportional gain factor is set to the square root of the ship's mass. Why? It was empirically shown to work.
    // The idea is that small ships are more sensitive to adjustments, and so small error corrections will do more harm than good.
    local pid_x is PIDLOOP(sqrt(SHIP:MASS), 0.01, 0.001, -1, 1).  // kp, ki, kd, minoutput, maxoutput
    local pid_y is PIDLOOP(sqrt(SHIP:MASS), 0.01, 0.001, -1, 1).
    local pid_z is PIDLOOP(sqrt(SHIP:MASS), 0.01, 0.001, -1, 1).

    //vdraw(SHIP:controlpart:position, translation_vector@, WHITE).
    //vdraw(SHIP:controlpart:position, relative_velocity@, GREEN).

    RCS on.
    until translation_vector:mag < 0.05 and relative_velocity():mag < 0.01 {
        set pid_x:setpoint to desired_velocity:x.
        set pid_y:setpoint to desired_velocity:y.
        set pid_z:setpoint to desired_velocity:z.

        local x is pid_x:update(TIME:seconds, relative_velocity():x).
        local y is pid_y:update(TIME:seconds, relative_velocity():y).
        local z is pid_z:update(TIME:seconds, relative_velocity():z).

        set SHIP:CONTROL:TRANSLATION to ship_raw_to_ship_control(V(x,y,z)).
        wait 0.  // wait one physics tick
    }
}