package kinectintruderalarm;
//
import java.io.BufferedReader;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.FileReader;
import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.io.PrintWriter;
import java.net.InetAddress;
import java.net.Socket;
import java.net.UnknownHostException;
import java.util.LinkedList;
import java.util.Scanner;
import java.util.logging.Level;
import java.util.logging.Logger;

/** KinectTCPClient contains all methods needed to communicate with a KinectTCP server.
 * It implements all commands defined in the documentation of KinectTCP. For more detailed
 * information, please see documentation of kinectTCP on https://sites.google.com/a/temple.edu/kinecttcp/
 * 
 * @author Rolf Lakaemper
 */
public class KinectTCPClientLIGHT extends Thread {

    private Socket socket = null;
    private OutputStream out;
    private InputStream in;
    public long timestamp;
    private int[] monoAudioBuffer;
    public int audioAmplitude;
    // ---------
    public static int CMD_HELLO = 0;
    public static int CMD_NUMDEVICES = 1;
    public static int CMD_STARTKINECT = 2;
    public static int CMD_STARTKINECTDEFAULT = 3;
    public static int CMD_ENABLEDEPTHXYZINDEX = 4;
    public static int CMD_DISABLEDEPTHXYZINDEX = 5;
    public static int CMD_ENABLEDEPTHXYZFULL = 6;
    public static int CMD_DISABLEDEPTHXYZFULL = 7;
    //
    public static int CMD_INITRGB640X480 = 20;
    public static int CMD_INITRGB1280X1024 = 21;
    public static int CMD_GETRGBMODE = 22;
    public static int CMD_INITDEPTH80X60 = 23;
    public static int CMD_INITDEPTH320X240 = 24;
    public static int CMD_INITDEPTH640X480 = 25;
    public static int CMD_GETDEPTHMODE = 26;
    public static int CMD_NORGB = 27;
    public static int CMD_NODEPTH = 28;
    //
    public static int CMD_READRGB = 30;
    public static int CMD_READDEPTH = 31;
    public static int CMD_READDEPTHXYZ = 32;
    public static int CMD_READDEPTHRGB = 33;
    public static int CMD_READDEPTHXYZRGB = 34;
    public static int CMD_READSKELETON = 35;
    public static int CMD_STREAMAUDIOSTART = 38;
    public static int CMD_STREAMAUDIOSTOP = 39;

    // ------------------------------------------------------------------------
    // Constructor, create socket connection
    /** Constructor, creates socket connection
     * 
     * @param ipaddress: ipaddress of kinectTCP server (kinectTCP server displays its ip-address)
     * @param port: port of kinectTCP server (kinectTCP server displays its port).
     */
    public KinectTCPClientLIGHT(String ipaddress, int port) {
        System.out.println("kinectTCP JAVA Client V1.2");
        try {
            socket = new Socket(InetAddress.getByName(ipaddress), port);
            out = socket.getOutputStream();
            in = socket.getInputStream();
        } catch (UnknownHostException e) {
            System.out.println("Unknown host");
            System.exit(1);
        } catch (IOException e) {
            System.out.println("No I/O. Is a Kinect connected?");
            System.exit(1);
        }
    }

    /**
     * Default constructor, connects to localhost, port 8001 
     */
    public KinectTCPClientLIGHT() {
        this("127.0.0.1", 8001);
    }

    // ------------------------------------------------------------------------
    //
    /**
     * start routine for thread. In case the client is used as audio client, this routine will
     * start the audio streaming
     */
    public void run() {
//        streamAudio();
    }

    /**Read byte stream and return signed bytes (=integer)
     * timestamp is written into field "timestamp",
     * the return array contains all data submitted after timestamp.
     *
     * @param in
     * @return data stream int[]
     */
    private int[] readData(InputStream in) {
        int l0, l1, l2, l3;
        try {
            // determine length of stream
            l0 = in.read();
            l1 = in.read();
            l2 = in.read();
            l3 = in.read();
            int length = l0 + (l1 << 8) + (l2 << 16) + (l3 << 24);
            length = length - 4;    // subtract timestamp

            // read timestamp
            l0 = in.read();
            if (l0 < 0) {
                l0 = 256 + l0;
            }
            l1 = in.read();
            if (l1 < 0) {
                l1 = 256 + l1;
            }
            l2 = in.read();
            if (l2 < 0) {
                l2 = 256 + l2;
            }
            l3 = in.read();
            if (l3 < 0) {
                l3 = 256 + l3;
            }
            timestamp = l0 | (l1 << 8) | (l2 << 16) | (l3 << 24);

            // read bytestream
            byte[] dataB = new byte[length];
            int totalRead = 0;
            while (totalRead < length) {
                totalRead += in.read(dataB, totalRead, length - totalRead);
            }

            // convert to unsigned (we hate JAVA for this)
            int[] data = new int[length];
            for (int i = 0; i < length; i++) {
                if (dataB[i] < 0) {
                    data[i] = 256 + dataB[i];
                } else {
                    data[i] = dataB[i];
                }
            }

            return (data);
        } catch (Exception e) {
            System.out.println("Error in readData: " + e.getMessage());
            return (null);
        }
    }

