uraxisdaemon/src/main/java/at/acdp/urweb/rtde/RTDEClient.java

package at.acdp.urweb.rtde;

import at.acdp.urweb.rtde.packets.IRtdeData;
import at.acdp.urweb.rtde.packets.RtdeRequestProtocolVersion;
import at.acdp.urweb.rtde.packets.RtdeRequestURVersion;
import at.acdp.urweb.rtde.packets.RtdeTextMessage;
import org.slf4j.LoggerFactory;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.net.Socket;
import java.net.SocketException;
import java.net.UnknownHostException;
import java.nio.ByteBuffer;
import java.util.List;
import java.util.concurrent.Semaphore;

import static at.acdp.urweb.rtde.CommandType.*;

public class RTDEClient implements Runnable {
    private final static org.slf4j.Logger logger = LoggerFactory.getLogger(RTDEClient.class);
    private final Semaphore waitingRequestSem = new Semaphore(0);
    private final Semaphore socketOpen = new Semaphore(0);

    private volatile IRtdeData waitingRequest=null;

    public static int RTDE_PROTOCOL_VERSION = 2;
    private final String ip;
    private final int port;
    private final boolean _running = true;
    private DataOutputStream dos;
    private DataInputStream dis;


    public RTDEClient(String ip, int port) {
        this.ip = ip;
        this.port = port;
    }

    public void start() throws IOException, InterruptedException {
        Thread readThread = new Thread(this);
        readThread.start();
        socketOpen.acquire();
        init();
    }

    private void request(IRtdeData data) throws IOException, InterruptedException {
        waitingRequest=data;
        data.send(dos);
        waitingRequestSem.acquire();
        return;
    }

    // Internal method that actually reads the data
    private void readSocket() throws IOException, InterruptedException {
        int length = dis.readShort();
        int type=dis.readByte();
        if(type==waitingRequest.getType()) {
            waitingRequest.read(dis);
            waitingRequestSem.release();
        } else switch(type) {
            case RTDE_TEXT_MESSAGE:
                var rpv = new RtdeTextMessage();
                request(rpv);
                logger.info("RTDE_TEXT_MESSAGE", rpv.text);
                break;

            default: {
                logger.warn(String.format("Unexpected package type: %d", type));
                byte[] buf=new byte[length];
                dis.readFully(buf);
            }
        }
    }

    @Override
    public void run() {
        try {
            Socket rt = new Socket(ip, port);
            rt.setSoTimeout(0);
            rt.setReuseAddress(true);
            rt.setTcpNoDelay(true);
            if (rt.isConnected()) {
                System.out.println("Connected to UR Realtime Client");
            }
            dos = new DataOutputStream(rt.getOutputStream());
            dis = new DataInputStream(rt.getInputStream());
            socketOpen.release();
            while (_running) {
                try {
                    readSocket();
                } catch (IOException | InterruptedException e) {
                    e.printStackTrace();
                }
            }
        } catch (IOException  e) {
            logger.warn("run",e);
        }
    }

    public void send_output_setup(List<String> variables, List<String> types, int frequency) throws IOException {
        var payload = String.join(",", variables);
        byte[] p = payload.getBytes();
        ByteBuffer bytes = ByteBuffer.allocate(8 + p.length);
        bytes.putDouble(frequency);
        bytes.put(p);
    }

    public void init() throws IOException, InterruptedException {
        var rpv = new RtdeRequestProtocolVersion();
        request(rpv);
        logger.info("RtdeRequestProtocolVersion succes:" + rpv.success);
        var ruv = new RtdeRequestURVersion();
        request(ruv);
        logger.info(String.format("RtdeRequestURVersion: %d.%d.%d.%d",ruv.major, ruv.minor, ruv.build, ruv.bugfix));
    }
}