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

import java.util.Arrays;
import java.util.Comparator;
import baseclasses.Point;
import baseclasses.Triangle;
import org.apache.commons.math3.linear.BlockRealMatrix;
import org.apache.commons.math3.linear.DecompositionSolver;
import org.apache.commons.math3.linear.RealMatrix;
import org.apache.commons.math3.linear.SingularValueDecomposition;

/**
 *
 * @author Pranav
 */
public class ClosestPoint {
    
    //========================================================================//
    
    //Methods=================================================================//
    
    /**
     * Calculate closest point on a triangle to a point.
     * @param a Point to be projected onto triangle
     * @param t Triangle being projected onto
     * @return closest point on triangle
     */
    public static Point findClosestPoint(Point a, Triangle t) {
        return ClosestPoint.findClosestPoint(a, t.getVertices());
    }
    
    
    /**
     * Calculate little C vectors to closest points on mesh.
     * @param pts points to find closest for.
     * @param tri triangles to calculate for.
     * @return set of points that were closest.
     */
    public static Point[] calcLilCCentroid(Point[] pts, 
            Triangle[] tri) {
        Point[] ans = new Point[pts.length];
        for (int i = 0; i < pts.length; i++) {
            Point curPt = pts[i];
            DistanceUtil[] disUtils = new DistanceUtil[tri.length];
            for (int j = 0; j < tri.length; j++) {
                double cen = Point.magnitude(Point.subtract(curPt, 
                        tri[j].getCentroid()));
                disUtils[j] = new DistanceUtil(cen, tri[j].getTriangleIndex(), 
                        tri[j].getRadius());
            }
            Arrays.sort(disUtils);
            //System.out.println(Arrays.toString(disUtils));
            Point closest = ClosestPoint.findClosestPoint(pts[i], 
                    tri[disUtils[0].getIndex()]);
            DistanceUtil closestInd = disUtils[0];
            Arrays.sort(disUtils, DistanceUtil.cUtilRDisComparator);
            //System.out.println(Arrays.toString(disUtils));
            double clLen = Point.magnitude(Point.subtract(closest, pts[i]));
            boolean flag = true;
            int j = 0;
            while (flag) {
                
                if (disUtils[j].getRDis() > closestInd.getDistance() 
                        + closestInd.getRadius()) {
                    flag = false;
                } else {
                    double cen = Point.magnitude(Point.subtract(curPt, 
                            tri[disUtils[j].getIndex()].getCentroid()));
                    Point cp = ClosestPoint.findClosestPoint(pts[i], 
                            tri[disUtils[j].getIndex()]);
                    double nLen = Point.magnitude(Point.subtract(cp, pts[i]));
                    if (nLen < clLen) {
                        closest = cp;
                        clLen = nLen;
                    }
                }
                j++;
            }
            ans[i] = closest;
        }
        return ans;
    }   
    /**
     * Calculate the closest point on a triangle to a point.
     * @param a Point to be projected onto triangle
     * @param vertices vertices of a triangle, called p, q, and r
     * @return closest point on triangle
     */
    public static Point findClosestPoint(Point a, Point[] vertices) {
        //The vector from Point a to vertex p
        Point ap = Point.subtract(a, vertices[0]);
        //The vector from between vertices q and p
        Point qp = Point.subtract(vertices[1], vertices[0]);
        //The vector from between vertices r and p
        Point rp = Point.subtract(vertices[2], vertices[0]);

        double[] coeffs = ClosestPoint.closestPointHelper(ap, qp, rp);
        double la = coeffs[0];
        double mu = coeffs[1];
        
        Point s1 = Point.add(vertices[0], Point.scale(qp, la));
        Point c = Point.add(s1, Point.scale(rp, mu));
        
        if (la >= 0 && mu >= 0 && la + mu <= 1) {
            return c;
        } else {
            return ClosestPoint.pointOnBorder(c, vertices, la, mu);
        }
    }

    //========================================================================//
    
    //Helper Methods==========================================================//
    
