from time import sleep

from .utils import kill_relative_velocity, correct_course, magnitude

from . import Maneuver


class DockingManeuver(Maneuver):
    def __init__(self, conn, vessel_id, docking_part, reference_frame):
        super().__init__(conn, vessel_id)
        self.docking_part = docking_part
        self.reference_frame = reference_frame

    def start(self):
        vessel = self.conn.space_center.active_vessel
        self.conn.drawing.add_direction((0, 1, 0), self.reference_frame)
        self.conn.drawing.add_direction((1, 0, 0), self.reference_frame)
        vessel.parts.controlling = self.docking_part

        kill_relative_velocity(self.conn, vessel, self.reference_frame)
        set_attitude_and_roll(self.conn, vessel, self.reference_frame)
        align_horizontally(self.conn, vessel, self.reference_frame)

        print("Starting docking procedure")
        vessel.control.set_action_group(0, True)
        rcs_push(vessel, {"y": 1}, .5)

        vessel.control.rcs = True
        try:
            while vessel.position(self.reference_frame)[1] > 0:
                print(vessel.position(self.reference_frame)[1])
                correct_course(self.conn, vessel, (0, 0, 0), self.reference_frame)
                sleep(1)
        except ValueError as e:
            vessel = self.conn.space_center.active_vessel
        finally:
            vessel.control.rcs = False

        return True


def set_attitude_and_roll(conn, vessel, reference_frame):
    fl = vessel.flight(reference_frame)
    vessel.control.rcs = False

    ap = vessel.auto_pilot
    ap.reference_frame = reference_frame
    ap.target_direction = (0, -1, 0)
    ap.target_roll = 0
    ap.sas = False
    ap.engage()
    ap.wait()
    ap.disengage()

    mj = conn.mech_jeb
    sa = mj.smart_ass

    sa.autopilot_mode = mj.SmartASSAutopilotMode.target_plus
    sa.update(False)

    while magnitude(vessel.angular_velocity(reference_frame)) > .1:
        sleep(.1)

    print("Ship pointing to dock")


def rcs_push(vessel, axis, duration):
    vessel.control.rcs = True
    if "x" in axis:
        vessel.control.up = axis["x"]
    elif "y" in axis:
        vessel.control.forward = axis["y"]
    elif "z" in axis:
        vessel.control.right = axis["z"]
    sleep(duration)
    if "x" in axis:
        vessel.control.up = 0
    elif "y" in axis:
        vessel.control.forward = 0
    elif "z" in axis:
        vessel.control.right = 0

    vessel.control.rcs = False


def kill_rcs_velocity(vessel, reference_frame):
    print("Killing RCS velocity")
    velo = vessel.velocity(reference_frame)
    vessel.control.rcs = True
    while any(abs(component) > .05 for component in velo) > .05:
        if abs(velo[0]) > .05:
            sign = -velo[0] / abs(velo[0])
            if abs(velo[0]) > .1:
                vessel.control.up = 1 * sign
            elif abs(velo[0]) > .05:
                vessel.control.up = .1 * sign
            else:
                vessel.control.up = 0

        if abs(velo[1]) > .05:
            sign = -velo[1] / abs(velo[1])
            if abs(velo[1]) > .1:
                vessel.control.forward = 1 * sign
            elif abs(velo[1]) > .05:
                vessel.control.forward = .1 * sign
            else:
                vessel.control.forward = 0

        if abs(velo[2]) > .05:
            sign = velo[2] / abs(velo[2])
            if abs(velo[2]) > .1:
                vessel.control.right = 1 * sign
            elif abs(velo[2]) > .05:
                vessel.control.right = .1 * sign
            else:
                vessel.control.right = 0
        sleep(.1)
        velo = vessel.velocity(reference_frame)
    vessel.control.rcs = False
    print("RCS velocity killed")


def align_horizontally(conn, vessel, reference_frame):
    conn.drawing.add_direction((1, 0, 0), vessel.reference_frame)

    while abs(vessel.position(reference_frame)[0]) > .1 \
            or abs(vessel.position(reference_frame)[2]) > .1:
        # determine power requirements of each
        sign_x = 1 if vessel.position(reference_frame)[0] > 0 else -1
        if abs(vessel.position(reference_frame)[0]) > 1:
            power_x = 1
        elif abs(vessel.position(reference_frame)[0]) > .1:
            power_x = .1
        else:
            power_x = 0
            sign_x = 0

        sign_z = 1 if vessel.position(reference_frame)[2] > 0 else -1
        if abs(vessel.position(reference_frame)[2]) > 1:
            power_z = 1
        elif abs(vessel.position(reference_frame)[2]) > .1:
            power_z = .1
        else:
            power_z = 0
            sign_z = 0

        axis = {}
        if power_x > 0:
            axis["x"] = -sign_x * power_x
        if power_z > 0:
            axis["z"] = sign_z * power_z

        rcs_push(vessel, axis, 1)

        while (sign_x > 0 and vessel.position(reference_frame)[0] > .1
               or sign_x < 0 and vessel.position(reference_frame)[0] < -.1
               or sign_x == 0) \
                and (sign_z > 0 and vessel.position(reference_frame)[2] > .1
                     or sign_z < 0 and vessel.position(reference_frame)[2] < -.1
                     or sign_z == 0):
            print(vessel.position(reference_frame))
            sleep(.1)

        kill_rcs_velocity(vessel, reference_frame)
    print("Vertical alignment done!")