/***************************************************************************
 *   Copyright (c) 2007 Joachim Zettler <Joachim.Zettler@gmx.de>           *
 *   Copyright (c) 2007 Human Rezai <human@mytum.de>                       *
 *                                                                         *
 *   This file is part of the FreeCAD CAx development system.              *
 *                                                                         *
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Library General Public           *
 *   License as published by the Free Software Foundation; either          *
 *   version 2 of the License, or (at your option) any later version.      *
 *                                                                         *
 *   This library  is distributed in the hope that it will be useful,      *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU Library General Public License for more details.                  *
 *                                                                         *
 *   You should have received a copy of the GNU Library General Public     *
 *   License along with this library; see the file COPYING.LIB. If not,    *
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
 *   Suite 330, Boston, MA  02111-1307, USA                                *
 *                                                                         *
 ***************************************************************************/

/*****************best_fit.CPP*****************
* Contains implementations from best_fit.h
*
*
*********************************************/

#include "PreCompiled.h"
#include "best_fit.h"
#include "routine.h"
#include <strstream>

#include <Mod/Mesh/App/Core/Grid.h>
#include <Mod/Mesh/App/Core/Builder.h>
#include <Mod/Mesh/App/Core/TopoAlgorithm.h>

#include <Base/Builder3D.h>

#include <BRep_Tool.hxx>
#include "BRepUtils.h"
#include <BRepBuilderAPI_Sewing.hxx>

//#include <BRepMeshAdapt.hxx>
#include <BRepTools.hxx>
#include <BRepMesh_IncrementalMesh.hxx>
#include <BRepMesh.hxx>

#include <BRepGProp.hxx>
#include <TopExp_Explorer.hxx>
#include <TopoDS.hxx>
#include <BRepBuilderAPI_Transform.hxx>
#include <GProp_PrincipalProps.hxx>

#include <Handle_Poly_Triangulation.hxx>
#include <Poly_Triangulation.hxx>

#include <ANN/ANN.h> // ANN declarations

#include <SMESH_Gen.hxx>


best_fit::best_fit()
{
    m_LSPnts.resize(2);
}

best_fit::~best_fit()
{
}

void best_fit::Load(const MeshCore::MeshKernel &mesh, const TopoDS_Shape &cad)
{
    m_Mesh = mesh;
    m_Cad  = cad;

    m_MeshWork = m_Mesh;

}

double best_fit::ANN()
{
    ANNpointArray   dataPts;                // data points
    ANNpoint        queryPt;                // query point
    ANNidxArray     nnIdx;                    // near neighbor indices
    ANNdistArray    dists;                    // near neighbor distances
    ANNkd_tree*     kdTree;                    // search structure

	Base::Builder3D log_error;

    //MeshCore::MeshPointArray meshPnts = m_MeshWork.GetPoints();
	m_pntCloud_2;
    Base::Vector3f projPoint;

    double error = 0.0;

    int a_dim = 3;
    int a_nbPnts =(int) m_pntCloud_2.size();        // Size of the scanned network
    int a_nbNear = 1;                               // number of return values
    queryPt = annAllocPt(a_dim);                    // allocate query point storage
    dataPts = annAllocPts(a_nbPnts, a_dim);         // allocate data points storage
    nnIdx = new ANNidx[a_nbNear];                   // allocate near neighbor indices
    dists = new ANNdist[a_nbNear];                  // allocate near neighbor dists

    m_LSPnts[0].clear();
    m_LSPnts[1].clear();

    for (int i=0; i<a_nbPnts; ++i)
    {
        dataPts[i][0] = m_pntCloud_2[i].x;
        dataPts[i][1] = m_pntCloud_2[i].y;
        dataPts[i][2] = m_pntCloud_2[i].z;
    }

    kdTree = new ANNkd_tree(        // build search structure
        dataPts,                    // the data points
        a_nbPnts,                   // number of points
        a_dim);                     // dimension of space


    for (unsigned int i = 0 ; i < m_pntCloud_1.size() ; i++ )
    {
        queryPt[0] = m_pntCloud_1[i].x;
        queryPt[1] = m_pntCloud_1[i].y;
        queryPt[2] = m_pntCloud_1[i].z;

        kdTree->annkSearch(                        // search
            queryPt,                               // query point
            a_nbNear,                              // number of near neighbors
            nnIdx,                                 // nearest neighbors (returned)
            dists                                  // distance (returned)
        );                                         // error bound

        m_LSPnts[1].push_back(m_pntCloud_1[i]);
        m_LSPnts[0].push_back(m_pntCloud_2[nnIdx[0]]);

		if(m_pntCloud_1[i].z <= m_pntCloud_2[nnIdx[0]].z)
		{
			log_error.addSingleLine(m_pntCloud_1[i],m_pntCloud_2[nnIdx[0]],8,1,0,0);
		}
		else
		{
			log_error.addSingleLine(m_pntCloud_1[i],m_pntCloud_2[nnIdx[0]],8,0,1,0);
		}

		//if(dists[0] > error)
			error += dists[0];

    }

	log_error.saveToFile("c:/errorVec_fit.iv");

    error /= double(m_pntCloud_1.size());
    m_weights_loc = m_weights;


    delete [] nnIdx; // clean things up
    delete [] dists;
    delete kdTree;
    annClose(); // done with ANN

    return error;
}

bool best_fit::Perform()
{
    Base::Matrix4D M;

    ofstream Runtime_BestFit;
	time_t sec1, sec2;

	Runtime_BestFit.open("c:/Runtime_BestFit.txt");
	Runtime_BestFit << "Runtime Best-Fit" << std::endl;


    cout << "tessellate shape" << endl;

	sec1 = time(NULL);
   	  Tesselate_Shape(m_Cad, m_CadMesh, 1); // Tessellates m_Cad Shape and stores Tessellation in m_CadMesh 
	sec2 = time(NULL);

	Runtime_BestFit << "Tessellate Shape: " << sec2 - sec1 << " sec" << std::endl;  

	sec1 = time(NULL);
      Comp_Weights(); // m_pntCloud_1, m_weights, m_normals des/r Cad-Meshs/Punktewolke werden hier gefüllt
    sec2 = time(NULL);

	Runtime_BestFit << "Compute Weights: " << sec2 - sec1 << " sec" << std::endl;  
	

	/*RotMat(M, 180, 1);
    m_MeshWork.Transform(M);
	return true;*/


	//MeshCore::MeshPointArray pntarr = m_MeshWork.GetPoints();
	//MeshCore::MeshFacetArray facetarr = m_MeshWork.GetFacets();

	//for(int i=0; i<pntarr.size(); ++i)
	//{
	//	pntarr[i].x -= 200;
	//	pntarr[i].y -= 200;
	//	pntarr[i].z += 50;
	//}

	//m_MeshWork.Assign(pntarr,facetarr);


    sec1 = time(NULL);
	
	MeshFit_Coarse();  // Transformation Mesh -> CAD
    ShapeFit_Coarse(); // Translation    CAD  -> Origin

	//return true;
	
    M.setToUnity();
    M[0][3] = m_cad2orig.X();
    M[1][3] = m_cad2orig.Y();
    M[2][3] = m_cad2orig.Z();

    m_CadMesh.Transform(M); // better: tessellation after the transformer!
    m_MeshWork.Transform(M);
    PointTransform(m_pntCloud_1,M);

	MeshCore::MeshPointArray pnts = m_MeshWork.GetPoints();

	for (unsigned int i=0; i<pnts.size(); ++i)
	{
		m_pntCloud_2.push_back(pnts[i]);

	}

    Runtime_BestFit << "- Error: " << ANN() << endl;

    Coarse_correction();

	sec2 = time(NULL);
	Runtime_BestFit << "Coarse Correction: " << sec2-sec1 << " sec" << endl;

    sec1 = time(NULL);
	LSM();
	sec2 = time(NULL);
    Runtime_BestFit << "Least-Square-Matching: " << sec2-sec1 << " sec" << endl;
	Runtime_BestFit.close();

    Base::Matrix4D T;
    T.setToUnity();
    T[0][3] = -m_cad2orig.X();
    T[1][3] = -m_cad2orig.Y();
    T[2][3] = -m_cad2orig.Z();
    m_MeshWork.Transform(T);
    m_CadMesh.Transform(T);

	m_Mesh = m_MeshWork;
    CompTotalError();

    return true;


}

