import sys
import termios
import time

from multiprocessing import Queue
from termios import (BRKINT, CS8, CSIZE, ECHO, ICANON, ICRNL, IEXTEN, INPCK,
                     ISTRIP, IXON, PARENB, VMIN, VTIME)
from typing import NoReturn

from openpilot.tools.sim.bridge.common import QueueMessage, control_cmd_gen

# Indexes for termios list.
IFLAG = 0
OFLAG = 1
CFLAG = 2
LFLAG = 3
ISPEED = 4
OSPEED = 5
CC = 6


KEYBOARD_HELP = """
  | key  |   functionality       |
  |------|-----------------------|
  |  1   | Cruise Resume / Accel |
  |  2   | Cruise Set    / Decel |
  |  3   | Cruise Cancel         |
  |  r   | Reset Simulation      |
  |  i   | Toggle Ignition       |
  |  q   | Exit all              |
  | wasd | Control manually      |
"""


def getch() -> str:
  STDIN_FD = sys.stdin.fileno()
  try:
    old_settings = termios.tcgetattr(STDIN_FD)
  except (termios.error, OSError):
    # No controlling terminal (e.g. headless / background run). Idle instead of crashing.
    return None
  try:
    # set
    mode = old_settings.copy()
    mode[IFLAG] &= ~(BRKINT | ICRNL | INPCK | ISTRIP | IXON)
    #mode[OFLAG] &= ~(OPOST)
    mode[CFLAG] &= ~(CSIZE | PARENB)
    mode[CFLAG] |= CS8
    mode[LFLAG] &= ~(ECHO | ICANON | IEXTEN)
    mode[CC][VMIN] = 1
    mode[CC][VTIME] = 0
    termios.tcsetattr(STDIN_FD, termios.TCSAFLUSH, mode)

    ch = sys.stdin.read(1)
  except (termios.error, OSError):
    ch = None
  finally:
    try:
      termios.tcsetattr(STDIN_FD, termios.TCSADRAIN, old_settings)
    except (termios.error, OSError):
      pass
  return ch

def print_keyboard_help():
  print(f"Keyboard Commands:\n{KEYBOARD_HELP}")

def keyboard_poll_thread(q: 'Queue[QueueMessage]'):
  print_keyboard_help()

  while True:
    c = getch()
    if c is None:
      # No terminal input available (headless/background); avoid busy-looping.
      time.sleep(0.05)
    elif c == '1':
      q.put(control_cmd_gen("cruise_up"))
    elif c == '2':
      q.put(control_cmd_gen("cruise_down"))
    elif c == '3':
      q.put(control_cmd_gen("cruise_cancel"))
    elif c == 'w':
      q.put(control_cmd_gen(f"throttle_{1.0}"))
    elif c == 'a':
      q.put(control_cmd_gen(f"steer_{-0.15}"))
    elif c == 's':
      q.put(control_cmd_gen(f"brake_{1.0}"))
    elif c == 'd':
      q.put(control_cmd_gen(f"steer_{0.15}"))
    elif c == 'z':
      q.put(control_cmd_gen("blinker_left"))
    elif c == 'x':
      q.put(control_cmd_gen("blinker_right"))
    elif c == 'i':
      q.put(control_cmd_gen("ignition"))
    elif c == 'r':
      q.put(control_cmd_gen("reset"))
    elif c == 'q':
      q.put(control_cmd_gen("quit"))
      break
    else:
      print_keyboard_help()

def test(q: 'Queue[str]') -> NoReturn:
  while True:
    print([q.get_nowait() for _ in range(q.qsize())] or None)
    time.sleep(0.25)

if __name__ == '__main__':
  from multiprocessing import Process, Queue
  q: 'Queue[QueueMessage]' = Queue()
  p = Process(target=test, args=(q,))
  p.daemon = True
  p.start()

  keyboard_poll_thread(q)