@LAZYGLOBAL off.

run once "lib/rendezvous".
run once "lib/rocket".
run once "lib/vectors".


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


function dock {
    //
    // Dock the ship with another vessel.
    // Prerequisites:
    //  Must be controlling from a local docking port (right click -> control from here).
    //  TARGET must be a docking port (right click -> set as target).
    //
    parameter max_translation_speed is 1.0.
    parameter final_docking_speed is 0.1.

    print "==================== DOCKING =====================".

    // Target port vectors
    local lock target_port to TARGET:nodeposition.
    local lock target_port_facing to TARGET:portfacing:vector.
    //vdraw(target_port@, target_port_facing@, CYAN).

    // Local port vectors
    local lock local_port to SHIP:controlpart:nodeposition.
    local lock local_port_facing to SHIP:controlpart:portfacing:vector.
    //vdraw(local_port@, local_port_facing@, CYAN).

    SAS off.
    set NAVMODE to "TARGET".

    print "==> Aligning with target port".
    local lock alignment_direction to lookdirup(-target_port_facing, TARGET:ship:facing:vector).  // align dock-on-dock but with UP the same direction
    lock STEERING to alignment_direction.
    wait until vang(SHIP:facing:vector, alignment_direction:vector) <= 1.

    print "==> Translating".
    // TODO: Translate "around" the target vessel on different axes first so we dont fail if behind the target docking port.
    translate({return (target_port + target_port_facing) - (local_port + local_port_facing).}, max_translation_speed).

    print "==> Docking".
    // TARGET will be unset the moment we dock, causing many of the calculations and local locks to cause errors.
    // Therefore, the final docking will be done using the information available to us now, without any error corrections.
    lock STEERING to SHIP:facing.
    set SHIP:CONTROL:TRANSLATION to ship_raw_to_ship_control(target_port - local_port).
    wait until relative_velocity():mag >= final_docking_speed.
    unlock_control().

    wait until not HASTARGET.
    print "==> DOCKING COMPLETE".
}


dock().




// TODO: Make GUI with dropdown of target:dockingports and SHIP:dockingports so user can't fuck it up.
//if not HASTARGET {
//    print "Please select a target".
//    return.
//}
//function find_docking_port {
//    parameter target.  // SHIP or TARGET.
//    parameter target_name is target:name.
//
//    if target:istype("DockingPort") {
//        return target.
//    }
//
//    local target_ports is target:dockingports.
//    if target_ports:empty {
//        print "No docking ports on " + target_name.
//        return.
//    }
//    if target_ports:length <> 1 {
//        print "Multiple docking ports on " + target_name + ", please select one and try again".
//        return.
//    }
//    return target_ports[0]
//}