from krpc.services.spacecenter import SASMode

import numpy as np
from time import time, sleep

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

from . import Maneuver


class ApproachRCSManeuver(Maneuver):

    SAFETY_RADIUS_MARGIN = 10

    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_rcs_velocity(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) + self.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
            planar_move_vector = unitary((pv[0], pv[2])) * (safety_radius - magnitude((pv[0], pv[2])))
            spacial_move_vector = np.array((planar_move_vector[0], 0, planar_move_vector[1]))

            pv = vessel.position(self.reference_frame)
            move_to_waypoint(self.conn, vessel, pv + spacial_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)
        point_toward_direction(vessel, (0, -1, 0), self.reference_frame)

        return True


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

    kill_relative_velocity(conn, vessel, reference_frame)

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

    start_position = np.array(vessel.position(reference_frame))
    vector = waypoint - start_position
    distance = magnitude(vector)
    direction = unitary(vector)
    acceleration_distance = distance / 4

    point_toward_direction(vessel, direction, reference_frame)

    print("Starting acceleration")
    remaining_distance = distance
    vessel.control.rcs = True
    vessel.control.forward = 1
    while remaining_distance > 3 * acceleration_distance:
        sleep(.1)
        remaining_distance = magnitude(waypoint - vessel.position(reference_frame))
    vessel.control.forward = 0
    print("Target velocity achieved")

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

    print("Starting deceleration")
    kill_rcs_velocity(vessel, reference_frame)
    print("Ship decelerated")

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