/*
 * 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 iterativeclosestpoint;

import baseclasses.Frame;
import baseclasses.ObjExport;
import baseclasses.ObjImport;
import baseclasses.Point;
import java.io.IOException;
import java.util.ArrayList;
import org.apache.commons.math3.linear.BlockRealMatrix;

/**
 *
 * @author Alex
 */
public class ICPRun {

    /**
     * @param args the command line arguments
     */
    public static void main(String[] args) {
        // TODO code application logic here
        
        ObjImport pointImport = new ObjImport("ICP_FaceRed.obj");
        ObjImport scanImport = new ObjImport("ICP_GroundFace.obj");
        try {
            pointImport.read();
            scanImport.read();
        } catch (IOException e) {
            System.out.println("wat - ICP Run");
        }
        
        Icp estimator = new Icp();
        
        Convergence conv = new IterationConvergence(40);
        //Convergence conv = new FlatlineConvergence(.005, 20, 200);
        //Convergence conv = new ThresholdConvergence(.002, 100);
        
        BlockRealMatrix ans = new BlockRealMatrix(3, 3);
        BlockRealMatrix trans = new BlockRealMatrix(3, 1);
        
        double theta = -3.0;
        theta = Math.toRadians(theta);
        double cos = Math.cos(theta);
        double sin = Math.sin(theta);
        
        ans.setEntry(0, 0, 1.0);
        ans.setEntry(1, 1, cos);
        ans.setEntry(1, 2, -1.0 * sin);
        ans.setEntry(2, 1, sin);
        ans.setEntry(2, 2, cos);
        
        trans.setEntry(0,0, -30.0);
        trans.setEntry(1,0, -30.0);
        
        Frame fr1 = new Frame(ans, trans);
        Point[] pts = pointImport.getPoints();
        pts = fr1.transform(pts);
        
        fr1.print();
        
        ObjExport ex = new ObjExport("ICP_FaceRedTrans.obj", 
                pts, pointImport.getTriangles());
        try {
            ex.write();
        } catch (IOException e) {
            System.out.println("Error on Export");
        }
        
        Frame fReg = estimator.fRegEstimateSpecNumIter(pts, scanImport.getTriangles(), conv);
        ArrayList<Double> err = estimator.getErrrors();
        fReg.print();
        
        ObjExport ex2 = new ObjExport("ICP_FaceRedICPTrans.obj", 
                fReg.transform(pts), pointImport.getTriangles());
        try {
            ex2.write();
        } catch (IOException e) {
            System.out.println("Error on Export");
        }
    }
    
}