    /**sendCommand: raw command routine to send a single byte command that expects
     * a single byte answer. For most of these commands, there is a specific routine
     * (e.g. sayHello, getRGBMode,...)
     *
     * @param command
     * @return answer from server
     */
    public int sendCommand(int command) {
        int a = 0;
        try {
            out.write(command);
            out.flush();
            a = in.read();
        } catch (Exception e) {
            System.out.println("Error in command: " + command + " : " + e.toString());
        }
        return (a);
    }

    // ------------------------------------------------------------------------
    // send command: say Hello
    /**
     * Send command: Hello.
     * @return Answers (int) from server. KinectTCP defines a valid answer as "214" (= -42).
     */
    public int sayHello() {
        int a = 0;
        try {
            out.write(0);
            out.flush();
            a = in.read();
            System.out.println("The server says hi (" + a + ").");
        } catch (Exception e) {
            System.out.println("Error in Hello Command: " + e.toString());
        }
        return (a);
    }

    // ------------------------------------------------------------------------
    /** Send command: getRGBMode.
     * 
     * @return 0 = noRGB, 3 = 640x480, 4 = 1280x1024
     */
    public int getRGBMode() {
        int a = 0;
        try {
            out.write(CMD_GETRGBMODE);
            out.flush();
            a = in.read();
        } catch (Exception e) {
            System.out.println("Error: " + e.toString());
        }
        return (a);
    }

    // ------------------------------------------------------------------------
    /** Send command: getDepthMode
     * 
     * @return 0 = no depth, 1 = 80x60, 2= 320x240, 3=640x480
     */
    public int getDepthMode() {
        int a = 0;
        try {
            out.write(CMD_GETDEPTHMODE);
            out.flush();
            a = in.read();
        } catch (Exception e) {
            System.out.println("Error: " + e.toString());
        }
        return (a);
    }

    // ------------------------------------------------------------------------
    /** Send command: enableDepthXYZIndex
     *
     * @return 1 if successful, 0 otherwise.
     */
    public int enableDepthXYZIndex() {
        int a = 0;
        try {
            out.write(CMD_ENABLEDEPTHXYZINDEX);
            out.flush();
            a = in.read();
        } catch (Exception e) {
            System.out.println("Error: " + e.toString());
        }
        return (a);
    }

    // ------------------------------------------------------------------------
    /** Send command: initRGB
     * 
     * @param resX= 640 | 1280 | 0
     * @return 1 if successful, 0 otherwise. A "0" would be returned from 
     * kinectTCP if the server is already initialized/started.
     */
    public int initRGB(int resX) {
        int cmd;
        if (resX == 640) {
            cmd = CMD_INITRGB640X480;
        } else if (resX == 1280) {
            cmd = CMD_INITRGB1280X1024;
        } else if (resX == 0) {
            cmd = CMD_NORGB;
        } else {
            return (0);
        }

        try {
            out.write(cmd);
            out.flush();
            int a = in.read();
            return (a);
        } catch (Exception e) {
            System.out.println("Error in Command: " + e.toString());
            return (0);
        }
    }

    // ------------------------------------------------------------------------
    /** Send command: initDepth
     *
     * @param resX= 640 | 320 | 80 | 0
     * @return 1 if successful, 0 otherwise. A "0" would be returned from
     * kinectTCP if the server is already initialized/started.
     */
    public int initDepth(int resX) {
        int cmd;
        if (resX == 640) {
            cmd = CMD_INITDEPTH640X480;
        } else if (resX == 320) {
            cmd = CMD_INITDEPTH320X240;
        } else if (resX == 80) {
            cmd = CMD_INITDEPTH80X60;
        } else if (resX == 0) {
            cmd = CMD_NODEPTH;
        } else {
            return (0);
        }

        try {
            out.write(cmd);
            out.flush();
            int a = in.read();
            return (a);
        } catch (Exception e) {
            System.out.println("Error in Command: " + e.toString());
            return (0);
        }
    }

