import time
import sys
import VisionCaptureApi
import PX4MavCtrlV4 as PX4MavCtrl
import cv2
import UE4CtrlAPI
import json
import numpy as np
import os
# xxx
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 30',0) # 设置UE4最大刷新频率，同时也是取图频率
ue.sendUE4Cmd('')
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() # 开启取图，并启用共享内存图像转发，转发到填写的目录


#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, -2!')
mav.SendPosNED(0, 0, -2, 0)

lastTime = time.time()

# 获取当前文件所在目录
current_directory = os.path.dirname(os.path.abspath(__file__))
# 构建完整的文件路径
config_file_path = os.path.join(current_directory, 'Config.json')

# 加载测距传感器配置文件
with open(config_file_path, 'r', encoding='utf-8') as config_file:
    config = json.load(config_file)
    sensors = config['VisionSensors']
    sensor = sensors[0]  
    sensor_distance = sensor['SensorPosXYZ'][0]  
    

while True:
    if vis.hasData[0] and vis.hasData[1]: # 是否成功取图和获取测距传感器

        img = vis.Img[0] # 获取0号传感器，图像数据指针，格式为opencv图像格式
        obj_distance = vis.Img[1] # 获取1号传感器，距离数据指针，格式见VisionCaptureApi.DistanceSensor()
        
        
        # 获取图像尺寸
        height, width, _ = img.shape
        # 计算中心坐标
        center_x = width // 2
        center_y = height // 2
        
        crosshair_length = 20
        crosshair_thickness = 2
        cv2.line(img, (center_x - crosshair_length, center_y), (center_x + crosshair_length, center_y), (0, 0, 255), crosshair_thickness)
        cv2.line(img, (center_x, center_y - crosshair_length), (center_x, center_y + crosshair_length), (0, 0, 255), crosshair_thickness)
        
        # 绘制距离信息
        distance_text = f"Distance: {obj_distance.Distance} m"
        cv2.putText(img, distance_text, (center_x - 50, center_y + crosshair_length + 20), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
        
        # 显示图像
        cv2.imshow('Image', img)
        cv2.waitKey(1)

    time.sleep(0.01)

# 注意：距离传感器的数据定义如下
# class DistanceSensor:
#     ## @brief DistanceSensor的构造函数
#     # @param 初始化类属性
#     def __init__(self):
#         ##  @var DistanceSensor.TimeStamp
#         #   @brief 这是当前消息的时间戳，初始化为 0
#         self.TimeStamp = 0
#         ##  @var DistanceSensor.Distance
#         #   @brief 这是距离传感器测量到的距离，初始化为 0
#         self.Distance = 0
#         ##  @var DistanceSensor.CopterID
#         #   @brief 用于标识直升机的ID，初始化为 0
#         self.CopterID = 0
#         ##  @var DistanceSensor.RayStart
#         #   @brief 这是射线起点的坐标，初始化为[0,0,0]
#         self.RayStart = [0,0,0]
#         ##  @var DistanceSensor.AngEular
#         #   @brief 这是传感器的欧拉角（Euler Angles），初始化为[0,0,0]
#         self.AngEular = [0,0,0]
#         ##  @var DistanceSensor.ImpactPoint
#         #   @brief 这是碰撞点的坐标，初始化为[0,0,0]
#         self.ImpactPoint = [0,0,0]
#         ##  @var DistanceSensor.BoxOri
#         #   @brief 这是盒子的原点或参考点，初始化为[0,0,0]
#         self.BoxOri = [0,0,0]