#!/usr/bin/env python
# -*- coding: utf-8 -*-
import roslib; roslib.load_manifest('teleop_twist_keyboard')
import rospy

from geometry_msgs.msg import Twist
from std_msgs.msg import Empty

import sys, select, termios, tty

msg = """
        ---------------------
takeoff>|  t|⇑ y|↖ u|↑ i|↗ o|  
        |---|---|---|---|---|----
  reset>|  g|⇐ h|← j|  k|→ l|⇒ m|
        |---|---|---|---|---|----
   land>|  b|⇓ n|↙ ,|↓ ;|↘ :| 
        ---------------------

a/w : increase/decrease max speeds by 10%
z/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
anything else : stop

CTRL-C to quit
"""

moveBindings = {
    #   x th y z
    'i':(1,0,0,0),
    'o':(1,-1,0,0),
    'j':(0,1,0,0),
    'l':(0,-1,0,0),
    'u':(1,1,0,0),
    ';':(-1,0,0,0),
    ':':(-1,1,0,0),
    ',':(-1,-1,0,0),
    'h':(0,0,-1,0),
    'm':(0,0,1,0),
    'y':(0,0,0,1),
    'n':(0,0,0,-1),
         }

speedBindings={
    'a':(1.1,1.1),
    'w':(.9,.9),
    'z':(1.1,1),
    'x':(.9,1),
    'e':(1,1.1),
    'c':(1,.9),
        }

def getKey():
  tty.setraw(sys.stdin.fileno())
  select.select([sys.stdin], [], [], 0)
  key = sys.stdin.read(1)
  termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
  return key

speed = 0.1
turn = 0.5

def vels(speed,turn):
  return "currently:\tspeed %s\tturn %s " % (speed,turn)

if __name__=="__main__":
  settings = termios.tcgetattr(sys.stdin)
  pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
  land = rospy.Publisher('/ardrone/land', Empty, queue_size=1)
  takeoff = rospy.Publisher('/ardrone/takeoff', Empty, queue_size=1)
  reset = rospy.Publisher('/ardrone/reset', Empty, queue_size=1)
  rospy.init_node('keyboard_azerty')
  x = 0
  th = 0
  y = 0
  z = 0
  status = 0
  try:
    print msg
    print vels(speed,turn)
    while(1):
      key = getKey()
      if (key == 't'):
        takeoff.publish(Empty())
        print "takeoff"
      elif (key == 'g'):
        reset.publish(Empty())
        print "reset"
      elif (key == 'b'):
        land.publish(Empty())
        print "land"
      else:
        if key in moveBindings.keys():
          x = moveBindings[key][0]
          th = moveBindings[key][1]
          y = moveBindings[key][2]
          z = moveBindings[key][3]
        elif key in speedBindings.keys():
          speed = speed * speedBindings[key][0]
          turn = turn * speedBindings[key][1]
          print vels(speed,turn)
          if (status == 14):
            print msg
          status = (status + 1) % 15
        else:
          x = 0
          th = 0
          y = 0
          z = 0
          if (key == '\x03'):
            break
        twist = Twist()
        twist.linear.x = x*speed
        twist.linear.y = y*speed
        twist.linear.z = z*speed
        twist.angular.x = 0; twist.angular.y = 0;
        twist.angular.z = th*turn
        pub.publish(twist)
  except:
    print e
  finally:
    twist = Twist()
    twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
    twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
    pub.publish(twist)
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)