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

package at.acdp.urweb.sclient;

import at.acdp.urweb.CountDataInputStream;
import at.acdp.urweb.sclient.data.*;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

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

public class SecondaryClient {
    private static final Logger log = LoggerFactory.getLogger(SecondaryClient.class);


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

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

    public VersionMessage connect() throws IOException {
        this.rt = new Socket(ip, port);
        this.os = rt.getOutputStream();
        this.in = new CountDataInputStream(rt.getInputStream());
        VersionMessage vm = new VersionMessage();
        vm.read(in, -1);
        return vm;
    }

    public void readReply(CountDataInputStream di) throws IOException {
        int beforeCount=di.getCount();
        int size = di.readInt(); //4
        int pType = di.readByte() & 0xff; //+1=5
        log.info("ptype: "+pType);
        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;
            case PackageType.CARTESIAN_INFO:
                readCartesianInfo(di, size);
                break;
            case PackageType.KINEMATICS_INFO:
                readKinemsticsInfo(di, size);
                break;
            case PackageType.CONFIGURATION_DATA:
                readConfigurationData(di, size);
                break;
            case PackageType.FORCE_MODE_DATA:
                readForceModeData(di, size);
                break;
            case PackageType.ADDITIONAL_INFO:
                readAdditionalInfo(di, size);
                break;
            case PackageType.NEEDED_FOR_CALIB_DATA:
                skip(PackageType.NEEDED_FOR_CALIB_DATA,di, size);
                break;
            case PackageType.SAFETY_DATA:
                skip(PackageType.SAFETY_DATA,di, size);
                break;
            case PackageType.TOOL_COMM_INFO:
                readToolCommInfo(di, size);
                break;


            default:
                log.warn("unknown ptype: "+pType+", size: "+size);
                byte[] pack=new byte[size-5];
                di.readFully(pack);
        }
        int afterCount=di.getCount();
        int diff=afterCount-beforeCount-size;
        if(diff!=0) {
            log.warn("size mismatch: " +diff + "package type: "+pType);
        }
    }

    private ToolCommInfo readToolCommInfo(CountDataInputStream di, int size) throws IOException {
        ToolCommInfo tci=new ToolCommInfo();
        tci.read(di, size);
        return tci;
    }

    private void skip(int pType, CountDataInputStream di, int size) throws IOException {
        log.trace("skip data for package type: "+pType);
        byte[] data=new byte[size-5];
        di.readFully(data);
    }


    private AdditionalInfo readAdditionalInfo(CountDataInputStream di, int size) throws IOException {
        AdditionalInfo ai=new AdditionalInfo();
        ai.read(di, size);
        return ai;

    }

    private ForceModeData readForceModeData(CountDataInputStream di, int size) throws IOException {
        ForceModeData fmd=new ForceModeData();
        fmd.read(di, size);
        return fmd;
    }

    private ConfigurationData readConfigurationData(CountDataInputStream di, int size) throws IOException {
        ConfigurationData cd=new ConfigurationData();
        cd.read(di, size);
        return cd;
    }

    void readRobotMessage(CountDataInputStream 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(CountDataInputStream 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);
        }
    }

    CartesianInfo readCartesianInfo(CountDataInputStream di, int size) throws IOException {
        CartesianInfo ci = new CartesianInfo();
        ci.read(di, size);
        return ci;
    }

    KinematicsInfo readKinemsticsInfo(CountDataInputStream di, int size) throws IOException {
        KinematicsInfo ki = new KinematicsInfo();
        ki.read(di, size);
        return ki;
    }

    ModeData readRobotModeData(CountDataInputStream 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(CountDataInputStream di, int size) throws IOException {
        int joints = size / 41;
        // byte[] buf=new byte[300];
        // di.readFully(buf);
        JointData[] jds = new JointData[size];
        for (int i = 0; i < joints; i++) {
            JointData jd = new JointData();
            jd.read(di, size);
            jds[i] = jd;
        }
        return jds;
    }


    public void writeCmd(String cmd) {
        try {
            System.out.println("send cmd:" + cmd);
            os.write(cmd.getBytes(StandardCharsets.UTF_8));

        } catch (IOException e) {
            e.printStackTrace();
        }
    }

    public void readPackage() throws IOException {
        readReply(in);
    }
}