    // ------------------------------------------------------------------------
    // command: start Kinect
    /** Send command: start kinectTCP
     *
     * @param deflt (boolean), true: starts kinectTCP with default settings,
     * otherwise kinectTCP will be started with settings submitted by initDepth and initRGB
     * @return 1 on success, 0 otherwise
     */
    public int startKinect(boolean deflt) {
        int cmd = CMD_STARTKINECT;
        if (deflt) {
            cmd = CMD_STARTKINECTDEFAULT;
        }
        try {
            out.write(cmd);
            out.flush();
            int a = in.read();
            return (a);
        } catch (Exception e) {
            System.out.println("Error in Command: " + e.toString());
            return (0);
        }
    }

    // ------------------------------------------------------------------------
    /** Read Depth Image. 
     * 
     * @return Returns a 2D array. First dimension contains distance, second 
     * dimension contains player index.
     */
    public int[][] readDepth() {
        try {
            out.write(CMD_READDEPTH);
            out.flush();
            int[] buffer = readData(in);
            // convert to color
            int[][] depth = new int[buffer.length / 2][2];
            for (int i = 0; i < depth.length; i++) {
                depth[i][0] = buffer[2 * i] + ((buffer[2 * i + 1] & 0x1f) * 256);
                depth[i][1] = (buffer[2 * i + 1] & 0xe0) >> 5;
            }
            return depth;
        } catch (IOException ex) {
            return (null);
        }
    }

    // ------------------------------------------------------------------------
    // command: READ DepthXYZ
    // depth as point cloud, [][0-2] = x,y,z
    /** Read Depth Point Cloud. kinectTCP only submits points which have a valid distance (>0),
     * hence the returned data stream has varying size.
     * This is the point cloud WITHOUT player index. For index see readDepthXYZIndex().
     *
     * @return 2D array of point coordinates. [][0-2] = x,y,z
     */
    public int[][] readDepthXYZ() {
        try {
            out.write(CMD_READDEPTHXYZ);
            out.flush();
            int[] data = readData(in);
            int[][] depthXYZ = new int[data.length / 6][3];
            int indexS = 0;
            int indexT = 0;
            while (indexS < data.length) {
                depthXYZ[indexT][0] = data[indexS] + (data[indexS + 1] << 8) - 32768; //x
                depthXYZ[indexT][1] = data[indexS + 2] + (data[indexS + 3] << 8) - 32768; //y
                depthXYZ[indexT][2] = data[indexS + 4] + (data[indexS + 5] << 8) - 32768; //z
                indexS += 6;
                indexT += 1;
            }
            return depthXYZ;
        } catch (IOException ex) {
            return (null);
        }
    }

    // ------------------------------------------------------------------------
    /** command: READ DepthXYZIndex.
     * depth as point cloud.
     * attention! this enables the index-reading (command 4), yet does NOT disable it!
     *
     * @return 2D array, depth in [][0-2]  = x,y,z, index in [][3]
     */
    public int[][] readDepthXYZIndex() {
        try {
            out.write(CMD_ENABLEDEPTHXYZINDEX); // (re-)enables index
            out.flush();
            int a = in.read();

            out.write(CMD_READDEPTHXYZ);
            out.flush();
            int[] data = readData(in);
            int[][] depthXYZ = new int[data.length / 6][4];  //3*2 bytes => single array-index
            int indexS = 0;
            int indexT = 0;
            while (indexS < data.length) {
                depthXYZ[indexT][0] = data[indexS] + (data[indexS + 1] << 8) - 32768;   //x
                depthXYZ[indexT][1] = data[indexS + 2] + (data[indexS + 3] << 8) - 32768; //y
                depthXYZ[indexT][2] = data[indexS + 4] + ((data[indexS + 5] & 0x1f) << 8);//z
                depthXYZ[indexT][3] = ((data[indexS + 5] & 0xe0) >> 5);                 //index
                indexS += 6;
                indexT += 1;
            }
            return depthXYZ;
        } catch (IOException ex) {
            return (null);
        }
    }

    // ------------------------------------------------------------------------
    /** command: READ RGB
     *
     * @return rgb as color image, alpha value = 0xff
     */
    public int[] readRGBImage() {
        try {
            out.write(CMD_READRGB);
            out.flush();
            int[] data = readData(in);
            int[] rgb = new int[data.length / 4];
            int index = 0;
            int count = 0;
            for (int i = 0; i < data.length; i += 4) {
                int b = data[i];
                int g = data[i + 1];
                int r = data[i + 2];
                if (r < 0) {
                    r = 256 + r;
                }
                if (g < 0) {
                    g = 256 + g;
                }
                if (b < 0) {
                    b = 256 + b;
                }
                rgb[count++] = (0xff << 24 | r << 16 | g << 8 | b);
            }
            return rgb;
        } catch (IOException ex) {
            return (null);
        }
    }

