//
//  hole_filling.cpp
//  test1
//
//  Created by Bac Alexandra on 12/04/2022.
//

#include <CGAL/boost/graph/IO/polygon_mesh_io.h>
#include <CGAL/IO/Color.h>
#include "hole_filling.h"

using namespace std ;

std::function<bool(Mesh::Vertex_index)> Hole_Filling::_f_select = Hole_Filling::f_true ;

const int nsteps = 20 ;

/* ************** Implicit RBF ************** */

Implicit_RBF::Implicit_RBF(vector<double> alpha, vector<double> beta, vector<K::Point_3> center) : _alpha(alpha), _beta(beta), _center(center)
{
	_n = _alpha.size() ;
	_d = _beta.size() ;
	if (_center.size() != _n)
		throw runtime_error("Inconsistent size of alpha and centers in Implicit_RBF constructor.");
}

// Computation of the value of the implicit surface at point X
double Implicit_RBF::val(K::Point_3 X) const
{
	double res = 0 ;
	// Computation of the sum of RBF at centers
	for(int i=0; i<_n; i++)
	{
		double r = sqrt((X-_center.at(i)).squared_length()) ;
		res += _alpha.at(i) * myphi(r) ;
	}
	// Computation of the polynomial part
	for(int j=0; j<_d; j++)
	{
		res += _beta.at(j) * myp(j, X) ;
	}
	return res ;
}

/* ************** Hole Filling ************** */

Hole_Filling::Hole_Filling(MyMesh_Norm &mesh) : _mesh(mesh)
{
	// Allocation de vprop
	bool created_vprop;
	boost::tie(vprop, created_vprop) = _mesh._m.add_property_map<vertex_descriptor,bool>("v:prop",false);
	assert(created_vprop);
    
	cout << "Starting hole filling ..." << endl ;
}


// ***** Computation of boundary and its neighborhood

Mesh::Halfedge_index Hole_Filling::find_boundary_edge()
{
	Mesh::Halfedge_range her = _mesh._m.halfedges() ;
	
	Mesh::Halfedge_range::iterator it = her.begin();
	while ( (it != her.end()) && (!_mesh._m.is_border(*it)) )
	{
		++it ;
	}
	if (it != her.end())
		return *it ;
	else
		throw std::runtime_error("Boundary HE does not exist") ;
}

vector<Mesh::Vertex_index> Hole_Filling::find_boundary(Mesh::Halfedge_index heh)
{
	Mesh::Halfedge_index heh_ini = heh ;
	vector<Mesh::Vertex_index> boundary ;

	// Follow (and memorize) boundary edges
	do
	{
		boundary.push_back(_mesh._m.target(heh));
		heh = _mesh._m.next(heh);
	} while (heh != heh_ini) ;
	return boundary ;
}

void Hole_Filling::init_mark_boundary(const vector<Mesh::Vertex_index> & bnd)
{
	Mesh::Vertex_range vr = _mesh._m.vertices() ;
	for (Mesh::Vertex_range::iterator v_it = vr.begin() ; v_it != vr.end(); ++v_it)
		vprop[*v_it] = false ;
	for (int i=0; i<bnd.size(); ++i)
		vprop[bnd.at(i)] = true ;
}

vector<Mesh::Vertex_index> Hole_Filling::next_neighbors(const vector<Mesh::Vertex_index> & bnd)
{
	// Visit bnd vertices to find and mark next circle
	vector<Mesh::Vertex_index> next_bnd ;
	for (int i=0; i<bnd.size(); i++)
	{
		CGAL::Vertex_around_target_circulator<Mesh> v_it(_mesh._m.halfedge(bnd.at(i)),_mesh._m), v_end(v_it) ;
		do {
			if(vprop[*v_it] == false) // new vertex
			{
				vprop[*v_it] = true ;
				next_bnd.push_back(*v_it);
			}
			++v_it ;
		} while (v_it != v_end) ;
	}

	return next_bnd ;
}

// ***** Computation of RBF

