import time
import math
import sys

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

# Create four MAVLink API instances, the CopterSim UDP ports are 20100+2*i, where i=1,2,3,4
# Although no CopterSim is required in NoPX4Mode, the port 20100+2*i is required to identify the CopterID
mav = PX4MavCtrl.PX4MavCtrler(1)
mav1 = PX4MavCtrl.PX4MavCtrler(2)
mav2 = PX4MavCtrl.PX4MavCtrler(3)
mav3 = PX4MavCtrl.PX4MavCtrler(4)


# sendUE4Cmd: RflySim3D API to modify scene display style
# Format: ue.sendUE4Cmd(cmd,windowID=-1), where cmd is a command string, windowID is the received window number (assuming multiple RflySim3D windows are opened at the same time), windowID =-1 means sent to all windows
# Augument: RflyChangeMapbyName command means to switch the map (scene), the following string is the map name, here will switch all open windows to the grass map
ue.sendUE4Cmd('RflyChangeMapbyName Grasslands')    
time.sleep(2)  

# Start MAVLink listening loop from CopterSim/Pixhawk
# initPointMassModel(intAlt=0,intState=[0,0,0]):
# intAlt is the ground high which should be acquired from the UE4 map
# intState=PosX, PosY, PosZ, which can obtain intAlt at CopterSim UI
mav.initPointMassModel(-8.086,[0,0,0])
mav1.initPointMassModel(-8.29,[2,0,0])
mav2.initPointMassModel(-7.835,[0,2,0])
mav3.initPointMassModel(-7.788,[2,2,0])
time.sleep(2)

# Display the position, velocity, angle, angular rate from Pixhawk/CopterSIm
print((mav.uavPosNED,mav.truePosNED,mav.uavVelNED,mav.uavAngEular,mav.uavAngRate,mav.uavGlobalPos))
print((mav1.uavPosNED,mav1.truePosNED,mav1.uavVelNED,mav1.uavAngEular,mav1.uavAngRate,mav1.uavGlobalPos))
print((mav2.uavPosNED,mav2.truePosNED,mav2.uavVelNED,mav2.uavAngEular,mav2.uavAngRate,mav2.uavPosGPS,mav2.uavGlobalPos))
print((mav3.uavPosNED,mav3.truePosNED,mav3.uavVelNED,mav3.uavAngEular,mav3.uavAngRate,mav3.uavPosGPS,mav3.uavGlobalPos))

# 设定最大速度，这个比较重要！，根据任务来匹配机型速度
mav.SendCopterSpeed(10)
mav1.SendCopterSpeed(10)
mav2.SendCopterSpeed(10)
mav3.SendCopterSpeed(10)
time.sleep(2)

# send desired poisition control command, fly to target postion 0,0,-1.7m with yaw 0 degree
mav.SendPosNED(0, 0, -1.7, 0) 
mav1.SendPosNED(2, 2, -1.7, 0) 
mav2.SendPosNED(-2, -2, -1.7, 0) 
mav3.SendPosNED(0, 0, -3, 0) 


print("Send target Pos")
time.sleep(0.5)
# Send arm command
mav.SendMavArm(True) 
time.sleep(2)
mav1.SendMavArm(True) 
time.sleep(2)
mav2.SendMavArm(True) 
time.sleep(2)
mav3.SendMavArm(True) 
print("Send Arm Command")

time.sleep(5)

# send desired NED velocity signals, 0.2m/s download
mav.SendVelNED(0, 0, 1, 0) 
mav1.SendVelNED(0, 0, 1, 0) 
mav2.SendVelNED(0, 0, 1, 0) 
mav3.SendVelNED(0, 0, 1, 0) 
print("Send Velocity Speed")

time.sleep(5)


# exit from MAVLink listening loop
print("End pointMass model")
mav.EndPointMassModel()
mav1.EndPointMassModel()
mav2.EndPointMassModel()
mav3.EndPointMassModel()
time.sleep(1)
# while True:
#    print(mav.uavPosNED)
#    time.sleep(2)