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 io.undertow.util.FileUtils;
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());
//        byte[] data=new byte[4096];
//        this.in.readFully(data);
//        FileOutputStream fos = new FileOutputStream("data2.txt");
//        fos.write(data);
//        fos.close();

        VersionMessage vm = new VersionMessage();
        vm.read(in, -1);
        return vm;
    }

    public int readReply(CountDataInputStream di) throws IOException {
        int beforeCount=di.getCount();
        int size = di.readInt(); //4
        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;
            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);
        }
        return size;
    }

    public int readSub(CountDataInputStream di) throws IOException {
        int beforeCount=di.getCount();
        int size = di.readInt(); //4
        int pType = di.readByte() & 0xff; //+1=5
        switch (pType) {
            case PackageType.ROBOT_MODE_DATA:
                var md=readRobotModeData(di, size);
                log.info(md.toString());
                break;
            case PackageType.JOINT_DATA:
                var jd=readJointData(di, size);
                log.info(jd.toString());
                break;
            case PackageType.TOOL_DATA:
                var td=readToolData(di, size);
                log.info(td.toString());
                break;
            case PackageType.MASTERBOARD_DATA:
                var mb=readMasterBoardData(di, size);
                log.info(mb.toString());
                break;
            case PackageType.CARTESIAN_INFO:
                var c=readCartesianInfo(di, size);
                log.info(c.toString());
                break;
            case PackageType.KINEMATICS_INFO:
                var ki=readKinemsticsInfo(di, size);
                log.info(ki.toString());
                break;
            case PackageType.CONFIGURATION_DATA:
                var cd=readConfigurationData(di, size);
                log.info(cd.toString());
                break;
            case PackageType.FORCE_MODE_DATA:
                var fmd=readForceModeData(di, size);
                log.info(fmd.toString());
                break;
            case PackageType.ADDITIONAL_INFO:
                var ai=readAdditionalInfo(di, size);
                log.info(ai.toString());
                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:
                var tci=readToolCommInfo(di, size);
                log.info(tci.toString());
                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);
        }
        return size;
    }

    private MasterBoardData readMasterBoardData(CountDataInputStream di, int size) throws IOException {
        MasterBoardData mb=new MasterBoardData();
        mb.read(di, size);
        return mb;
    }

    private ToolData readToolData(CountDataInputStream di, int size) throws IOException {
        ToolData td=new ToolData();
        td.read(di, size);
        return td;
    }

    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();
        byte source = di.readByte();
        int robotMessageType = di.readUnsignedByte();
        switch (robotMessageType) {
            default:
                log.info("unknown msg: " + (int) robotMessageType);
                byte[] buf = new byte[size - 15];
                di.readFully(buf);
        }
    }

    void readRobotState(CountDataInputStream di, int size) throws IOException {
        int remaining=size-5;
        while(remaining>0)
            remaining-=readSub(di);
    }

    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();
        return md;
    }

    JointData[] readJointData(CountDataInputStream di, int size) throws IOException {
        int joints = size / 41;
        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);
    }
}