    /**
     * Interpolation helper.
     * Calculates the coefficients of the interpolated point.
     * @param ap Point a - p
     * @param qp Point q - p
     * @param rp Point r - p
     * @return lambda and mu, coefficients for interpolated point
     */
    private static double[] closestPointHelper(Point ap, Point qp, Point rp) {
        BlockRealMatrix aMat = new BlockRealMatrix(3, 2);
        aMat.setColumn(0, qp.getCoor());
        aMat.setColumn(1, rp.getCoor());
        
        BlockRealMatrix bMat = new BlockRealMatrix(3, 1);
        bMat.setColumn(0, ap.getCoor());
        
        SingularValueDecomposition svd;
        svd = new SingularValueDecomposition(aMat);
        DecompositionSolver solver = svd.getSolver();
        RealMatrix solve = solver.solve(bMat);
        double[] coeffs = {solve.getEntry(0, 0), solve.getEntry(1, 0)};
        
        return coeffs;
    }
    
    /**
     * Determine on to which segment to project.
     * @param cp closest point in plane, to be projected onto triangle border
     * @param verts vertices of the triangle
     * @param a coefficient used in interpolation
     * @param b coefficient used in interpolation
     * @return closest point on border of triangle
     */
    private static Point pointOnBorder(Point cp, Point[] verts, 
            double a, double b) {
        if (a < 0) {
            return ClosestPoint.projectOnSegment(cp, 
                    verts[2], verts[0]);
        } else if (b < 0) {
            return ClosestPoint.projectOnSegment(cp, 
                    verts[0], verts[1]);
        } else if (a + b > 1) {
            return ClosestPoint.projectOnSegment(cp, 
                    verts[1], verts[2]);
        }
        return null;
    }
    
    /**
     * If closest point is outside triangle, project onto edge segment.
     * @param cp point in plane
     * @param vert1 vertex of closest edge
     * @param vert2 other vertex of closest edge
     * @return projection of point onto segment
     */
    private static Point projectOnSegment(Point cp, Point vert1, Point vert2) {
        double numer = Point.dot(
                Point.subtract(cp, vert1),
                Point.subtract(vert2, vert1));
        double denom = Point.dot(
                Point.subtract(vert2, vert1),
                Point.subtract(vert2, vert1));
        double lam = numer / denom;
        double lambda = Math.max(0, Math.min(lam, 1));
        
        Point cProj = Point.add(vert1, 
                Point.scale(Point.subtract(vert2, vert1), lambda));
        return cProj;
    }

    
}


/**
 * Distance Utility Class for Centroids.
 * @author Alex
 */
class DistanceUtil implements Comparable {
    
    /**
     * Index of object.
     */
    private final int index;
    
    /**
     * Distance of associated index.
     */
    private final double distance;
    
    /**
     * Distance minus radius of associated index.
     */
    private final double rDistance;
    
    /**
     * Distance minus radius of associated index.
     */
    private final double radius;
    
    /**
     * Construct a DistanceUtil using a specified distance and index.
     * @param dis double distance. 
     * @param ind index.
     * @param rad radius of the triangle.
     */
    public DistanceUtil(double dis, int ind, double rad) {
        this.index = ind;
        this.distance = dis;
        this.radius = rad;
        this.rDistance = Math.abs(dis - rad);
    }

    /**
     * Return the stored distance.
     * @return the store distance.
     */
    public double getDistance() {
        return this.distance;
    }
    
    /**
     * Return the stored index.
     * @return the stored index.
     */
    public int getIndex() {
        return this.index;
    }
    
    /**
     * Return the stored index.
     * @return the stored index.
     */
    public double getRDis() {
        return this.rDistance;
    }
    
    /**
     * Return the stored index.
     * @return the stored index.
     */
    public double getRadius() {
        return this.radius;
    }
    
    /**
     * Compare to for sorting.
     * @param o object to compare to.
     * @return  -1, 0, 1 according to comparable specifications.
     */
    @Override
    public int compareTo(Object o) {
        DistanceUtil other = (DistanceUtil) o;
        if (this.distance - other.getDistance() > 0) {
            return 1;
        } else if (this.distance - other.getDistance() < 0) {
            return -1;
        } else {
            return 0;
        }
    }
    
    /**
     * Basic to String for printing.
     * @return the Distance util as a string.
     */
    @Override
    public String toString() {
        return this.distance + " " + this.index;
    }
    
    /**
     * Comparator object for sorting by rDistance. 
    */
    public static Comparator<DistanceUtil> cUtilRDisComparator = new 
        Comparator<DistanceUtil>() {
 
            @Override
            public int compare(DistanceUtil dUtil1, DistanceUtil dUtil2) {

                if (dUtil1.getRDis() - dUtil2.getRDis() > 0) {
                    return 1;
                } else if (dUtil1.getRDis() - dUtil2.getRDis() < 0) {
                    return -1;
                } else {
                    return 0;
                }
            }
        };
}