FBT client reference impl.

// --- FBTClient.java --- //
package tholin.motionTracker.trackerTester.driver;

import java.io.*;
import java.net.*;
import java.nio.ByteBuffer;
import java.util.*;

import org.joml.Quaternionf;

public class FBTClient implements Runnable {

    public static final int STATUS_OK                      =  0;
    public static final int CONNECTION_REFUSED_SERVER_FULL =  1;
    public static final int INVALID_SERVER_RESPONSE        = -1;
    public static final int RESPONSE_TIMEOUT               = -2;

    private String IP;
    private int port;
    private Socket socket;
    private Thread t;
    private List<FBTEventHandler> eventHandlers = new ArrayList<FBTEventHandler>();

    public FBTClient(String IP) throws UnknownHostException, IOException {
        this(IP, 8086);
    }

    public FBTClient(String IP, int port) throws UnknownHostException, IOException {
        this.IP = IP;
        this.port = port;
    }

    @Override
    public void run() {
        try {
            ByteBuffer buff = ByteBuffer.allocate(16);
            InputStream in = socket.getInputStream();
            while(true) {
                if(in.available() > 0) {
                    int ID = in.read() & 0xFF;
                    switch(ID) {
                    case 0x07:
                        for(FBTEventHandler handler:eventHandlers) handler.pingReturn();
                        break;
                    case 0x05:
                        int disconnectCause = in.read() & 0xFF;
                        for(FBTEventHandler handler:eventHandlers) handler.clientDisconnect(disconnectCause);
                        socket.close();
                        return;
                    case 0x02:
                        int crashCause = in.read() & 0xFF;
                        for(FBTEventHandler handler:eventHandlers) handler.serverCrash(crashCause);
                        socket.close();
                        return;
                    case 0x09:
                        int sensorID = in.read() & 0xFF;
                        int sensorError = in.read() & 0xFF;
                        for(FBTEventHandler handler:eventHandlers) handler.sensorFailure(sensorID, sensorError);
                        return;
                    case 0x0A:
                        sensorID = in.read() & 0xFF;
                        for(int i = 0; i < 16; i++) {
                            buff.put((byte)in.read());
                        }
                        buff.flip();
                        Quaternionf quat = new Quaternionf();
                        quat.w = buff.getFloat();
                        quat.x = buff.getFloat();
                        quat.y = buff.getFloat();
                        quat.z = buff.getFloat();
                        buff.rewind();
                        for(FBTEventHandler handler:eventHandlers) handler.sensorData(sensorID, quat);
                        break;
                    default:    //TODO: throw exception on invalid packet ID
                        break;
                    }
                }
                Thread.sleep(2);
            }
        }catch(Exception e) {
            e.printStackTrace();
        }
    }

    public void addEventHandler(FBTEventHandler handler) {
        this.eventHandlers.add(handler);
    }

    public int openConnection() throws Exception {
        if(socket != null && socket.isConnected()) {
            throw new Exception("Connection already established.");
        }
        socket = new Socket(IP, port);
        t = new Thread(this);
        t.start();
        InputStream in = socket.getInputStream();
        long startTime = System.currentTimeMillis();
        while(System.currentTimeMillis() - startTime < 10000) {
            if(in.available() > 0) {
                int ID = in.read() & 0xFF;
                if(ID == 0x03) {
                    return STATUS_OK;
                }else if(ID == 0x04) {
                    int reason = in.read() & 0xFF;
                    return reason;
                }else {
                    return INVALID_SERVER_RESPONSE;
                }
            }
        }
        return RESPONSE_TIMEOUT;
    }

}

// --- FBTEventHandler --- //
package tholin.motionTracker.trackerTester.driver;

import org.joml.Quaternionf;

public interface FBTEventHandler {

    void pingReturn();

    void clientDisconnect(int cause);

    void serverCrash(int cause);

    void sensorFailure(int sensorID, int error);

    void sensorData(int sensorID, Quaternionf quat);
}