@LAZYGLOBAL off.

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


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 crash if behind the target docking port.
    rcs_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".
}