import serial
import array

debug = True

class rob3IF:
    B_COMMAND_HEAD = 0x20
    B_COMMAND_TAIL = 0x03

    D_BODY = 0b000 
    D_SHOULDER = 0b001
    D_ELBOW = 0b010
    D_HAND_BEND = 0b011
    D_HAND_TORQUE = 0b100
    D_GRIP = 0b101
    D_ALL = 0b111

    FLAG_ACKNOWLEDGEMENT = 0b1000
    FLAG_SPEEDY = 0b1110000
    FLAG_ASKPOS = 0b1000000

    SEQ_INIT = [ 0x15, 0xF1, 0xF2, 0xF3 ]
    SEQ_MOTOR_OFF = [ 0x60 ]
    SEQ_MOTOR_ON = [ 0x61 ]
    SEQ_SERIAL_NUMBER = [ 0x63 ]
    

    def __init__(self, port = '/dev/ttyUSB0', baudrate = 9600, timeout = 30):
        self.serial = serial.Serial(port = port, baudrate = baudrate, timeout = timeout)
        if debug:
            print(self.serial)
        self.on()
        self._send(self.commandformat(self.SEQ_INIT))
#        print(self.serno())
#        print(self.get_position_all())

    def _send(self, data):
        raw = array.array('B', data).tostring()
        if debug:
            print('-> ' + raw)
        self.serial.write(raw)
        self.serial.flush()

    def _receive(self):
        data = []
        while not self.serial.inWaiting():
            data.append(ord(self.serial.read()))
            if debug and len(data) % 16 == 0:
                print('<-. %s' % ','.join( str(b) for b in data[-16:]))
        if debug and len(data) % 16:
            print('<-  %s' % ','.join( str(b) for b in data[-(len(data) % 16):]))
        return data

    @staticmethod
    def commandformat(data):
       data.insert(0, rob3IF.B_COMMAND_HEAD)
       data.append(rob3IF.B_COMMAND_TAIL)
       return data

    def on(self):
        self._send(self.commandformat(self.SEQ_MOTOR_ON))

    def off(self):
        self._send(self.commandformat(self.SEQ_MOTOR_OFF))

    def serno(self):
        self._send(self.commandformat(self.SEQ_SERIAL_NUMBER))
        return self._receive()

    def get_position_axis(self, ax = D_BODY):
        self._send(self.commandformat([ax | self.FLAG_ASKPOS]))
        return self._receive()

    def set_position_axis(self, ax = D_BODY, pos = 0, acknowledge = True):
        assert pos >= 0 and pos <= 255
        cmd = ax | (self.FLAG_ACKNOWLEDGEMENT & acknowledge)
        self._send(self.commandformat([cmd, pos]))
    #FIXME: ack eseten varni

    def get_position_all(self):
        self._send(self.commandformat([self.D_ALL | self.FLAG_ASKPOS]))
        return self._receive()

    def set_position_all(self, positions = [0x7f] * 6, acknowledge = True):
        assert len(positions) == 6
        assert min(positions) >= 0 and max(positions) <= 255
        cmd = self.D_ALL | (self.FLAG_ACKNOWLEDGEMENT & acknowledge)
        self._send(self.commandformat([cmd, pos]))
    #FIXME: ack eseten varni

    def position_speedy_axis(self, ax = D_BODY, pos = 0, tempo = 6, acknowledge = True):
        assert pos >= 0 and pos <= 255
        assert tempo >= 0 and pos <= 255
        cmd = ax | (self.FLAG_ACKNOWLEDGEMENT & acknowledge) | self.FLAG_SPEEDY
        self._send(self.commandformat([cmd, pos, tempo]))
    #FIXME: ack eseten varni

    def position_speedy_all(self, pos = [0x7f] * 6, tempo = [0] * 6, acknowledge = True):
        assert len(positions) == 6 and len(tempo) == 6
        assert min(positions) >= 0 and max(positions) <= 255
        assert min(tempo) >= 0 and max(tempo) <= 255
        cmd = self.D_ALL | (self.FLAG_ACKNOWLEDGEMENT & acknowledge) | self.FLAG_SPEEDY
        vec = [cmd]
        vec.extend(pos)
        vec.extend(tempo)
        self._send(vec)
    #FIXME: ack eseten varni