#!/usr/bin/env python2

#  nog geen 3 omdat serial er nog niet bij zit. (nog steeds?)

#  To control Nexus robot car
#          Via /dev/ttyUSB?
#

import sys, os, serial, time, traceback
from struct import pack, unpack
import numpy as np
import re

def key_description(character):
    """generate a readable description for a key"""
    ascii_code = ord(character)
    if ascii_code < 32:
        return 'Ctrl+%c' % (ord('@') + ascii_code)
    else:
        return repr(character)

# voorlopig alleen posix
if os.name == 'posix':
    import termios, sys, os
    class Console:
        def __init__(self):
            self.fd = sys.stdin.fileno()

        def setup(self):
            self.old = termios.tcgetattr(self.fd)
            new = termios.tcgetattr(self.fd)
            #            new[3] = new[3] & ~termios.ICANON & ~termios.ECHO & ~termios.ISIG
            new[3] = new[3] & ~termios.ICANON  # & ~termios.ISIG
            new[6][termios.VMIN] = 1
            new[6][termios.VTIME] = 0
            termios.tcsetattr(self.fd, termios.TCSANOW, new)

        #  Uiteindelijk willen we meer tegelijk lezen..
        #   zeker als er grotere hoeveelheden data verstuurd worden door het target
        def getkey(self):
            c = os.read(self.fd, 1)
            return c

        def cleanup(self):
            termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.old)

    console = Console()

    def cleanup_console():
        console.cleanup()

    console.setup()
    sys.exitfunc = cleanup_console      #terminal modes have to be restored on exit...

else:
    raise NotImplementedError( "Sorry no implementation for your platform (%s) available." % sys.platform)


CONVERT_CRLF = 2
CONVERT_CR   = 1
CONVERT_LF   = 0
NEWLINE_CONVERISON_MAP = ('\n', '\r', '\r\n')
LF_MODES = ('LF', 'CR', 'CR/LF')

REPR_MODES = ('raw', 'some control', 'all control', 'hex')

class CarControl:
    def __init__(self, port, baudrate, parity, rtscts, xonxoff, echo=False, convert_outgoing=CONVERT_CRLF, repr_mode=0):
        self.serial = serial.Serial(port, baudrate, parity=parity, rtscts=rtscts, xonxoff=xonxoff, timeout = 1)
        self.echo = echo
        self.repr_mode = repr_mode
        self.convert_outgoing = convert_outgoing
        self.newline = NEWLINE_CONVERISON_MAP[self.convert_outgoing]
        self.dtr_state = True
        self.rts_state = True
        self.break_state = False

        # Het lijkt er op of er na een power up van de Nexus twee characters (waarde 0) gezonden worden. FTDI chip?
        # De writer pas wat laten doen als we daar geen last meer van hebben.
        tmp = self.serial.read(1)
        #print 'pretmp', len(tmp)
        #if len(tmp) > 0:
        #    print ord(tmp[0])
        time.sleep(1)
        tmp = self.serial.read(1)
        #print 'prettmp', len(tmp)
        #if len(tmp) > 0:
        #    print ord(tmp[0])

    def stop(self):
        print 'Waar is de stop?'
        self.serial.close()

    def getinfopacket(self):
        try:
            d = self.serial.read(24)
            print len(d) #, ord(d[0])
            if ord(d[0]) <> 2 or ord(d[23]) <> 3:  # STX and ETX
                print 'Packet framing error!'
            else:
                print 'New info packet received.'
                if ord(d[1]) == 1:   # scope data packet (try that first)
                    status_control = ord(d[3]);
                    error_code  = ord(d[4]);
                    status_car  = ord(d[5]);
                    #(sensor,)   = unpack('h', d[10:12]);
                    (speed,)    = unpack('h', d[6:8]);
                    (debtime,)    = unpack('h', d[10:12]);
                    # we gebruiken dit in de write thread en bij autoreply.
                    # evt. korte lock bij uitpakken data
                    waitreq = 1
                else:
                    print 'Packet type %x received, not implemented.' % d[1]
                print 'Info packet received with error code %x %x %d and %d' % (error_code, status_car, speed, debtime)
        except:
            print 'Wanneer hier?'
            traceback.print_exc(file=sys.stdout)


    """
    Packets are 8 bytes.
         { stx, command, par's ..., etx }
             parameters dependant upon the command

    Nexus robot functions of type void(*function[11])(unsigned int speedMMPS) :
      {goAhead, turnLeft, turnRight, rotateRight, rotateLeft, allStop,  backOff,  upperLeft, lowerLeft, upperRight, lowerRight};
      {  0,        1,        2,         3,            4,         5,        6,         7,         8,         9,         10     }
    this wil go in command.
    Parameter speedMMPS in par

    -700 < speedMMPS < 700
    speedMMPS = 200  komt overeen met 0.21 m/sec
      maximale snelheid 0.735 m/sec
      van m/s naar speedMMPS : maal 952


    Other commands:
    12: setWheelspeedcommand (set speed of 4 wheels)
    13: set speedMMPS on Nexus
    20: ask info packet
          a 24 byte packet will be received
    21: autoshutdown off (default)
    22: autoshutdown on
    23: autoreply off (default)
    24: autoreply on. reply with a infopacket
    25: start standalone?
    26: stop standalone
    27: keep alive
    28: repeat mode info packets off (default)
    29: repeat mode info packete on

    """

    def sendpacket(self, command, par):
         outpkt = bytearray(16)
         outpkt[0]  = 2   # STX
         outpkt[1]  = command
         outpkt[2:4]  = pack('h', par)
         outpkt[15]  = 3   # ETX
         self.serial.write(outpkt)             # send packet
         print 'sendpacket', outpkt[0], outpkt[1], outpkt[2], outpkt[3], outpkt[4], outpkt[5], outpkt[6], ' ... ', outpkt[15]

    def sendwheelscommand(self, frontleft, backleft, frontright, backright):
        outpkt = bytearray(16)
        outpkt[0]  = 2   # STX
        outpkt[1]  = 12  # setwheels command
        outpkt[2:4]  = pack('h', frontleft)
        outpkt[4:6]  = pack('h', backleft)
        outpkt[6:8]  = pack('h', frontright)
        outpkt[8:10]  = pack('h', backright)

        outpkt[15]  = 3   # ETX
        self.serial.write(outpkt)             # send packet
        #print 'outpkt: ', unpack('h', outpkt[2:4]), unpack('h', outpkt[4:6]), unpack('h', outpkt[6:8]), unpack('h',outpkt[8:10])

    def setspeed(self, lin,rot):
        #  lineaire (m/sec) and rotatie (rad/sec) snelheid 
        # convert to nexus (calibrated with nexus1)
        speed = lin * 1024
        rot = rot * 300
        self.sendwheelscommand(speed -rot, speed - rot, speed + rot , speed + rot) 


