/*
 *  Alex Mathews, amathews@jhu.edu
 *  Pranav Lakshminarayanan, plakshm1@jhu.edu
 *  Computer Integrated Surgery I - Fall 2014
 */
package baseclasses;
import baseclasses.Point;
import org.apache.commons.math3.linear.BlockRealMatrix;
import org.apache.commons.math3.linear.EigenDecomposition;
import org.apache.commons.math3.linear.RealMatrix;
/**
 *
 * @author Alex
 */
public class PointCloudtoCloud {
    
    //Global Variables========================================================//
    
    /**
     * First cloud of points.
     */
    private final Point[] cloudA;
    
    /**
     * Second cloud of points.
     */
    private final Point[] cloudB;
    
    /**
     * Centroid Relative first cloud of points.
     */
    private final Point[] cenCloudA;
    
    /**
     * Centroid Relative second cloud of points.
     */
    private final Point[] cenCloudB;
    
    /**
     * Assuming 3 space. Cannot be changed.
     */
    private final int dimension = 3;
    
    /**
     * scaledRotation, only not null after calculateR is called.
     */
    private BlockRealMatrix scaledRotation; 
    
    /**
     * translation vector, only not null after calculateR is called.
     */
    private BlockRealMatrix translation;
    //========================================================================//
    
    
    //Constructor=============================================================//
    
    /**
     * PointCloudtoCloud - constructor, specify two Point[]'s.
     * @param pCloudA - first cloud of points.
     * @param pCloudB - second cloud of points.
     */
    public PointCloudtoCloud(Point[] pCloudA, Point[] pCloudB) {
        if (pCloudA.length != pCloudB.length) {
            throw new IllegalArgumentException("Point cloud lengths do not"
                    + "match.");
        }
        this.cloudA = pCloudA;
        this.cloudB = pCloudB;    
        this.cenCloudA = this.formCentroidPoints(this.cloudA);
        this.cenCloudB = this.formCentroidPoints(this.cloudB);        
    }
    //========================================================================//
    
    
    //Methods=================================================================//
    
    /**
     * calculateFrame - creates a frame of rot and trans between 2 point clouds.
     * @return a frame with scaled rotation and translation. 
     */
    public Frame calculateFrame() {
        BlockRealMatrix rot = this.scaledRotation;
        if (this.scaledRotation == null) {
            rot = this.calculateR();
        }
        BlockRealMatrix trans = this.calculateTran(rot);
        return new Frame(rot, trans);
    }
    
    /**
     * Calculate translation vector between two points.
     * @param rot - rotation matrix.
     * @return the translation vector.
     */
    private BlockRealMatrix calculateTran(BlockRealMatrix rot) {
        Point centroidb = Point.calculateAvgPt(this.cloudB);
        BlockRealMatrix zTrans = new BlockRealMatrix(3, 1);
        Frame fr = new Frame(rot, zTrans);
        Point[] transCloudb = fr.transform(this.cloudA);
        Point centroidB = Point.calculateAvgPt(transCloudb);
        BlockRealMatrix trans = Point.vectorBetweenPoints(centroidB, centroidb);
        return trans;
    }
    
    /**
     * calculateScale - find scaling of rotation matrix.
     * @param rot - rotation matrix.
     * @return the scaling between the two. 
     */    
    private double calculateScale(BlockRealMatrix rot) {
        BlockRealMatrix zTrans = new BlockRealMatrix(3, 1);
        Frame fr = new Frame(rot, zTrans);
        Point[] transCloudb = fr.transform(this.cenCloudA);
        double adotB = 0.0;
        double aNorm = 0.0;
        for (int i = 0; i < this.cenCloudA.length; i++) {
            double[] pointsA = this.cenCloudA[i].getCoor();
            double[] pointsB1 = this.cenCloudB[i].getCoor();
            double[] pointsB2 = transCloudb[i].getCoor();
            for (int j = 0; j < 3; j++) {
                adotB += pointsB1[j] * pointsB2[j];
                aNorm += pointsA[j] * pointsA[j];
            }
        }
        double scale = adotB / aNorm;
        return scale;
    }
    
