# -*- coding: utf-8 -*-
import cv2
import sys
import time
import VisionCaptureApi
import os
import UE4CtrlAPI
ue = UE4CtrlAPI.UE4CtrlAPI()


# 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 720x405w',0) # 设置UE4窗口分辨率，注意本窗口仅限于显示，取图分辨率在json中配置，本窗口设置越小，资源需求越少。
ue.sendUE4Cmd('t.MaxFPS 200',0) # 设置UE4最大刷新频率，同时也是取图频率
ue.sendUE4Cmd('stat fps',0) #在UE4窗口显示当前刷新频率
time.sleep(2)    

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

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

#vis.isUE4DirectUDP=True
# 注意，手动修改本命令能强制将图片发送机制为UE4直接发出UDP图片到指定IP地址
# 如果不修改这个值，那么发送机制由json文件中SendProtocol[0]中定义

isSuss = vis.sendReqToUE4() # 向RflySim3D发送取图请求，并验证
if not isSuss: # 如果请求取图失败，则退出
    sys.exit(0)


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


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

# 启用时间戳打印功能
#vis.isPrintTime=True

print('Start Image Reciver')

time.sleep(5)

for i in range(len(vis.Img)):
    print('Data for #',vis.VisSensor[i].SeqID)
    print('rflyStartStmp',vis.rflyStartStmp[i]) # rflyStartStmp[i]是CopterSim启动
    #（TargetCopter对应的飞机）时，py所在电脑的时间（系统时间或ROS时间）
    # 注意：如果isEnableRosTrans被启用，rflyStartStmp[i]使用的是ROS时间，反之使用的是系统UNIX时间
    print('timeStmp',vis.timeStmp[i]) # timeStmp[i]是从CopterSim启动到当前数据生成的时间
    print('imgStmp',vis.imgStmp[i]) # imgStmp[i] = rflyStartStmp[i]+timeStmp[i]是图像真实时间戳


    print('imu.rflyStartStmp',vis.imu.rflyStartStmp)# rflyStartStmp是CopterSim启动时，py所在电脑的系统或ROS时间戳
    print('imu.timestmp',vis.imu.timestmp) # timeStmp是从CopterSim启动到当前数据生成的时间
    print('imu.imuStmp',vis.imu.imuStmp) # imuStmp=rflyStartStmp+timeStmp是IMU真实时间戳
    print('')