bool best_fit::Perform_PointCloud()
{
	Base::Matrix4D M;

	ofstream Runtime_BestFit;
	time_t sec1, sec2;

	Runtime_BestFit.open("c:/Runtime_BestFit_PntCld.txt");
	Runtime_BestFit << "Runtime Best-Fit" << std::endl;

	PointCloud_Coarse();  

	M.setToUnity();

	for(unsigned int i=0; i<m_pntCloud_1.size(); i++)
		m_weights.push_back(1.0);
	
	M[0][3] = m_cad2orig.X();
	M[1][3] = m_cad2orig.Y();
	M[2][3] = m_cad2orig.Z();

	PointTransform(m_pntCloud_1,M);
	PointTransform(m_pntCloud_2,M);

	//Runtime_BestFit << "- Error: " << ANN() << endl;
    sec1 = time(NULL);
	Coarse_correction();
	sec2 = time(NULL);
	Runtime_BestFit << "Coarse Correction: " << sec2-sec1 << " sec" << endl;

	//M[0][3] = m_cad2orig.X();
	//M[1][3] = m_cad2orig.Y();
	//M[2][3] = m_cad2orig.Z();

	//PointTransform(m_pntCloud_1,M);
	//PointTransform(m_pntCloud_2,M);

	sec1 = time(NULL);
	LSM();
	sec2 = time(NULL);
	Runtime_BestFit << "Least-Square-Matching: " << sec2-sec1 << " sec" << endl;
	Runtime_BestFit.close();

	Base::Matrix4D T;
	T.setToUnity();
	T[0][3] = -m_cad2orig.X();
	T[1][3] = -m_cad2orig.Y();
	T[2][3] = -m_cad2orig.Z();
	PointTransform(m_pntCloud_1, T);
	PointTransform(m_pntCloud_2, T);
	m_MeshWork.Transform(T);
	m_CadMesh.Transform(T);

	m_Mesh = m_MeshWork;
	CompTotalError();

	return true;
}

/*
bool best_fit::Intersect(const Base::Vector3f &normal,const MeshCore::MeshKernel &mesh, Base::Vector3f &P, Base::Vector3f &I)
{
    MeshCore::MeshFacetIterator f_it(mesh);

    for (f_it.Begin(); f_it.More(); f_it.Next())
    {
        if (intersect_RayTriangle(normal, *f_it, P, I) == 1)
            return true;
    }

    return false;
}

int best_fit::intersect_RayTriangle(const Base::Vector3f &normal,const MeshCore::MeshGeomFacet &T, Base::Vector3f &P, Base::Vector3f &I)
{
    Base::Vector3f    u, v, n, null(0.0,0.0,0.0), J;          // triangle vectors
    Base::Vector3f    dir, w0, w;                             // ray vectors
    float             r, a, b;                                // params to calc ray-plane intersect

    J = P+normal;

    // get triangle edge vectors and plane normal
    u = T._aclPoints[1] - T._aclPoints[0];
    v = T._aclPoints[2] - T._aclPoints[0];
    n = u % v;                // cross product
    if (n == null)            // triangle is degenerate
        return -1;            // do not deal with this case

    dir = normal;    // ray direction vector
    w0  = P - T._aclPoints[0];
    a   = n * w0;
    b   = n * dir;
    if (fabs(b) < SMALL_NUM)      // ray is parallel to triangle plane
    {
        if (a == 0)                // ray lies in triangle plane
            return 2;
        else return 0;             // ray disjoint from plane
    }

    // get intersect point of ray with triangle plane
    r = -a / b;

    //if (r < 0.0)                   // ray goes away from triangle
    //    return 0;                  // => no intersect
    // for a segment, also test if (r > 1.0) => no intersect

    dir.Scale(r,r,r);
    I = P + dir;              // intersect point of ray and plane

    // is I inside T?
    float    uu, uv, vv, wu, wv, D;
    uu = u*u;
    uv = u*v;
    vv = v*v;
    w = I - T._aclPoints[0];
    wu = w*u;
    wv = w*v;
    D = uv * uv - uu * vv;

    // get and test parametric coords
    float s, t;
    s = (uv * wv - vv * wu) / D;
    if (s < 0.0 || s > 1.0)        // I is outside T
        return 0;
    t = (uv * wu - uu * wv) / D;
    if (t < 0.0 || (s + t) > 1.0)  // I is outside T
        return 0;

    return 1;                      // I is in T
}
*/

bool best_fit::Coarse_correction()
{
    double error, error_tmp, rot = 0.0;
    Base::Matrix4D M,T;
	ofstream CoarseCorr;

	CoarseCorr.open("c:/CoarseCorr.txt");

    std::vector<Base::Vector3f> m_pntCloud_Work = m_pntCloud_2;

    T.setToUnity();
    best_fit befi; 

	//error = CompError_GetPnts(m_pnts, m_normals)[0];  // start error    int n=360/rstep_corr;

	error = ANN();

    for (int i=1; i<4; ++i)
    {
        RotMat(M, 180, i);
        PointTransform(m_pntCloud_2, M);
		//m_MeshWork.Transform(M);

        error_tmp = ANN();
		
		//error_tmp = CompError_GetPnts(m_pnts, m_normals)[0];
        //error_tmp = befi.CompTotalError(m_MeshWork);
		
		CoarseCorr << i << ", " << error_tmp << endl;

        if (error_tmp < error)
        {
            T = M;
            error = error_tmp;
        }

        m_pntCloud_2 = m_pntCloud_Work;
    }

	CoarseCorr << "BEST CHOICE: " << error << endl;
	CoarseCorr.close();
    PointTransform(m_pntCloud_2, T);
	m_MeshWork.Transform(T);


    return true;
}


bool best_fit::LSM()
{
    double TOL  = 0.05;          // Termination criterion of the Newton method
    int maxIter = 100;            // maximum number of iterations for the case,
                                 // that the termination criterion is not met

   int mult = 2;                 // to halve the step size in the event of failure of the Newton method

    double val, tmp = 1e+10, delta, delta_tmp = 0.0;
    Base::Matrix4D Tx,Ty,Tz,Rx,Ry,Rz,M;   // Transformation matrices

	ofstream anOutputFile;
	anOutputFile.open("c:/outputBestFit.txt");
    anOutputFile.precision(7);

      int c=0; // Run variable



    std::vector<double> del_x(3,0.0);
    std::vector<double>     x(3,0.0); // Start parameter corresponds to zero vector

    Base::Vector3f centr_l,centr_r, cent;  // Focal points of the point sets

    // Newton's method: 1. Solve  H*del_x = -J
    //                  2. Set    x = x + del_x

    std::vector<double> Jac(3);           // 1. Deriving the error function (Jacobi-Matrix)
    std::vector< std::vector<double> > H; // 2. Deriving the error function (Hesse-Matrix)

    time_t seconds1, seconds2, sec1, sec2;
    seconds1 = time(NULL);

    while (true)
    {

        seconds1 = time(NULL);
        //m_Mesh = m_MeshWork;
        
		// Error calculation from CAD -> Mesh
        //tmp = CompError_GetPnts(m_pnts, m_normals);      // here: - Calculation of the LS point sets
        //      CompTotalError()                           //       - Calculation of the corresponding weightings

		delta = delta_tmp;
        delta_tmp = ANN();          // returns average absolute errors
		delta = delta - delta_tmp ; // the error correction for the previous iteration step is stored here

		if (c==maxIter || delta < ERR_TOL && c>1) break; // Abort criterion (if maximum iteration steps are reached or if
										                 //                  the change in error is insignificantly small)

        seconds2 = time(NULL);
		anOutputFile << c << ", " << delta_tmp << ", " << delta << "    -    Time: " << seconds2 - seconds1 << " sec" << endl;
		seconds1 = time(NULL);

		sec1 = time(NULL);
        for (unsigned int i=0; i<x.size(); ++i) x[i] = 0.0; // sets the starting values for newton to zero


        // Calculate weighted centroids and shift the point sets accordingly:
        centr_l.Scale(0.0,0.0,0.0);
        centr_r.Scale(0.0,0.0,0.0);

        Base::Vector3f p,q;
		double Sum = 0.0;
        for (unsigned int i=0; i<m_LSPnts[0].size(); ++i)
        {
            p = m_LSPnts[0][i];
            q = m_LSPnts[1][i];

            p.Scale((float) m_weights_loc[i],(float) m_weights_loc[i],(float) m_weights_loc[i]);
            q.Scale((float) m_weights_loc[i],(float) m_weights_loc[i],(float) m_weights_loc[i]);

			Sum += m_weights_loc[i];
			centr_l += p;
            centr_r += q;
        }

		centr_l.Scale( -1.0f/float(Sum), -1.0f/float(Sum), -1.0f/float(Sum));
		centr_r.Scale( -1.0f/(float)Sum, -1.0f/(float)Sum, -1.0f/(float)Sum);

       /* float s = (float) m_LSPnts[0].size();
        s = float(-1.0/s);

        centr_l.Scale(s,s,s);
        centr_r.Scale(s,s,s);*/



        // Shifting the focus to the origin
        TransMat(Tx,centr_l.x,1);
        TransMat(Ty,centr_l.y,2);
        TransMat(Tz,centr_l.z,3);

        M = Tx*Ty*Tz;
        PointTransform(m_LSPnts[0],M);
		PointTransform(m_pntCloud_1,M);
		PointTransform(m_pntCloud_2,M);
        m_MeshWork.Transform(M);

        TransMat(Tx,centr_r.x,1); // Calculation of the translation matrix in x-direction
        TransMat(Ty,centr_r.y,2); // Calculation of the translation matrix in y-direction
        TransMat(Tz,centr_r.z,3); // Calculation of the translation matrix in z-direction

        M = Tx*Ty*Tz;                  // Merge into an overall translation matrix
        PointTransform(m_LSPnts[1],M); // Applying the translation to m_LSPnts
        PointTransform(m_pntCloud_1,M);
		//PointNormalTransform(m_pnts, m_normals, M); // Application of translation to m_pnts
        m_CadMesh.Transform(M);                       // Application of translation to the CadMesh

        sec2 = time(NULL);
		anOutputFile << c+1 << " - Initialization and transformation around weighted focal points: " << sec2 - sec1 << " sec" << endl;

		sec1 = time(NULL);
        // Newton's method for calculating the rotation matrix:
        while (true)
        {

            Jac = Comp_Jacobi(x);  // compute 1st derivative
            H   = Comp_Hess(x);    // compute 2nd derivative

            val = 0.0;
            for (unsigned int i=0; i<Jac.size(); ++i)
            {
                val += Jac[i]*Jac[i];
            }
            val = sqrt(val);

            if (val < TOL) break; // Termination criterion of the Newton method

            if (val>tmp && mult < 1e+4)
            {
                for (unsigned int i=0; i<del_x.size(); ++i)
                {
                    x[i] -= del_x[i]/double(mult);  // Halve the increment if no improvement
                }
                mult *= 2;
                continue;
            }
            else
            {
                mult = 2;
            }

            tmp = val;

            del_x = Routines::NewtonStep(Jac,H);      // solves equation system:  H*del_x = -J
            for (unsigned int i=0; i<x.size(); ++i)   // next iteration value:    x = x + del_x
            {
                x[i] += del_x[i];
            }
        }

		sec2 = time(NULL);
		anOutputFile << c+1 << " - Newton: " << seconds2 - seconds1 << " sec" << endl;
        sec1 = time(NULL);
        // Rotate and shift back to the origin of the !!! CAD geometry !!!
        RotMat  (Rx,(x[0]*180.0/PI),1);
        RotMat  (Ry,(x[1]*180.0/PI),2);
        RotMat  (Rz,(x[2]*180.0/PI),3);

		Base::Matrix4D R = Rx*Ry*Rz;
		centr_l.Scale(-1.0, -1.0, -1.0);

		cent.x =(float)  (R[0][0]*centr_l.x + R[0][1]*centr_l.y + R[0][2]*centr_l.z);
		cent.y =(float)  (R[1][0]*centr_l.x + R[1][1]*centr_l.y + R[1][2]*centr_l.z);
		cent.z =(float)  (R[2][0]*centr_l.x + R[2][1]*centr_l.y + R[2][2]*centr_l.z);
			 
		TransMat(Tx, -centr_r.x - cent.x + centr_l.x, 1);
        TransMat(Ty, -centr_r.y - cent.y + centr_l.y, 2);
		TransMat(Tz, -centr_r.z - cent.z + centr_l.z, 3);

        M = Tx*Ty*Tz*Rx*Ry*Rz; // Rotate first !!! (Rotations always around the zero point ...)
        
		PointTransform(m_pntCloud_2,M);
		m_MeshWork.Transform(M);

		TransMat(Tx, -centr_r.x, 1);
		TransMat(Ty, -centr_r.y, 2);
		TransMat(Tz, -centr_r.z, 3);

        M = Tx*Ty*Tz;
		PointTransform(m_pntCloud_1,M);
        m_CadMesh.Transform(M);
        //PointNormalTransform(m_pnts, m_normals, M);

   		sec2 = time(NULL);
		
		anOutputFile << c+1 << " - Trafo: " << seconds2 - seconds1 << " sec" << endl;
        ++c;  //Increase run variable 
    }

	anOutputFile.close();
return true;

    /*TransMat(Tx,-centr_l.x,1);
       TransMat(Ty,-centr_l.y,2);
    TransMat(Tz,-centr_l.z,3);

    M = Tx*Ty*Tz;
    PointTransform(m_LSPnts[0],M);

    TransMat(Tx,-centr_r.x,1);
       TransMat(Ty,-centr_r.y,2);
    TransMat(Tz,-centr_r.z,3);

    M = Tx*Ty*Tz;
    PointTransform(m_LSPnts[1],M);

    Base::Builder3D log;

    for(unsigned int i=0; i<m_LSPnts[0].size(); ++i)
     log.addSingleArrow(m_LSPnts[0][i],m_LSPnts[1][i]);

    log.saveToFile("c:/newton_pnts.iv");*/

  
}


