/*
 *  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.MatrixUtils;

/**
 *
 * @author Pranav, Alex
 */
public class Frame {
    
    //Global Variables========================================================//
    /**
     * Rotation matrix of the frame.
     */
    private final BlockRealMatrix rot;
    
    /**
     * Translation vector of the frame.
     */
    private final BlockRealMatrix tran;
    
    /**
     * Flag to determine if the inverse has already been calculated.
     */
    private boolean inverseCalculated = false;
    
    /**
     * Inverse of the rotation matrix for inverse frame stored for multiple use.
     */
    private BlockRealMatrix inverseRot;
     /**
     * Inverse of the translation vector for inverse frame stored for multiple 
     * use.
     */
    
    private BlockRealMatrix inverseTran;
       
    /**
     * Inverse of the frame stored for multiple use.
     */
    private Frame inverseFrame;
    //========================================================================//
    
    
    //Constructors============================================================//
    
    /**
     * Frame - default constructor of the Frame object.
     * Forms an identity matrix.
     */
    public Frame() {
        this.rot = this.identity(3);
        BlockRealMatrix transl = new BlockRealMatrix(3, 1);
        transl.setEntry(0, 0, 0);
        transl.setEntry(1, 0, 0);
        transl.setEntry(2, 0, 0);
        this.tran = transl;
    }
    
    /**
     * Frame - normal constructor with rotation and translation specified.
     * @param pRot - Rotation Matrix.
     * @param pTran - Translation Vector.
     */
    public Frame(BlockRealMatrix pRot, BlockRealMatrix pTran) {
        this.rot = pRot;
        this.tran = pTran;
    }
    //========================================================================//
    
    
    //Methods=================================================================//
    
    /**
     * Access the rotation element of the Frame.
     * @return rotation matrix
     */
    public BlockRealMatrix getRot() {
        return this.rot;
    }

    /**
     * Access the translation element of the Frame.
     * @return translation vector
     */
    public BlockRealMatrix getTrans() {
        return this.tran;
    }
    
    /**
     * compose - composition of two Frames.
     * @param frame2 - frame to be composed with.
     * @return - new frame that represents the composition of the two vectors.
     */
    public Frame compose(Frame frame2) {     
        BlockRealMatrix rComp = this.rot.multiply(frame2.getRot());
        BlockRealMatrix pComp = this.rot.multiply(frame2.getTrans()).add(
                this.tran);
        return new Frame(rComp, pComp);
    }
    
    /**
     * inverse - the inverse of a frame.
     * @return - the inverted frame. Use old value if already calculated.
     */
    public Frame inverse() {
        if (this.inverseCalculated) {
            return this.inverseFrame;
        } else {
            this.inverseRot = ((BlockRealMatrix) MatrixUtils.inverse(this.rot));
            this.inverseTran = (((BlockRealMatrix) MatrixUtils.inverse(this.rot)
                    .scalarMultiply(-1.0))).multiply(this.tran);
            this.inverseFrame = new Frame(this.inverseRot, this.inverseTran);
            this.inverseCalculated = true;
            return this.inverseFrame;
        }
    }
    
    /**
     * transform - transform a group of points.
     * @param points - the points to be transformed.
     * @return - Point[] of transformed points.
     */
    public Point[] transform(Point[] points) {
        Point[] transformed = new Point[points.length];
        
        for (int i = 0; i < points.length; i++) {
            transformed[i] = this.transform(points[i]);
        }
        return transformed;
    }
    
    /**
     * Perform frame transformation on a point.
     * @param pt column vector of point (3x1)
     * @return point in new reference frame
     */
    public Point transform(Point pt) {
        BlockRealMatrix point = new BlockRealMatrix(3, 1);
        point.setEntry(0, 0, pt.getCoor()[0]);
        point.setEntry(1, 0, pt.getCoor()[1]);
        point.setEntry(2, 0, pt.getCoor()[2]);
        
        BlockRealMatrix tf = (this.rot.multiply(point)).add(this.tran);
        double[] rgb = pt.getRGBColor();
        Point tfPt = new Point(tf.getEntry(0, 0), tf.getEntry(1, 0), 
                tf.getEntry(2, 0), rgb[0], rgb[1], rgb[2] );
        return tfPt;
    }
    
    /**
     * Return an square identityMatrix matrix.
     * @param n the dimensions of the matrix.
     * @return an n x n identityMatrix BlockRealMatrix.
     */
    private BlockRealMatrix identity(int n) {
        BlockRealMatrix ans = new BlockRealMatrix(n, n);
        for (int i = 0; i < n; i++) {
            ans.setEntry(i, i, 1.0);
        }
        return ans;
    }
    
    /**
     * print - print utility for frames.
     */
    public void print() {
        //System.out.println("\n==========================");
        System.out.print("Rotation Matrix:");
        System.out.println(this.rot.toString());
        System.out.print("Translation Matrix:");
        System.out.println(this.tran.toString() + "\n");
        //System.out.println("==========================\n");
    }
}
