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.util.List;

import static at.acdp.urweb.rtde.CommandType.RTDE_GET_URCONTROL_VERSION;
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);
    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 {
        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());

            negotiate_protocol_version();
            //Thread readThread = new Thread(this);
            //readThread.start();
            // 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 void receive(RtdeData cmd) throws IOException {
        logger.info(cmd.getType().toString());
        cmd.read(dis);
    }

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

        while (_running) {
            try {
                readSocket();
            } catch (IOException e) {
                e.printStackTrace();
            }
        }
    }

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

    private void sendAndReceive(RtdeData cmd) throws IOException {
        cmd.send(dos);
        cmd.read(dis);
    }

    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);

        // sendAndReceive(rpv, bytes.array());
    }

    public void negotiate_protocol_version() throws IOException {
        var rpv = new RtdeRequestProtocolVersion();
        sendAndReceive(rpv);
    }

    private interface RtdeData<T> {
        CommandType getType();

        int getSize();

        void setSize(int i);


        T read(DataInputStream di) throws IOException;

        T send(DataOutputStream dos) throws IOException;

        default void sendHeader(DataOutputStream dos) throws IOException {
            dos.writeShort(getSize());
            dos.writeByte(getType().getVal());
        }

        default void readHeader(DataInputStream dis) throws IOException {
            setSize(dis.readShort());
            int cmd = dis.readByte();
            if (cmd != getType().getVal())
                throw new IOException(String.format("Expected %d, got %d", getType().getVal(), cmd));
        }

    }

    private class RtdeRequestURVersion implements RtdeData {
        private int size;

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

        @Override
        public int getSize() {
            return 5;
        }

        @Override
        public void setSize(int s) {
            size = s;
        }

        @Override
        public RtdeRequestURVersion read(DataInputStream di) throws IOException {
            readHeader(di);
            return this;
        }

        @Override
        public RtdeRequestURVersion send(DataOutputStream dos) throws IOException {
            sendHeader(dos);
            return this;
        }
    }

    private class RtdeRequestProtocolVersion implements RtdeData {
        public boolean success;
        private int size;

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

        @Override
        public int getSize() {
            return size;
        }

        @Override
        public void setSize(int size) {
            this.size = size;
        }

        @Override
        public RtdeRequestProtocolVersion read(DataInputStream d) throws IOException {
            readHeader(dis);
            success = dis.readBoolean();
            return this;
        }

        @Override
        public RtdeRequestProtocolVersion send(DataOutputStream dos) throws IOException {
            sendHeader(dos);
            dos.writeShort(RTDE_PROTOCOL_VERSION);
            dos.flush();
            return this;
        }
    }
}