package kinectintruderalarm;

import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.event.MouseEvent;
import java.awt.event.MouseListener;
import java.awt.event.MouseMotionListener;
import java.util.Comparator;
import javax.swing.JPanel;

/**Depth Panel, central projection depth view of point cloud.
 * Rotation and zoom by mouse.
 *
 */
public class KoDepthPanelLIGHT extends JPanel implements MouseMotionListener, MouseListener, Comparator {

    public static Color backgroundColor = Color.black;
    public boolean painting = false;
    private boolean mouseEntered = false;
    private boolean mousePressed = false;
    private int mousebutton = 0;
    private int mouseLastX, mouseLastY;
    int sizeX, sizeY;
    double[][] projection = null;
    public int[][] xyz = null;
    int maxX, maxY, minX, minY;
    int[] rgb = null;
    double scale;
    double PLANEDISTANCE = 300.0;                                           // distance to projection plane
    double OPENING = PLANEDISTANCE * Math.tan(28.5 / 180 * Math.PI);              // factor to get a 57 degree lens
    double RCX = 0.0;                                                       // rotation center
    double RCY = 0.0;
    double RCZ = 1800.0;
    double rotY = 45.0 * Math.PI / 180.0;                                       // Yaw
    double rotX = 0.0 * Math.PI / 180.0;                                        // Pitch
    double TRANSLATEDISTANCE = 0;
    double[][] rotMatrix;
    public boolean labeled = false;                                                     // if true, the 4th index in xyz is a label, not a person index.
    Color[] colors;
    final int colormax = 1000;

    public KoDepthPanelLIGHT(int sx, int sy) {
        super();
        sizeX = sx;
        sizeY = sy;
        setPreferredSize(new Dimension(sx, sy));
        rotMatrix = new double[3][3];
        determineRotMatrix();
        this.addMouseListener(this);
        this.addMouseMotionListener(this);

        // in case points are labeled, use this random color table
        colors = new Color[colormax];
        for (int i = 0; i < colormax; i++) {
            int r = (int) (Math.random() * 200 + 55);
            int g = (int) (Math.random() * 200 + 55);
            int b = (int) (Math.random() * 200 + 55);
            colors[i] = new Color(r, g, b);
        }
    }

    // --------------------------------------------------------------------
    public void setViewParameters(double irX, double irY, double iDist) {
        rotX = irX / 180 * Math.PI;
        rotY = irY / 180 * Math.PI;
        TRANSLATEDISTANCE = iDist;
        this.determineRotMatrix();
    }
    // --------------------------------------------------------------------