    // ------------------------------------------------------------------------
    /** Command Read Skeleton.
     * Usually followed by "get joint positions" for more convenient access.
     *
     * @return raw skeleton data int[]. See documentation of kinectTCP.
     */
    public int[] readSkeleton() {
        try {
            out.write(CMD_READSKELETON);
            out.flush();
            int[] data = readData(in);
            return data;
        } catch (Exception ex) {
            System.out.println("Error in readSkeleton " + ex.toString());
            return (null);
        }
    }

    // ------------------------------------------------------------------------
    /** getJointPositions: convenience routine to extract joints from skeleton data
     *
     * @param skel: skeleton data array, as obtained from readSkeleton()
     * @param skelIndex: index of skeleton (1 or 2)
     * @return
     */
    public static int[][] getJointPositions(int[] skel, int skelIndex) {

        if (skel[0] < skelIndex) {
            return (null);
        }

        int dataIndex = 17 + 149 * (skelIndex - 1) + 9;
        int[][] joints = new int[20][4]; // tracked x y z
        for (int jIndex = 0; jIndex < 20; jIndex++) {
            joints[jIndex][0] = skel[dataIndex++];
            joints[jIndex][1] = (skel[dataIndex] | skel[dataIndex + 1] * 256) - 32768;
            dataIndex += 2;
            joints[jIndex][2] = (skel[dataIndex] | skel[dataIndex + 1] * 256) - 32768;
            dataIndex += 2;
            joints[jIndex][3] = (skel[dataIndex] | skel[dataIndex + 1] * 256) - 32768;
            dataIndex += 2;
        }
        return (joints);
    }

//    // ------------------------------------------------------------------------
//    /** Stream Audio: an example method for audio handling.
//     * Opens a JFrame and shows the amplitude of the audio data.
//     * This method is called by the 'run()' method of the thread.
//     *
//     */
//    public void streamAudio() {
//        JFrame jf = new JFrame();
//        jf.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
//        AudioPanel ap = new AudioPanel();
//        jf.getContentPane().add(ap);
//        jf.pack();
//        jf.setVisible(true);
//
//        int buffersize = 512;
//        try {
//            flushTCPin();
//            out.write(CMD_STREAMAUDIOSTART);
//            out.flush();
//
//
//            byte[] audioBuffer = new byte[buffersize];
//            int[] unsignedBuffer = new int[buffersize];
//            monoAudioBuffer = new int[buffersize / 4];
//            while (true) {
//                int l = in.read(audioBuffer, 0, buffersize);
//                for (int i = 0; i < l; i++) {
//                    if (audioBuffer[i] < 0) {
//                        unsignedBuffer[i] = audioBuffer[i] + 256;
//                    } else {
//                        unsignedBuffer[i] = audioBuffer[i];
//                    }
//                }
//
//                for (int i = 0; i < buffersize; i += 2) {
//                    unsignedBuffer[i] = unsignedBuffer[i] + 256 * unsignedBuffer[i + 1];
//                    if (unsignedBuffer[i] > 32767) {
//                        unsignedBuffer[i] = unsignedBuffer[i] - 65536;
//                    }
//                }
//
//                audioAmplitude = 0;
//                for (int i = 0; i < buffersize / 4; i++) {
//                    monoAudioBuffer[i] = (unsignedBuffer[4 * i] + unsignedBuffer[4 * i + 2]) / 2;
//                    audioAmplitude += Math.abs(monoAudioBuffer[i]);
//                }
//
//                System.arraycopy(monoAudioBuffer, 0, ap.buffer, 0, monoAudioBuffer.length);
//                ap.repaint();
//            }
//        } catch (Exception e) {
//            System.out.println(e.getMessage());
//        }
//    }
    // -------------------------------------------------------------------------
    /**
     * Command to stop audio streaming.
     */
    public void stopAudio() {
        try {
            out.write(CMD_STREAMAUDIOSTOP);
            out.flush();
            flushTCPin();
        } catch (IOException ex) {
            Logger.getLogger(KinectTCPClientLIGHT.class.getName()).log(Level.SEVERE, null, ex);
        }
    }

    // -------------------------------------------------------------------------
    /**
     * empty buffer from audio data before return
     */
    private void flushTCPin() {
        byte[] audioBuffer = new byte[4096];
        while (true) {
            int l = 0;
            try {
                l = in.available();
            } catch (IOException ex) {
                Logger.getLogger(KinectTCPClientLIGHT.class.getName()).log(Level.SEVERE, null, ex);
            }
            if (l > 0) {
                try {

                    int ln = in.read(audioBuffer, 0, Math.min(l, audioBuffer.length));
                    System.out.println("Read " + ln);
                } catch (IOException ex) {
                    Logger.getLogger(KinectTCPClientLIGHT.class.getName()).log(Level.SEVERE, null, ex);
                }
            } else {
                break;
            }
        }
    }