pair<pair<Eigen::MatrixXd &, Eigen::VectorXd &>, vector<K::Point_3> &> Hole_Filling::compute_approx_mat(vector<Mesh::Vertex_index> vlist, double scale)
{
	const int n(vlist.size()), d(10) ;

	vector<K::Point_3> & pts_list = *(new vector<K::Point_3>);
	//Append vertices to pts_list
	for (int i=0; i<n; i++)
	{
		pts_list.push_back(_mesh._m.point(vlist.at(i))) ;
	}
	//Append vertices+normals to pts_list
	int n1 = 0 ;
	for (int i=0; i<n; i++)
	{
		K::Vector_3 normale = _mesh.vnormal[vlist.at(i)] ;
		
		if (_f_select(vlist.at(i)))
		{
			++n1 ;
			pts_list.push_back(_mesh._m.point(vlist.at(i)) + scale*normale) ;
		}
	}
	//Append vertices-normals to pts_list
	for (int i=0; i<n; i++)
	{
		if (_f_select(vlist.at(i)))
			pts_list.push_back(_mesh._m.point(vlist.at(i)) - scale*_mesh.vnormal[vlist.at(i)]) ;
	}

	int nn = pts_list.size() ;

	Eigen::MatrixXd & A = *(new Eigen::MatrixXd(nn+d,nn+d)) ;
	Eigen::MatrixXd Phi(nn,nn);
	Eigen::MatrixXd P(nn,d);
	Eigen::VectorXd & B = *(new Eigen::VectorXd(nn+d));

	// Compute corresponding B vector (0 / 1 / -1 )
	B << Eigen::VectorXd::Zero(n), Eigen::VectorXd::Ones(n1), -Eigen::VectorXd::Ones(n1), Eigen::VectorXd::Zero(d) ;

	// Fill Phi matrix
	for (int i=0; i< nn; i++)
	{
		for (int j=0; j<i; j++)
		{
			Phi(i,j) = myphi(sqrt((pts_list.at(i)-pts_list.at(j)).squared_length())) ; // phi(|| c_i - c_j ||)
			Phi(j,i) = Phi(i,j) ;
		}
		Phi(i,i) = 0 ;
	}

	// Fill P matrix
	for (int i=0; i<nn; i++)
	{
		for (int j=0; j<d; j++)
		{
			P(i,j) = myp(j,pts_list.at(i)) ;
		}
	}
	// Set final A matrix
	/* A = Phi | P
	 *      P' | 0    */

	A << Phi, P, P.transpose(), Eigen::MatrixXd::Zero(d,d) ;
	cout << "size of pts_list : " << nn << endl ;
	cout << "End computation of matrices" << endl ;

	return {{A,B},pts_list} ;
}

pair<vector<double>&, vector<double>&> Hole_Filling::solve_approx(const pair<Eigen::MatrixXd &, Eigen::VectorXd &> &p, int n, int d)
{
	Eigen::MatrixXd & A = p.first ;
	Eigen::VectorXd & B = p.second ;

	Eigen::VectorXd res = A.householderQr().solve(B) ;

	cout << "End of solver" << endl ;
	cout << "res : " << endl << res.head(10) << endl ;
	vector<double> & alpha = *(new vector<double>);
	vector<double> & beta = *(new vector<double>);

	if (res.size() != (n+d))
	{
		cout << "taille du res : " << res.size() << endl ;
		throw std::runtime_error("Error in solve_approx") ;
	}
	for (int i=0; i<n; i++)
	{
		alpha.push_back(res(i)) ;
	}

	for (int j=0; j<d; j++)
	{
		beta.push_back(res(n+j)) ;
	}

	return {alpha, beta} ;
}

// ***** Hole filling

std::shared_ptr<Implicit_RBF>  Hole_Filling::fill_hole(std::vector<Mesh::Vertex_index>& boundary)
{
    CGAL::IO::write_polygon_mesh("test_norm.obj", _mesh._m, CGAL::parameters::vertex_normal_map(_mesh.vnormal)) ;
    try
    {
        // Find an initial boundary halfedge
        Mesh::Halfedge_index heh = find_boundary_edge() ;
        cout << "demi arete de bord trouvée" << endl ;
        // Compute the connected vector of boundary vertices
        boundary = find_boundary(heh) ;
        cout << "bord de " << boundary.size() << " vertex trouves" << endl ;
        // Mark these boundary vertices
        init_mark_boundary(boundary) ;
        // Compute the neighborhood of these vertices (so the 2-neighborhood of the hole)
        // Corresponding vertices are "marked" as true in the vprop property
        vector<Mesh::Vertex_index> bnd2 = next_neighbors(boundary) ;
        cout << "1er voisins : " << bnd2.size() << " vertex trouves" << endl ;
        
        // Colorize (red) the marked vertices
        colorize_prop();
        
        // Merge 1 and 2-neighbor vertices
        boundary.insert(boundary.end(), bnd2.begin(), bnd2.end()) ;
        
        // Vizualisation of the boundary
//        CGAL::IO::write_OFF("boundary.off", _mesh._m, CGAL::parameters::vertex_color_map(_mesh.vcolor));
        
        cout << "nombre total de sommets pour l'approximation : " << boundary.size() << endl ;
        
        cout << "starting computation of approx matrices" << endl ;
        // Compute matrices A and B of the interpolation problem (see TDP4 subject)
        pair<pair<Eigen::MatrixXd &,Eigen::VectorXd &>,vector<K::Point_3> &> sol = compute_approx_mat(boundary, .01) ;
        // Extract/test returned data
        // Centers
        vector<K::Point_3> & centers(sol.second) ;
        // Matrix A
        cout << "starting solving" << endl ;
        cout << "taille de A : " << (sol.first.first).rows() << ", " << (sol.first.first).cols() << endl ;
        // Vector B
        cout << "taille de B : " << (sol.first.second).size() << endl ;
        cout << "nombre de centres : " << centers.size() << endl;
        
        // Solve the linear system AX=B
        pair<vector<double>, vector<double>> p_coefs = solve_approx(sol.first, centers.size(), 10) ;
        cout << "taille de alpha : " << p_coefs.first.size() << endl ;
        cout << "taille de beta : " << p_coefs.second.size() << endl ;
        
        cout << "end solving" << endl ;
        
        // DEBUG
//        cout << "centers = [" ;
//        K::Point_3 p(centers.at(0)) ;
//        cout << p.x() << ", " << p.y() << ", " << p.z() ;
//        for (int i=1; i<centers.size(); ++i)
//        {
//            K::Point_3 p(centers.at(i)) ;
//            cout << ";" << endl << p.x() << ", " << p.y() << ", " << p.z() ;
//        }
//        cout << "] ;" << endl ;
//        
//        cout << "B = [" ;
//        double tmp = sol.first.second(0) ;
//        cout << tmp ;
//        for (int i=1; i<sol.first.second.size(); ++i)
//        {
//            double tmp = sol.first.second(i) ;
//            cout << ", " << tmp ;
//        }
//        cout << "] ;" << endl ;
        // END DEBUG
        
        // Return a shared_ptr over the implicit function
        std::shared_ptr<Implicit_RBF> implicit = std::make_shared<Implicit_RBF>(p_coefs.first, p_coefs.second, centers) ;
        return implicit;
    }
    catch (const runtime_error& error)
    {
        cout << "Erreur ..." << endl ;
    }
}
    
