/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package colorsegmentation;

import baseclasses.Frame;
import baseclasses.ObjImport;
import baseclasses.ObjExport;
import baseclasses.Triangle;
import baseclasses.PointCloudtoCloud;
import baseclasses.Point;
import java.awt.Color;
import java.io.IOException;
import java.util.ArrayList;

/**
 *
 * @author Alex
 */
public class ScanColorDotRegistration {
    
    /**
     * Filename of Scan A.
     */
    private final String scanA; 
    
    /**
     * Scan A import object.
     */
    private ObjImport scanAImport;
    
    /**
     * Filename of Scan B.
     */
    private final String scanB;
    
    /**
     * Scan B import object.
     */
    private ObjImport scanBImport;
    
    /**
     * Specification on colors of dots and how many dots.
     */
    private final int[][] dotSpecifications;
    
    /**
     * Specification on colors of dots and how many dots.
     */
    private final double[][] dotTolerances;
    
    /**
     * Transformation between specified files.
     */
    private Frame transformation = null;
    
    /**
     * Constructor of a Scan Dot Registration object.
     * @param a filename of Scan A. 
     * @param b filename of Scan B. 
     * @param dots specification of dot colors and number.
     * @param tol tolerances of color dots.
     */
    public ScanColorDotRegistration(String a, String b, int[][] dots, 
            double[][] tol) {
        this.scanA = a; 
        this.scanB = b;
        this.dotSpecifications = dots;
        this.dotTolerances = tol;
        if (this.dotSpecifications.length != this.dotTolerances.length) {
            throw new IllegalArgumentException("Dot Specificaitons and "
                    + "Tolerance lengths do not match.");
        }
    }
    
    /**
     * Find the Frame transformation between the two files specified.
     * @return a Frame holding transformation information.
     */
    public Frame findTransformation() {
        if (this.transformation != null) {
            return this.transformation;
        }
        
        ///Import files A and B
        this.importScans();
        
        //Find Marker Points and order them for registration
        this.debugPrint("Finding relevant points in File A");
        Point[][] ptsA = this.markerPoints(this.scanAImport);
        this.showSelected(ptsA, this.scanA);
        Point[] avgPtsA = this.orderPoints(this.averagePoints(ptsA));
        
        //Find Marker Points and order them for registration
        this.debugPrint("Finding relevant points in File B");
        Point[][] ptsB = this.markerPoints(this.scanBImport);
        this.showSelected(ptsB, this.scanB);
        Point[] avgPtsB = this.orderPoints(this.averagePoints(ptsB));
        
        //Calculate tranfromation with associated points.
        this.debugPrint("Calculating Transformation");
        PointCloudtoCloud aToB = new PointCloudtoCloud(avgPtsA, avgPtsB);
        this.transformation = aToB.calculateFrame();
        return this.transformation;
    }
    
    /**
     * Find Markers on imported files.
     * @param imported ObjImport objects.
     * @return array of Point arrays for each marker.
     */
    private Point[][] markerPoints(ObjImport imported) {
        Triangle[][] fileADots = this.findDots(imported);
        
        Point[] fileAPoints = imported.getPoints();
        Point[][] relPointsA = new Point[fileADots.length * 2][];
        for (int i = 0; i < fileADots.length; i++) {
            System.out.println("Color " + (i + 1) + ": " + fileADots[i].length 
                    + " found.");
            if (fileADots[i].length < 20) {
                throw new IllegalArgumentException("Not enough colors found.");
            }
        }
        for (int i  = 0; i < fileADots.length; i++) {
            Point[] a = this.condensePoints(fileAPoints, fileADots[i]);
            ClusterAnalysis test = new ClusterAnalysis(a, 20);
            Point[][] clusters = test.clusterPoints(2);
            relPointsA[2 * i] = clusters[0];
            relPointsA[2 * i + 1] = clusters[1];
        }
        return relPointsA;
    }
    
    /**
     * Condense all the relevant Triangle faces to just array of Points.
     * @param objPts array of points from object file.
     * @param tri array of triangles.
     * @return array of Points.
     */
    private Point[] condensePoints(Point[] objPts, Triangle[] tri) {
        ArrayList<Integer> ptInds = new ArrayList<Integer>();
        for (Triangle tTri : tri) {
            int[] inds = tTri.getVertIndices();
            for (int i = 0; i < inds.length; i++) {
                if (!ptInds.contains(inds[i])) {
                    ptInds.add(inds[i]);
                }
            }
        }
        Point[] pts = new Point[ptInds.size()];
        for (int i = 0; i < ptInds.size(); i++) {
            pts[i] = objPts[ptInds.get(i)];
        }
        return pts;
    }
    
    /**
     * Find the dots specified within files.
     * @param file Obj import object to search in.
     * @return a double array of triangle faces that match.
     */
    private Triangle[][] findDots(ObjImport file) {
        Triangle[][] goodFaces = new Triangle[this.dotSpecifications.length][];
        for (int i = 0; i < this.dotSpecifications.length; i++) {
            float[] hsb = this.hsbColorConvert(this.dotSpecifications[i]);
            Triangle[] tFaces = file.getColorPoints(hsb, this.dotTolerances[i]);
            goodFaces[i] = tFaces;
        }
        return goodFaces;
    }
    
    /**
     * Import scans to ObjImport Classes.
     */
    private void importScans() {
        this.scanAImport = new ObjImport(this.scanA);
        this.scanBImport = new ObjImport(this.scanB);
        
        try {
            this.debugPrint("Importing File A");
            this.scanAImport.read();
            this.debugPrint("Importing File B");
            this.scanBImport.read();
        } catch (IOException e) {
            System.err.println(e.toString());
            System.err.println("ScanColorDotRegistration - Likely a filename "
                    + "error");
        }
        
    }
    
