"""
mavsimPy
    - Chapter 4 assignment for Beard & McLain, PUP, 2012
    - Update history:  
        12/27/2018 - RWB
        1/17/2019 - RWB
"""
import sys
sys.path.append('..')
import numpy as np
import parameters.simulation_parameters as SIM

from chap2.mav_viewer import mav_viewer
# from chap2.video_writer import video_writer
from chap3.data_viewer import data_viewer
from chap4.mav_dynamics import mav_dynamics
from chap4.wind_simulation import wind_simulation
from time import sleep

# initialize the visualization
VIDEO = False  # True==write video, False==don't write video
mav_view = mav_viewer()  # initialize the mav viewer
data_view = data_viewer()  # initialize view of data plots
if VIDEO == True:
    video = video_writer(video_name="chap4_video.avi",
                         bounding_box=(0, 0, 1000, 1000),
                         output_rate=SIM.ts_video)

# initialize elements of the architecture
wind = wind_simulation(SIM.ts_simulation)
mav = mav_dynamics(SIM.ts_simulation)

# initialize the simulation time
sim_time = SIM.start_time

# main simulation loop
# sleep(5)
print("Press Command-Q to exit...")
while sim_time < SIM.end_time:
    #-------set control surfaces-------------
    if(sim_time<25):
        delta_e = -0.1
        delta_t = 1.0  # 0.5
        delta_a = 0.0  # 0.0
        delta_r = 0.0  # 0.005
        delta = np.array([[delta_e, delta_t, delta_a, delta_r]]).T  # transpose to make it a column vector
    else:
        delta_e = -0.3
        delta_t = 1.0#0.5
        delta_a = 0.01#0.0
        delta_r = 0.00025#0.005
        delta = np.array([[delta_e, delta_t, delta_a, delta_r]]).T  # transpose to make it a column vector

    #-------physical system-------------
    current_wind = wind.update()  # get the new wind vector
    # print("current wind: ", current_wind)
    mav.update_state(delta, current_wind)  # propagate the MAV dynamics

    #-------update viewer-------------
    mav_view.update(mav.msg_true_state)  # plot body of MAV
    data_view.update(mav.msg_true_state, # true states
                     mav.msg_true_state, # estimated states
                     mav.msg_true_state, # commanded states
                     SIM.ts_simulation)
    if VIDEO == True:
        video.update(sim_time)

    #-------increment time-------------
    sim_time += SIM.ts_simulation

if VIDEO == True:
    video.close()





[((9, 0, 9, 21), 'sys.path.append', 'sys.path.append', ({(9, 16, 9, 20): '""".."""'}, {}), "('..')", False, 'import sys\n'), ((22, 11, 22, 23), 'chap2.mav_viewer.mav_viewer', 'mav_viewer', ({}, {}), '()', False, 'from chap2.mav_viewer import mav_viewer\n'), ((23, 12, 23, 25), 'chap3.data_viewer.data_viewer', 'data_viewer', ({}, {}), '()', False, 'from chap3.data_viewer import data_viewer\n'), ((30, 7, 30, 41), 'chap4.wind_simulation.wind_simulation', 'wind_simulation', ({(30, 23, 30, 40): 'SIM.ts_simulation'}, {}), '(SIM.ts_simulation)', False, 'from chap4.wind_simulation import wind_simulation\n'), ((31, 6, 31, 37), 'chap4.mav_dynamics.mav_dynamics', 'mav_dynamics', ({(31, 19, 31, 36): 'SIM.ts_simulation'}, {}), '(SIM.ts_simulation)', False, 'from chap4.mav_dynamics import mav_dynamics\n'), ((46, 16, 46, 64), 'numpy.array', 'np.array', ({(46, 25, 46, 63): '[[delta_e, delta_t, delta_a, delta_r]]'}, {}), '([[delta_e, delta_t, delta_a, delta_r]])', True, 'import numpy as np\n'), ((52, 16, 52, 64), 'numpy.array', 'np.array', ({(52, 25, 52, 63): '[[delta_e, delta_t, delta_a, delta_r]]'}, {}), '([[delta_e, delta_t, delta_a, delta_r]])', True, 'import numpy as np\n')]