#!/usr/bin/env python
# -*- coding: utf-8 -*-
import tello		# tello.pyをインポート
import time			# time.sleepを使いたいので
import pygame		# pygameでジョイスティックを読む

def main():
	# pygameの初期化とジョイスティックの初期化
	pygame.init()
	joy = pygame.joystick.Joystick(0)	# ジョイスティック番号はjstest-gtkで確認しておく
	joy.init()

	# Telloクラスを使って，droneというインスタンス(実体)を作る
	#   コマンドの応答タイムアウトを0.01秒(10ms)にして，rcコマンドの連送に耐えられるようにする
	drone = tello.Tello('', 8889, command_timeout=.01 )

	time.sleep(0.5)		# 通信が安定するまでちょっと待つ
        recorded = time.time()
	#Ctrl+cが押されるまでループ
	try:
		while True:
			# Joystickの読み込み
			#   get_axisは　-1.0〜0.0〜+1.0 で変化するので100倍して±100にする
			#   プラスマイナスの方向が逆の場合は-100倍して反転させる
			a = joy.get_axis(3)		# aは左右移動
			b = joy.get_axis(4)*-1.		# bは前後移動
			c = joy.get_axis(1)*-1.		# cは上下移動
			d = joy.get_axis(0)		# dは旋回
			btn0 = joy.get_button(0)
			btn1 = joy.get_button(1)
			btn2 = joy.get_button(2)
			btn3 = joy.get_button(3)
			pygame.event.pump()		# イベントの更新

			# プラスマイナスの方向や離陸/着陸に使うボタンを確認するためのprint文
			#print("l/r=%d  f/b=%d  u/d=%d  cw/ccw=%d  btn0=%d  btn1=%d  btn2=%d  btn3=%d"%(a, b, c, d, btn0, btn1, btn2, btn3))

			# rcコマンドを送信
                        elasped=time.time()-recorded
                        if elasped > 0.5:
                            aabs=abs(a)
                            babs=abs(b)
                            cabs=abs(c)
                            dabs=abs(d)
                            comp_ax={'lr': aabs,'fb': babs, 'ud': cabs, 'rot': dabs}
                            max_k = max(comp_ax, key = comp_ax.get)
                            if max_k == 'lr':
                                if a < 0.:
                                    drone.move('left', aabs )
                                else:
                                    drone.move('right', aabs )
                            elif max_k == 'fb':
                                if b < 0.:
                                    drone.move('back', babs )
                                else:
                                    drone.move('forward', babs )
                            elif max_k == 'ud':
                                #distance = int(round(cabs * 100)) 
                                if c < 0.:
                                    drone.move('down', cabs )
                                else:
                                    drone.move('up', cabs )
                            else:
                                angle=int( dabs * 90. )
                                if d < 0.:
                                    drone.send_command( 'ccw %s' % angle )
                                else:
                                    drone.send_command( 'cw %s' % angle )
                                

			    #drone.send_command( 'rc %s %s %s %s'%(a, b, c, d) )
                            recorded=time.time()
			if btn1 == 1:		# 離陸
				drone.takeoff()
			elif btn2 == 1:		# 着陸
				drone.land()

			time.sleep(0.03)	# 適度にウェイトを入れてCPU負荷を下げる

	except( KeyboardInterrupt, SystemExit):    # Ctrl+cが押されたら離脱
		print( "SIGINTを検知" )

	# telloクラスを削除
	del drone


# "python main.py"として実行された時だけ動く様にするおまじない処理
if __name__ == "__main__":		# importされると"__main__"は入らないので，実行かimportかを判断できる．
	main()    # メイン関数を実行