import struct
import socket

sensorData = [
    [0,0,0, 0,0,0],
    [0,0,0, 0,0,0],
    [0,0,0, 0,0,0],
    [0,0,0, 0,0,0]
    ]

controllerButtonData = [False] * 32
controllerValuatorData = [0.0] * 32

def init(trackerPortNum = 4505, controllerPortNum=4510):
    global trackerSocket, controllerSocket
    trackerSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    trackerSocket.bind(('',trackerPortNum))
    trackerSocket.setblocking(0)
    controllerSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    controllerSocket.bind(('',controllerPortNum))
    controllerSocket.setblocking(0)

def sensor(id):
    return sensorData[id]

def button(id):
    return controllerButtonData[id]

def valuator(id):
    return controllerValuatorData[id]

def update():
    while True:
        try:
            data, fromAddress = trackerSocket.recvfrom(65536)
        except socket.error: break
        intdata = struct.unpack('!lllllllllll',data)
        id = intdata[0] - 1
        x = intdata[5] / 100000.0
        y = intdata[6] / 100000.0
        z = intdata[7] / 100000.0
        rx = intdata[8] / 100000.0
        ry = intdata[9] / 100000.0
        rz = intdata[10] / 100000.0
        if id >= 0 and id < 4:
            sensorData[id] = [x,y,z, rx,ry,rz]
    while True:
        try:
            data, fromAddress = controllerSocket.recvfrom(65536)
        except socket.error: break
        intdata = struct.unpack('!lllllllllllBBBB',data)
        controllerValuatorData[0] = intdata[9]/1000.0
        controllerValuatorData[1] = intdata[10]/1000.0
        controllerButtonData[0] = (intdata[11] & 1) != 0
        controllerButtonData[1] = (intdata[11] & 2) != 0
        controllerButtonData[2] = (intdata[11] & 4) != 0