    /**
     * calculateR - form the rotation matrix for b cloud to cloud trans.
     * @return - b rotation matrix.
     */
    private BlockRealMatrix calculateR() {
        BlockRealMatrix mMatrix = this.computeMMatrix();
        BlockRealMatrix nMatrix = this.computeNMatrix(mMatrix);
        EigenDecomposition eig = new EigenDecomposition(nMatrix);
        double[] eigs = eig.getRealEigenvalues();
        int maxInd = 0;
        for (int i = 1; i < eigs.length; i++) {
            if (eigs[maxInd] < eigs[i]) {
                maxInd = i;
            }
        }
        RealMatrix eigVectors = eig.getV();
        BlockRealMatrix quat = new BlockRealMatrix(
                eigVectors.getColumnMatrix(maxInd).getData());
        double quatNorm = quat.getEntry(0, 0) * quat.getEntry(0, 0) 
                + quat.getEntry(1, 0) * quat.getEntry(1, 0)
                + quat.getEntry(2, 0) * quat.getEntry(2, 0)
                + quat.getEntry(3, 0) * quat.getEntry(3, 0);
        quatNorm = Math.sqrt(quatNorm);
        BlockRealMatrix uQuat = new BlockRealMatrix(4, 1);
        uQuat.setEntry(0, 0, quat.getEntry(0, 0) / quatNorm);
        uQuat.setEntry(1, 0, quat.getEntry(1, 0) / quatNorm);
        uQuat.setEntry(2, 0, quat.getEntry(2, 0) / quatNorm);
        uQuat.setEntry(3, 0, quat.getEntry(3, 0) / quatNorm);
        BlockRealMatrix rMatrix = this.rFromUQuat(quat);
        double scale = this.calculateScale(rMatrix);
        BlockRealMatrix tran = this.calculateTran((BlockRealMatrix) 
                rMatrix.scalarMultiply(scale));
        this.translation = tran;
        this.scaledRotation = (BlockRealMatrix) rMatrix.scalarMultiply(scale);
        return this.scaledRotation;
    }
    
    /**
     * rFromUQuat - return the the Rotation matrix from unit quat.
     * Matrix is set according to Horn's paper, referenced in project write up.
     * @param q - unit quaternion.
     * @return the rotation matrix.
     */
    private BlockRealMatrix rFromUQuat(BlockRealMatrix q) {
        double[] e = new double[]{q.getEntry(0, 0), q.getEntry(1, 0), 
            q.getEntry(2, 0), q.getEntry(3, 0)};
        BlockRealMatrix rMatrix = new BlockRealMatrix(3, 3);
        
        rMatrix.setEntry(0, 0, e[0] * e[0] + e[1] * e[1] - e[2] * e[2] 
                - e[3] * e[3]);
        rMatrix.setEntry(1, 1, e[0] * e[0] - e[1] * e[1] + e[2] * e[2] 
                - e[3] * e[3]);
        rMatrix.setEntry(2, 2, e[0] * e[0] - e[1] * e[1] - e[2] * e[2] 
                + e[3] * e[3]);
       
        rMatrix.setEntry(0, 1, 2 * (e[1] * e[2] - e[0] * e[3]));
        rMatrix.setEntry(0, 2, 2 * (e[1] * e[3] + e[0] * e[2]));
        
        rMatrix.setEntry(1, 0, 2 * (e[2] * e[1] + e[0] * e[3]));
        rMatrix.setEntry(1, 2, 2 * (e[2] * e[3] - e[0] * e[1]));
        
        rMatrix.setEntry(2, 0, 2 * (e[3] * e[1] - e[0] * e[2]));
        rMatrix.setEntry(2, 1, 2 * (e[3] * e[2] + e[0] * e[1]));
        return rMatrix;
    }
    
