from krpc.services.spacecenter import SASMode

import numpy as np
from time import time, sleep

from .utils import magnitude, unitary, kill_relative_velocity, correct_course, get_safety_radius, point_toward_direction

from . import Maneuver


class ApproachManeuver(Maneuver):

    def __init__(self, conn, mission_control, reference_frame):
        super().__init__(conn, mission_control)
        self.reference_frame = reference_frame

    def start(self):
        sc = self.conn.space_center
        vessel = sc.active_vessel
        target = sc.target_vessel

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

        self.conn.drawing.add_direction((0, 1, 0), self.reference_frame)

        vessel.control.rcs = False

        pv = vessel.position(self.reference_frame)

        safety_radius = get_safety_radius(vessel) + get_safety_radius(target) + SAFETY_RADIUS_MARGIN

        # if under and inside safety cylinder's circle
        if pv[1] < safety_radius and pow(pv[0], 2) + pow(pv[2], 2) <= pow(safety_radius, 2):
            print("We're under the target and inside the safety cylinder, getting out")
            # get out of the cylinder
            plane_move_vector = unitary(tuple((pv[0], pv[2]))) * (safety_radius - magnitude(tuple((pv[0], pv[2]))))

            pv = vessel.position(self.reference_frame)
            move_vector = np.array((plane_move_vector[0], 0, plane_move_vector[1]))
            move_to_waypoint(self.conn, vessel, pv + move_vector, self.reference_frame)

        print("We're outside of the safety cylinder, setting vertical distance")
        pv = vessel.position(self.reference_frame)
        move_to_waypoint(self.conn, vessel, (pv[0], safety_radius, pv[2]), self.reference_frame)

        # should be above and outside => get inside
        print("We're at the right vertical distance to the target, setting horizontal position")
        move_to_waypoint(self.conn, vessel, (0, safety_radius, 0), self.reference_frame)

        point_toward_direction(vessel, - np.array(vessel.position(self.reference_frame)), self.reference_frame)

        return True


THROTTLE = .1
VELOCITY_TOLERANCE = .1


def thrust(vessel, delta_v, reference_frame):
    print("Starting velocity change")
    starting_velocity = magnitude(vessel.velocity(reference_frame))
    vessel.control.throttle = THROTTLE

    if delta_v < 0:
        while magnitude(vessel.velocity(reference_frame)) - starting_velocity > delta_v + VELOCITY_TOLERANCE:
            while magnitude(vessel.velocity(reference_frame)) - starting_velocity > delta_v + .3:
                sleep(.01)
            vessel.control.throttle = THROTTLE / 10
            sleep(.01)
    else:
        while magnitude(vessel.velocity(reference_frame)) - starting_velocity < delta_v - VELOCITY_TOLERANCE:
            while magnitude(vessel.velocity(reference_frame)) - starting_velocity < delta_v - .3:
                sleep(.01)
            vessel.control.throttle = THROTTLE / 10
            sleep(.01)

    vessel.control.throttle = 0
    print("Velocity change achieved")


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

    kill_relative_velocity(conn, vessel, reference_frame)

    conn.drawing.add_line(vessel.position(reference_frame), waypoint, reference_frame)
    waypoint = np.array(waypoint)

    distance = magnitude(waypoint - vessel.position(reference_frame))
    if distance > 250:
        velocity = 10
    elif distance > 50:
        velocity = 5
    elif distance > 25:
        velocity = 2
    else:
        velocity = 1

    direction = waypoint - np.array(vessel.position(reference_frame))
    point_toward_direction(vessel, direction, reference_frame)

    start_position = np.array(vessel.position(reference_frame))
    print("Starting acceleration")
    thrust(vessel, velocity, reference_frame)
    print("Target velocity achieved")
    acceleration_distance = magnitude(np.array(vessel.position(reference_frame)) - start_position)

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

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

    vessel.control.rcs = True
    while magnitude(waypoint - vessel.position(reference_frame)) > acceleration_distance:
        print(magnitude(waypoint - vessel.position(reference_frame)), " ", acceleration_distance)
        sleep(.1)
        correct_course(conn, vessel, waypoint, reference_frame)

    vessel.control.rcs = False
    vessel.control.up = 0
    vessel.control.right = 0

    print("Starting deceleration")
    thrust(vessel, -velocity, reference_frame)
    print("Ship decelerated")

    sa.autopilot_mode = mj.SmartASSAutopilotMode.off
    sa.update(True)

    print("destination position: {}".format(waypoint))
    print("end position:         {}".format(np.array(vessel.position(reference_frame))))