    /**
     * Convert RGB to HSB.
     * @param rgb int array of R, G, B. 
     * @return a float of HSB colors.
     */
    public float[] hsbColorConvert(int[] rgb) {
        float[] tVertColor = new float[3];
        tVertColor = Color.RGBtoHSB(rgb[0], rgb[1], rgb[2], null);
        tVertColor[0] = tVertColor[0] * 360.0f;
        tVertColor[1] = tVertColor[1] * 100.0f;
        tVertColor[2] = tVertColor[2] * 100.0f;
        return tVertColor;
    }
    
    /**
     * Color shift points for debugging.
     * @param pts points from file.
     * @param tri triangles to shift.
     * @return new set of points for export.
     */
    private Point[] colorShift(Point[] pts, Triangle[][] tri) {
        for (int i = 0; i < tri.length; i++) {
            for (Triangle good : tri[i]) {
                int[] tPts = good.getVertIndices();
                for (int goodPtInd : tPts) {
                    Point temp = pts[goodPtInd];
                    double[] tempCoor = temp.getCoor();
                    Point nTemp = new Point(tempCoor[0], tempCoor[1], 
                            tempCoor[2], 1.0, 0.0, 1.0);
                    pts[goodPtInd] = nTemp;
                }
            }
        }
        return pts;
    }
    
    /**
     * Color shift points for debugging.
     * @param pts points from file.
     * @return new set of points for export.
     */
    private Point[] colorShift(Point[] pts) {
        for (int i = 0; i < pts.length; i++) {
            Point temp = pts[i];
            double[] tempCoor = temp.getCoor();
            Point nTemp = new Point(tempCoor[0], tempCoor[1], 
                    tempCoor[2], 1.0, 0.0, 1.0);
            pts[i] = nTemp;
        }
        return pts;
    }
    
    /**
     * Find average points for each collection.
     * @param points Array of Point Arrays describing each marker.
     * @return average Point for each marker.
     */
    private Point[] averagePoints(Point[][] points) {
        Point[] avgPtsA = new Point[points.length];
        for (int i = 0; i < avgPtsA.length; i++) {
            Point j = Point.calculateAvgPt(points[i]);
            avgPtsA[i] = this.closestPointsOnSurface(points[i], j);
        }
        return avgPtsA;
    }
    
    /**
     * Find closest point on the surface to the average point.
     * @param surface Point array describing surface.
     * @param average average Point of a marker.
     * @return Closest Point on the surface to the average Point.
     */
    private Point closestPointsOnSurface(Point[] surface, Point average) {
        Point closest = surface[0];
        double min = Point.distance(surface[0], average);
        for (int i = 1; i < surface.length; i++) {
            double dis = Point.distance(surface[i], average);
            if (dis < min) {
                min = dis;
                closest = surface[i];
            }
        }
        return closest;
    }
    
    /**
     * Order the Points of the array according to the pattern.
     * @param points Array of average points.
     * @return correctly ordered points for registration.
     */
    private Point[] orderPoints(Point[] points) {
        Point[] clusterA = new Point[3];
        Point[] clusterB = new Point[3];
        //assign reds
        clusterA[0] = points[0];
        clusterB[0] = points[1];
        double distance1 = Point.distance(points[0], points[2]);
        double distance2 = Point.distance(points[0], points[3]);
        if (distance1 < distance2) {
            clusterA[1] = points[2];
            clusterB[1] = points[3];
        } else {
            clusterA[1] = points[3];
            clusterB[1] = points[2];
        }
        
        double distance3 = Point.distance(points[0], points[4]);
        double distance4 = Point.distance(points[0], points[5]);
        if (distance3 < distance4) {
            clusterA[2] = points[4];
            clusterB[2] = points[5];
        } else {
            clusterA[2] = points[5];
            clusterB[2] = points[4];
        }
        
        Point[] ans = new Point[6];
        double distance5 = Point.distance(clusterA[0], clusterA[1]);
        double distance6 = Point.distance(clusterB[0], clusterB[1]);
        if (distance5 < distance6) {
            ans[0] = clusterA[0];
            ans[1] = clusterA[1];
            ans[2] = clusterA[2];
            ans[3] = clusterB[0];
            ans[4] = clusterB[1];
            ans[5] = clusterB[2];
        } else {
            ans[0] = clusterB[0];
            ans[1] = clusterB[1];
            ans[2] = clusterB[2];
            ans[3] = clusterA[0];
            ans[4] = clusterA[1];
            ans[5] = clusterA[2];
        }
        
        return ans;
    }
    
    /**
     * Show the selected markers in data.
     * @param selected points that were selected.
     * @param file file name of input.
     */
    private void showSelected(Point[][] selected, String file) {
        int len = 0;
        for (int i = 0; i < selected.length; i++) {
            len += selected[i].length;
        }
        Point[] selCondensed = new Point[len];
        int index = 0;
        for (int i = 0; i < selected.length; i++) {
            for (int j = 0; j < selected[i].length; j++) {
                selCondensed[index] = selected[i][j];
                index++;
            }
        }
        ObjExport exp = new ObjExport("Selected_" + file, selCondensed, null);
        try {
            exp.write();
        } catch (IOException e) {
            System.out.println("Show Selected error in file");
        }
    }
    
    /**
     * Debug print statement.
     * @param debug debug statement.
     */
    private void debugPrint(String debug) {
        String out = "\nScanColorDotRegistration - " + debug;
        int j = out.length();
        out += "\n";
        for (int i = 0; i < j; i++) {
            out += "=";
        }
        System.out.println(out);
    }
}
