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

package at.acdp.urweb.rtde;

import org.slf4j.LoggerFactory;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.net.Socket;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.List;

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

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

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


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

    public void start() throws IOException {
        try (Socket rt = new Socket(ip, port);) {
            rt.setSoTimeout(0);
            if (rt.isConnected()) {
                System.out.println("Connected to UR Realtime Client");
            }
            dis = new DataInputStream(rt.getInputStream());
            dos = new DataOutputStream(rt.getOutputStream());
            Thread readThread = new Thread(this);
            //readThread.start();
            negotiate_protocol_version();
            send_output_setup(List.of(OutParams.timestamp), List.of(), 125);
        }
    }

    // Internal method that actually reads the data
    private void readSocket() throws IOException {
        while(true) {
            int length = dis.readInt();
            double[] rtm = new double[length];
            rtm[0] = length;
            // Calculate how much data is available from the length
            int data_available = (length - 4) / 8;
            for(int i=0; i<data_available; i++){
                rtm[i] = dis.readDouble();
            }
        }

    }

    private interface RtdeData<T> {
        public CommandType getType();
        public T read(DataInputStream di) throws IOException;

    }
    private class RtdeRequestProtocolVersion implements RtdeData{

        public  int major;
        public  int minor;
        public  int bugfix;
        public  int build;

        @Override
        public CommandType getType() {
            return RTDE_REQUEST_PROTOCOL_VERSION;
        }

        public RtdeRequestProtocolVersion read(DataInputStream d) throws IOException {
             major = d.readInt();
             minor = d.readInt();
             bugfix = d.readInt();
             build = d.readInt();
             return this;
        }
    }


    private void receive(RtdeData cmd) throws IOException {
        switch (cmd) {
            case RTDE_REQUEST_PROTOCOL_VERSION:
                logger.info("RTDE_REQUEST_PROTOCOL_VERSION");
                var pv=new ProtocolVersion();
                return pv.read(dis);
                break;
            case RTDE_GET_URCONTROL_VERSION:
                logger.info("RTDE_GET_URCONTROL_VERSION");
                break;
            case RTDE_TEXT_MESSAGE:
                logger.info("RTDE_TEXT_MESSAGE");
                break;
            case RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS:
                logger.info("RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS");
                break;
            case RTDE_CONTROL_PACKAGE_SETUP_INPUTS:
                logger.info("RTDE_CONTROL_PACKAGE_SETUP_INPUTS");
                break;
            case RTDE_CONTROL_PACKAGE_START:
                logger.info("RTDE_CONTROL_PACKAGE_START");
                break;
            case RTDE_CONTROL_PACKAGE_PAUSE:
                logger.info("RTDE_CONTROL_PACKAGE_PAUSE");
                break;
            case RTDE_DATA_PACKAGE:
                logger.info("RTDE_DATA_PACKAGE");
                break;
        }
        return null;
    }

    @Override
    public void run() {
        while(_running) {
            try {
                readSocket();
            } catch (IOException e) {
                e.printStackTrace();
            }
        }
    }

    public void send_input_setup(List<String> variables, List<String> types) {
        var cmd = CommandType.RTDE_CONTROL_PACKAGE_SETUP_INPUTS;
        var payload = String.join(",", variables);
        var rpv = new RtdeRequestProtocolVersion();
        sendAndReceive(rpv, payload.getBytes());
    }

    private void sendAndReceive(RtdeData cmd, byte[] payload) {
        sendall(cmd, payload);
        receive(cmd);
    }


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


    public void negotiate_protocol_version() {
        var cmd = RTDE_REQUEST_PROTOCOL_VERSION;
        ByteBuffer bytes = ByteBuffer.allocate(2);
        bytes.putShort((short) RTDE_PROTOCOL_VERSION);
        sendAndReceive(cmd, bytes.array());
    }


    public void  sendall(RtdeData cmd, byte[] payload) {
        try {
            int size = 3 + payload.length;
            ByteBuffer bytes = ByteBuffer.allocate(size);
            bytes.order(ByteOrder.BIG_ENDIAN);
            bytes.order( ByteOrder.BIG_ENDIAN);
            bytes.putShort((short) size);
            bytes.put((byte) cmd.getVal());
            bytes.put(payload);
            bytes.rewind();
            byte[] arr = new byte[bytes.remaining()];
            bytes.get(arr);
            dos.write(arr);
            dos.flush();
        } catch (IOException e) {
            e.printStackTrace();
        }
    }
}