#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
import sys
import select
import termios
import tty

msg = """
Control Your Aunchan Robot!
---------------------------
Moving around:
        w
   a    s    d
        x
w/x : increase/decrease linear velocity
a/d : increase/decrease angular velocity
space key, s : force stop
CTRL-C to quit
"""


def getKey():
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key


def vels(target_linear_vel, target_angular_vel):
    return "currently: linear-> %s\t angular-> %s " % (target_linear_vel, target_angular_vel)


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

    rospy.init_node('aunchan_teleop')
    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)

    status = 0
    target_linear_vel = 0
    target_angular_vel = 0
    control_linear_vel = 0
    control_angular_vel = 0
    try:
        print(msg)
        while(1):
            key = getKey()
            if key == 'w':
                target_linear_vel += 0.1
                status += 1
                print(vels(target_linear_vel, target_angular_vel))
            elif key == 'x':
                target_linear_vel -= 0.1
                status += 1
                print(vels(target_linear_vel, target_angular_vel))
            elif key == 'a':
                target_angular_vel += 0.4
                status += 1
                print(vels(target_linear_vel, target_angular_vel))
            elif key == 'd':
                target_angular_vel -= 0.4
                status += 1
                print(vels(target_linear_vel, target_angular_vel))
            elif key == ' ' or key == 's':
                target_linear_vel = 0
                control_linear_vel = 0
                target_angular_vel = 0
                control_angular_vel = 0
                print(vels(0, 0))
            elif status >= 14:
                print(msg)
                status = 0
            else:
                if key == '\x03':
                    break

            control_linear_vel = target_linear_vel
            control_angular_vel = target_angular_vel
            # if target_linear_vel > control_linear_vel:
            #     control_linear_vel = min(
            #         target_linear_vel, control_linear_vel + (0.01/4.0))
            # else:
            #     control_linear_vel = target_linear_vel

            # if target_angular_vel > control_angular_vel:
            #     control_angular_vel = min(
            #         target_angular_vel, control_angular_vel + (0.1/4.0))
            # else:
            #     control_angular_vel = target_angular_vel

            twist = Twist()
            twist.linear.x = control_linear_vel
            twist.linear.y = 0
            twist.linear.z = 0
            twist.angular.x = 0
            twist.angular.y = 0
            twist.angular.z = control_angular_vel
            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)