"""
Execute an approach towards landing on a runway.
env.jets[1] is used as a reference position for landing.
"""

from vpython import *
from ..global_variables import *
from ..fast_jet import dir_to_vec


X_RANGE = (-120, -80)
Y_RANGE = (10, 20)
Z_RANGE = (-60, 60)

HDG_DEV, PITCH_DEV, ROLL_DEV = pi/8, pi/8, pi/4


def reset(env):
    pos = vector(env.np_random.uniform(*X_RANGE), env.np_random.uniform(*Y_RANGE), env.np_random.uniform(*Z_RANGE))
    hdg = env.np_random.uniform(-HDG_DEV, HDG_DEV)
    pitch = env.np_random.uniform(-PITCH_DEV, +PITCH_DEV)
    roll = env.np_random.uniform(-ROLL_DEV, +ROLL_DEV)
    vel_norm, _ = dir_to_vec(hdg, pitch, roll)
    vel = vel_norm * env.np_random.uniform(START_SPEED_MIN, START_SPEED_MAX)
    env.jets[0].reset(pos=pos, vel=vel, hdg=hdg, pitch=pitch, roll=roll)
    env.jets[1].reset(pos=vector(100,0,0), pitch=pi/12) # Reference jet is always at the same position

def terminate(env):
    if env.jets[0].pos.x >= 100: env.phase = 1

task = {
    "obs": DEFAULT_OBS,
    "obs_lims": DEFAULT_OBS_LIMS,
    "reset": reset,
    "phases": (2, terminate),
    "reference_jet": {},
    "render": lambda env: (
        (box(canvas=env.scene, pos=vector(150,0,0), size=vector(150,0.1,10), color=vector(0.2,0.2,0.2))),
         box(canvas=env.scene, pos=vector(150,0,0), size=vector(150,0.11,0.5)))
}
