#!/usr/bin/env python

import pygame
import pygame.midi
import select
import sys
import yaml
import roslib

try:
  from jsk_teleop_joy.midi_util import MIDIParse, MIDICommand, MIDIException
except:
  roslib.load_manifest("jsk_teleop_joy")
  from jsk_teleop_joy.midi_util import MIDIParse, MIDICommand, MIDIException

# use raw_input for python2 c.f. https://stackoverflow.com/questions/5868506/backwards-compatible-input-calls-in-python
if hasattr(__builtins__, 'raw_input'):
    input = raw_input

G_DEVICE_INFO = {
  "device_name": "",
  "analogs": [] #[[command, index], [command, index], ...
  }

class ParseException(Exception):
  pass

def parseDeviceName():
  global G_DEVICE_INFO
  devices = pygame.midi.get_count()
  print("===========================================")
  print("First, we choose device name:")
  for d in range(devices):
    info = pygame.midi.get_device_info(d)
    if info[2] == 1:
      print("  [%d] %s (%s)" % (d, info[1], "input"))
    else:
      print("  [%d] %s (%s)" % (d, info[1], "output"))
  val = input("Please select the device by number[%d-%d]:" % (0, d))
  try:
    parsed_number = int(val)
    if parsed_number >= 0 and parsed_number <= d:
      name = pygame.midi.get_device_info(parsed_number)[1]
      G_DEVICE_INFO["device_name"] = name
      print("")
      print("device_name: %s"  % (name))
      return parsed_number
    else:
      raise ParseException("please input number bewtween %d to %d" % (0, d))
  except ValueError:
    raise ParseException("please input number")

def configAnalogInputs(controller):
  global G_DEVICE_INFO
  print("===========================================")
  print("Please move ALL the inputs")
  print("===========================================")
  print("The order you move them will be mapped into Joy/axes.")
  print("If you want to finish analog mapping, please type 'q'")
  analog_configs = []
  while True:
    ready = select.select([sys.stdin], [], [], 0.1)[0]
    if ready:
      line = sys.stdin.readline()
      if line.startswith("q"):
        print("We installed %d analog inputs" % (len(analog_configs)))
        G_DEVICE_INFO["analogs"] = analog_configs
        return
    while controller.poll():
      data = controller.read(1)
      for elem_set in data:
        try:
          (command, index, val) = MIDIParse(elem_set)
          if (command, index) not in analog_configs:
            print("(%d, %d) installing into %d" % (command, index, len(analog_configs)))
            analog_configs.append((command, index))
        except MIDIException as e:
          print("(%d, %d, %d) is not supported" % (elem_set[0][0], elem_set[0][1], elem_set[0][2]))

def main():
  pygame.midi.init()
  while True:
    try:
      device_num = parseDeviceName()
      break
    except ParseException as e:
      print(e.message)
      print("")
      continue
  controller = pygame.midi.Input(device_num)
  configAnalogInputs(controller)
  f = open('/tmp/midi.yaml', 'w')
  f.write(yaml.dump(G_DEVICE_INFO))
  f.close()
  print("writing the configuration to /tmp/midi.yaml")
  
if __name__ == "__main__":
  main()

  