std::vector<double> best_fit::Comp_Jacobi(const std::vector<double> &x)
{
    std::vector<double> F(3,0.0);
    double s1 = sin(x[0]), c1 = cos(x[0]);
    double s2 = sin(x[1]), c2 = cos(x[1]);
    double s3 = sin(x[2]), c3 = cos(x[2]);

    Base::Vector3f p,q;

    for (unsigned int i=0; i<m_LSPnts[0].size(); ++i)
    {
        p = m_LSPnts[0][i];
        q = m_LSPnts[1][i];

        F[0] += ( q.y * ( (-c1*s2*c3-s1*s3) * p.x +  (c1*s2*s3-s1*c3) * p.y + (-c1*c2) * p.z ) +
                  q.z * ( (-s1*s2*c3+c1*s3) * p.x +  (s1*s2*s3+c1*c3) * p.y + (-s1*c2) * p.z ) )*m_weights_loc[i];

        F[1] += ( q.x * (          (-s2*c3) * p.x +           (s2*s3) * p.y +    (-c2) * p.z ) +
                  q.y * (       (-s1*c2*c3) * p.x +        (s1*c2*s3) * p.y +  (s1*s2) * p.z ) +
                  q.z * (        (c1*c2*c3) * p.x +       (-c1*c2*s3) * p.y + (-c1*s2) * p.z ) )*m_weights_loc[i];

        F[2] += ( q.x * (          (-c2*s3) * p.x +          (-c2*c3) * p.y) +
                  q.y * (  (s1*s2*s3+c1*c3) * p.x +  (s1*s2*c3-c1*s3) * p.y) +
                  q.z * ( (-c1*s2*s3+s1*c3) * p.x + (-c1*s2*c3-s1*s3) * p.y) )*m_weights_loc[i];
    }

    return F;
}

std::vector<std::vector<double> > best_fit::Comp_Hess(const std::vector<double> &x)
{
    std::vector<std::vector<double> > DF(3);
    for (unsigned int i=0; i<DF.size(); ++i)
    {
        DF[i].resize(3,0.0);
    }

    double s1 = sin(x[0]), c1 = cos(x[0]);
    double s2 = sin(x[1]), c2 = cos(x[1]);
    double s3 = sin(x[2]), c3 = cos(x[2]);

    double sum1 = 0.0, sum2 = 0.0, sum3 = 0.0, sum4 = 0.0, sum5 = 0.0, sum6 = 0.0;

    Base::Vector3f p,q;

    for (unsigned int i=0; i<m_LSPnts[0].size(); ++i)
    {
        p = m_LSPnts[0][i];
        q = m_LSPnts[1][i];

        sum1      = q.y * (  (s1*s2*c3-c1*s3) * p.x + (-s1*s2*s3-c1*c3) * p.y +  (s1*c2) * p.z ) +
                    q.z * ( (-c1*s2*c3-s1*s3) * p.x +  (c1*s2*s3-s1*c3) * p.y + (-c1*c2) * p.z );

        sum2      = q.x * (          (-c2*c3) * p.x +           (c2*s3) * p.y +     (s2) * p.z ) +
                    q.y * (        (s1*s2*c3) * p.x +       (-s1*s2*s3) * p.y +  (s1*c2) * p.z ) +
                    q.z * (       (-c1*s2*c3) * p.x +        (c1*s2*s3) * p.y + (-c1*c2) * p.z );

        sum3      = q.x * (          (-c2*c3) * p.x +           (c2*s3) * p.y) +
                    q.y * (  (s1*s2*c3-c1*s3) * p.x + (-s1*s2*s3-c1*c3) * p.y) +
                    q.z * ( (-c1*s2*c3-s1*s3) * p.x +  (c1*s2*s3-s1*c3) * p.y);

        sum4      = q.y * (       (-c1*c2*c3) * p.x +        (c1*c2*s3) * p.y +  (c1*s2) * p.z ) +
                    q.z * (       (-s1*c2*c3) * p.x +        (s1*c2*s3) * p.y +  (s1*s2) * p.z );

        sum5      = q.y * (  (c1*s2*s3-s1*c3) * p.x +  (c1*s2*c3+s1*s3) * p.y) +
                    q.z * (  (s1*s2*s3+c1*c3) * p.x +  (s1*s2*c3-c1*s3) * p.y);

        sum6      = q.x * (           (s2*s3) * p.x +           (s2*c3) * p.y) +
                    q.y * (        (s1*c2*s3) * p.x +        (s1*c2*c3) * p.y) +
                    q.z * (       (-c1*c2*s3) * p.x +       (-c1*c2*c3) * p.y);

        DF[0][0] += sum1*m_weights_loc[i];
        DF[1][1] += sum2*m_weights_loc[i];
        DF[2][2] += sum3*m_weights_loc[i];
        DF[0][1] += sum4*m_weights_loc[i];
        DF[1][0] += sum4*m_weights_loc[i];
        DF[0][2] += sum5*m_weights_loc[i];
        DF[2][0] += sum5*m_weights_loc[i];
        DF[1][2] += sum6*m_weights_loc[i];
        DF[2][1] += sum6*m_weights_loc[i];
    }

    return DF;
}