    private void determineRotMatrix() {
        double[][] ry = new double[3][3];
        double[][] rx = new double[3][3];

        double cs = Math.cos(rotX);
        double sn = Math.sin(rotX);
        // rot around X
        rx[0][0] = 1.0;
        rx[1][1] = cs;
        rx[1][2] = -sn;
        rx[2][1] = sn;
        rx[2][2] = cs;

        // rot around Y
        cs = Math.cos(rotY);
        sn = Math.sin(rotY);
        ry[1][1] = 1.0;
        ry[0][0] = cs;
        ry[0][2] = -sn;
        ry[2][0] = sn;
        ry[2][2] = cs;

        // matrix multiplication
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                rotMatrix[i][j] = 0.0;
                for (int k = 0; k < 3; k++) {
                    rotMatrix[i][j] += rx[i][k] * ry[k][j];
                }
            }
        }
    }

    // --------------------------------------------------------------------
    private int[] translate(int[] vec, int factor) {
        vec[0] -= RCX * factor;
        vec[1] -= RCY * factor;
        vec[2] -= RCZ * factor;
        return (vec);
    }
    // --------------------------------------------------------------------

    private int[] rotate(int[] vec) {

        if (rotMatrix != null) {
            double[] result = new double[3];
            for (int i = 0; i < 3; i++) {
                for (int j = 0; j < 3; j++) {
                    result[i] += rotMatrix[i][j] * vec[j];
                }
            }
            vec[0] = (int) (result[0]);
            vec[1] = (int) (result[1]);
            vec[2] = (int) (result[2]);
        }
        return (vec);
    }

    // --------------------------------------------------------------------
    public void paintComponent(Graphics g) {
        painting = true;    // semaphor (this is bad...)

        // short flash when mouse enters
        if (!mouseEntered) {
            g.setColor(backgroundColor);
        } else {
            g.setColor(new Color(30, 100, 0));
            mouseEntered = false;
        }
        g.fillRect(0, 0, sizeX, sizeY);
        g.setColor(Color.white);

        //
        if (xyz == null) {
            g.setColor(Color.red);
            g.drawString("No Point Cloud", sizeX / 2, sizeY / 2);
        } else {
            // transform
            // (a) rotate around RCX, RCY, RCZ
            for (int i = 0; i < xyz.length; i++) {
                xyz[i] = translate(xyz[i], 1);
                xyz[i] = rotate(xyz[i]);
                xyz[i] = translate(xyz[i], -1);
            }

            // add TRANSLATEDISTANCE to z

            for (int i = 0; i < xyz.length; i++) {
                xyz[i][2] += TRANSLATEDISTANCE;
            }

            // sorting
            // Arrays.sort(xyz, this);

            // project
            projection = new double[xyz.length][3];
            int scrRadX = sizeX / 2;
            int scrRadY = sizeY / 2;
            for (int index = 0; index < xyz.length; index++) {
                if (xyz[index][2] > PLANEDISTANCE) {
                    // projection
                    double x = (int) Math.round(PLANEDISTANCE * (double) xyz[index][0] / (double) xyz[index][2]);
                    double y = (int) Math.round(PLANEDISTANCE * (double) xyz[index][1] / (double) xyz[index][2]);
                    // opening angle 53 degrees
                    x = x * scrRadX / OPENING + scrRadX;
                    y = y * scrRadX / OPENING + scrRadY;
                    projection[index][0] = x;
                    projection[index][1] = y;

                    // distance => grey (for fog)
                    projection[index][2] = (int) (Math.sqrt(xyz[index][0] * xyz[index][0] + xyz[index][1] * xyz[index][1] + xyz[index][2] * xyz[index][2]));
                    projection[index][2] = (8000 - projection[index][2]) * 256 / (8000 - PLANEDISTANCE);
                    projection[index][2] = Math.min(255, projection[index][2]);
                    projection[index][2] = Math.max(20, projection[index][2]);

                }
            }


            // plot
            for (int index = 0; index < xyz.length; index++) {
                int c = (int) projection[index][2];

                // set color
                if (labeled) {
                    // labeled
                    int colIndex = xyz[index][3] % colors.length;
                    g.setColor(colors[colIndex]);
                } else {
                    // unlabeled with person index
                    if (xyz[0].length == 4 && xyz[index][3] > 0) {
                        g.setColor(new Color(c, 0, 0));
                    } else {
                        // unlabeled without index
                        g.setColor(new Color(c, c, c));
                    }
                }
                int ix = (int) Math.round(projection[index][0]);
                int iy = (int) Math.round(projection[index][1]);
                g.drawLine(ix, iy, ix, iy);
            }
        }
        painting = false;
        g.setColor(Color.black);
        g.fillRect(0, 0, 75, 15);
        g.setColor(Color.green);
        g.drawString("Point Cloud", 5, 10);
    }

    public void mouseDragged(MouseEvent e) {
        double dx = e.getX() - mouseLastX;
        double dy = e.getY() - mouseLastY;
        mouseLastX = e.getX();
        mouseLastY = e.getY();

        if (mousebutton == 1) {
            rotX += dy / 100;
            rotY += dx / 100;
            this.determineRotMatrix();
        } else {
            TRANSLATEDISTANCE += dy * 10;
        }

    }

    public void mouseMoved(MouseEvent e) {
    }

    public void mouseClicked(MouseEvent e) {
    }

    public void mousePressed(MouseEvent e) {
        this.mousePressed = true;
        mouseLastX = e.getX();
        mouseLastY = e.getY();
        mousebutton = e.getButton();
    }

    public void mouseReleased(MouseEvent e) {
        mousebutton = 0;
    }

    public void mouseEntered(MouseEvent e) {
        mouseEntered = true;
    }

    public void mouseExited(MouseEvent e) {
    }

    public int compare(Object o1, Object o2) {
        if (((int[]) o1)[2] > ((int[]) o2)[2]) {
            return (-1);
        } else {
            return (1);
        }
    }
}
