package at.acdp.urweb.web;

import java.io.DataInputStream;
import java.io.IOException;
import java.net.Socket;

import java.net.SocketException;
import java.util.HashMap;
import java.util.Map;

public class GetRobotRealtimeData implements Runnable {

    private final String ip;
    private final int port;
    private volatile boolean _running=true;

    /*****
     * Creates a new RealTime reader
     * Default Constructor
     * Uses localhost (127.0.0.1) and port 30003
     */
    public GetRobotRealtimeData(String ip, int port) {
        this.ip=ip;
        this.port=port;

    }

    // Internal method that actually reads the data
    private void readSocket() throws IOException {
        try(Socket rt = new Socket(ip, port);){
            rt.setSoTimeout(1000);
            if (rt.isConnected()){
                System.out.println("Connected to UR Realtime Client");
            }
            DataInputStream in = new DataInputStream(rt.getInputStream());
            while(true) {
                int length = in.readInt();
                double[] rtm = new double[length];
                rtm[0] = length;
                // Calculate how much data is available from the length
                int data_available = (length - 4) / 8;
                for(int i=0; i<data_available; i++){
                    rtm[i] = in.readDouble();
                }
            }
        }
    }

    @Override
    public void run() {
        while(_running) {
            try {
                readSocket();
            } catch (IOException e) {
                e.printStackTrace();
            }
        }
    }

    /*
     * Creating enum to map start index and lengths of data entries
     * According to specification of RealTimeClient in Excel sheet
     */
    private enum RTinfo {
        // name			(index in plot, number of doubles)
        q_target		(2, 6),
        qd_target		(8, 6),
        qdd_target		(14, 6),
        q_actual		(32, 6),
        qd_actual		(38, 6),
        TCP_actual		(56, 6),
        TCPd_actual		(62, 6),
        TCP_force		(68, 6),
        TCP_target		(74, 6),
        TCPd_target		(80, 6),
        temp_joint		(87, 6),
        robotmode		(95, 1),
        jointmode		(96, 6),
        safetymode		(97, 1),
        tcp_accel		(109, 3),
        speedscaling	(118, 1),
        prgstate		(132, 1);
        // More data points could be added if desired, by following above example and according to specification.

        private final int index;
        private final int count;
        RTinfo(int index, int count){
            this.index = index;
            this.count = count;
        }
        private int index() {return index;}
        private int count() {return count;}
    }

    /*****************************************************************
     * Methods for returning the relevant data to the calling classes
     *****************************************************************/

    /*
     * Example get function, to read actual joint positions as double[]
     */
    public double[] getActualJointPose(double []rtm){
        double[] val = new double[RTinfo.q_actual.count()];
        int i = 0;
        while (i < RTinfo.q_actual.count()){
            val[i] = rtm[RTinfo.q_actual.index()+i];
            ++i;
        }
        return val;
    }

    /*
     * Example get function, to read actual TCP position as double[]
     */
    public double[] getActualTcpPose(double[] rtm){
        double[] val = new double[RTinfo.TCP_actual.count()];
        int i = 0;
        while (i < RTinfo.TCP_actual.count()){
            val[i] = rtm[RTinfo.TCP_actual.index()+i];
            ++i;
        }
        return val;
    }

    /*
     * Example get function, to read joint temperatures as double[]
     */
    public double[] getJointTemperatures(double[] rtm){
        double[] val = new double[RTinfo.temp_joint.count()];
        int i = 0;
        while (i < RTinfo.temp_joint.count()){
            val[i] = rtm[RTinfo.temp_joint.index()+i];
            ++i;
        }
        return val;
    }

    /*
     * Example to read joint state as useful information
     */
    public String[] getJointStatus(double[] rtm){
        // Create a map binding message code to state message
        // According to Excel sheet sclient interface specification
        Map<Double, String> jointStates = new HashMap<Double, String>();
        jointStates.put((double) 236, "SHUTTING_DOWN");
        jointStates.put((double) 237, "DUAL_CALIB_MODE");
        jointStates.put((double) 238, "BACKDRIVE");
        jointStates.put((double) 239, "POWER_OFF");
        jointStates.put((double) 245, "NOT_RESPONDING");
        jointStates.put((double) 246, "MOTOR_INIT");
        jointStates.put((double) 247, "BOOTING");
        jointStates.put((double) 248, "DUAL_CALIB_ERROR");
        jointStates.put((double) 249, "BOOTLOADER");
        jointStates.put((double) 250, "CALIBRATION_MODE");
        jointStates.put((double) 252, "FAULT");
        jointStates.put((double) 253, "RUNNING");
        jointStates.put((double) 255, "IDLE");

        String[] val = new String[RTinfo.jointmode.count()];
        int i = 0;
        while (i < RTinfo.jointmode.count()){
            // Read the code for given joint
            double code = rtm[RTinfo.jointmode.index()+i];

            // Check if the key is known in the map
            if(jointStates.containsKey(code)){
                // Read corresponding message
                val[i] = jointStates.get(code);
            }
            else{
                // If unknown code, show "unknown"
                val[i] = "UNKNOWN_CODE";
            }
            ++i;
        }
        return val;
    }

    /*
     * Example to read the actual safety limited speed scaling in percent
     */
    public double getSpeedScaling(double[] rtm){
        double scaling = rtm[RTinfo.speedscaling.index()];
        return scaling * 100; // Converted to percent
    }
}