bool best_fit::Comp_Weights()
{
	double weight_low = 1, weight_high = 2;
    TopExp_Explorer aExpFace;
    MeshCore::MeshKernel FaceMesh;
    MeshCore::MeshFacetArray facetArr;

    MeshCore::MeshKernel mesh1,mesh2;
    MeshCore::MeshBuilder builder1(mesh1);
    MeshCore::MeshBuilder builder2(mesh2);
    builder1.Initialize(1000);
    builder2.Initialize(1000);

    Base::Vector3f Points[3];
    TopLoc_Location aLocation;

    bool bf;

    // explores all faces  ------------  Main loop
    for (aExpFace.Init(m_Cad,TopAbs_FACE);aExpFace.More();aExpFace.Next())
    {
        TopoDS_Face aFace = TopoDS::Face(aExpFace.Current());

        bf = false;
        for (unsigned int i=0; i<m_LowFaces.size(); ++i)
        {
            if (m_LowFaces[i].HashCode(IntegerLast()) == aFace.HashCode(IntegerLast())) bf = true;
        }

        // takes the triangulation of the face aFace:
        Handle_Poly_Triangulation aTr = BRep_Tool::Triangulation(aFace,aLocation);
        if (!aTr.IsNull()) // if this triangulation is not NULL
        {
            // takes the array of nodes for this triangulation:
            const TColgp_Array1OfPnt& aNodes = aTr->Nodes();

            // takes the array of triangles for this triangulation:
            const Poly_Array1OfTriangle& triangles = aTr->Triangles();

            // create array of node points in absolute coordinate system
            TColgp_Array1OfPnt aPoints(1, aNodes.Length());
            for ( Standard_Integer i = 1; i <= aNodes.Length(); i++)
                aPoints(i) = aNodes(i).Transformed(aLocation);

            // Takes the node points of each triangle of this triangulation.
            // takes a number of triangles:
            Standard_Integer nnn = aTr->NbTriangles();
            Standard_Integer nt,n1,n2,n3;
            for ( nt = 1 ; nt < nnn+1 ; nt++)
            {
                // takes the node indices of each triangle in n1,n2,n3:
                triangles(nt).Get(n1,n2,n3);
                // takes the node points:
                gp_Pnt aPnt1 = aPoints(n1);
                Points[0].Set(float(aPnt1.X()),float(aPnt1.Y()),float(aPnt1.Z()));
                gp_Pnt aPnt2 = aPoints(n2);
                Points[1].Set((float) aPnt2.X(),(float) aPnt2.Y(),(float) aPnt2.Z());
                gp_Pnt aPnt3 = aPoints(n3);
                Points[2].Set((float) aPnt3.X(),(float) aPnt3.Y(),(float) aPnt3.Z());

                // give the occ faces to the internal mesh structure of freecad
                MeshCore::MeshGeomFacet Face(Points[0],Points[1],Points[2]);

                if (bf == false) builder1.AddFacet(Face);
                else   builder2.AddFacet(Face);

            }
        }
    }

    builder1.Finish();
    builder2.Finish();

    m_pntCloud_1.clear();
    m_weights.clear();

    MeshCore::MeshPointArray pnts = mesh1.GetPoints();

    for (unsigned int i=0; i<pnts.size(); ++i)
    {
        m_pntCloud_1.push_back(pnts[i]);
        m_weights.push_back(weight_low);
    }

    pnts = mesh2.GetPoints();

    for (unsigned int i=0; i<pnts.size(); ++i)
    {
        m_pntCloud_1.push_back(pnts[i]);
        m_weights.push_back(weight_high);
    }

    m_normals = Comp_Normals(mesh1);
    std::vector<Base::Vector3f> tmp = Comp_Normals(mesh2);

    for (unsigned int i=0; i<tmp.size(); ++i)
    {
        m_normals.push_back(tmp[i]);
    }

    return true;
}

bool best_fit::RotMat(Base::Matrix4D &M, double degree, int axis)
{
    M.setToUnity();
    degree = 2*PI*degree/360;  // transformer bend

    switch (axis)
    {
    case 1:
        M[1][1]=cos(degree);
        M[1][2]=-sin(degree);
        M[2][1]=sin(degree);
        M[2][2]=cos(degree);
        break;
    case 2:
        M[0][0]=cos(degree);
        M[0][2]=-sin(degree);
        M[2][0]=sin(degree);
        M[2][2]=cos(degree);
        break;
    case 3:
        M[0][0]=cos(degree);
        M[0][1]=-sin(degree);
        M[1][0]=sin(degree);
        M[1][1]=cos(degree);
        break;
    default:
        throw Base::RuntimeError("second input value differs from 1,2,3 (x,y,z)");
    }

    return true;
}

bool best_fit::TransMat(Base::Matrix4D &M, double trans, int axis)
{
    M.setToUnity();

    switch (axis)
    {
    case 1:
        M[0][3] = trans;
        break;
    case 2:
        M[1][3] = trans;
        break;
    case 3:
        M[2][3] = trans;
        break;
    default:
        throw Base::RuntimeError("second input value differs from 1,2,3 (x,y,z)");
    }

    return true;
}

bool best_fit::PointNormalTransform(std::vector<Base::Vector3f> &pnts,
                                    std::vector<Base::Vector3f> &normals,
                                    Base::Matrix4D              &M)
{
    int m = pnts.size();
    Base::Vector3f pnt,normal;

    for (int i=0; i<m; ++i)
    {
        pnt.x  = float(M[0][0]*pnts[i].x + M[0][1]*pnts[i].y + M[0][2]*pnts[i].z + M[0][3]);
        pnt.y  = float(M[1][0]*pnts[i].x + M[1][1]*pnts[i].y + M[1][2]*pnts[i].z + M[1][3]);
        pnt.z  = float(M[2][0]*pnts[i].x + M[2][1]*pnts[i].y + M[2][2]*pnts[i].z + M[2][3]);

        pnts[i] = pnt;

        normal.x = float(M[0][0]*normals[i].x + M[0][1]*normals[i].y + M[0][2]*normals[i].z);
        normal.y = float(M[1][0]*normals[i].x + M[1][1]*normals[i].y + M[1][2]*normals[i].z);
        normal.z = float(M[2][0]*normals[i].x + M[2][1]*normals[i].y + M[2][2]*normals[i].z);

        normals[i] = normal;
    }

    return true;
}

bool best_fit::output_best_fit_mesh()
{
	
	SMDS_NodeIteratorPtr aNodeIter = m_meshtobefit->GetMeshDS()->nodesIterator();

	for(;aNodeIter->more();) 
	{
		const SMDS_MeshNode* aNode = aNodeIter->next();
		m_meshtobefit->GetMeshDS()->MoveNode(aNode,m_pntCloud_2[(aNode->GetID()-1)].x,m_pntCloud_2[(aNode->GetID()-1)].y,m_pntCloud_2[(aNode->GetID()-1)].z);
	}
	m_meshtobefit->ExportUNV("c:/best_fit_mesh.unv");

	return true;
}

bool best_fit::Initialize_Mesh_Geometrie_1()
{
	m_aMeshGen1 = new SMESH_Gen();
	m_referencemesh = m_aMeshGen1->CreateMesh(1,false);
	m_referencemesh->UNVToMesh("c:/cad_mesh_cenaero.unv");

	m_pntCloud_1.clear();

	//add the nodes
	SMDS_NodeIteratorPtr aNodeIter = m_referencemesh->GetMeshDS()->nodesIterator();
	for(;aNodeIter->more();) {
		const SMDS_MeshNode* aNode = aNodeIter->next();
		Base::Vector3f a3DVector;
		a3DVector.Set((float) aNode->X(),(float)  aNode->Y(),(float)  aNode->Z()),
		m_pntCloud_1.push_back(a3DVector);
    }



	return true;
}



