/*[Update:[Tue Sep 11 IST 2018]]
**Mon Sep 10 2018
   Added a working sendToAoI()
 */

/*
  How to use this program:
    Step 0: 
       First you need a photo that shows a rectangle (e.g., a tabletop) with perspective distortion 
       (i.e., opposite sides not parallel). Visualise this is as the rectangle [0,1]x[0,z] in the 
       positive quadrant of the x-z plane. Keep in mind that the choice of x- and z-axes determines
       the orientation of the y-axis.
    Step 1:
       Start this program as 
       java CameraMatcher5 <img file name>
    Step 2:
      Click on the vertices of the rectangle in this order: (0,0), (1,0), (1,z), (0,z). The software
      will display th estimated handles and also print the camera position and orientation for AoI.
    Step 3: 
      Open AoI. Select the camera. Open its Properties panel. With the window of the CameraMatcher5 
      software active take the mouse over the X textfield under Position (shown in red in [[./aoi.png]]).
      Now press 'a'. This should copy the appropriate values to the AoI. 
    Step 4:
      Render. If the object is not visible, then you may need to increase the field of view. You should not 
      need to do this if the rectangle is near the centre of the image.
 */
import java.awt.*;
import javax.swing.*;
import java.awt.event.*;
import javax.imageio.*;
import java.awt.image.*;
import java.io.*;
import java.awt.datatransfer.*;

