#!/usr/bin/env python
import rospy
from std_msgs.msg import String

from geometry_msgs.msg import Twist
from pynput import mouse
from pynput import keyboard
from gazebo_msgs.msg import WorldState

msg = """
Control Your multiple Turtlebots!
---------------------------
Moving around:
   w  move forward
   s  move backward
   a  turn left
   d  turn right
Init model:  0 

e/q : increase/decrease max speeds by 0.1
c/z : increase/decrease max angular speeds by 0.1
alt+num: change the mode to control different robot
ctrl-C to quit
"""

ID_Robot = ('0', '1', '2','3','4','5','6','7','8','9', 'c', 'b')

def on_press(key):
    try:
        global target_speed, target_turn, ranges, altflag, mode, angular
        press = key.char
        if press == 'w':
            target_speed = 1 * ranges
        if press == 's':
            target_speed = -1 * ranges
        if press == 'd':
            target_turn = -1 * angular
        if press == 'a':
            target_turn = 1 * angular
        if altflag == 1 and press in ID_Robot:
            if press == 'c':
                mode = '10'
            elif press == 'b':
                mode = '11'
            else:
                mode = str(press)
              
	    print('Current model is {0}'.format(mode))
    except AttributeError:

        if key == keyboard.Key.alt:
            altflag = 1
            #print(1)
            
def on_release(key):
        global target_speed, target_turn, ranges, altflag, angular
        if key not in keyboard.Key:
                press = key.char
                if press == 'e' and ranges < 1:
                        ranges = ranges + 0.1
                        print('increase speed to {0}'.format(ranges))
                if press == 'q' and ranges > 0.1:
                        ranges = ranges - 0.1
                        print('decrease speed to {0}'.format(ranges))
		if press == 'c' and angular < 1:
                        angular = angular + 0.1
                        print('increase angular speed to {0}'.format(angular))
                if press == 'z' and angular > 0.1:
                        angular = angular - 0.1  
                        print('decrease angular speed to {0}'.format(angular))      
                if press == 'w' or press == 's':
                        target_speed = 0
                if press == 'a' or press == 'd':
                        target_turn = 0
        if key == keyboard.Key.alt:
           altflag = 0
'''
def on_move(x, y):
    print('Pointer moved to {0}'.format(
        (x, y)))

def on_click(x, y, button, pressed):
    print('{0} at {1}'.format(
        'Pressed' if pressed else 'Released',
        (x, y)))
    #if not pressed:
        # Stop listener
        #return False

def on_scroll(x, y, dx, dy):
    print('Scrolled {0} at {1}'.format(
        'down' if dy < 0 else 'up',
        (x, y)))
'''
def trans(target, control, acc_for, acc_back):
    if target > control:
        control = min(target, control + acc_for)
    elif target < control:
        control = max(target, control - acc_back)
    else:
        control = target
    return control


if __name__=="__main__":    
    rospy.init_node('multiKey')
    pub = rospy.Publisher('/key_vel', WorldState, queue_size = 10)
    
    altflag = 0
    mode = '0' # control model 1~9 robot
    control_speed = 0
    control_turn = 0

    ranges = 0.4
    angular = 0.9

    target_speed = 0
    target_turn = 0

    acc_for = 0.02 
    acc_back = 0.02
    
    listener_key = keyboard.Listener(on_press=on_press, on_release=on_release)
    listener_key.start() 
    #listener_mouse = mouse.Listener(on_move=on_move, on_click=on_click, on_scroll=on_scroll)
    #listener_mouse.start()
    vel = WorldState()
    twist = Twist()

    vel.name.append('agent0')
    vel.twist.append(twist)

    rate = rospy.Rate(50)
    print(msg)
    while not rospy.is_shutdown():
        control_speed = trans(target_speed, control_speed, acc_for, acc_back)
        #control_turn = trans(target_turn, control_turn, acc_for, acc_back)
        control_turn = target_turn;

        vel.name[0] = "agent" + mode
        vel.twist[0].linear.x = control_speed
        vel.twist[0].angular.z = control_turn
        vel.twist[0].linear.z = 1 # to judge whether this is the message from keyboard: non-zero
        pub.publish(vel)
        #print(vel)
        rate.sleep()
      

  