bool best_fit::Initialize_Mesh_Geometrie_2()
{

	m_aMeshGen2 = new SMESH_Gen();
	m_meshtobefit = m_aMeshGen2->CreateMesh(1,false);
	m_meshtobefit->UNVToMesh("c:/mesh_cenaero.unv");

	m_pntCloud_2.clear();

	//add the nodes
	SMDS_NodeIteratorPtr aNodeIter = m_meshtobefit->GetMeshDS()->nodesIterator();
	for(;aNodeIter->more();) {
		const SMDS_MeshNode* aNode = aNodeIter->next();
		Base::Vector3f a3DVector;
		a3DVector.Set((float) aNode->X(),(float)  aNode->Y(), (float) aNode->Z()),
		m_pntCloud_2.push_back(a3DVector);
	}

	////add the 2D edge-Elements
	//SMDS_EdgeIteratorPtr 	anEdgeIter = Reference_Mesh->GetMeshDS()->edgesIterator();
	//for(;anEdgeIter->more();) {
	//	const SMDS_MeshEdge* anElem = anEdgeIter->next();
	//	myElements.push_back( anElem->GetID() );
	//}
	////add the 2D-Planar Elements like triangles 
	//SMDS_FaceIteratorPtr 	aFaceIter = Reference_Mesh->GetMeshDS()->facesIterator();
	//for(;aFaceIter->more();) {
	//	const SMDS_MeshFace* anElem = aFaceIter->next();
	//	int element_node_count = anElem->NbNodes();
	//	myElements.push_back( anElem->GetID() );
	//}
	////Add the Volume-Elements
	//SMDS_VolumeIteratorPtr aVolumeIter = Reference_Mesh->GetMeshDS()->volumesIterator();
	//for(;aVolumeIter->more();) {
	//	const SMDS_MeshVolume* anElem = aVolumeIter->next();
	//	myElements.push_back( anElem->GetID() );
	//}

	//int testsize = myElements.size();

	//SMDS_VolumeTool aTooling;


	////Now take the Element-Vector and work with the elements
	////check validity of element
	//for (unsigned int i=0;i<myElements.size();i++)
	//{
	//	const SMDS_MeshElement* CurrentElement = Reference_Mesh->GetMeshDS()->FindElement(myElements[i]);
	//	if (CurrentElement->GetType() == SMDSAbs_Volume) 
	//	{
	//		//We encountered a Surface-Element like a triangle and we have to check if its a triangle or not
	//		aTooling.Set(CurrentElement);
	//		//Now we have to check what kind of volume element we have
	//		if(aTooling.GetVolumeType()== SMDS_VolumeTool::HEXA)
	//		{
	//			//Found a HEXA Element
	//		}
	//	}
	//}

	return true;
}

bool best_fit::PointTransform(std::vector<Base::Vector3f> &pnts, const Base::Matrix4D &M)
{
    int m = pnts.size();
    Base::Vector3f pnt;

    for (int i=0; i<m; ++i)
    {
        pnt.x  =  float(M[0][0]*pnts[i].x + M[0][1]*pnts[i].y + M[0][2]*pnts[i].z + M[0][3]);
        pnt.y  =  float(M[1][0]*pnts[i].x + M[1][1]*pnts[i].y + M[1][2]*pnts[i].z + M[1][3]);
        pnt.z  =  float(M[2][0]*pnts[i].x + M[2][1]*pnts[i].y + M[2][2]*pnts[i].z + M[2][3]);

        pnts[i] = pnt;
    }

    return true;
}

bool best_fit::PointCloud_Coarse()
{
	GProp_GProps prop;
	GProp_PrincipalProps pprop;

	MeshCore::PlaneFit FitFunc_1, FitFunc_2;

	Base::Vector3f pnt(0.0,0.0,0.0);
	Base::Vector3f DirA_1, DirB_1, DirC_1, Grav_1,
  		           DirA_2, DirB_2, DirC_2, Grav_2;
	Base::Vector3f x,y,z;
	Base::Builder3D log3d_mesh, log3d_cad;
	gp_Pnt orig;

	gp_Vec v1,v2,v3,v,vec; // Major axis directions
	gp_Trsf trafo;

	FitFunc_1.Clear();
	FitFunc_2.Clear();

	FitFunc_1.AddPoints(m_pntCloud_1);
	FitFunc_2.AddPoints(m_pntCloud_2);
	
	FitFunc_1.Fit();
	FitFunc_2.Fit();

	DirA_1 = FitFunc_1.GetDirU();
	DirB_1 = FitFunc_1.GetDirV();
	DirC_1 = FitFunc_1.GetNormal();
	Grav_1 = FitFunc_1.GetGravity();

	m_cad2orig.SetX(-Grav_1.x);
	m_cad2orig.SetY(-Grav_1.y);
	m_cad2orig.SetZ(-Grav_1.z);

	DirA_2 = FitFunc_2.GetDirU();
	DirB_2 = FitFunc_2.GetDirV();
	DirC_2 = FitFunc_2.GetNormal();
	Grav_2 = FitFunc_2.GetGravity();

	Base::Matrix4D T5, T1;

	// Fills matrix T5 
	T5[0][0] = DirA_1.x;
	T5[1][0] = DirA_1.y;
	T5[2][0] = DirA_1.z;

	T5[0][1] = DirB_1.x;
	T5[1][1] = DirB_1.y;
	T5[2][1] = DirB_1.z;

	T5[0][2] = DirC_1.x;
	T5[1][2] = DirC_1.y;
	T5[2][2] = DirC_1.z;

	T5[0][3] = Grav_1.x;
	T5[1][3] = Grav_1.y;
	T5[2][3] = Grav_1.z;

	/*T5[0][0] = DirA_1.x;
	T5[0][1] = DirA_1.y;
	T5[0][2] = DirA_1.z;

	T5[1][0] = DirB_1.x;
	T5[1][1] = DirB_1.y;
	T5[1][2] = DirB_1.z;

	T5[2][0] = DirC_1.x;
	T5[2][1] = DirC_1.y;
	T5[2][2] = DirC_1.z;

	T5[0][3] = Grav_1.x;
	T5[1][3] = Grav_1.y;
	T5[2][3] = Grav_1.z;*/


	// Fills matrix T1
	T1[0][0] = DirA_2.x;
	T1[1][0] = DirA_2.y;
	T1[2][0] = DirA_2.z;

	T1[0][1] = DirB_2.x;
	T1[1][1] = DirB_2.y;
	T1[2][1] = DirB_2.z;

	T1[0][2] = DirC_2.x;
	T1[1][2] = DirC_2.y;
	T1[2][2] = DirC_2.z;

	T1[0][3] = Grav_2.x;
	T1[1][3] = Grav_2.y;
	T1[2][3] = Grav_2.z;

	v1.SetX(T5[0][0]);v1.SetY(T5[0][1]);v1.SetZ(T5[0][2]);
	v2.SetX(T5[1][0]);v2.SetY(T5[1][1]);v2.SetZ(T5[1][2]);
	v3.SetX(T5[2][0]);v3.SetY(T5[2][1]);v3.SetZ(T5[2][2]);

	v1.Normalize();
	v2.Normalize();
	v3.Normalize();

	v = v1;
	v.Cross(v2);

	// right-hand-system check
	if ( v.Dot(v3) < 0.0 )
		v3 *= -1;

	T1.inverse();

	orig.SetX(T5[0][3]);orig.SetY(T5[1][3]);orig.SetZ(T5[2][3]);

	// plot CAD -> local coordinate system

	x.x =  50.0f*(float)v1.X();	x.y =  50.0f*(float)v1.Y();	x.z =  50.0f*(float)v1.Z();
	y.x =  50.0f*(float)v2.X();	y.y =  50.0f*(float)v2.Y();	y.z =  50.0f*(float)v2.Z();
	z.x =  50.0f*(float)v3.X();	z.y =  50.0f*(float)v3.Y();	z.z =  50.0f*(float)v3.Z();

	pnt.x = (float) orig.X();
	pnt.y = (float) orig.Y();
	pnt.z = (float) orig.Z();

	log3d_cad.addSingleArrow(pnt,x,3,1,0,0);
	log3d_cad.addSingleArrow(pnt,y,3,0,1,0);
	log3d_cad.addSingleArrow(pnt,z,3,0,0,1);

	//log3d_cad.addSinglePoint(pnt,6,1,1,1);

	log3d_cad.saveToFile("c:/CAD_CoordSys.iv");

	PointTransform(m_pntCloud_2,T5*T1);

	//m_MeshWork.Transform(T1);
	// plot Mesh -> local coordinate system

	v1.SetX(T1[0][0]);v1.SetY(T1[0][1]);v1.SetZ(T1[0][2]);
	v2.SetX(T1[1][0]);v2.SetY(T1[1][1]);v2.SetZ(T1[1][2]);
	v3.SetX(T1[2][0]);v3.SetY(T1[2][1]);v3.SetZ(T1[2][2]);

	T1.inverse();
	orig.SetX(T1[0][3]);orig.SetY(T1[1][3]);orig.SetZ(T1[2][3]);

	x.x =  50.0f*(float)v1.X();	x.y =  50.0f*(float)v1.Y();	x.z =  50.0f*(float)v1.Z();
	y.x =  50.0f*(float)v2.X();	y.y =  50.0f*(float)v2.Y();	y.z =  50.0f*(float)v2.Z();
	z.x =  50.0f*(float)v3.X();	z.y =  50.0f*(float)v3.Y();	z.z =  50.0f*(float)v3.Z();

	pnt.x = (float) orig.X();
	pnt.y = (float) orig.Y();
	pnt.z = (float) orig.Z();

	log3d_mesh.addSingleArrow(pnt,x,3,1,0,0);log3d_mesh.addSingleArrow(pnt,y,3,0,1,0);log3d_mesh.addSingleArrow(pnt,z,3,0,0,1);
	log3d_mesh.addSinglePoint(0,0,0,20,1,1,1); // plot origins
	//log3d_mesh.addSinglePoint(pnt,6,0,0,0);
	log3d_mesh.saveToFile("c:/Mesh_CoordSys.iv");

	/*for(int i=0; i< m_pntCloud_2.size(); i++)
	{
		m_pntCloud_2[i].x = m_pntCloud_2[i].x + Grav_1.x - Grav_2.x;
		m_pntCloud_2[i].y = m_pntCloud_2[i].y + Grav_1.y - Grav_2.y;
		m_pntCloud_2[i].z = m_pntCloud_2[i].z + Grav_1.z - Grav_2.z;
	}*/

	return true;
}
bool best_fit::MeshFit_Coarse()
{

    GProp_GProps prop;
    GProp_PrincipalProps pprop;

    Base::Vector3f pnt(0.0,0.0,0.0);
    Base::Vector3f x,y,z;
    Base::Builder3D log3d_mesh, log3d_cad;
    gp_Pnt orig;

    gp_Vec v1,v2,v3,v,vec; // Major axis directions
    gp_Trsf trafo;

   /* BRepGProp::SurfaceProperties(m_Cad, prop);
    pprop = prop.PrincipalProperties();

    v1 = pprop.FirstAxisOfInertia();
    v2 = pprop.SecondAxisOfInertia();
    v3 = pprop.ThirdAxisOfInertia();*/

	MeshCore::MeshEigensystem pca(m_CadMesh);
    pca.Evaluate();
    Base::Matrix4D T5 =  pca.Transform();

	v1.SetX(T5[0][0]);v1.SetY(T5[0][1]);v1.SetZ(T5[0][2]);
    v2.SetX(T5[1][0]);v2.SetY(T5[1][1]);v2.SetZ(T5[1][2]);
    v3.SetX(T5[2][0]);v3.SetY(T5[2][1]);v3.SetZ(T5[2][2]);

    v1.Normalize();
    v2.Normalize();
    v3.Normalize();


    v = v1;
    v.Cross(v2);

    // right-hand-system check
    if ( v.Dot(v3) < 0.0 )
        v3 *= -1;

	T5.inverse();

    orig.SetX(T5[0][3]);orig.SetY(T5[1][3]);orig.SetZ(T5[2][3]);
    //orig  = prop.CentreOfMass();

    // plot CAD -> local coordinate system

	x.x =  50.0f*(float)v1.X();	x.y =  50.0f*(float)v1.Y();	x.z =  50.0f*(float)v1.Z();
    y.x =  50.0f*(float)v2.X();	y.y =  50.0f*(float)v2.Y();	y.z =  50.0f*(float)v2.Z();
    z.x =  50.0f*(float)v3.X();	z.y =  50.0f*(float)v3.Y();	z.z =  50.0f*(float)v3.Z();

    pnt.x = (float) orig.X();
	pnt.y = (float) orig.Y();
	pnt.z = (float) orig.Z();

    log3d_cad.addSingleArrow(pnt,x,3,1,0,0);
	log3d_cad.addSingleArrow(pnt,y,3,0,1,0);
	log3d_cad.addSingleArrow(pnt,z,3,0,0,1);
	
	//log3d_cad.addSinglePoint(pnt,6,1,1,1);
	
	log3d_cad.saveToFile("c:/CAD_CoordSys.iv");

    MeshCore::MeshEigensystem pca2(m_MeshWork);
    pca2.Evaluate();
    Base::Matrix4D T1 =  pca2.Transform();
    m_MeshWork.Transform(T5*T1);
	//m_MeshWork.Transform(T1);
    // plot Mesh -> local coordinate system

	
    v1.SetX(T1[0][0]);v1.SetY(T1[0][1]);v1.SetZ(T1[0][2]);
    v2.SetX(T1[1][0]);v2.SetY(T1[1][1]);v2.SetZ(T1[1][2]);
    v3.SetX(T1[2][0]);v3.SetY(T1[2][1]);v3.SetZ(T1[2][2]);


    
    T1.inverse();
    orig.SetX(T1[0][3]);orig.SetY(T1[1][3]);orig.SetZ(T1[2][3]);

	x.x = 50.0f*(float)v1.X();	x.y = 50.0f*(float)v1.Y();	x.z = 50.0f*(float)v1.Z();
    y.x = 50.0f*(float)v2.X();	y.y = 50.0f*(float)v2.Y();	y.z = 50.0f*(float)v2.Z();
    z.x = 50.0f*(float)v3.X();	z.y = 50.0f*(float)v3.Y();	z.z = 50.0f*(float)v3.Z();

    pnt.x = (float) orig.X();
	pnt.y = (float) orig.Y();
	pnt.z = (float) orig.Z();

	log3d_mesh.addSingleArrow(pnt,x,3,1,0,0);log3d_mesh.addSingleArrow(pnt,y,3,0,1,0);log3d_mesh.addSingleArrow(pnt,z,3,0,0,1);
	log3d_mesh.addSinglePoint(0,0,0,20,1,1,1); // plot origins
    //log3d_mesh.addSinglePoint(pnt,6,0,0,0);
	log3d_mesh.saveToFile("c:/Mesh_CoordSys.iv");

    return true;
}

