from time import sleep

import numpy as np
import numpy.linalg as la

# from docking import *
from utils import magnitude
from rendezvous import maneuver_to_rendezvous
from approach import maneuver_to_approach
from docking import dock_ship
import krpc


conn = krpc.connect()
print(conn.krpc.get_status().version)


def undock(conn):
    print('Undocking')
    sc = conn.space_center
    v = sc.active_vessel
    target = sc.target_vessel

    decouplers = [d for d in v.parts.decouplers if d.staged and not d.decoupled]

    tugship = decouplers[-1].decouple()
    sc.active_vessel = tugship
    v = sc.active_vessel
    v.name = "Tugship " + target.name
    sc.target_vessel = target

    v.control.throttle = 0

    v.control.rcs = True
    v.control.up = -1
    sleep(5)
    v.control.up = 0

    v.control.solar_panels = True
    v.parts.engines[0].active = True
    print('Undocking sequence finished')


sc = conn.space_center
vessel = sc.active_vessel
target = sc.target_vessel

if vessel.name == "TugGrape":
    if target is None:
        print("Must set a target")
        exit(1)

    undock(conn)

maneuver_to_rendezvous(conn, vessel, target)

reference_frame = sc.ReferenceFrame.create_relative(target.reference_frame, rotation=(1., 0., 0., 0.))

if magnitude(vessel.position(vessel.reference_frame) - np.array(target.position(vessel.reference_frame))) < 1000:
    maneuver_to_approach(conn, reference_frame)

docking_part = vessel.parts.root.children[0].children[10].children[0].children[0].children[0].children[0]
dock_ship(conn, vessel, docking_part, reference_frame)

conn.close()