#!/usr/bin/env python
# -*- coding: utf-8 -*-
import socket			# UDP通信用
import threading		# マルチスレッド用
import time				# ウェイト時間用
import numpy as np		# 画像データの配列用
#import libh264decoder	# H.264のデコード用(自分でビルドしたlibh264decoder.so)

class Tello:
	"""Telloドローンと通信するラッパークラス"""

	def __init__(self, local_ip, local_port, imperial=False, command_timeout=.3, tello_ip='192.168.10.1', tello_port=8889):
		"""
		クラスの初期化．ローカルのIP/ポートをバインドし，Telloをコマンドモードにする．

		:param local_ip (str): バインドする(UDPサーバにする)ローカルのIPアドレス
		:param local_port (int): バインドするローカルのポート番号
		:param imperial (bool): Trueの場合，速度の単位はマイル/時，距離の単位はフィート．
								Falseの場合, 速度の単位はkm/h，距離はメートル．デフォルトはFalse
		:param command_timeout (int|float): コマンドの応答を待つ時間．デフォルトは0.3秒．
		:param tello_ip (str): TelloのIPアドレス．EDUでなければ192.168.10.1
		:param tello_port (int): Telloのポート.普通は8889
		"""

		self.abort_flag = False		# 中断フラグ
		#self.decoder = libh264decoder.H264Decoder()	# H.264のデコード関数を登録
		self.command_timeout = command_timeout		# タイムアウトまでの時間
		self.imperial = imperial	# 速度と距離の単位を選択
		self.response = None  	# Telloが応答したデータが入る
		self.frame = None		# BGR並びのnumpy配列 -- カメラの出力した現在の画像
		self.is_freeze = False	# カメラ出力を一時停止(フリーズ)するかどうかのフラグ
		self.last_frame = None	# 一時停止時に出力する画像
		self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)		  # コマンド送受信のソケット
		self.socket_video = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  # ビデオストリーム受信用のソケット
		self.tello_address = (tello_ip, tello_port)		# IPアドレスとポート番号のタプル(変更不可能)
		self.local_video_port = 11111  					# ビデオ受信のポート番号
		self.last_height = 0 							# get_heightで確認した最終の高度
		self.socket.bind((local_ip, local_port))		# コマンド受信のUDPサーバのスタート(バインド)

		# コマンドに対する応答の受信スレッド
		self.receive_thread = threading.Thread(target=self._receive_thread)		# スレッドの作成
		self.receive_thread.daemon = True	# メインプロセスの終了と一緒にスレッドが死ぬように設定

		self.receive_thread.start()			# スレッドスタート

		# ビデオ受信の開始 -- コマンド送信: command, streamon
		self.socket.sendto(b'command', self.tello_address)			# 'command'を送信し，TelloをSDKモードに
		print ('sent: command')
		#self.socket.sendto(b'streamon', self.tello_address)			# 'streamon'を送信し，ビデオのストリーミングを開始
		print ('sent: streamon')

		#self.socket_video.bind((local_ip, self.local_video_port))	# ビデオ受信のUDPサーバのスタート(バインド)

		# ビデオ受信のスレッド
		#self.receive_video_thread = threading.Thread(target=self._receive_video_thread)		# スレッドの作成
		#self.receive_video_thread.daemon = True	# メインプロセスの終了と一緒にスレッドが死ぬように設定

		#self.receive_video_thread.start()		# スレッドスタート

	def __del__(self):
		"""ローカルのソケットを閉じる"""

		self.socket.close()			# コマンド送受信のソケットを閉じる
		#self.socket_video.close()	# ビデオ受信のソケットを閉じる
	
	def read(self):
		"""カメラで受信した最新の画像を返す"""
		if self.is_freeze:			# 一時停止フラグがTrueのときは，保存してある画像を返す
			return self.last_frame
		else:						# そうでないときは，最新の画像を返す
			return self.frame

	def video_freeze(self, is_freeze=True):
		"""ビデオ出力の一時停止 -- is_freezeフラグをTrueにセットすること"""
		self.is_freeze = is_freeze	# 一時停止フラグの状態をセット
		if is_freeze:				# Trueのときは，現在の画像をlast_frameに保存しておく
			self.last_frame = self.frame

	def _receive_thread(self):
		"""
		Telloからの応答を監視する

		スレッドとして走らせる．Telloが最後に返した応答をself.responseに格納する

		"""
		while True:
			try:
				self.response, ip = self.socket.recvfrom(3000)		# Telloからの応答を受信（最大3000バイトまで一度に受け取れる）
				#print(self.response)
			except socket.error as exc:		# エラー時の処理
				print ("Caught exception socket.error : %s" % exc)

	def _receive_video_thread(self):
		"""
		Telloからのビデオストリーミング(H.264のrawデータ)を監視する

		スレッドとして走らせる．Telloから受信した最新の画像をself.frameに格納する

		"""
		packet_data = ""	# 変数を初期化
		while True:
			try:
				res_string, ip = self.socket_video.recvfrom(2048)	# Telloからの画像データを受信(最大2048バイトまで一度に受け取れる)
				packet_data += res_string		# packet_dataに受信データを連結して１つの長いデータにする
				# フレームの最後
				if len(res_string) != 1460:		# 受信データのバイト数が1460以外のとき，packet_dataをデコードしframeを得る．
					for frame in self._h264_decode(packet_data):	# デコードしたデータには何枚分かの画像が入っているので，枚数分繰り返す
						self.frame = frame
					packet_data = ""	# 変数を初期化

			except socket.error as exc:
				print ("Caught exception socket.error : %s" % exc)
	
	def _h264_decode(self, packet_data):
		"""
		Telloから受信したH.264の生データをデコードする
		
		:param packet_data: H.264のrawデータ
		
		:return: デコードされた画像のリスト(複数枚の画像が入っていることもある)
		"""
		res_frame_list = []		# リストの初期化
		frames = self.decoder.decode(packet_data)	# packet_dataをデコードする
		for framedata in frames:	# 何枚分かの画像が入っているので，枚数分繰り返す
			(frame, w, h, ls) = framedata	# データの分解
			if frame is not None:	# frameの中身が空でないとき
				# print 'frame size %i bytes, w %i, h %i, linesize %i' % (len(frame), w, h, ls)

				frame = np.fromstring(frame, dtype=np.ubyte, count=len(frame), sep='')		# 文字列データをnp.ubyte型の配列に作りなおす
				frame = (frame.reshape((h, ls / 3, 3)))		# RGBを考慮して3次元配列にする
				frame = frame[:, :w, :]						# 画像の幅のぶんだけ取り出し，右側のゴミは捨てる
				res_frame_list.append(frame)				# リストの要素として追加

		return res_frame_list	# 複数枚の画像が入ったリストとして返す

	def send_command(self, command):
		"""
		Telloへコマンドを送信し，応答を待つ

		:param command: 送信するコマンド
		:return (str): Telloの応答

		"""

		print (">> send cmd: {}".format(command))
		self.abort_flag = False		# 中断フラグを倒す
		timer = threading.Timer(self.command_timeout, self.set_abort_flag)		# タイムアウト時間が立ったらフラグを立てるタイマースレッドを作成

		self.socket.sendto(command.encode('utf-8'), self.tello_address)		# コマンドを送信

		timer.start()	# スレッドスタート
		while self.response is None:		# タイムアウト前に応答が来たらwhile終了
			if self.abort_flag is True:		# タイムアウト時刻になったらブレイク
				break
		timer.cancel()	# スレッド中断
		
		if self.response is None:		# 応答データが無い時
			response = 'none_response'
		else:							# 応答データがあるとき
			response = self.response.decode('utf-8')

		self.response = None	# _receive_threadスレッドが次の応答を入れてくれるので，ここでは空にしておく

		return response		# 今回の応答データを返す
	
	def set_abort_flag(self):
		"""
		self.abort_flagのフラグをTrueにする
		
		send_command関数の中のタイマーで呼ばれる．

		この関数が呼ばれるということは，応答が来なくてタイムアウトした，ということ．

		"""

		self.abort_flag = True

	def takeoff(self):
		"""
		離陸開始

		Returns:
			str: Telloからの応答．'OK'または'FALSE'.

		"""

		return self.send_command('takeoff')

	def set_speed(self, speed):
		"""
		スピードを設定

		この関数の引数にはkm/hかマイル/hを使う．
		Tello APIは 1〜100 センチメートル/秒を使う

		Metric: .1 to 3.6 km/h
		Imperial: .1 to 2.2 Mile/h

		Args:
			speed (int|float): スピード

		Returns:
			str: Telloからの応答．'OK'または'FALSE'.

		"""

		speed = float(speed)

		if self.imperial is True:		# 単位系に応じて計算
			speed = int(round(speed * 44.704))		# Mile/h -> cm/s
		else:
			speed = int(round(speed * 27.7778))		# km/h -> cm/s

		return self.send_command('speed %s' % speed)

	def rotate_cw(self, degrees):
		"""
		時計回りの旋回

		Args:
			degrees (int): 旋回角度， 1〜360度

		Returns:
			str: Telloからの応答．'OK'または'FALSE'.

		"""

		return self.send_command('cw %s' % degrees)

	def rotate_ccw(self, degrees):
		"""
		反時計回りの旋回

		Args:
			degrees (int): 旋回角度， 1〜360度.

		Returns:
			str: Telloからの応答．'OK'または'FALSE'.

		"""
		return self.send_command('ccw %s' % degrees)

	def flip(self, direction):
		"""
		宙返り

		Args:
			direction (str): 宙返りする方向の文字， 'l', 'r', 'f', 'b'.

		Returns:
			str: Telloからの応答．'OK'または'FALSE'.
		"""

		return self.send_command('flip %s' % direction)

	def get_response(self):
		"""
		Telloの応答を返す

		Returns:
			int: Telloの応答

		"""
		response = self.response
		return response

	def get_height(self):
		"""
		Telloの高度(dm)を返す

		Returns:
			int: Telloの高度(dm)

		"""
		height = self.send_command('height?')
		height = str(height)
		height = filter(str.isdigit, height)
		try:
			height = int(height)
			self.last_height = height
		except:
			height = self.last_height
			pass
		return height

	def get_battery(self):
		"""
		バッテリー残量をパーセンテージで返す

		Returns:
			int: バッテリー残量のパーセンテージ

		"""
		
		battery = self.send_command('battery?')

		try:
			battery = int(battery)
		except:
			pass

		return battery

	def get_flight_time(self):
		"""
		飛行時間を秒数で返す

		Returns:
			int: 飛行の経過時間

		"""

		flight_time = self.send_command('time?')

		try:
			flight_time = int(flight_time)
		except:
			pass

		return flight_time

	def get_speed(self):
		"""
		現在のスピードを返す

		Returns:
			int: 現在スピード， km/h または Mile/h

		"""

		speed = self.send_command('speed?')

		try:
			speed = float(speed)

			if self.imperial is True:
				speed = round((speed / 44.704), 1)		# cm/s -> mile/h
			else:
				speed = round((speed / 27.7778), 1)		# cm/s -> km/h
		except:
			pass

		return speed

	def land(self):
		"""
		着陸を開始

		Returns:
			str: Telloからの応答．'OK'または'FALSE'.

		"""

		return self.send_command('land')

	def move(self, direction, distance):
		"""
		direction の方向へ distance の距離だけ移動する．

		この引数にはメートルまたはフィートを使う．
		Tello API は 20〜500センチメートルを使う．

		Metric: .02 〜 5 メートル
		Imperial: .7 〜 16.4 フィート

		Args:
			direction (str): 移動する方向の文字列，'forward', 'back', 'right' or 'left'．
			distance (int|float): 移動する距離．(メートルまたはフィート)

		Returns:
			str: Telloからの応答．'OK'または'FALSE'.

		"""

		distance = float(distance)

		if self.imperial is True:
			distance = int(round(distance * 30.48))		# feet -> cm
		else:
			distance = int(round(distance * 100))		# m -> cm

		return self.send_command('%s %s' % (direction, distance))

	def move_backward(self, distance):
		"""
		distance の距離だけ後進する．

		Tello.move()のコメントを見ること．

		Args:
			distance (int): 移動する距離

		Returns:
			str: Telloからの応答．'OK'または'FALSE'.

		"""

		return self.move('back', distance)

	def move_down(self, distance):
		"""
		distance の距離だけ降下する．

		Tello.move()のコメントを見ること．

		Args:
			distance (int): 移動する距離
			
		Returns:
			str: Telloからの応答．'OK'または'FALSE'.

		"""

		return self.move('down', distance)

	def move_forward(self, distance):
		"""
		distance の距離だけ前進する．

		Tello.move()のコメントを見ること．

		Args:
			distance (int): 移動する距離

		Returns:
			str: Telloからの応答．'OK'または'FALSE'.

		"""
		return self.move('forward', distance)

	def move_left(self, distance):
		"""
		distance の距離だけ左移動する．

		Tello.move()のコメントを見ること．

		Args:
			distance (int): 移動する距離

		Returns:
			str: Telloからの応答．'OK'または'FALSE'.

		"""
		return self.move('left', distance)

	def move_right(self, distance):
		"""
		distance の距離だけ右移動する．

		Tello.move()のコメントを見ること．

		Args:
			distance (int): 移動する距離

		Returns:
			str: Telloからの応答．'OK'または'FALSE'.

		"""
		return self.move('right', distance)

	def move_up(self, distance):
		"""
		distance の距離だけ上昇する．

		Tello.move()のコメントを見ること．

		Args:
			distance (int): 移動する距離

		Returns:
			str: Telloからの応答．'OK'または'FALSE'.

		"""

		return self.move('up', distance)