#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
import sys
import select
import termios
import tty

def get_key_non_blocking():
    """???????"""
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    try:
        tty.setraw(fd)
        # ???????
        if select.select([sys.stdin], [], [], 0.05)[0]:
            key = sys.stdin.read(1)
        else:
            key = 'None'  # ????
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
    return key

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

def clamp(value, min_value, max_value):
    return max(min(value, max_value), min_value)

if __name__ == "__main__":
    settings = termios.tcgetattr(sys.stdin)

    rospy.init_node('keyboard_control')
    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

    max_linear = 0.8
    max_angular = 0.6
    speed_increment = 0.05
    decay_rate = 0.02

    linear_speed = 0.0
    angular_speed = 0.0
    active_keys = set()

    rate = rospy.Rate(15)

    try:
        print "Control the robot with keyboard:"
        print "w/x: Increase/Decrease linear speed"
        print "a/d: Increase/Decrease angular speed"
        print "s: Stop"
        print "Ctrl+C: Exit"

        while not rospy.is_shutdown():
            # if sys.stdin in select.select([sys.stdin], [], [], 0)[0]:
            # key = get_key()
            key = get_key_non_blocking()

            if key == 'w':
                active_keys.add('w')
            elif key == 'x':
                active_keys.add('x')
            elif key == 'a':
                active_keys.add('a')
            elif key == 'd':
                active_keys.add('d')
            elif key == 'q':
                active_keys.add('q')
            elif key == 'e':
                active_keys.add('e')
            elif key == 's':
                active_keys.clear()
                linear_speed = 0.0
                angular_speed = 0.0
                print "Robot stopped."
            elif key == '1':
                max_linear += 0.1
                print "max_linear = {:.2f}, max_angular = {:.2f}".format(max_linear, max_angular)
            elif key == '2':
                max_linear -= 0.1
                print "max_linear = {:.2f}, max_angular = {:.2f}".format(max_linear, max_angular)
            elif key == '3':
                max_angular += 0.1
                print "max_linear = {:.2f}, max_angular = {:.2f}".format(max_linear, max_angular)
            elif key == '4':
                max_angular -= 0.1
                print "max_linear = {:.2f}, max_angular = {:.2f}".format(max_linear, max_angular)
            elif key == '\x03':  # Ctrl+C
                break
            else:
                # print "Invalid key! Use 'w', 'x', 'a', 'd', or 's'."
                pass

            # Process active keys
            if 'q' in active_keys:
                if linear_speed + speed_increment < max_linear:     # w
                    linear_speed += speed_increment
                else:
                    linear_speed = max_linear

                if angular_speed + speed_increment < max_angular:   # a
                    angular_speed += speed_increment
                else:
                    angular_speed = max_angular

            if 'e' in active_keys:
                if linear_speed + speed_increment < max_linear:     # w
                    linear_speed += speed_increment
                    # linear_speed = 0.4
                else:
                    linear_speed = max_linear

                if angular_speed - speed_increment > -max_angular:  # d
                    angular_speed -= speed_increment
                else:
                    angular_speed = -max_angular

            if 'w' in active_keys:
                if linear_speed + speed_increment < max_linear:
                    linear_speed += speed_increment
                else:
                    linear_speed = max_linear
                
            elif 'x' in active_keys:
                if linear_speed - speed_increment > -max_linear:
                    linear_speed -= speed_increment
                else:
                    linear_speed = -max_linear
            else:
                if linear_speed > 0.03:
                    linear_speed -= decay_rate  # ????
                elif linear_speed < -0.03:
                    linear_speed += decay_rate  # ????
                else:
                    linear_speed = 0.0

            if 'a' in active_keys:
                if angular_speed + speed_increment < max_angular:
                    angular_speed += speed_increment
                else:
                    angular_speed = max_angular

            elif 'd' in active_keys:
                if angular_speed - speed_increment > -max_angular:
                    angular_speed -= speed_increment
                else:
                    angular_speed = -max_angular
            else:
                if angular_speed > 0.03:
                    angular_speed -= decay_rate  # ????
                elif angular_speed < -0.03:
                    angular_speed += decay_rate  # ????
                else:
                    angular_speed = 0.0

            # Remove released keys
            active_keys -= {'w', 'x', 'a', 'd', 'q', 'e'}

            # Publish Twist message
            twist = Twist()
            twist.linear.x = linear_speed
            twist.angular.z = angular_speed
            pub.publish(twist)
            if linear_speed != 0.0 or angular_speed != 0.0:
                print "Linear speed: {:.2f}, Angular speed: {:.2f}".format(linear_speed, angular_speed)
            rate.sleep()

    except rospy.ROSInterruptException:
        pass
    finally:
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)