#!/usr/bin/env python
# -*- coding: utf-8 -*-
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コマンドの連送に耐

        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 = int( joy.get_axis(3)*100 )          # aは左右移動
                        b = int( joy.get_axis(4)*-100 )         # bは前後移動
                        c = int( joy.get_axis(1)*-100 )         # cは上下移動
                        d = int( joy.get_axis(0)*100 )          # 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:
                            #drone.send_command( 'rc %s %s %s %s'%(a, b, c, d) )
                            recorded=time.time()
                            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))
                        if btn1 == 1:           # 離陸
                                #drone.takeoff()
                                print("takeoff()")
                        elif btn2 == 1:         # 着陸
                                #drone.land()
                                print("land()")

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

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

        # telloクラスを削除
        #del drone
main()