urweb/src/main/java/at/acdp/urweb/sclient/SecondaryClient.java

package at.acdp.urweb.sclient;

import at.acdp.urweb.sclient.data.MessageType;
import at.acdp.urweb.sclient.data.PackageType;
import at.acdp.urweb.sclient.data.RobotMessageType;
import at.acdp.urweb.sclient.data.VersionMessage;

import java.io.*;
import java.net.Socket;
import java.nio.charset.StandardCharsets;

public class SecondaryClient {

    private final String ip;
    private final int port;
    private volatile boolean _running = true;
    private Socket rt;
    private OutputStream os;
    private DataInputStream in;

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

    public void connect() throws IOException {
        this.rt = new Socket(ip, port);
        this.os = rt.getOutputStream();
        this.in = new DataInputStream(rt.getInputStream());
        VersionMessage vm = new VersionMessage();
        vm.readVersionMessage(in);
        System.out.println(vm);
        readReply(in);
    }

    private void readReply(DataInputStream di) throws IOException {

        int size=di.readInt(); //4
        System.out.println("size: " +size);
        int pType= di.readByte() &0xff; //+1=5
        switch (pType) {
            case MessageType.ROBOT_MESSAGE:
                readRobotMessage(di, size);
                break;
            case MessageType.ROBOT_STATE:
                readRobotState(di, size);
                break;
            case PackageType.ROBOT_MODE_DATA:
                readRobotModeData(di, size);
                break;
            case PackageType.JOINT_DATA:
                readJointData(di, size);
                break;
        }
    }
    void readRobotMessage(DataInputStream di, int size) throws IOException {
        long ts=di.readLong();
        char source= (char) (di.readByte() & 0xFF);
        char robotMessageType= (char) (di.readByte() & 0xFF);
        switch (robotMessageType) {
            default:
                System.out.println("unknown msg" + (int)robotMessageType);
                //byte[] buf=new byte[size-11];
                byte[] buf=new byte[size-10];
                di.readFully(buf);
                System.out.println(buf);
        }
    }

    void readRobotState(DataInputStream di, int size) throws IOException {
        long ts=di.readLong();
        char source= (char) (di.readByte() & 0xFF);
        char robotMessageType= (char) (di.readByte() & 0xFF);
        switch (robotMessageType) {
            default:
                System.out.println("unknown msg" + robotMessageType);
                byte[] buf=new byte[size-10];
                di.readFully(buf);
        }
    }

    ModeData readRobotModeData(DataInputStream di, int size) throws IOException {
        long ts=di.readLong();
        ModeData md=new ModeData();
        md.realRobotConnected=di.readBoolean();
        md.realRobotEnabled=di.readBoolean();
        md.robotPoweredOn=di.readBoolean();
        md.emergencyStopped=di.readBoolean();
        md.protectiveStopped=di.readBoolean();
        md.isProgramRunning=di.readBoolean();
        md.isProgramPaused=di.readBoolean();
        md.robotMode=di.readByte();
        md.controlMode=di.readByte();
        md.targetSpeedFraction=di.readDouble();
        md.speedScaling=di.readDouble();
        md.targetSpeedFractionLimit=di.readDouble();
        md.internal=di.readByte();
        //byte[] buf=new byte[50];
        //di.readFully(buf);
        return md;
    }

    JointData[] readJointData(DataInputStream di, int size) throws IOException {
        int joints=size/49;
        JointData[] jds=new JointData[5];
        for(int i=0;i<joints;i++) {
            JointData jd=new JointData();
            jd.read(di);
            jds[i]=jd;
        }
        return jds;
    }



        public void writeCmd(String cmd) {
        try {
            System.out.println("send cmd:" + cmd);
            os.write(cmd.getBytes(StandardCharsets.UTF_8));
            readReply(in);
            readReply(in);
            readReply(in);
        } catch (IOException e) {
            e.printStackTrace();
        }
    }
}