#!/usr/bin/env python3
import sys, select, termios, tty

import rospy
from std_msgs.msg import String


COMMAND_MAP = {
    "i": "forward",
    ",": "stop",
    "j": "left",
    "l": "right",
    "k": "back",
    "\x03": "break",
}

command = "stop"
last_command = "stop"

msg = """
Reading from the keyboard and publishing to /command!
-----------------------------------------------------
Moving around:
      i
j     k     l
      ,
CTRL-C to quit
"""


def broadcaster():
    global command
    global last_command

    pub = rospy.Publisher("/command", String, queue_size=10)
    rospy.init_node("keyb_commander", anonymous=True, log_level=rospy.INFO)
    rate = rospy.Rate(10)

    print(msg)

    while not rospy.is_shutdown():
        key_timeout = 0.6  # seconds
        k = get_key(key_timeout)

        command = COMMAND_MAP.get(k)

        if command == "break":
            # shut it down
            break

        

        if command != last_command and command is not None:
            # print("Published command: {}".format(command))
            rospy.loginfo("Published command: {}".format(command))
            last_command = command
        
        if command is not None:
            pub.publish(command)
        
        rate.sleep()


def get_key(key_timeout):
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], key_timeout)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ""
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key


if __name__ == "__main__":
    settings = termios.tcgetattr(sys.stdin)
    try:
        broadcaster()
    except rospy.ROSInterruptException:
        pass