    /**
     * computeNMatrix - compute the quat N matrix from M Matrix.
     * Matrix is set according to Horn's paper, referenced in project write up.
     * @param mMatrix - M matrix of S coordinates.
     * @return - N matrix.
     */
    private BlockRealMatrix computeNMatrix(BlockRealMatrix mMatrix) {
        BlockRealMatrix nMatrix = new BlockRealMatrix(4, 4);
        
        //set diagonal
        nMatrix.setEntry(0, 0, mMatrix.getEntry(0, 0) + mMatrix.getEntry(1, 1) 
                + mMatrix.getEntry(2, 2));
        nMatrix.setEntry(1, 1, mMatrix.getEntry(0, 0) - mMatrix.getEntry(1, 1) 
                - mMatrix.getEntry(2, 2));
        nMatrix.setEntry(2, 2, -1 * mMatrix.getEntry(0, 0) 
                + mMatrix.getEntry(1, 1) - mMatrix.getEntry(2, 2));
        nMatrix.setEntry(3, 3, -1 * mMatrix.getEntry(0, 0) 
                - mMatrix.getEntry(1, 1) + mMatrix.getEntry(2, 2));

        double a = mMatrix.getEntry(1, 2) - mMatrix.getEntry(2, 1);
        nMatrix.setEntry(1, 0, a);
        nMatrix.setEntry(0, 1, a);
        
        double b = mMatrix.getEntry(2, 0) - mMatrix.getEntry(0, 2);
        nMatrix.setEntry(2, 0, b);
        nMatrix.setEntry(0, 2, b);        
        
        double c = mMatrix.getEntry(0, 1) - mMatrix.getEntry(1, 0);
        nMatrix.setEntry(3, 0, c);
        nMatrix.setEntry(0, 3, c);
        
        double d = mMatrix.getEntry(0, 1) + mMatrix.getEntry(1, 0);
        nMatrix.setEntry(1, 2, d);
        nMatrix.setEntry(2, 1, d);
        
        double e = mMatrix.getEntry(2, 0) + mMatrix.getEntry(0, 2);
        nMatrix.setEntry(1, 3, e);
        nMatrix.setEntry(3, 1, e);
        
        double f = mMatrix.getEntry(1, 2) + mMatrix.getEntry(2, 1);
        nMatrix.setEntry(2, 3, f);
        nMatrix.setEntry(3, 2, f);
        
        return nMatrix;
    
    }
    
    /**
     * computeMMatrix - compute the matrix of s functions.
     * Matrix is set according to Horn's paper, referenced in project write up.
     * @return - 3x3 matrix
     */
    private BlockRealMatrix computeMMatrix() {
        BlockRealMatrix mMatrix = new BlockRealMatrix(3, 3);
        double[] aPointCoor;
        double[] bPointCoor;
        for (int r = 0; r < 3; r++) {
            //row
            for (int c = 0; c < 3; c++) {
                //column
                double summation = 0.0;
                //create M matrix by index
                for (int i = 0; i < this.cenCloudA.length; i++) {
                    aPointCoor = this.cenCloudA[i].getCoor();
                    bPointCoor = this.cenCloudB[i].getCoor();
                    summation += aPointCoor[r] * bPointCoor[c];
                }
                mMatrix.setEntry(r, c, summation);
            }
        }
        return mMatrix;
    }
    
    /**
     * formCentroidPoints - form point cloud arrays relative to b centroid.
     * @param cloud - points to calculate for. 
     * @return b final array of centroid relative points.
     */
    public final Point[] formCentroidPoints(Point[] cloud) {
        Point aCentroid = Point.calculateAvgPt(cloud);
        Point[] cenCloud = new Point[cloud.length];
        
        //enter relative points into an array
        for (int i = 0; i < this.cloudA.length; i++) {
            cenCloud[i] = Point.relativePoint(aCentroid, cloud[i]);
        }
        
        return cenCloud;
    }
}
