import time
import math
import sys
import VisionCaptureApi
import PX4MavCtrlV4 as PX4MavCtrl
import cv2
import UE4CtrlAPI
ue = UE4CtrlAPI.UE4CtrlAPI()

#Create a new MAVLink communication instance, UDP sending port (CopterSim’s receving port) is 20100
mav = PX4MavCtrl.PX4MavCtrler(1)

# The IP should be specified by the other computer
vis = VisionCaptureApi.VisionCaptureApi()

# Send command to UE4 Window 1 to change resolution 
ue.sendUE4Cmd('r.setres 1280x720w',0) # 设置UE4窗口分辨率，注意本窗口仅限于显示，取图分辨率在json中配置，本窗口设置越小，资源需求越少。
ue.sendUE4Cmd('t.MaxFPS 90',0) # 设置UE4最大刷新频率90hz，注意，在json中图片帧率为90Hz，因此，UE每1个Tick取一次图，每次取图延迟1Tick=1/90.0s
time.sleep(2)    

# VisionCaptureApi 中的配置函数
vis.jsonLoad() # 加载Config.json中的传感器配置文件

# vis.RemotSendIP = '192.168.3.80'
# 注意，手动修改RemotSendIP的值，可以将图片发送到本地址
# 如果不修改这个值，那么发送的IP地址为json文件中SendProtocol[1:4]定义的IP
# 图片的发送端口，为json中SendProtocol[5]定义好的。

isSuss = vis.sendReqToUE4() # 向RflySim3D发送取图请求，并验证
if not isSuss: # 如果请求取图失败，则退出
    sys.exit(0)
vis.startImgCap() # 开启取图，并启用共享内存图像转发，转发到填写的目录

time.sleep(1) 
# 请求1号飞机的IMU数据，回传IP如果RemotSendIP赋值了就传给这个IP，否则传给127.0.0.1
vis.sendImuReqCopterSim(1)


#mav.InitMavLoop(UDPMode), where UDPMode=0,1,2,3,4
# Use MAVLink_Full Mode to control PX4
# In this mode, this script will send MAVLinkOffboard message to PX4 directly
# and receive MAVLink data from PX4
# Obviously, MAVLink_Full mode is slower than UDP mode, but the functions and data are more comprehensive
mav.InitMavLoop() # the same as mav.InitMavLoop() in other PythonVision demos

time.sleep(1)
print('Start Offboard Send!')
mav.initOffboard()
time.sleep(1)


# Check if the PX4'EKF has correctlly initialized, which is necessary for offboard control
if not mav.isPX4Ekf3DFixed:
    print('CopterSim/PX4 still not 3DFxied, please wait and try again.')
    sys.exit(0)
else:
    print('CopterSim/PX4 3D Fixed, ready to fly.')

mav.SendMavArm(True)
print('Fly to pos 0, 0, -10!')
mav.SendPosNED(0, 0, -10, 0) 

# 下面的程序非必需，仅用于观察图像用，在电脑性能不足时，请删除
lastTime = time.time()
num=0
lastClock=time.time()
while True:
    lastTime = lastTime + 1/30.0
    sleepTime = lastTime - time.time()
    if sleepTime > 0:
        time.sleep(sleepTime)
    else:
        lastTime = time.time()   
        
    if vis.hasIMUData:
        print('IMU: ',vis.imu.timestmp,vis.imu.acc,vis.imu.rate)
                
    for i in range(len(vis.hasData)):
        if vis.hasData[i]:
            # Process your image here
            cv2.imshow('Img'+str(i),vis.Img[i])
            #print('Img'+str(i),': ',vis.timeStmp[i])
            cv2.waitKey(1)
        

            
        # if i==0: # 更新0号相机的参数
        #     # 以下代码用于实时更新相机参数（位置、焦距、角度、装载飞机和形式）
        #     vs = vis.VisSensor[0] #获取第0号相机基本参数
        #     # 修改其中的可变部分，只修改需要改变的部分即可
        #     vs.TargetCopter=1  #修改视角绑定的飞机ID
        #     vs.TargetMountType=0  # 修改视角绑定类型，固连飞机还是地面
        #     vs.CameraFOV=90   # 修改视角的视场角（焦距），可以模拟对焦相机
        #     vs.SensorPosXYZ=[0.3,-0.15,0]  # 修改相机的位置，可以调整相机初始位置
        #     vs.SensorAngEular=[0,0,0]  # 修改相机的姿态，可以模拟云台转动
        #     vis.sendUpdateUEImage(vs) # 发送更新数据
            

#mav.endOffboard()
#mav.stopRun()