# -*- coding: utf-8 -*-
# Frame_1 : 获取Tello的摄像头
# Frame_2 : 获取Tello的摄像头,并获取电量和高度信息

from cv2 import cv2
import socket
import time

FONT_SIZE = 0.5

socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

# ステータス受信用のUDPサーバの設定
LOCAL_IP = "" # "0.0.0.0"と同じ意味．すなわち「全てのネットワークインターフェイスを使う」
LOCAL_PORT = 8890 # ステータス受信は8890ポート
socket.bind((LOCAL_IP, LOCAL_PORT))

# ビデオ受信用のUDPサーバの設定
LOCAL_IP = ""# 同"0.0.0.0"
LOCAL_PORT_VIDEO = 11111
address = "udp://" + LOCAL_IP + ":" + str(LOCAL_PORT_VIDEO)

# コマンド送信用の設定
TELLO_IP = "192.168.10.1"
TELLO_PORT = 8889
tello_address = (TELLO_IP, TELLO_PORT)

# 最初に"command"を送ってSDKモードを開始しないとステータスが出てこない
socket.sendto("command".encode("utf-8"),tello_address)
socket.sendto("streamon".encode("utf-8"),tello_address)
camera = cv2.VideoCapture(address)

# 获取原Frame大小(960,720)
frame_width = int(camera.get(cv2.CAP_PROP_FRAME_WIDTH))
frame_height = int(camera.get(cv2.CAP_PROP_FRAME_HEIGHT))
frame_size = (frame_width, frame_height) 
print("size:"+repr(frame_size))

fps = 0
frameCount = 0
start_time = time.time()
begin_time = time.time()

    
while(camera.isOpened()):

    # 读取视频流
    ret,frame = camera.read()

    if not ret:
        print("No camera") 
        break
    
    # 将Frame大小调整到(640,480)
    frame = cv2.resize(frame, dsize = (int(frame_width/1.5),int(frame_height/1.5)))

    # 复制两个Frame
    original_frame = frame
    perform_frame = frame

    cv2.imshow("Original Frame",original_frame)
    cv2.moveWindow("Original Frame", 50, 100)

    # 受信は最大1024バイトまで．受信結果はresponse変数に入る
    # data, addr = socket.recvfrom(1024)
     
    # if data == "ok": # 受信データがokだけだったら再ループ
    #     continue

    # data = str(data).split(";") 
    
    # try:
    #     bat = data[15].replace("bat","Battery")+(" %") 
    #     h = data[16].replace("baro","Height")+(" cm") 
    # except IndexError:
    #     continue
    
    # 图像,文字内容,坐标,字体,大小,颜色,字体厚度
    # cv2.putText(perform_frame, bat, (10, 30), cv2.FONT_HERSHEY_SIMPLEX, FONT_SIZE, (255, 255, 255), 2)
    # cv2.putText(perform_frame, h, (10, 60), cv2.FONT_HERSHEY_SIMPLEX, FONT_SIZE, (255, 255, 255), 2)

    # 显示FPS
    cv2.putText(perform_frame,"FPS: " + str(fps),(10,30),cv2.FONT_HERSHEY_SIMPLEX, FONT_SIZE, (255, 255, 255), 2)

    frameCount  = frameCount  + 1
    end_time = time.time()
    diff_time = end_time - start_time
    if (diff_time > 1):
        fps = frameCount
        frameCount = 0
        start_time = end_time

    # battery = socket.sendto("battery?".encode("utf-8"),tello_address)
    # battery = int(battery)
    # cv2.putText(perform_frame,"Battery: " + str(battery) + "%",(10,60),cv2.FONT_HERSHEY_SIMPLEX, FONT_SIZE, (255, 255, 255), 2)

    # 输出运行时间      
    tm = round(time.time()-begin_time,3)
    cv2.putText(perform_frame, "Time: " + str(tm), (10, 90), cv2.FONT_HERSHEY_SIMPLEX, FONT_SIZE, (255, 255, 255), 2)

    cv2.imshow("Perform Frame", perform_frame)
    cv2.moveWindow("Perform Frame", 750, 100)

    if(cv2.waitKey(1) & 0xFF) == ord("q"): 
        break 

    elif(cv2.waitKey(1) & 0xFF) == ord("l"):
        socket.sendto("rc 0 0 0 0".encode("utf-8"),tello_address)  
        socket.sendto("land".encode("utf-8"),tello_address)
        time.sleep(1)

    elif(cv2.waitKey(1) & 0xFF) == ord("j"):

        socket.sendto("takeoff".encode("utf-8"),tello_address)
        time.sleep(0.5)

    elif(cv2.waitKey(1) & 0xFF) == ord("k"):
        socket.sendto("rc -10 0 0 0".encode("utf-8"),tello_address)  
        print("1")
            
camera.release()
cv2.destroyAllWindows()