public class CameraMatcher5 extends JPanel 
    implements MouseListener, MouseMotionListener, KeyListener, ClipboardOwner {

    enum StateValue {START, INITED, ADJUST_X, ADJUST_Z};
    StateValue state = StateValue.START;
    int n = 0;
    double oldXX1, oldYY1, oldXX3, oldYY3;
    double[] u1, u2, u3;
    
    static final double factor = 180/Math.PI;
    public void mouseClicked(MouseEvent me) {
        //System.err.println("Clicked!");
        if(state != StateValue.START) return;
        
        pp[n] = me.getX();
        qq[n] = me.getY();
        n++;
        if(n>3) {
            computeAndShow();
            state = StateValue.INITED;
        }
        repaint();
    }
    
    Point aoi;
    public void keyTyped(KeyEvent ke) {
        char c = ke.getKeyChar();
        base = MouseInfo.getPointerInfo().getLocation();
        aoi.x = base.x;
        aoi.y= base.y;
        Point tmp = getLocationOnScreen();
        base.x -= tmp.x;
        base.y -= tmp.y;
        switch(c) {
        case 'x': 
            if(state == StateValue.ADJUST_X) {
                oldXX1 = xx1;
                oldYY1 = yy1;
                state = StateValue.INITED;
            }
            else if(state == StateValue.INITED) {
                state = StateValue.ADJUST_X;
            }
            break;
        case 'z': 
            if(state == StateValue.ADJUST_Z) {
                oldXX3 = xx3;
                oldYY3 = yy3;
                state = StateValue.INITED;
            }
            else if(state == StateValue.INITED) {
                state = StateValue.ADJUST_Z;
            }
            break;
           
        case 'a':
            if(state == StateValue.INITED) {
                Thread t = new Thread(new Runnable() {
                        public void run() {
                            sendToAOI();
                        }
                    });
                t.start();
            }
            break;
        case 'r':
            state = StateValue.START;
            n = 0;
            repaint();
            break;
        }
    }

    double val[];
    private BufferedImage img;
    private int iw, ih;
    JLabel msg;
    Robot robot;
    //Clipper clpr;
    Clipboard cb;
    public CameraMatcher5(JLabel msg, String filename) {
        val = new double[6];
        aoi = new Point();
        Toolkit tools = Toolkit.getDefaultToolkit();
        cb = tools.getSystemClipboard();
      
        try {
	    robot = new Robot();
	}
	catch(AWTException awtex) {}
        
        //clpr = new Clipper();

        this.msg = msg;
        try {
            img = ImageIO.read(new File(filename));
            ih = img.getHeight()/2;
            iw = img.getWidth()/2;
            setPreferredSize(new Dimension(2*iw,2*ih));
        }
        catch(Exception ex) {
            ex.printStackTrace();
        }
        

        addMouseListener(this);
        addMouseMotionListener(this);
        pp = new double[4];
        qq = new double[4];
        p = new double[4];
        q = new double[4];
    }

    int h,w;

    double pp[],qq[],p[],q[];
    Point base;

    public void keyPressed(KeyEvent ke) {}
    public void keyReleased(KeyEvent ke) {}
    
    void computeAndShow() {
        computeVP();
        oldXX1 = xx1; 
        oldYY1 = yy1;
        oldXX3 = xx3; 
        oldYY3 = yy3;
        computeScaleRot();
        computeShift();
        dump();
        //reconstruct();
        state = StateValue.ADJUST_X;
        repaint();
    }

    Cursor cross = new Cursor(Cursor.CROSSHAIR_CURSOR);
    Cursor usual = new Cursor(Cursor.DEFAULT_CURSOR);
    public void mousePressed(MouseEvent me) {}
    public void mouseReleased(MouseEvent me) {}
    public void mouseEntered(MouseEvent me) {
        setCursor(cross);
    }
    public void mouseExited(MouseEvent me) {
        setCursor(usual);
    }

 
    public void mouseMoved(MouseEvent me) {
        if(state!=StateValue.ADJUST_X && state!=StateValue.ADJUST_Z) return;
        if(state==StateValue.ADJUST_X) {
            xx1 = oldXX1 + (me.getX()-base.x);
            yy1 = oldYY1 + (base.y-me.getY());
        }
        else {
            xx3 = oldXX3 + (me.getX()-base.x);
            yy3 = oldYY3 + (base.y-me.getY());
        }

        computeScaleRot();
        computeShift();
        //reconstruct();
        dump();
        repaint();
    }
    public void mouseDragged(MouseEvent me) { }
    double[] xtip, ytip, ztip;
    public void paintComponent(Graphics g) {
        super.paintComponent(g);
        g.drawImage(img,0,0,this);
        
        g.setColor(Color.RED);
        for(int i=0;i<n;i++)
            g.fillOval((int)pp[i]-3,(int)qq[i]-3,7,7);

        if(state==StateValue.START) return;

        // {{{ Draw VP connectors
        g.setColor(Color.BLUE);
        for(int i=0;i<4;i++) 
            g.drawLine((int)pp[i],(int)qq[i],(int)xx1+iw,ih-(int)yy1);

        g.setColor(Color.GREEN);

        for(int i=0;i<4;i++) 
            g.drawLine((int)pp[i],(int)qq[i],(int)xx3+iw,ih-(int)yy3);
        // }}}

        
        // {{{ Draw estimated axes
        g.setColor(Color.MAGENTA);
        Point temp = proj(xtip);
        g.fillOval(temp.x-3,temp.y-3,7,7);
        g.drawLine((int)pp[0],(int)qq[0],temp.x,temp.y);
        g.setColor(Color.PINK);
        temp = proj(ytip);
        g.fillOval(temp.x-3,temp.y-3,7,7);
        g.drawLine((int)pp[0],(int)qq[0],temp.x,temp.y);
        g.setColor(Color.YELLOW);
        temp = proj(ztip);
        g.fillOval(temp.x-3,temp.y-3,7,7);
        g.drawLine((int)pp[0],(int)qq[0],temp.x,temp.y);
        // }}}

    }
    double[] ztip2;
    Point proj(double vec[]) {
            int ytipx = iw-(int)(sigma*vec[0]/vec[2]);
            int ytipy = ih+(int)(sigma*vec[1]/vec[2]);
            return new Point(ytipx,ytipy);
    }
    boolean obtuse;
    Color col[] = {Color.BLACK, Color.BLUE, Color.YELLOW, Color.MAGENTA, Color.RED};

    double xx1, yy1, xx3, yy3, x1, y1, x3, y3;

    void computeVP() {
        for(int i=0;i<4;i++) {
            p[i] = pp[i]-iw;
            q[i] = ih - qq[i];
            //System.err.format("%d:[%.2f, %.2f]",i,p[i],q[i]);
        }
        System.err.println();
        double m01 = (q[0]-q[1])/(p[0]-p[1]);
        double c01 = q[0]-m01*p[0];
        double m23 = (q[2]-q[3])/(p[2]-p[3]);
        double c23 = q[2]-m23*p[2];

        xx1 = (c23-c01)/(m01-m23);
        yy1 = c01+m01*xx1;

        double m03 = (q[0]-q[3])/(p[0]-p[3]);
        double c03 = q[0]-m03*p[0];
        double m12 = (q[1]-q[2])/(p[1]-p[2]);
        double c12 = q[1]-m12*p[1];

        xx3 = (c12-c03)/(m03-m12);
        yy3 = c03+m03*xx3;
        //System.err.format("\nX:[%.2f, %.2f], Z:[%.2f, %.2f]\n",xx1,yy1,xx3,yy3);
    }

    /*
#(%i10) R1.R2.R3;
#                [      c2 c3            - c2 s3         s2    ]
#                [                                             ]
#(%o10)          [ c1 s3 + c3 s1 s2  c1 c3 - s1 s2 s3  - c2 s1 ]
#                [                                             ]
#                [ s1 s3 - c1 c3 s2  c1 s2 s3 + c3 s1   c1 c2  ]
    */

    private double[][] euler2mat(Euler eu) {
        double[][] result = new double[3][3];
        double s1=Math.sin(eu.x);
        double c1=Math.cos(eu.x);
        double s2=Math.sin(eu.y);
        double c2=Math.cos(eu.y);
        double s3=Math.sin(eu.z);
        double c3=Math.cos(eu.z);
        result[0][0] = c2*c3;
        result[0][1] = -c2*s3;
        result[0][2] = s2;

        result[1][0] = c1*s3 + c3 * s1 *s2;
        result[1][1] = c1*s3-s1*s2*s3;
        result[1][2] = -c2*s1;

        result[2][0] = s1*s3 - c1 * c3 *s2;
        result[2][1] = c1*s2*s3+c3*s1;
        result[2][2] = c1*c2;

        return result;
    }
    
    private static Euler mat2euler(double mat[][]) {
        double theta2 = Math.asin(mat[0][2]);
        double c2 = Math.cos(theta2);
        double c3 = mat[0][0]/c2;
        double s3 = -mat[0][1]/c2;
        double theta3 = Math.atan2(s3,c3);

        double s1 = -mat[1][2]/c2;
        double c1 = mat[2][2]/c2;
        double theta1 = Math.atan2(s1,c1);

        return new Euler(factor * theta1, factor * theta2, factor * theta3);
    }

    private static double[] cross(double[] a, double[] b) {
        double[] result = new double[3];
        result[0] = a[1]*b[2]-a[2]*b[1];
        result[1] = a[2]*b[0]-a[0]*b[2];
        result[2] = a[0]*b[1]-a[1]*b[0];

        return result;
    }

    private static double[] matMult(double[][] mat, double[] vec) {
        double[] result = new double[3];

        for(int i=0;i<3;i++) {
                double sum = 0.0;
                for(int k=0;k<3;k++) sum += mat[i][k] * vec[k];
                result[i] = sum;
        }
        return result;
    }
    double[][] R;
    double sigma;
    void computeScaleRot() {
        // {{{ sigma = scale factor
        obtuse = true;
        double temp = xx1*xx3+yy1*yy3;
        if(temp >= 0) {
            obtuse = false;
            msg.setText("Negative scale!"+temp);
            return;
        }
        sigma = Math.sqrt(-temp);
        msg.setText("OK "+sigma);
        // }}}

        x1 = xx1/sigma; y1 = yy1/sigma;
        x3 = xx3/sigma; y3 = yy3/sigma;

        // {{{ u1, u2, u3 are the ONB vectors
        double norm1 = Math.sqrt(1+x1*x1+y1*y1);
        double norm3 = Math.sqrt(1+x3*x3+y3*y3);
        u1 = new double[3];
        u3 = new double[3];

        u1[0] = x1/norm1; u1[1] = y1/norm1; u1[2] = -1/norm1;
        u3[0] = x3/norm3; u3[1] = y3/norm3; u3[2] = -1/norm3;
        if((xx1 < p[0] && p[0] < p[1]) || (xx1 > p[0] && p[0] > p[1])) {
            u1[0] = -u1[0]; u1[1] = -u1[1]; u1[2] = -u1[2];
        }

        if((xx3 < p[0] && p[0] < p[3]) || (xx3 > p[0] && p[0] > p[3])) {
            u3[0] = -u3[0]; u3[1] = -u3[1]; u3[2] = -u3[2];
        }

        u2 = cross(u3,u1);
        // }}}

        // {{{ R is the transpose (inverse) of the rotation matrix
        R = new double[3][3];

        R[0][0] = u1[0]; R[0][1] = u1[1]; R[0][2] = u1[2];
        R[1][0] = u2[0]; R[1][1] = u2[1]; R[1][2] = u2[2];
        R[2][0] = u3[0]; R[2][1] = u3[1]; R[2][2] = u3[2];

        // }}}

    }

    double normsq(double[] v) {
        return v[0]*v[0]+v[1]*v[1]+v[2]*v[2];
    }
    double orig[];

    void dbg(String msg, double[] what) {
        System.err.format("%s: [%.2f, %.2f, %.2f]\n",msg, what[0], what[1], what[2]);
    }
    
    void computeShift() {
        if(!obtuse) return;
        double t = (p[1]*u1[2]+u1[0]*sigma)/(p[1]-p[0]);
        orig = new double[3];
        orig[0] = p[0]*t/sigma;
        orig[1] = q[0]*t/sigma;
        orig[2] = -t;

        xtip = new double[3];
        xtip[0] = orig[0] + u1[0];
        xtip[1] = orig[1] + u1[1];
        xtip[2] = orig[2] + u1[2];
        ytip = new double[3];
        ytip[0] = orig[0] + u2[0];
        ytip[1] = orig[1] + u2[1];
        ytip[2] = orig[2] + u2[2];
        ztip = new double[3];
        //dbg("u3 from dump",u3);
        ztip[0] = orig[0] + u3[0];
        ztip[1] = orig[1] + u3[1];
        ztip[2] = orig[2] + u3[2];
        System.err.format("|u1|=%.2f, |u2|=%.2f, |u3|=%.2f\n",normsq(u1),normsq(u2),normsq(u3));
        System.err.format("u1.u2=%.2f, u1.u3=%.2f, u2.u3=%.2f\n",dot(u1,u2),dot(u1,u3),dot(u2,u3));
        tetra();
    }

    void tetra() {
        try {
            PrintWriter pw = new PrintWriter(new File("silly.obj"));

            pw.format("v %.2f %.2f %.2f\n",orig[0],orig[1],orig[2]);
            pw.format("v %.2f %.2f %.2f\n",xtip[0],xtip[1],xtip[2]);
            pw.format("v %.2f %.2f %.2f\n",ytip[0],ytip[1],ytip[2]);
            pw.format("v %.2f %.2f %.2f\n",ztip[0],ztip[1],ztip[2]);
            pw.format("f 1 2 3\n");
            pw.format("f 1 3 4\n");
            pw.format("f 1 4 2\n");
            pw.close();
        }
        catch(Exception ex) {
            ex.printStackTrace();
        }
    }
    double dot(double[] u, double[] v) {
        return u[0]*v[0]+u[1]*v[1]+u[2]*v[2];
    }
    
    double[] cameraPos;
    Euler angles;

    void dump() {
        if(!obtuse) return;
        cameraPos = matMult(R,orig);
        angles = mat2euler(R);

        System.out.format("Location of camera [%.2f, %.2f, %.2f]\n",
                          -cameraPos[0], -cameraPos[1], -cameraPos[2]);
        System.out.format("Euler angles for camera: [x=%.2f, y=%.2f, z=%.2f]\n",
                          angles.x, angles.y, angles.z);
    }

    public static void main(String args[]) {
        JFrame f = new JFrame("CameraMatcher5");
        f.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
        JLabel south = new JLabel("Messages will be shown here.");
        CameraMatcher5 cm = new CameraMatcher5(south,(args.length==1? args[0]: "silly.png"));
        f.add(new JScrollPane(cm));
        f.addKeyListener(cm);

        f.add(BorderLayout.SOUTH,south);
        f.pack();
        f.setSize(800,400);
        f.setVisible(true);
    }
    // {{{ Selection related stuff

    void copy(String str) {
        cb.setContents(new StringSelection(str),this);
    }
    
    void sendToAOI() {
        dump();
        clickAt(aoi.x,aoi.y);
    
        selectAll();
        copy(String.format("%.2f",-cameraPos[0])); paste();
        tabForward();

        copy(String.format("%.2f",-cameraPos[1])); paste();
        tabForward();

        copy(String.format("%.2f",-cameraPos[2])); paste();
        tabForward();

        copy("0"); paste(); tabForward();
        copy("180"); paste(); tabForward();
        copy("0"); paste(); 

        ctrlT();
        tabForward(); tabForward(); tabForward();

        copy(String.format("%.2f",angles.x)); paste();
        tabForward();
        copy(String.format("%.2f",angles.y)); paste();
        tabForward();
        copy(String.format("%.2f",angles.z)); paste();
    }

    private void ctrlT() {
	robot.keyPress(KeyEvent.VK_CONTROL);
	robot.keyPress(KeyEvent.VK_T);
	robot.delay(100);
	robot.keyRelease(KeyEvent.VK_T);
        robot.keyRelease(KeyEvent.VK_CONTROL);
	robot.delay(100);
    }

    private void selectAll() {
	robot.keyPress(KeyEvent.VK_CONTROL);
	robot.keyPress(KeyEvent.VK_A);
	robot.delay(100);
	robot.keyRelease(KeyEvent.VK_A);
        robot.keyRelease(KeyEvent.VK_CONTROL);
	robot.delay(100);
    }

    private void tabForward() {
	robot.keyPress(KeyEvent.VK_TAB);
	robot.delay(100);
	robot.keyRelease(KeyEvent.VK_TAB);
	robot.delay(100);
    }
    
    private void paste() {
	robot.keyPress(KeyEvent.VK_CONTROL);
	robot.keyPress(KeyEvent.VK_V);
	robot.delay(100);
	robot.keyRelease(KeyEvent.VK_V);
	robot.keyRelease(KeyEvent.VK_CONTROL);
        robot.delay(100);
    }
    
    private void clickAt(int x, int y) {
	robot.mouseMove(x,y);
	robot.delay(100);
	robot.mousePress(InputEvent.BUTTON1_MASK);
	robot.delay(100);
	robot.mouseRelease(InputEvent.BUTTON1_MASK);
    }

    
   public void lostOwnership(Clipboard clip, 
                             Transferable transfer) {
       System.err.println("Lost ownnership["+transfer+"]");
   }
    // }}}
}

class Euler {
    double x,y,z;
    Euler(double t1, double t2, double t3) {
        x = t1;
        y = t2;
        z = t3;
    }
}