package at.acdp.urweb.web;
import java.io.*;
import java.net.Socket;
import java.nio.charset.StandardCharsets;
import java.util.HashMap;
import java.util.Map;

public class RTDE implements Runnable {

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

    public RTDE(String ip, int port) {
        this.ip=ip;
        this.port=port;
    }
    private void connect() throws IOException {
        this.rt = new Socket(ip, port);
        this.os=rt.getOutputStream();
        this.in=new DataInputStream(rt.getInputStream());
        readVersionMessage();
    }

    private void readVersionMessage() throws IOException {
        int msgSize=in.readInt();
        int msgType=in.readByte()&0xff;
        long tstamp=in.readLong();
        byte source=in.readByte();
        byte robotMsgType=in.readByte();
        byte projectNameSize=in.readByte();
        byte[] name=new byte[projectNameSize];
        in.read(name);
        String nameS=new String(name);
        int  majorVersion=in.readByte() & 0xff;
        int  minorVersion=in.readByte() & 0xff;
        int bugFixVersion=in.readInt();
        int buildNumber=in.readInt();
        byte[] buildDateB=new byte[msgSize-(16+projectNameSize+10)];
        in.read(buildDateB);
        String buildDate=new String(buildDateB);



    }

    private void writeCmd(String cmd) {
        try {
            System.out.println("send cmd:"+cmd);
            os.write(cmd.getBytes(StandardCharsets.UTF_8));
            int length=in.readInt();
            System.out.println("len: "+length);
            byte[] buf=new byte[length];
            //in.read(buf);
            int mType = in.readByte() & 0xff;
            System.out.println("mtype: "+mType);
            while(true) {
                int sublength=in.readInt();
                System.out.println("sublength: " + sublength);

                int subType = in.readByte();
                System.out.println("subtype: "+subType);
                byte[] res=new byte[sublength-5];
                in.read(res);


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

    @Override
    public void run() {
            try {
                connect();
                writeCmd("set_digital_out(1,True)\n");
                writeCmd("set_digital_out(2,True)\n");
                writeCmd("movej([-1.95,-1.58,-1.16,-1.15,-1.55,1.18], a=1.0, v=0.1)\n");

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