SAFETY_RADIUS_MARGIN = 10


# DEPRECATED
def maneuver_to_approach(conn, reference_frame):
    print("Handling approach")
    sc = conn.space_center
    vessel = sc.active_vessel
    target = sc.target_vessel
    kill_relative_velocity(conn, vessel, reference_frame)
    conn.drawing.add_direction((0, 1, 0), reference_frame)

    vessel.control.rcs = False

    pv = vessel.position(reference_frame)

    safety_radius = get_safety_radius(vessel) + get_safety_radius(target) + SAFETY_RADIUS_MARGIN

    # if under and inside safety cylinder's circle
    if pv[1] < safety_radius and pow(pv[0], 2) + pow(pv[2], 2) <= pow(safety_radius, 2):
        print("We're under the target and inside the safety cylinder, getting out")
        # get out of the cylinder
        plane_move_vector = unitary(tuple((pv[0], pv[2]))) * (safety_radius - magnitude(tuple((pv[0], pv[2]))))

        pv = vessel.position(reference_frame)
        move_vector = np.array((plane_move_vector[0], 0, plane_move_vector[1]))
        move_to_waypoint(conn, vessel, pv + move_vector, reference_frame)

    print("We're outside of the safety cylinder, setting vertical distance")
    pv = vessel.position(reference_frame)
    move_to_waypoint(conn, vessel, (pv[0], safety_radius, pv[2]), reference_frame)

    # should be above and outside => get inside
    print("We're at the right vertical distance to the target, setting horizontal position")
    move_to_waypoint(conn, vessel, (0, safety_radius, 0), reference_frame)

    point_toward_direction(vessel, - np.array(vessel.position(reference_frame)), reference_frame)

    print("Approach handled")


TARGET_VELOCITY = 2


# DEPRECATED
def move_with_vector(conn, vessel, vector, reference_frame):
    mj = conn.mech_jeb
    sa = mj.smart_ass

    kill_relative_velocity(conn, vessel, reference_frame)

    position = np.array(vessel.position(reference_frame))
    vector = np.array(vector)
    destination = position + vector

    conn.drawing.add_line(vessel.position(reference_frame), destination, reference_frame)

    print("Pointing to acceleration")
    point_toward_direction(vessel, unitary(vector), reference_frame)
    print("Pointed")
    trip_duration = magnitude(vector) / TARGET_VELOCITY
    acceleration_start = time()
    print("Starting acceleration")
    vessel.control.throttle = THROTTLE
    while magnitude(vessel.velocity(reference_frame)) < TARGET_VELOCITY:
        sleep(.01)
    vessel.control.throttle = 0
    print("Target velocity achieved")

    acceleration_duration = time() - acceleration_start

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

    while acceleration_duration < trip_duration - (time() - acceleration_start):
        sleep(.01)

    print("Starting deceleration")
    vessel.control.throttle = THROTTLE

    deceleration_start = time()
    while time() - deceleration_start < acceleration_duration:
        sleep(.01)
    vessel.control.throttle = 0
    print("Deceleration done")

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

    print("starting position:    {}".format(position))
    print("destination position: {}".format(destination))
    print("end position:         {}".format(np.array(vessel.position(reference_frame))))