import time
import os
import math

import sys
import PX4MavCtrlV4 as PX4MavCtrl
import UE4CtrlAPI
ue = UE4CtrlAPI.UE4CtrlAPI()

# Create a MAVLink communication instance with UDP 
# port 20100 (coresponding to the port on CopterSim)
mav = PX4MavCtrl.PX4MavCtrler(1)
time.sleep(1)

# Switch to a night scene
#ue.sendUE4Cmd('RflyChangeMapbyName ModernCityNight')    
ue.sendUE4Cmd('RflyChangeMapbyName LightShow')    
time.sleep(4)

# send 100 vehicles to RflySim3D to form a helix
z0 = -50
x0 = 50
y0 = 40
for i in range(100):
    s = 1 + i/5.0
    th = math.pi/3 + math.pi/10*i
    y = s*math.cos(th)
    z = -s*math.sin(th)
    ue.sendUE4Pos(i+1,3,0,[x0,y+y0,z0+z],[0,0,0])

# Switch viewpoint target vehicle to vehicle 1
ue.sendUE4Cmd('RflyChangeViewKeyCmd B 1')
time.sleep(0.5)  

# Switch to the N2 ground observation viewpoint for target vehicle
ue.sendUE4Cmd('RflyChangeViewKeyCmd N 2')
time.sleep(0.5)  

# Set the observation position and angle, 1.7m above the ground, with 30 degree pitch
ue.sendUE4Cmd('RflyCameraPosAng -10 40 -1.7 0 30 0')
time.sleep(2)  

idx=1
while True:
    for i in range(100):
        str0 = "RflyChange3DModel "+str(i+1)+" -1"
        #print(str0)
        ue.sendUE4Cmd(str0.encode())
        time.sleep(0.05)

sys.exit()