void Hole_Filling::output_mesh_implicit(std::shared_ptr<Implicit_RBF> implicit, const vector<Mesh::Vertex_index>& boundary, string out_file = "patch.off")
{
        Tr tr;            // 3D-Delaunay triangulation
        C2t3 c2t3 (tr);   // 2D-complex in 3D-Delaunay triangulation

    // defining the surface
        auto implicit_fun = [&implicit](Point_3 p)
        {
            K::Point_3 pp(p.x(), p.y(), p.z()) ;
            return implicit->val(pp) ;
        } ;
        
        GT::Sphere_3 domain(estimate_BS(boundary)) ;
        Surface_3 surface(implicit_fun, // implicit_function,             // pointer to function
                          domain); // bounding sphere
        
        
        
        CGAL::Surface_mesh_default_criteria_3<Tr> criteria(30.,  // angular bound
                                                           sqrt(domain.squared_radius())/nsteps,  // radius bound
                                                           0.1); // distance bound
        // meshing surface
        CGAL::make_surface_mesh(c2t3, surface, criteria, CGAL::Non_manifold_tag());
        Surface_mesh sm;
        CGAL::facets_in_complex_2_to_triangle_mesh(c2t3, sm);
        
        std::ofstream out(out_file);
        out << sm << std::endl;
        std::cout << "Size of the patch mesh: " << tr.number_of_vertices() << "\n";
        
    }


GT::Sphere_3 Hole_Filling::estimate_BS(const vector<Mesh::Vertex_index> &vlist)
{
	// BS estimate :
	//   center : barycenter
	//   radius : largest distance to barycenter
	
	K::Point_3 bary(barycenter(vlist, _mesh._m)) ;
	double r = sqrd_distance(_mesh._m.point(vlist.at(0)), bary) ;
	for (int i=1; i<vlist.size(); i++)
	{
		double tmp = sqrd_distance(_mesh._m.point(vlist.at(i)), bary) ;
		if (tmp > r)
			r = tmp ;
	}

	GT::Point_3 center(bary.x(), bary.y(), bary.z());
	GT::Sphere_3 domain(center, 3.*r) ;
	
	return domain ;
}

// ***** VColorProp_mesh

void Hole_Filling::colorize_prop()
{
	Mesh::Vertex_range vr = _mesh._m.vertices() ;
	for (Mesh::Vertex_range::iterator v_it = vr.begin(); v_it != vr.end() ; ++v_it)
	{
		if(vprop[*v_it] == true)
			_mesh.vcolor[*v_it] = CGAL::Color(255, 0, 0) ;
		else
            _mesh.vcolor[*v_it] = CGAL::Color(200, 200, 200) ;
	}
}

void Hole_Filling::colorize_verts(const vector<Mesh::Vertex_index> &vlist)
{
	Mesh::Vertex_range vr = _mesh._m.vertices() ;
	for (Mesh::Vertex_range::iterator v_it = vr.begin(); v_it != vr.end() ; ++v_it)
	{
        _mesh.vcolor[*v_it] = CGAL::Color(200, 200, 200) ;
	}

	for(int i=0; i<vlist.size(); i++)
	{
        _mesh.vcolor[vlist.at(i)] = CGAL::Color(255, 0, 0) ;
	}
}
