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

package at.acdp.urweb.sclient;

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

import java.io.*;
import java.net.InetSocketAddress;
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 final URBot urBot;
    private volatile boolean _running = true;
    private Socket rt;
    private OutputStream os;
    private CountDataInputStream in;
    private MasterBoardData lastMB=new MasterBoardData();

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

    public VersionMessage connect() throws IOException {
        this.rt = new Socket();
        var adr=new InetSocketAddress(ip, port);
        log.info("Connecting to {}", adr.toString());
        this.rt=new Socket();
        this.rt.connect(adr, 5000);
        this.os = rt.getOutputStream();
        this.in = new CountDataInputStream(rt.getInputStream());
        VersionMessage vm = new VersionMessage();
        vm.read(in, -1);
        return vm;
    }

    public MasterBoardData getLastMB() {
        return lastMB;
    }

    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;
            case MessageType.PROGRAM_STATE_MESSAGE:
                var psm=new ProgramStateMessage();
                psm.read(di, size);
                URLog.add(psm);
                break;
            case MessageType.MODBUS_INFO_MESSAGE:
                var m=new ModbusInfoMessage();
                m.read(di, size);
                URLog.add(m);
                break;
            case MessageType.SAFETY_COMPLIANCE_TOLERANCES_MESSAGE:
                var sc=new SafetyComplianceTolerancesMessage();
                sc.read(di, size);
                URLog.add(sc);
                break;
            case MessageType.SAFETY_SETUP_BROADCAST_MESSAGE:
                var ss=new SafetySetupBroadcastMessage();
                ss.read(di, size);
                URLog.add(ss);
                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);
                URLog.add(md);
                log.debug(md.toString());
                break;
            case PackageType.JOINT_DATA:
                var jd=readJointData(di, size);
                URLog.add(jd);
                log.debug(jd.toString());
                break;
            case PackageType.TOOL_DATA:
                var td=readToolData(di, size);
                URLog.add(td);
                log.debug(td.toString());
                break;
            case PackageType.MASTERBOARD_DATA:
                var mb=readMasterBoardData(di, size);
                URLog.add(mb);
                log.debug(mb.toString());
                break;
            case PackageType.CARTESIAN_INFO:
                var c=readCartesianInfo(di, size);
                URLog.add(c);
                log.debug(c.toString());
                break;
            case PackageType.KINEMATICS_INFO:
                var ki=readKinemsticsInfo(di, size);
                URLog.add(ki);
                log.debug(ki.toString());
                break;
            case PackageType.CONFIGURATION_DATA:
                var cd=readConfigurationData(di, size);
                URLog.add(cd);
                log.debug(cd.toString());
                break;
            case PackageType.FORCE_MODE_DATA:
                var fmd=readForceModeData(di, size);
                URLog.add(fmd);
                log.debug(fmd.toString());
                break;
            case PackageType.ADDITIONAL_INFO:
                var ai=readAdditionalInfo(di, size);
                URLog.add(ai);
                log.debug(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);
                URLog.add(tci);
                log.debug(tci.toString());
                break;
            default:
                log.warn("unknown ptype: "+pType+", size: "+size);
                skip(43, di, size);
        }
        int afterCount=di.getCount();
        int diff=afterCount-beforeCount-size;
        if(diff!=0) {
            log.warn("size mismatch: " +diff + "package type: "+pType);
            byte[] pack=new byte[-diff];
            di.readFully(pack);
        }
        return size;
    }

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

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

    private JointDataList readJointData(CountDataInputStream di, int size) throws IOException {
        JointDataList td=new JointDataList();
        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) {
            case RobotMessageType.RUNTIME_EXCEPTION:
                var rte=new RobotRuntimeException();
                rte.read(di, size);
                URLog.add(rte);
                log.debug(rte.toString());
                break;
            case RobotMessageType.KEY:
                var rmk=new RobotMessageKey();
                rmk.read(di, size);
                URLog.add(rmk);
                log.debug(rmk.toString());
                break;
            case RobotMessageType.PROGRAM_LABEL:
                var rpl=new RobotProgramLabel();
                rpl.read(di, size);
                URLog.add(rpl);
                log.debug(rpl.toString());
                urBot.ackNumber(rpl);
                break;
            case RobotMessageType.ERROR_CODE:
                var rme=new RobotMessageError();
                rme.read(di, size);
                URLog.add(rme);
                log.debug(rme.toString());
                break;
            case RobotMessageType.ZWOELF:
                var zm=new ZwoelfMessageKey();
                zm.read(di, size);
                URLog.add(zm);
                log.debug(zm.toString());
                break;
            case RobotMessageType.TEXT:
                var t=new RobotMessageText();
                t.read(di, size);
                URLog.add(t);
                log.debug(t.toString());
                break;
            default:
                log.info("unknown msg:  {} size: {}", (int) robotMessageType, size);
                byte[] buf = new byte[size - 15];
                di.readFully(buf);
                break;
        }
    }


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

    public void writeCmd(String cmd) {
        try {
            os.write(cmd.getBytes(StandardCharsets.UTF_8));
        } catch (IOException e) {
            log.warn("writecmd", e);
        }
    }

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