/*
 *  Alex Mathews, amathews@jhu.edu
 *  Pranav Lakshminarayanan, plakshm1@jhu.edu
 *  Computer Integrated Surgery I - Fall 2014
 */
package iterativeclosestpoint;

import baseclasses.Frame;
import baseclasses.Point;
import baseclasses.PointCloudtoCloud;
import baseclasses.Triangle;
import java.util.ArrayList;

/**
 *
 * @author Alex
 */
public class Icp {
    
    /**
     * ArrayList of errors collected.
     */
    private ArrayList<Double> errors;
    
    /**
     * ICP Constructor.
     */
    public Icp() {
    }
    
    /**
     * Estimate fReg.
     * @param lilD little d vectors.
     * @param tri triangle mesh.
     * @param conv convergence object.
     * @return an estimate F_reg.
     */
    public Frame fRegEstimateSpecNumIter(Point[] lilD, Triangle[] tri, 
            Convergence conv) {
        int iteration = 1;
        this.errors = new ArrayList<Double>();
        Point[] lilC = null;
        Frame fReg = new Frame();
        Point[] lilS = fReg.transform(lilD);
        lilC = ClosestPoint.calcLilCCentroid(lilS, tri);
        double error = this.diff(lilS, lilC);
        System.out.println("Iteration " + iteration + ": " + error);
        iteration++;
        while (!conv.isConverged(error)) {
            PointCloudtoCloud estimate = new PointCloudtoCloud(lilD, lilC);
            fReg = estimate.calculateFrame();
            lilS = fReg.transform(lilD);
            lilC = ClosestPoint.calcLilCCentroid(lilS, tri);
            error = this.diff(lilS, lilC);
            this.errors.add(error);
            System.out.println("Iteration " + iteration + ": " + error);
            iteration++;
        }
        return fReg;
    }
    
    /**
     * Diff two arrays of points.
     * @param a Array of points.
     * @param b Array of points.
     * @return an array of double with diff.
     */
    public double diff(Point[] a, Point[] b) {
        if (a.length != b.length) {
            throw new IllegalArgumentException("Point array lengths do not "
                    + "match in diff!");
        }
        double ans = 0.0;
        for (int i = 0; i < a.length; i++) {
            double[] aP = a[i].getCoor();
            double[] bP = b[i].getCoor();
            Point ptA = new Point(aP[0], aP[1], aP[2]);
            Point ptB = new Point(bP[0], bP[1], bP[2]);
            ans += Math.abs(Point.magnitude(Point.subtract(ptA, ptB)));
        }
        
        return ans / a.length;
    }
    
    /**
     * Get the errors over time.
     * @return an ArrayList of the errors in order.
     */
    public ArrayList<Double> getErrrors() {
        return this.errors;
    }
}
