import time
import math
import sys

import PX4MavCtrlV4 as PX4MavCtrl

# Create four MAVLink API instances, the CopterSim UDP ports are 20100+2*i, where i=1,2,3,4
mav = PX4MavCtrl.PX4MavCtrler(1)
mav1 = PX4MavCtrl.PX4MavCtrler(2)
mav2 = PX4MavCtrl.PX4MavCtrler(3)
mav3 = PX4MavCtrl.PX4MavCtrler(4)

# Start MAVLink listening loop from CopterSim/Pixhawk
# MAVLink Full mode is adopted here
mav.InitMavLoop()
mav1.InitMavLoop()
mav2.InitMavLoop()
mav3.InitMavLoop()
time.sleep(2)

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


# Start Offboard mode of Pixhawk
mav.initOffboard()
mav1.initOffboard()
mav2.initOffboard()
mav3.initOffboard()


# 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(10)

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

time.sleep(10)

# exit from Offboard control mode of Pixhawk
print("Send offboard stop")
mav.endOffboard()
mav1.endOffboard()
mav2.endOffboard()
mav3.endOffboard()
time.sleep(1)

# exit from MAVLink listening loop
print("Send Mavlink stop")
mav.stopRun()
mav1.stopRun()
mav2.stopRun()
mav3.stopRun()
time.sleep(1)
#while True:
#    print(mav.uavPosNED)
#    time.sleep(2)