#!/usr/bin/env python3
"""
Keyboard control node for robot chassis.
W/S: forward/backward
A/D: turn left/right
I/K: increase/decrease linear speed
J/L: increase/decrease angular speed
X: stop
Q: quit
"""

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import sys
import select
import termios
import tty

HELP = """
========================================
  Keyboard Control
========================================
  W/S    : forward / backward
  A/D    : turn left / turn right
  I/K    : linear  speed +/- (step 0.05)
  J/L    : angular speed +/- (step 0.1)
  X/Space: stop
  Q      : quit
========================================
  Current: lin=%.2f  ang=%.2f
"""


class KeyboardControl(Node):
    def __init__(self):
        super().__init__("keyboard_control")
        self.pub = self.create_publisher(Twist, "/cmd_vel", 10)

        self.lin_speed = 0.2
        self.ang_speed = 0.5
        self.lin_step = 0.05
        self.ang_step = 0.1

        self.print_help()
        self.create_timer(0.05, self.read_key)

    def print_help(self):
        print(HELP % (self.lin_speed, self.ang_speed))

    def get_key(self):
        fd = sys.stdin.fileno()
        old = termios.tcgetattr(fd)
        try:
            tty.setraw(fd)
            r, _, _ = select.select([sys.stdin], [], [], 0.05)
            if r:
                return sys.stdin.read(1)
            return None
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old)

    def read_key(self):
        key = self.get_key()
        if key is None:
            return

        twist = Twist()

        if key == 'w':
            twist.linear.x = self.lin_speed
        elif key == 's':
            twist.linear.x = -self.lin_speed
        elif key == 'a':
            twist.angular.z = self.ang_speed
        elif key == 'd':
            twist.angular.z = -self.ang_speed
        elif key == 'i':
            self.lin_speed = round(self.lin_speed + self.lin_step, 2)
            self.print_help()
            return
        elif key == 'k':
            self.lin_speed = round(max(0.0, self.lin_speed - self.lin_step), 2)
            self.print_help()
            return
        elif key == 'j':
            self.ang_speed = round(self.ang_speed + self.ang_step, 2)
            self.print_help()
            return
        elif key == 'l':
            self.ang_speed = round(max(0.0, self.ang_speed - self.ang_step), 2)
            self.print_help()
            return
        elif key == 'x' or key == ' ':
            twist.linear.x = 0.0
            twist.angular.z = 0.0
            print("*** STOP ***")
        elif key == 'q' or ord(key) == 3:
            twist.linear.x = 0.0
            twist.angular.z = 0.0
            self.pub.publish(twist)
            self.get_logger().info("Quitting...")
            raise SystemExit
        else:
            return

        self.pub.publish(twist)


def main():
    rclpy.init()
    try:
        rclpy.spin(KeyboardControl())
    except SystemExit:
        pass
    finally:
        rclpy.shutdown()


if __name__ == "__main__":
    main()