bool best_fit::ShapeFit_Coarse()
{
    GProp_GProps prop;
    BRepGProp    SurfProp;
    gp_Trsf      trafo;
    gp_Pnt       orig;

    SurfProp.SurfaceProperties(m_Cad, prop);
    orig  = prop.CentreOfMass();

    // CAD-Mesh -> back to the origin
    m_cad2orig.SetX(-orig.X());
    m_cad2orig.SetY(-orig.Y());
    m_cad2orig.SetZ(-orig.Z());

    trafo.SetTranslation(m_cad2orig);
    BRepBuilderAPI_Transform trsf(trafo);

    trsf.Perform(m_Cad);
    m_Cad = trsf.Shape();

    return true;
}

bool best_fit::Tesselate_Face(const TopoDS_Face &aface, MeshCore::MeshKernel &mesh, float deflection)
{
    Base::Builder3D aBuild;
    MeshCore::MeshBuilder builder(mesh);
    builder.Initialize(1000);
    Base::Vector3f Points[3];
    if (!BRepTools::Triangulation(aface,0.1))
    {
        // removes all the triangulations of the faces,
        // and all the polygons on the triangulations of the edges:
        BRepTools::Clean(aface);

        // adds a triangulation of the shape aShape with the deflection aDeflection:
        //BRepMesh_IncrementalMesh Mesh(pcShape->getShape(),aDeflection);

/*The next two lines have been from the occ6.2 adapt mesh library. They do not work within OCC6.3
      TriangleAdapt_Parameters MeshingParams;
       BRepMeshAdapt::Mesh(aface,deflection,MeshingParams);
*/
       BRepMesh_IncrementalMesh Mesh(aface,deflection);
    }
    TopLoc_Location aLocation;
    // takes the triangulation of the face aFace:
    Handle_Poly_Triangulation aTr = BRep_Tool::Triangulation(aface,aLocation);
    if (!aTr.IsNull()) // if this triangulation is not NULL
    {
        // takes the array of nodes for this triangulation:
        const TColgp_Array1OfPnt& aNodes = aTr->Nodes();
        // takes the array of triangles for this triangulation:
        const Poly_Array1OfTriangle& triangles = aTr->Triangles();
        // create array of node points in absolute coordinate system
        TColgp_Array1OfPnt aPoints(1, aNodes.Length());
        for ( Standard_Integer i = 1; i < aNodes.Length()+1; i++)
            aPoints(i) = aNodes(i).Transformed(aLocation);
        // Takes the node points of each triangle of this triangulation.
        // takes a number of triangles:
        Standard_Integer nnn = aTr->NbTriangles();
        Standard_Integer nt,n1,n2,n3;
        for ( nt = 1 ; nt < nnn+1 ; nt++)
        {
            // takes the node indices of each triangle in n1,n2,n3:
            triangles(nt).Get(n1,n2,n3);
            // takes the node points:
            gp_Pnt aPnt1 = aPoints(n1);
            Points[0].Set(float(aPnt1.X()),float(aPnt1.Y()),float(aPnt1.Z()));
            gp_Pnt aPnt2 = aPoints(n2);
            Points[1].Set((float) aPnt2.X(),(float) aPnt2.Y(),(float) aPnt2.Z());
            gp_Pnt aPnt3 = aPoints(n3);
            Points[2].Set((float) aPnt3.X(),(float) aPnt3.Y(),(float) aPnt3.Z());
            // give the occ faces to the internal mesh structure of freecad
            MeshCore::MeshGeomFacet Face(Points[0],Points[1],Points[2]);
            builder.AddFacet(Face);
        }

    }
    // if the triangulation of only one face is not possible to get
    else
    {
        throw Base::RuntimeError("Empty face triangulation\n");
    }

    // finish FreeCAD Mesh Builder and exit with new mesh
    builder.Finish();
    return true;
}