    // ------------------------------------------------------------------------
    /** MULTIBYTECOMMAND: write to communication channel
     *
     * @param channel (int, 0..9)
     * @param data (byte[], max. length: 65535)
     * @return 1 on success, 0 otherwise.
     */
    public int writeDataToCommunactionChannel(int channel, byte[] data) {
        try {
            out.write(255);                                                     // multibyte command
            int length = data.length + 2;                                       // length
            out.write(length & 0xff);                                           // lowbyte
            out.write(length >> 8);                                             // highbyte
            out.write(0);                                                       // command: set channel data
            out.write(channel);                                                 // channel
            out.write(data);                                                    // data
            out.flush();
            return (in.read());                                                 // answer: 1=ok, 0=trouble
        } catch (IOException ex) {
            return (0);
        }
    }

    // ------------------------------------------------------------------------
    /** MULTIBYTECOMMAND: read data from communication channel
     *
     * @param channel (int, 0..9)
     * @return int[] data, or null if not successful.
     */
    public int[] readDataFromCommunactionChannel(int channel) {
        try {
            out.write(255);                                                     // multibyte command
            int length = 2;                                                     // length
            out.write(length & 0xff);                                           // lowbyte
            out.write(length >> 8);                                               // highbyte
            out.write(1);                                                       // command: read channel data
            out.write(channel);                                                 // channel
            out.flush();
            int[] data = readData(in);
            return (data);                                                       // answer: data
        } catch (IOException ex) {
            return (null);
        }
    }

    // ------------------------------------------------------------------------
    /** for debugging: write 1D array to a data file
     *
     * @param data int[]
     * @param filename
     */
    public static void writeArrayToASCIIFile(int[] data, String filename) {
        System.out.println("Attention! Writing File! This should only be done for debugging purposes.");
        FileOutputStream outStream = null;
        try {
            File myFile = new File(filename);
            outStream = new FileOutputStream(myFile);
            PrintWriter out = new PrintWriter(outStream);

            int i;
            for (i = 0; i < data.length; i++) {
                out.print(data[i] + ", ");
            }
            out.flush();
        } catch (FileNotFoundException ex) {
            Logger.getLogger(Utils.class.getName()).log(Level.SEVERE, null, ex);
        } finally {
            try {

                outStream.close();
            } catch (IOException ex) {
                Logger.getLogger(Utils.class.getName()).log(Level.SEVERE, null, ex);
            }
        }
    }

    // ----------------------------------------------
    public static void write2DArrayToASCIIFile(int[][] data, String filename) {
        System.out.println("Attention! Writing File! This should only be done for debugging purposes.");
        FileOutputStream outStream = null;
        try {
            File myFile = new File(filename);
            outStream = new FileOutputStream(myFile);
            PrintWriter out = new PrintWriter(outStream);

            int i;
            for (i = 0; i < data.length; i++) {
                for (int j = 0; j < data[i].length; j++) {
                    out.print(data[i][j] + " ");
                }
                out.println("");
            }
            out.flush();
        } catch (FileNotFoundException ex) {
            Logger.getLogger(Utils.class.getName()).log(Level.SEVERE, null, ex);
        } finally {
            try {

                outStream.close();
            } catch (IOException ex) {
                Logger.getLogger(Utils.class.getName()).log(Level.SEVERE, null, ex);
            }
        }
    }
    // ----------------------------------------------
    public static int[][] readDepthFileTo2DArray(String filename) {
        int[][] data = null;
        LinkedList<Integer> list = new LinkedList<Integer>();
        //System.out.println("Attention! Reading File! This should only be done for debugging purposes.");
        FileInputStream inStream = null;
        Scanner s = null;
        int count = 0;
        try {

            s = new Scanner(
                    new BufferedReader(
                    new FileReader(filename)));

            while (s.hasNext()) {
                String number = s.next();
                //System.out.println(number);
                if (count%4 != 3){
                    list.addLast(new Integer(number));
                }
                count++;
            }
        } catch (Exception e) {
        } finally {
            if (s != null) {
                s.close();
            }
        }
        if (list.size()>0){
            data = new int[list.size()/3][3];
            count = 0;
            for (Integer i:list){
                int v = i.intValue();
                data[count/3][count%3] = v;
                count++;
            }
        }
        return(data);
    }
}
