uraxisdaemon/src/main/java/at/acdp/urweb/rtde/packets/RtdeInDataPackage.java

package at.acdp.urweb.rtde.packets;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.util.HashMap;

import static at.acdp.urweb.rtde.CommandType.RTDE_DATA_PACKAGE;
import static at.acdp.urweb.rtde.CommandType.RTDE_TEXT_MESSAGE;

public class RtdeInDataPackage implements IRtdeData {
    private final RtdeSetupInputs inputs;
    private HashMap<String, String> pdata=new HashMap<>();


    public RtdeInDataPackage(RtdeSetupInputs inputs) {
        this.inputs=inputs;
    }

    @Override
    public int getType() {
        return RTDE_DATA_PACKAGE;
    }

    public HashMap<String, String> getPdata() {
        return pdata;
    }


    @Override
    public RtdeInDataPackage read(DataInputStream di, int length) throws IOException {
        return this;
    }

    private int calcSize() {
        int size = 0;
        for (int i = 0; i < inputs.variables.size(); i++) {
            String output = inputs.variables.get(i);
            String type = inputs.types.get(i);
            switch (type) {
                case "BOOL":
                case "UINT8":
                    size+=1;
                    break;
                case "UINT32":
                case "INT32":
                    size+=4;
                    break;
                case "UINT64":
                case "DOUBLE":
                    size+=8;
                    break;
                case "VECTOR3D":
                case "VECTOR6INT32":
                case "VECTOR6UINT32":
                    size+=24;
                    break;
                case "VECTOR6D":
                    size+=48;
                    break;
            }
        }
        return size;
    }

    @Override
    public RtdeInDataPackage send(DataOutputStream dos) throws IOException {
        sendHeader(dos,calcSize()+1);
        dos.writeByte(inputs.recipe);
        for(int i=0; i<inputs.variables.size(); i++) {
            String output = inputs.variables.get(i);
            String type = inputs.types.get(i);
            switch (type) {
                case "BOOL":
                    dos.writeBoolean(Boolean.parseBoolean(pdata.get(output)));
                    break;
                case "UINT8":
                    dos.writeByte(Byte.parseByte(pdata.get(output)));
                    break;
                case "UINT32":
                case "INT32":
                    dos.writeInt(Integer.parseInt(pdata.get(output)));
                    break;
                case "UINT64":
                    dos.writeLong(Long.parseLong(pdata.get(output)));
                    break;
                case "DOUBLE":
                    dos.writeDouble(Double.parseDouble(pdata.get(output)));
                    break;
                case "VECTOR3D": {
                    String s=pdata.get(output);
                    var doubles=s.split(" ");
                    dos.writeDouble(Double.parseDouble(doubles[0]));
                    dos.writeDouble(Double.parseDouble(doubles[1]));
                    dos.writeDouble(Double.parseDouble(doubles[2]));
                    break;}
                case "VECTOR6D": {
                    String s=pdata.get(output);
                    var doubles=s.split(" ");
                    dos.writeDouble(Double.parseDouble(doubles[0]));
                    dos.writeDouble(Double.parseDouble(doubles[1]));
                    dos.writeDouble(Double.parseDouble(doubles[2]));
                    dos.writeDouble(Double.parseDouble(doubles[3]));
                    dos.writeDouble(Double.parseDouble(doubles[4]));
                    dos.writeDouble(Double.parseDouble(doubles[5]));
                    break;}
                case "VECTOR6INT32":
                case "VECTOR6UINT32": {
                    String s=pdata.get(output);
                    var ints=s.split(" ");
                    dos.writeInt(Integer.parseInt(ints[0]));
                    dos.writeInt(Integer.parseInt(ints[1]));
                    dos.writeInt(Integer.parseInt(ints[2]));
                    dos.writeInt(Integer.parseInt(ints[3]));
                    dos.writeInt(Integer.parseInt(ints[4]));
                    dos.writeInt(Integer.parseInt(ints[5]));
                    break;}

                default:
                    throw new IllegalStateException("Unexpected value: " + type);
            }
        }
        return this;
    }

    public void put(String key, String value) {
        pdata.put(key, value);
    }

    @Override
    public boolean hasReply() {
        return false;
    }
}