bool best_fit::Tesselate_Shape(const TopoDS_Shape &shape, MeshCore::MeshKernel &mesh, float deflection)
{
    Base::Builder3D aBuild;

    MeshCore::MeshDefinitions::_fMinPointDistanceD1 = (float) 0.0001;
    MeshCore::MeshBuilder builder(mesh);
    builder.Initialize(1000);
    Base::Vector3f Points[3];

    //bool check = BRepUtils::CheckTopologie(shape);
    //if (!check)
    //{
    //    cout <<"an error"<< endl;
    //}

    //BRepBuilderAPI_Sewing aSewer;
    //aSewer.Load(shape);
    //aSewer.Perform();
    //aSewer.IsModified(shape);
    //const TopoDS_Shape& asewedShape = aSewer.SewedShape();
    //if (asewedShape.IsNull())
    //{
    //    cout << "Nothing Sewed" << endl;
    //}
    //int test = aSewer.NbFreeEdges();
    //int test1 = aSewer.NbMultipleEdges();
    //int test2 = aSewer.NbDegeneratedShapes();

    // adds a triangulation of the shape aShape with the deflection deflection:
/*
    TriangleAdapt_Parameters MeshParams;
    MeshParams._minAngle = 30.0;
    //MeshParams._minNbPntsPerEdgeLine = 3;
    //MeshParams._minNbPntsPerEdgeOther = 3;
    //MeshParams._minEdgeSplit = 3;
	MeshParams._maxTriangleSideSize = 10; //10
	MeshParams._maxArea = 10; //50
	*/
    BRepMesh_IncrementalMesh Mesh(shape,deflection);
    //BRepMesh::Mesh(shape,deflection);
    TopExp_Explorer aExpFace;


    for (aExpFace.Init(shape,TopAbs_FACE);aExpFace.More();aExpFace.Next())
    {

        TopoDS_Face aFace = TopoDS::Face(aExpFace.Current());

        TopLoc_Location aLocation;

        // takes the triangulation of the face aFace:
        Handle_Poly_Triangulation aTr = BRep_Tool::Triangulation(aFace,aLocation);
        if (!aTr.IsNull()) // if this triangulation is not NULL
        {
            // takes the array of nodes for this triangulation:
            const TColgp_Array1OfPnt& aNodes = aTr->Nodes();
            // takes the array of triangles for this triangulation:
            const Poly_Array1OfTriangle& triangles = aTr->Triangles();
            // create array of node points in absolute coordinate system
            TColgp_Array1OfPnt aPoints(1, aNodes.Length());
            for ( Standard_Integer i = 1; i <= aNodes.Length(); i++)
                aPoints(i) = aNodes(i).Transformed(aLocation);
            // Takes the node points of each triangle of this triangulation.
            // takes a number of triangles:
            Standard_Integer nnn = aTr->NbTriangles();
            Standard_Integer nt,n1,n2,n3;
            for ( nt = 1 ; nt < nnn+1 ; nt++)
            {
                // takes the node indices of each triangle in n1,n2,n3:
                triangles(nt).Get(n1,n2,n3);
                // takes the node points:
                gp_Pnt aPnt1 = aPoints(n1);
                Points[0].Set(float(aPnt1.X()),float(aPnt1.Y()),float(aPnt1.Z()));
                gp_Pnt aPnt2 = aPoints(n2);
                Points[1].Set((float) aPnt2.X(),(float) aPnt2.Y(),(float) aPnt2.Z());
                gp_Pnt aPnt3 = aPoints(n3);
                Points[2].Set((float) aPnt3.X(),(float) aPnt3.Y(),(float) aPnt3.Z());
                // give the occ faces to the internal mesh structure of freecad
                MeshCore::MeshGeomFacet Face(Points[0],Points[1],Points[2]);
                builder.AddFacet(Face);
            }
        }
        // if the triangulation of only one face is not possible to get
        else
        {
            throw Base::RuntimeError("Empty face triangulation\n");
        }
    }
    // finish FreeCAD Mesh Builder and exit with new mesh
    builder.Finish();

    /*MeshCore::MeshAlgorithm algo(mesh);
    std::list< std::vector <unsigned long> > BoundariesIndex;
    algo.GetMeshBorders(BoundariesIndex);*/

    return true;
}

std::vector<Base::Vector3f> best_fit::Comp_Normals(MeshCore::MeshKernel &M)
{
    Base::Builder3D log3d;
    Base::Vector3f normal,local_normal,origPoint;
    MeshCore::MeshRefPointToFacets rf2pt(M);
    MeshCore::MeshGeomFacet        t_face;
    MeshCore::MeshPoint mPnt;
    std::vector<Base::Vector3f>    normals;

    int NumOfPoints = M.CountPoints();
    float local_Area;
    float fArea;

    for (int i=0; i<NumOfPoints; ++i)
    {
        // Set of triangles at each point
        mPnt = M.GetPoint(i);
        origPoint.x = mPnt.x;
        origPoint.y = mPnt.y;
        origPoint.z = mPnt.z;

        const std::set<unsigned long>& faceSet = rf2pt[i];
        fArea = 0.0;
        normal.Set(0.0,0.0,0.0);

        // Iterate over the triangles to each point
        for (std::set<unsigned long>::const_iterator it = faceSet.begin(); it != faceSet.end(); ++it)
        {
            // Dereference twice to get to the MeshFacet and hand over
            // to the kernel that it delivers a MeshGeomFacet
            t_face = M.GetFacet(*it);
            // Sum up the area content
            local_Area = t_face.Area();
            local_normal = t_face.GetNormal();
            if (local_normal.z < 0)
            {
                local_normal = local_normal * (-1);
            }

            fArea  = fArea  + local_Area;
            normal = normal + local_Area*local_normal;

        }

        normal.Normalize();
        normals.push_back(normal);

        log3d.addSingleArrow(origPoint,origPoint+normal,1,0,0,0);
    }

    log3d.saveToFile("c:/normals.iv");

    return normals;
}

/*
double best_fit::CompError(std::vector<Base::Vector3f> &pnts, std::vector<Base::Vector3f> &normals)
{
    double err_avg = 0.0;
    double err_max = 0.0;
    double sqrdis = 0.0;

    MeshCore::MeshFacetGrid aFacetGrid(m_CadMesh);
    MeshCore::MeshAlgorithm malg(m_CadMesh);
    MeshCore::MeshAlgorithm malg2(m_CadMesh);

    Base::Vector3f origPoint, projPoint, distVec;
    unsigned long  facetIndex;

    int NumOfPoints = pnts.size();
    int c = 0;

    for (int i=0; i<NumOfPoints; ++i)
    {
        if (!malg.NearestFacetOnRay(pnts[i], normals[i], aFacetGrid, projPoint, facetIndex))  // gridoptimiert
        {
            if (malg2.NearestFacetOnRay(pnts[i], normals[i], projPoint, facetIndex))
            {
                distVec  = projPoint-pnts[i];
                sqrdis   = distVec*distVec;
                err_avg += sqrdis;
            }
            else
                ++c;
        }
        else
        {
            distVec  = projPoint-pnts[i];
            sqrdis   = distVec*distVec;
            err_avg += sqrdis;
        }
    }

    if (c==NumOfPoints)
        return 1e+10;

    return err_avg/(NumOfPoints-c);
}

std::vector<double> best_fit::CompError_GetPnts(std::vector<Base::Vector3f> pnts,
        std::vector<Base::Vector3f> &normals)

{
    double err_avg = 0.0;
    double dist;

    std::vector<double> errVec(2);    // errVec[0]: avg. error, errVec[1]: max. error

    MeshCore::MeshFacetGrid aFacetGrid(m_Mesh);
    MeshCore::MeshAlgorithm malg(m_Mesh);
    unsigned long  facetIndex;

    Base::Vector3f origPoint, projPoint, distVec;
    //Base::Builder3D log;

    unsigned int NumOfPoints = pnts.size();
    int c = 0;

    m_LSPnts[0].clear();
    m_LSPnts[1].clear();

    m_weights_loc.clear();

    for (unsigned int i=0; i<NumOfPoints; ++i)
    {
        if (!malg.NearestFacetOnRay(pnts[i], normals[i], aFacetGrid, projPoint, facetIndex))
               // !Intersect(normals[i], *m_Mesh, pnts[i], projPoint)  // gridoptimiert
        {
            ++c;
        }
        else
        {
            m_LSPnts[0].push_back(projPoint);
            m_LSPnts[1].push_back(pnts[i]);
            m_weights_loc.push_back(m_weights[i]);

            //log.addSingleArrow(pnts[i], projPoint);
        }
    }

    double max_err = 0.0;

    for (unsigned int i=0; i<NumOfPoints-c; ++i)
    {
        distVec  = m_LSPnts[0][i]-m_LSPnts[1][i];
        dist     = distVec*distVec;
        err_avg += dist;

        if (dist > max_err)
            max_err = dist;
    }

    //log.saveToFile("c:/intersection.iv");

    errVec[0] = err_avg /= (NumOfPoints-c);  // durchschnittlicher Fehlerquadrat
    errVec[1] = max_err;         // maximaler Fehlerquadrat

    //cout << c << " projections failed" << endl;

    return errVec;
}

double best_fit::CompError(std::vector<Base::Vector3f> &pnts, std::vector<Base::Vector3f> &normals, bool plot)
{
    if (plot==false)
        return CompError(pnts, normals);
    else
    {
        Base::Builder3D log3d;
        double err_avg = 0.0;
        double err_max = 0.0;
        double sqrdis  = 0.0;


        MeshCore::MeshFacetGrid aFacetGrid(m_CadMesh);
        MeshCore::MeshAlgorithm malg(m_CadMesh);
        MeshCore::MeshAlgorithm malg2(m_CadMesh);

        Base::Vector3f projPoint, distVec;
        unsigned long  facetIndex;

        int NumOfPoints = pnts.size();
        int c=0;

        for (int i=0; i<NumOfPoints; ++i)
        {
            if (!malg.NearestFacetOnRay(pnts[i], normals[i], aFacetGrid, projPoint, facetIndex))   // gridoptimiert
            {
                if (malg2.NearestFacetOnRay(pnts[i], normals[i], projPoint, facetIndex))
                {

                    log3d.addSingleArrow(pnts[i],projPoint, 3, 0,0,0);
                    distVec  = projPoint-pnts[i];
                    sqrdis   = distVec*distVec;
                    err_avg += sqrdis;
                }
                else
                    c++;

            }
            else
            {
                log3d.addSingleArrow(pnts[i],projPoint, 3, 0,0,0);
                distVec  = projPoint-pnts[i];
                sqrdis   = distVec*distVec;
                err_avg += sqrdis;
            }
        }

        log3d.saveToFile("c:/projection.iv");

        if (c>(NumOfPoints/2))
            return 1e+10;

        return err_avg/(NumOfPoints-c);
    }
}
*/

