from time import time, sleep

import numpy as np


def magnitude(vector):
    return np.linalg.norm(vector)


def node_thrust_time(vessel, node):
    return (node.delta_v * vessel.mass) / vessel.available_thrust


def unitary(vector):
    return np.array(vector) / magnitude(vector)


THROTTLE = .1


def kill_relative_velocity(conn, vessel, reference_frame):
    mj = conn.mech_jeb
    sa = mj.smart_ass

    vessel.control.throttle = 0
    print("Killing relative velocity")
    while magnitude(vessel.velocity(reference_frame)) > .05:
        sa.autopilot_mode = mj.SmartASSAutopilotMode.relative_minus
        sa.update(False)
        while magnitude(vessel.angular_velocity(reference_frame)) > .1:
            sleep(.1)

        vessel.control.throttle = THROTTLE if magnitude(vessel.velocity(reference_frame)) > 1 else THROTTLE / 10
        current_speed = magnitude(vessel.velocity(reference_frame))
        previous_speed = current_speed
        while current_speed <= previous_speed:
            sleep(.1)
            previous_speed = current_speed
            current_speed = magnitude(vessel.velocity(reference_frame))

        vessel.control.throttle = 0

    print("Relative velocity killed")
    sa.autopilot_mode = mj.SmartASSAutopilotMode.off
    sa.update(False)


def correct_course(conn, vessel, waypoint, reference_frame):
    waypoint = np.array(conn.space_center.transform_position(tuple(waypoint), reference_frame, vessel.reference_frame))

    waypoint_x = round(waypoint[0], 0)
    if waypoint_x < 0:
        vessel.control.right = -.1
    elif waypoint_x > 0:
        vessel.control.right = .1
    else:
        vessel.control.right = 0

    waypoint_z = round(waypoint[2], 0)
    if waypoint_z < 0:
        vessel.control.up = .1
    elif waypoint_z > 0:
        vessel.control.up = -.1
    else:
        vessel.control.up = 0


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 get_safety_radius(vessel):
    bbox = vessel.bounding_box(vessel.reference_frame)
    return max(magnitude(bbox[0]), magnitude(bbox[1]))


def point_toward_direction(vessel, direction, reference_frame):
    ap = vessel.auto_pilot
    ap.reference_frame = reference_frame
    ap.target_direction = unitary(direction)
    ap.target_roll = 0
    ap.sas = False
    ap.engage()
    sleep(1)
    ap.wait()

    ap.disengage()
    ap.sas_mode = SASMode.stability_assist
    ap.sas = True