#! /usr/bin/python
#coding=utf-8
"""Programita para probar el carro
"""

from serial import Serial
import struct
import trama
import sys


class AGV(Serial):

  modos = dict(
  AVANZAR = "+"*4,
  RETROCEDER = "-"*4,
  FRENAR = "F"*4,
  POSICION = "P"*4,
  )

  def __init__(self, *args, **kwargs):
    #Asegura que el timeout tenga un valor rasonable
    timeout = kwargs.get('timeout',0.1)
    if timeout < 0.01: timeout = 0.1
    kwargs['timeout'] = timeout
    Serial.__init__(self, *args, **kwargs)
    self.buf = ''

  def readline(self, timeout=1):
    """timeout en segundos es el máximo tiempo que debe esperar para una trama completa"""
    tries = 0
    while 1:
      self.buf += self.read(512)
      pos = self.buf.find('*')
      if pos > 0:
        line, self.buf = self.buf[:pos+1], self.buf[pos+1:]
        return line
      tries += 1
      if tries * self.timeout > timeout:
        break
    line, self.buf = self.buf, ''
    return line

  def readlines(self, sizehint=None, timeout=1):
    """Lee todas las tramas que hay disponibles. Aborta después del timeout
    o cuando no hay mas datos."""
    lines = []
    while 1:
      line = self.readline(timeout=timeout)
      if line:
        lines.append(line)
      if not line or line[-1:] != '*':
        break
    return lines

  def trama2int(self, lista):
    """Devuelve el valor en entero obtenido del método leer registro"""
    cadena = "".join(lista)
    if len(cadena) != 6:
      return
    valor = ""
    #Sacamos el @ y el registro que pedimos, por eso empezamos del 2
    for i in xrange(2, len(cadena)):
      valor += cadena[i]
    return struct.unpack("i", valor)

  def imprimir_registros(self, *muchos_registros):
    """Pide con leer registro el valor y luego lo
    imprime en pantalla"""
    salida_pantalla = ""
    for registro in muchos_registros:
      self.write(trama.leer_registro(registro))
      salida_pantalla += "%s %s    " % (registro, self.trama2int(self.readlines(timeout=0.01)))
    print salida_pantalla

  def set_registro(self, registro, dato):
    """Le envía el valor del registro correspondiente"""
    if type(dato) == str:
      dato_int = int(dato, 16)
    else:
      dato_int = dato
    dato_string = struct.pack("i", dato_int)
    self.write(trama.dato(registro, dato_string))

  def reset(self):
    """Envía reset al Carro"""
    self.write(trama.leer_registro("RESET"))

  def prender(self):
    """Envía power al Carro"""
    self.write(trama.dato("PRENDER", "0000"))

  def apagar(self):
    """Envía apagar al Carro"""
    self.write(trama.dato("APAGAR", "0000"))

  def set_modo(self, modo):
    """Setea el modo de funcionamiento"""
    self.write(trama.dato("MODO", self.modos[modo]))

if __name__=='__main__':
  PORT = "/dev/ttyUSB0"
  BAUDRATE = 9600
  try:
    modulo_config = sys.argv[1]
    if modulo_config.endswith(".py"): modulo_config = modulo_config[:len(modulo_config)-3:]
  except:
    print "hay que pasar un archivo de configuración"
  config = __import__(modulo_config)
  s = AGV(PORT, BAUDRATE)
  s.prender()
  s.reset()
  s.set_modo("POSICION")
  s.set_registro("Ki", config.Ki)
  s.set_registro("Kd", config.Kd)
  s.set_registro("Kp", config.Kp)
  s.set_registro("ACELERACION", config.ACELERACION)
  s.set_registro("FRENADO", config.FRENADO)
  s.set_registro("VELOCIDAD_FINAL", config.VELOCIDAD_FINAL)
  s.set_registro("POSICION_FINAL", 600)
  s.imprimir_registros("Ki", "Kd", "Kp", "ACELERACION", "POSICION_FINAL")
  for j in range(50):
    s.imprimir_registros("CUENTA", "VELOC_ACTUAL")
  s.set_registro("POSICION_FINAL", 0)
  for j in range(50):
    s.imprimir_registros("CUENTA", "VELOC_ACTUAL")
  s.reset()
  s.apagar()