double best_fit::CompTotalError()
{
    Base::Builder3D log3d;
    double err_avg = 0.0;
    double err_max = 0.0;
    double sqrdis  = 0.0;

    std::vector<int> FailProj;

    MeshCore::MeshFacetGrid aFacetGrid(m_Mesh,10);
    MeshCore::MeshAlgorithm malg(m_Mesh);
    MeshCore::MeshAlgorithm malg2(m_Mesh);
    MeshCore::MeshPointIterator p_it(m_CadMesh);

    Base::Vector3f projPoint, distVec, projPoint2;
    unsigned long  facetIndex;
    std::stringstream text;
    m_error.resize(m_CadMesh.CountPoints());

    unsigned int c=0;
    int i=0;

	
    m_LSPnts[0].clear();
    m_LSPnts[1].clear();
    for (p_it.Begin(); p_it.More(); p_it.Next())
    {
        if (malg.NearestFacetOnRay(*p_it, m_normals[i], aFacetGrid, projPoint, facetIndex))   // grid-optimized
        {
            log3d.addSingleArrow(*p_it, projPoint, 3, 0,0,0);
            distVec  = projPoint - *p_it;
            sqrdis   = distVec*distVec;

			m_LSPnts[1].push_back(*p_it);
            m_LSPnts[0].push_back(projPoint);

            if (((projPoint.z - p_it->z) / m_normals[i].z ) > 0)
                m_error[i] = sqrt(sqrdis);
            else
                m_error[i] = -sqrt(sqrdis);

            err_avg += sqrdis;
        }
        else
        {

            if (!malg2.NearestFacetOnRay(*p_it, m_normals[i], projPoint, facetIndex))   // not grid-optimized
            {
                c++;
                //m_normals[i].Scale(-10,-10,-10);
                text << p_it->x << ", " << p_it->y << ", " << p_it->z << "; " << m_normals[i].x << ", " << m_normals[i].y << ", " << m_normals[i].z;
                //log3d.addSingleArrow(*p_it, *p_it + m_normals[i], 4, 1,0,0);
                //log3d.addText(*p_it,(text.str()).c_str());
            }
            else
			{
				log3d.addSingleArrow(*p_it, projPoint, 3, 0,0,0);
				distVec  = projPoint - *p_it;
				sqrdis   = distVec*distVec;

				m_LSPnts[1].push_back(*p_it);
				m_LSPnts[0].push_back(projPoint);

				if (((projPoint.z - p_it->z) / m_normals[i].z ) > 0)
					m_error[i] = sqrt(sqrdis);
				else
					m_error[i] = -sqrt(sqrdis);

				err_avg += sqrdis;
			}
		}
        

        ++i;
    }

    //for (p_it.Begin(); p_it.More(); p_it.Next())
//   {
    //    if (!malg.NearestFacetOnRay(*p_it, m_normals[i], aFacetGrid, projPoint, facetIndex))   // grid-optimized
//       {
//           if (malg2.NearestFacetOnRay(*p_it, m_normals[i], projPoint, facetIndex))
//           {

//               log3d.addSingleArrow(*p_it, projPoint, 3, 0,0,0);
//               distVec  = projPoint - *p_it;
//               sqrdis   = distVec*distVec;

//               if (((projPoint.z - p_it->z) / m_normals[i].z ) > 0)
//                   m_error[i] = sqrt(sqrdis);
//               else
//                   m_error[i] = -sqrt(sqrdis);

//               err_avg += sqrdis;
//           }
//           else
//           {
//               c++;
//               FailProj.push_back(i);
//           }
//       }
//       else
//       {
//           distVec  = projPoint-*p_it;
//           sqrdis   = distVec*distVec;

//           m_normals[i].Scale(-1,-1,-1);

//           if (malg.NearestFacetOnRay(*p_it, m_normals[i], aFacetGrid, projPoint2, facetIndex))
//           {
//               distVec  = projPoint2-*p_it;
//               if (sqrdis > distVec*distVec)
//               {
//                   sqrdis   = distVec*distVec;
//                   log3d.addSingleArrow(*p_it, projPoint2, 3, 0,0,0);
//               }
//               else
//               {
//                   log3d.addSingleArrow(*p_it, projPoint, 3, 0,0,0);
//               }

//           }
//           m_normals[p_it.Position()].Scale(-1,-1,-1);

//           if (((projPoint.z - p_it->z) / m_normals[i].z ) > 0)
//               m_error[i] = sqrt(sqrdis);
//           else
//               m_error[i] = -sqrt(sqrdis);

//           err_avg += sqrdis;
//       }
//       ++i;
    //}


    MeshCore::MeshRefPointToPoints vv_it(m_CadMesh);
    MeshCore::MeshPointArray::_TConstIterator v_beg = m_CadMesh.GetPoints().begin();

    std::set<unsigned long>::const_iterator v_it;
    for (unsigned int i=0; i<FailProj.size(); ++i)
    {
        const std::set<unsigned long>& PntNei = vv_it[FailProj[i]];
        m_error[FailProj[i]] = 0.0;

        for (v_it = PntNei.begin(); v_it !=PntNei.end(); ++v_it)
        {
            m_error[FailProj[i]] += m_error[v_beg[*v_it]._ulProp];
        }

        m_error[FailProj[i]] /= double(PntNei.size());
    }


    log3d.saveToFile("c:/projection.iv");

    if (c>(m_CadMesh.CountPoints()/2))
        return 1e+10;

    return err_avg/(m_CadMesh.CountPoints()-c);

}

double best_fit::CompTotalError(MeshCore::MeshKernel &mesh)
{
    double err_avg = 0.0;
    double err_max = 0.0;
    double sqrdis  = 0.0;

    std::vector<int> FailProj;

    MeshCore::MeshFacetGrid aFacetGrid(mesh,10);
    MeshCore::MeshAlgorithm malg(mesh);
    MeshCore::MeshAlgorithm malg2(mesh);
    MeshCore::MeshPointIterator p_it(m_CadMesh);

    Base::Vector3f projPoint, distVec, projPoint2;
    unsigned long  facetIndex;
    std::stringstream text;
    m_error.resize(m_CadMesh.CountPoints());

    unsigned int c=0;
    int i=0;

    for (p_it.Begin(); p_it.More(); p_it.Next())
    {
        if (malg.NearestFacetOnRay(*p_it, m_normals[i], aFacetGrid, projPoint, facetIndex))   // grid-optimized
        {
            distVec  = projPoint - *p_it;
            sqrdis   = distVec*distVec;

            if (((projPoint.z - p_it->z) / m_normals[i].z ) > 0)
                m_error[i] += sqrt(sqrdis);
            else
                m_error[i] += -sqrt(sqrdis);

            err_avg += sqrdis;
        }
        else
        {

            if (!malg2.NearestFacetOnRay(*p_it, m_normals[i], projPoint, facetIndex))   // not grid-optimized
            {
                c++;
                FailProj.push_back(i);
            }
            else
            {
				distVec  = projPoint - *p_it;
				sqrdis   = distVec*distVec;

				if (((projPoint.z - p_it->z) / m_normals[i].z ) > 0)
					m_error[i] += sqrt(sqrdis);
				else
					m_error[i] += -sqrt(sqrdis);

				err_avg += sqrdis;
			}
		}
        

        ++i;
    }

    MeshCore::MeshRefPointToPoints vv_it(m_CadMesh);
    MeshCore::MeshPointArray::_TConstIterator v_beg = m_CadMesh.GetPoints().begin();

	double error;
	std::set<unsigned long>::const_iterator v_it;
    for (unsigned int i=0; i<FailProj.size(); ++i)
    {
        const std::set<unsigned long>& PntNei = vv_it[FailProj[i]];
		error = 0.0;


        for (v_it = PntNei.begin(); v_it !=PntNei.end(); ++v_it)
        {
			error += m_error[v_beg[*v_it]._ulProp];
        }

		error /= double(PntNei.size());
        m_error[FailProj[i]] += error;
    }


    if (c>(m_CadMesh.CountPoints()/2))
        return 1e+10;

    return err_avg/(m_CadMesh.CountPoints()-c);
}