# import required libraries
# pip3 install pymavlink pyserial

import cv2
import numpy as np
import time
import VisionCaptureApi
import PX4MavCtrlV4 as PX4MavCtrl
import math
# import matplotlib.pyplot as plt
import ReqCopterSim
import Open3DShow

show3d=Open3DShow.Open3DShow()


vis = VisionCaptureApi.VisionCaptureApi()

# VisionCaptureApi 中的配置函数
vis.jsonLoad()
isSuss = vis.sendReqToUE4()

vis.startImgCap()  # 开启取图循环，执行本语句之后，已经可以通过vis.Img[i]读取到图片了
print('Start Image Reciver')

# vis.sendImuReqCopterSim(1,'127.0.0.1') # 发送请求，从目标飞机CopterSim读取IMU数据,回传地址为127.0.0.1，默认频率为200Hz
# 执行本语句之后，会自动开启数据监听，已经可以通过vis.imu读取到IMU数据了。


mav = PX4MavCtrl.PX4MavCtrler(1)


time.sleep(2)
# Start MAV loop with UDP mode: MAVLINK_FULL
mav.InitMavLoop()
mav.InitTrueDataLoop()

# Enter Offboard mode to start vehicle control
time.sleep(2)
mav.initOffboard()

# Get the takeoff position of each vehicle to the UE4 Map
# this can be adopted to obtain the global position of a vehicle in swarm simulation
time.sleep(2)
Error2UE4Map = []

Error2UE4Map = -np.array([mav.uavGlobalPos[0]-mav.uavPosNED[0],
                                        mav.uavGlobalPos[1]-mav.uavPosNED[1], mav.uavGlobalPos[2]-mav.uavPosNED[2]])

# fly to 10m high above its takeoff position
mav.SendPosNED(0, 0, -8, 0)
mav.SendCopterSpeed(3)
#time.sleep(10)


#创建点云显示窗口
show3d.CreatShow(0)


lastTime = time.time()
startTime = time.time()
timeInterval = 1/30.0  # here is 0.0333s (30Hz)
while True:
    lastTime = lastTime + timeInterval
    sleepTime = lastTime - time.time()
    if sleepTime > 0:
        time.sleep(sleepTime)  # sleep until the desired clock
    else:
        lastTime = time.time()
    # The following code will be executed 30Hz (0.0333s)

    t = time.time()-startTime


    # target position in UE4 map global frame
    if t < 10:
        # fly to 15 meters high in 0 to 10s
        targetPosE = np.array([-0, 0, -5])
    elif t<20:  # fly circle after 10s
        targetPosE = np.array(
            [15,5, -5])
    else:
        targetPosE = np.array([15,-10, -5])
        
    # target position in vehilce takeoff frame
    targetPosE = targetPosE+Error2UE4Map
    mav.SendPosNED(targetPosE[0], targetPosE[1], targetPosE[2], 0)

    if vis.hasData[0]:
        
        # 更新点云
        show3d.UpdateShow(vis.Img[0])
        
        # 标记
        vis.hasData[0]=False
        
    #isShowCloud = False
    if t > 200:  # end simulation when 200s
        break


mav.endOffboard()

time.sleep(1)
mav.stopRun()