// ErrorEstimator.cpp: ErrorEstimator クラスのインプリメンテーション
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "MeshEditor.h"
#include "ErrorEstimator.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// 構築/消滅
//////////////////////////////////////////////////////////////////////

ErrorEstimator::ErrorEstimator()
{
	original = NULL;
	noisy = NULL;

	original_f = NULL;
	noisy_f = NULL;

	mesh = NULL;

	v_error = NULL;
	f_error = NULL;
}

ErrorEstimator::~ErrorEstimator()
{
	if(original != NULL)
		delete[] original;
	if(noisy != NULL)
		delete[] noisy;

	if(original_f != NULL)
		delete[] original_f;
	if(noisy_f != NULL)
		delete[] noisy_f;

	if(v_error != NULL)
		delete[] v_error;

	if(f_error != NULL)
		delete[] f_error;
}

void ErrorEstimator::setMeshData(MeshData *mesh)
{
	this->mesh = mesh;
	updateOriginal();
}

void ErrorEstimator::updateOriginal()
{
	if(original != NULL)
		delete[] original;

	if(original_f != NULL)
		delete[] original_f;

	int vertex_N = mesh->vertex_N;
	float (*vertex_tmp)[3] = mesh->vertex;
	mesh->vertex = new float[vertex_N][3];
	for(int i=0; i<vertex_N; i++){
		mesh->vertex[i][0] = vertex_tmp[i][0];
		mesh->vertex[i][1] = vertex_tmp[i][1];
		mesh->vertex[i][2] = vertex_tmp[i][2];
	}
	mesh->undoNormalizeScale();
	original = mesh->vertex;

	int face_N = mesh->face_N;
	original_f = new float[face_N][3];
	for(i=0; i<face_N; i++){
		mesh->faceCenter(original_f[i], i);
	}

	mesh->vertex = vertex_tmp;

	if(v_error != NULL)
		delete[] v_error;
	v_error = new float[vertex_N];

	if(f_error != NULL)
		delete[] f_error;
	f_error = new float[face_N];
}

double ErrorEstimator::estimateVertexError(double &max)
{
	int vertex_N = mesh->vertex_N;
	int *degree = mesh->degree_f;
	int **link = mesh->vertex_link_f;

	max = 0;

	float (*vertex_tmp)[3] = mesh->vertex;
	mesh->vertex = new float[vertex_N][3];
	for(int i=0; i<vertex_N; i++){
		mesh->vertex[i][0] = vertex_tmp[i][0];
		mesh->vertex[i][1] = vertex_tmp[i][1];
		mesh->vertex[i][2] = vertex_tmp[i][2];
	}
	mesh->undoNormalizeScale();

	int face_N = mesh->face_N;
	int *id1 = new int[face_N];
	//int *id2 = new int[face_N];
	for(i=0; i<face_N; i++){
		id1[i] = -1;
		//id2[i] = -1;
	}
	float (*vertex)[3] = mesh->vertex;
	BOOL* isBound = mesh->isBound;
	double err = 0;
	//double err2 = 0;
	double total = 0;
	//double total2 = 0;
	for(i=0; i<vertex_N; i++){
		if(isBound[i])
			continue;

		int deg = degree[i];
		int* l = link[i];
		double a = 0;
		double a_o = 0;
		for(int j=0; j<deg; j++){
			a += mesh->faceArea(l[j]);
			//a_o += faceAreaOriginal(l[j]);
		}
		//total += a + a_o;
		total += a;
			//total2 += a_o;

		//double d1 = dist2ToNearestTriangle(i, original[i], vertex, id1, 4);
		double d2 = dist2ToNearestTriangle(i, vertex[i], original, id1, 4); //44 dist2ToNearestVertex(i, id, 2);
		//err += a*d2 + a_o*d1;

		err += a*d2;
		//err2 += a_o*d1;

		v_error[i] = (float)sqrt(d2);

		d2 = sqrt(d2);
		if(max < d2)
			max = d2;
		//d1 = sqrt(d1);
		//if(max < d1)
			//max = d1;
	}
	delete[] mesh->vertex;
	mesh->vertex = vertex_tmp;

	delete [] id1;
	//delete [] id2;

	//return 0.5*(sqrt(err/(3.0*total)) + sqrt(err2/(3.0*total2)));

	return sqrt(err/(3.0*total));
}

double ErrorEstimator::dist2ToNearestVertex(int i)
{
	int *link = mesh->vertex_link_v[i];
	int deg = mesh->degree_v[i];
	float* p = mesh->vertex[i];
	
	double min = MeshData::DIST2(p, original[i]);

	for(int j=0; j<deg; j++){
		double d2 = MeshData::DIST2(p, original[link[j]]);
		if(min > d2)
			min = d2;
	}
	return min;
}

void ErrorEstimator::setNoisy()
{	
	if(noisy != NULL)
		delete[] noisy;

	if(noisy_f != NULL)
		delete[] noisy_f;

	int vertex_N = mesh->vertex_N;
	float (*vertex_tmp)[3] = mesh->vertex;
	mesh->vertex = new float[vertex_N][3];
	for(int i=0; i<vertex_N; i++){
		mesh->vertex[i][0] = vertex_tmp[i][0];
		mesh->vertex[i][1] = vertex_tmp[i][1];
		mesh->vertex[i][2] = vertex_tmp[i][2];
	}
	mesh->undoNormalizeScale();
	noisy = mesh->vertex;
	
	int face_N = mesh->face_N;
	noisy_f = new float[face_N][3];
	for(i=0; i<face_N; i++){
		mesh->faceCenter(noisy_f[i], i);
	}

	mesh->vertex = vertex_tmp;
}

int ErrorEstimator::searchNearestVertex(int i)
{
	int *link = mesh->vertex_link_v[i];
	int deg = mesh->degree_v[i];
	float* p = mesh->vertex[i];
	
	int index = i;
	double min = MeshData::DIST2(p, noisy[i]);

	for(int j=0; j<deg; j++){
		double d2 = MeshData::DIST2(p, noisy[link[j]]);
		if(min > d2){
			index = link[j];
			min = d2;
		}
	}
	return index;
}

double ErrorEstimator::estimateCorelationVertex()
{
	int vertex_N = mesh->vertex_N;
	int *degree = mesh->degree_f;
	int **link = mesh->vertex_link_f;

	float (*vertex_tmp)[3] = mesh->vertex;
	mesh->vertex = new float[vertex_N][3];
	for(int i=0; i<vertex_N; i++){
		mesh->vertex[i][0] = vertex_tmp[i][0];
		mesh->vertex[i][1] = vertex_tmp[i][1];
		mesh->vertex[i][2] = vertex_tmp[i][2];
	}
	mesh->undoNormalizeScale();

	float (*vertex)[3] = mesh->vertex;
	double co = 0;
	double vari1 = 0;
	double vari2 = 0;
	double total = 0;
	for(i=0; i<vertex_N; i++){
		int deg = degree[i];
		int* l = link[i];
		double a = 0;
		for(int j=0; j<deg; j++)
			a += mesh->faceArea(l[j]);
		total += a;
		
		int k = this->searchNearestVertex(i);
		float v[3];
		MeshData::VEC(v, noisy[k], vertex[i]);
		co += a*MeshData::DOT(v, vertex[i]);
		vari1 += a*(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
		vari2 += a*(vertex[i][0]*vertex[i][0] + vertex[i][1]*vertex[i][1] + vertex[i][2]*vertex[i][2]);
	}
	delete[] mesh->vertex;
	mesh->vertex = vertex_tmp;

	return fabs(co)/(sqrt(vari1*vari2)); //(3.0*total);
}

double ErrorEstimator::dist2ToNearestVertex(int i, int *id, int R)
{
	int **link = mesh->vertex_link_v;
	int *degree= mesh->degree_v;
	float* p = mesh->vertex[i];

	Node* Q = new Node;
	Q->append(i, 0);
	id[i] = i;
	
	double min = 10000000000;
	while(Q->next != NULL){
		int c = Q->v;
		int r = Q->f;
		Node* tmp = Q;
		Q = Q->next;
		Q->tail = tmp->tail;
		tmp->next = NULL;
		delete tmp;

		double d2 = MeshData::DIST2(p, original[c]);
		if(d2 < min)
			min = d2;

		if(r == R)
			continue;

		int* l = link[c];
		int deg = degree[c];
		for(int j=0; j<deg; j++){
			int k = l[j];
			if(id[k] == i)
				continue;
			Q->append(k, r+1);
			id[k] = i;
		}
	}
	delete Q;
	return min;
}

void ErrorEstimator::normalNearestFace(float n[], int i, int *id, int R)
{
	int (*link)[3] = mesh->face_link_E;
	float p[3];
	mesh->faceCenter(p, i);

	Node* Q = new Node;
	Q->append(i, 0);
	id[i] = i;
	
	double min = 10000000000;
	int index;
	while(Q->next != NULL){
		int c = Q->v;
		int r = Q->f;
		Node* tmp = Q;
		Q = Q->next;
		Q->tail = tmp->tail;
		tmp->next = NULL;
		delete tmp;

		double d2 = MeshData::DIST2(p, original_f[c]);
		if(d2 < min){
			min = d2;
			index = c;
		}

		if(r == R)
			continue;

		int* l = link[c];
		for(int j=0; j<3; j++){
			int k = l[j];
			if(k < 0)
				continue;
			if(id[k] == i)
				continue;
			Q->append(k, r+1);
			id[k] = i;
		}
	}
	delete Q;
	
	int *f = mesh->face[index];
	float v1[3], v2[3];
	MeshData::VEC(v1, original[f[0]], original[f[1]]);
	MeshData::VEC(v2, original[f[0]], original[f[2]]);
	double cross[3];
	MeshData::CROSS(cross, v1, v2);
	double len = MeshData::LENGTH(cross);
	if((float)len != 0){
		n[0] = (float)(cross[0]/len);
		n[1] = (float)(cross[1]/len);
		n[2] = (float)(cross[2]/len);
	}
	else{
		n[0] = n[1] = n[2] = 0;
	}
}

double ErrorEstimator::estimateFaceError(double &max)
{
	int vertex_N = mesh->vertex_N;
	int face_N = mesh->face_N;
	max = 0;
	float (*vertex_tmp)[3] = mesh->vertex;
	mesh->vertex = new float[vertex_N][3];
	for(int i=0; i<vertex_N; i++){
		mesh->vertex[i][0] = vertex_tmp[i][0];
		mesh->vertex[i][1] = vertex_tmp[i][1];
		mesh->vertex[i][2] = vertex_tmp[i][2];
	}
	mesh->undoNormalizeScale();

	int *id1 = new int[face_N];
	//int *id2 = new int[face_N];
	for(i=0; i<face_N; i++){
		id1[i] = -1;
		//id2[i] = -1;
	}
	mesh->computeFaceNormal();
	float (*vertex)[3] = mesh->vertex;
	double err = 0;
	double err2 = 0;
	double total = 0;
	double total2 = 0;
	for(i=0; i<face_N; i++){
		double a1 = mesh->faceArea(i);
		double a2 = faceAreaOriginal(i);
		total += a1;
		total2 += a2;

		float *n1 = mesh->normal_f[i];
		float m1[3];
		float c1[3];
		mesh->faceCenter(c1, i);

		//float n2[3], m2[3], c2[3];
		//faceNormalOriginal(n2, i);
		//faceCenterOriginal(c2, i);

		normalNearestTriangle(m1, i, c1, original, id1, 5);
		//normalNearestTriangle(m2, i, c2, vertex, id2, 5);
		//normalNearestFace(m, i, id, 4);
		err += 2.0*a1*(1.0 - MeshData::DOT(n1, m1));
		//err2 += 2.0*a2*(1.0 - MeshData::DOT(n1, m2));
		

		float mn[3];
		mn[0] = m1[0] - n1[0];
		mn[1] = m1[1] - n1[1];
		mn[2] = m1[2] - n1[2];
		double len = MeshData::LENGTH(mn);
		if(max < len)
			max = len;

		f_error[i] = (float)len;
/*
		mn[0] = m2[0] - n2[0];
		mn[1] = m2[1] - n2[1];
		mn[2] = m2[2] - n2[2];
		len = MeshData::LENGTH(mn);
		if(max < len)
			max = len;
			*/
	}
	delete[] mesh->vertex;
	mesh->vertex = vertex_tmp;

	delete [] id1;
	//delete [] id2;

	//return 0.5*(sqrt(err/total) + sqrt(err2/total));
	return sqrt(err/total);
}

double ErrorEstimator::dist2ToSegment(float A[], float B[], float P[])
{
	float AB[3];
	MeshData::VEC(AB, A, B);

	double AB2 = AB[0]*AB[0] + AB[1]*AB[1] + AB[2]*AB[2];
	double dot = (P[0]-A[0])*AB[0] + (P[1]-A[1])*AB[1] + (P[2]-A[2])*AB[2];

	double AC[3];
	AC[0] = dot*AB[0]/AB2;
	AC[1] = dot*AB[1]/AB2;
	AC[2] = dot*AB[2]/AB2;

	double dot2 = MeshData::DOT(AC, AB);

	if(dot2 < 0)
		return MeshData::DIST2(A, P);  
	else if(AB2 < AC[0]*AC[0]+AC[1]*AC[1]+AC[2]*AC[2])
		return MeshData::DIST2(B, P);
	else
		return (AC[0]+A[0]-P[0])*(AC[0]+A[0]-P[0]) + 
				(AC[1]+A[1]-P[1])*(AC[1]+A[1]-P[1]) + 
					(AC[2]+A[2]-P[2])*(AC[2]+A[2]-P[2]);
}

double ErrorEstimator::dist2ToTriangle(float A[], float B[], float C[], float P[])
{
	float AB[3], AC[3];
	MeshData::VEC(AB, A, B);
	MeshData::VEC(AC, A, C);
	double n[3];
	MeshData::CROSS(n, AB, AC);
	double len2 = n[0]*n[0] + n[1]*n[1] + n[2]*n[2];
	if((float)len2 != 0){
		float AP[3];
		MeshData::VEC(AP, A, P);
		double dot = MeshData::DOT(AP, n);
		double Q[3];
		Q[0] = P[0] - dot*n[0]/len2;
		Q[1] = P[1] - dot*n[1]/len2;
		Q[2] = P[2] - dot*n[2]/len2;

		double c[3], v[3];
		MeshData::CROSS(c, AB, n);
		MeshData::VEC(v, A, Q);
		if(MeshData::DOT(c, v) > 0){
			goto OUTER;
		}
		float BC[3];
		MeshData::VEC(BC, B, C);
		MeshData::CROSS(c, BC, n);
		MeshData::VEC(v, B, Q);
		if(MeshData::DOT(c, v) > 0){
			goto OUTER;
		}
		MeshData::CROSS(c, AC, n);
		MeshData::VEC(v, C, Q);
		if(MeshData::DOT(c, v) < 0){
			goto OUTER;
		}	
		return MeshData::DIST2(Q, P);
	}
	OUTER:
	double min = dist2ToSegment(A, B, P);
	double d2 = dist2ToSegment(B, C, P);
	if(d2 < min)
		min = d2;
	d2 = dist2ToSegment(C, A, P);
	if(d2 < min)
		min = d2;
	return min;
}

double ErrorEstimator::dist2ToNearestTriangle(int i, float p[], float (*vertex)[3], int *id, int R)
{
	int (*link)[3] = mesh->face_link_E;
	int (*face)[3] = mesh->face;

	Node* Q = new Node;
	int deg = mesh->degree_f[i];
	int* l = mesh->vertex_link_f[i];
	for(int j=0; j<deg; j++){
		int k= l[j];
		Q->append(k, 0);
		id[k] = i;
	}
	
	double min = 10000000000;
	while(Q->next != NULL){
		int c = Q->v;
		int r = Q->f;
		Node* tmp = Q;
		Q = Q->next;
		Q->tail = tmp->tail;
		tmp->next = NULL;
		delete tmp;

		int* f = face[c];
		double d2 = dist2ToTriangle(vertex[f[0]], vertex[f[1]], vertex[f[2]], p);
		if(d2 < min)
			min = d2;

		if(r == R)
			continue;

		int* l = link[c];
		for(j=0; j<3; j++){
			int k = l[j];
			if(k < 0)
				continue;
			if(id[k] == i)
				continue;
			Q->append(k, r+1);
			id[k] = i;
		}
	}
	delete Q;
	return min;
}

void ErrorEstimator::normalNearestTriangle(float n[], int i, float p[], float (*vertex)[3], int *id, int R)
{
	int (*link)[3] = mesh->face_link_E;
	int (*face)[3] = mesh->face;

	Node* Q = new Node;
	Q->append(i, 0);
	id[i] = i;
	
	double min = 10000000000;
	int index;
	while(Q->next != NULL){
		int c = Q->v;
		int r = Q->f;
		Node* tmp = Q;
		Q = Q->next;
		Q->tail = tmp->tail;
		tmp->next = NULL;
		delete tmp;

		int* f = face[c];
		double d2 = dist2ToTriangle(vertex[f[0]], vertex[f[1]], vertex[f[2]], p);
		if(d2 < min){
			min = d2;
			index = c;
		}

		if(r == R)
			continue;

		int* l = link[c];
		for(int j=0; j<3; j++){
			int k = l[j];
			if(k < 0)
				continue;
			if(id[k] == i)
				continue;
			Q->append(k, r+1);
			id[k] = i;
		}
	}
	delete Q;
	
	int *f = mesh->face[index];
	float v1[3], v2[3];
	MeshData::VEC(v1, original[f[0]], original[f[1]]);
	MeshData::VEC(v2, original[f[0]], original[f[2]]);
	double cross[3];
	MeshData::CROSS(cross, v1, v2);
	double len = MeshData::LENGTH(cross);
	if((float)len != 0){
		n[0] = (float)(cross[0]/len);
		n[1] = (float)(cross[1]/len);
		n[2] = (float)(cross[2]/len);
	}
	else{
		n[0] = n[1] = n[2] = 0;
	}
}

double ErrorEstimator::faceAreaOriginal(int i)
{
	int* f = mesh->face[i];
	return MeshData::AREA(original[f[0]], original[f[1]], original[f[2]]);
}

void ErrorEstimator::faceCenterOriginal(float c[], int i)
{
	int* f = mesh->face[i];
	c[0] = (original[f[0]][0] + original[f[1]][0] + original[f[2]][0])/3.0f;
	c[1] = (original[f[0]][1] + original[f[1]][1] + original[f[2]][1])/3.0f;
	c[2] = (original[f[0]][2] + original[f[1]][2] + original[f[2]][2])/3.0f;
}

void ErrorEstimator::faceNormalOriginal(float n[], int i)
{
	int* f = mesh->face[i];
	float v1[3], v2[3];
	MeshData::VEC(v1, original[f[0]], original[f[1]]);
	MeshData::VEC(v2, original[f[0]], original[f[2]]);
	double c[3];
	MeshData::CROSS(c, v1, v2);
	double l = MeshData::LENGTH(c);
	if((float)l != 0){
		n[0] = (float)(c[0]/l);
		n[1] = (float)(c[1]/l);
		n[2] = (float)(c[2]/l);
	}
	else
		n[0] = n[1] = n[2] = 0;
}

double ErrorEstimator::estimateHnError(double &max)
{
	int vertex_N = mesh->vertex_N;
	int *degree = mesh->degree_f;
	int **link = mesh->vertex_link_f;

	max = 0;

	float (*vertex_tmp)[3] = mesh->vertex;
	mesh->vertex = new float[vertex_N][3];
	for(int i=0; i<vertex_N; i++){
		mesh->vertex[i][0] = vertex_tmp[i][0];
		mesh->vertex[i][1] = vertex_tmp[i][1];
		mesh->vertex[i][2] = vertex_tmp[i][2];
	}
	mesh->undoNormalizeScale();

	int *id1 = new int[vertex_N];
	for(i=0; i<vertex_N; i++){
		id1[i] = -1;
	}
	float (*vertex)[3] = mesh->vertex;
	BOOL* isBound = mesh->isBound;
	double err = 0;
	double total = 0;
	for(i=0; i<vertex_N; i++){
		if(isBound[i])
			continue;

		int deg = degree[i];
		int* l = link[i];
		double a = 0;
		double a_o = 0;
		for(int j=0; j<deg; j++){
			a += mesh->faceArea(l[j]);
			a_o += faceAreaOriginal(l[j]);
		}
		total += a + a_o;

		double Hn1[3], Hn2[3];
		mesh->meanCurvatureNormalWithoutA(i, Hn1);

		int index = searchNearestVertex(i, id1, 3);
		meanCurvatureNormalOriginal(index, Hn2);

		double diff[3];
		diff[0] = Hn1[0] - Hn2[0];
		diff[1] = Hn1[1] - Hn2[1];
		diff[2] = Hn1[2] - Hn2[2];
		double d2 = diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2];

		err += (a + a_o)*d2;

		d2 = sqrt(d2);
		if(max < d2)
			max = d2;
	}
	delete[] mesh->vertex;
	mesh->vertex = vertex_tmp;

	delete [] id1;

	return sqrt(err/(6.0*total));

	return 0;
}

int ErrorEstimator::searchNearestVertex(int i, int *id, int R)
{
	int **link = mesh->vertex_link_v;
	int *degree= mesh->degree_v;
	float* p = mesh->vertex[i];

	Node* Q = new Node;
	Q->append(i, 0);
	id[i] = i;
	
	double min = 10000000000;
	int index;
	while(Q->next != NULL){
		int c = Q->v;
		int r = Q->f;
		Node* tmp = Q;
		Q = Q->next;
		Q->tail = tmp->tail;
		tmp->next = NULL;
		delete tmp;

		double d2 = MeshData::DIST2(p, original[c]);
		if(d2 < min){
			index = c;
			min = d2;
		}

		if(r == R)
			continue;

		int* l = link[c];
		int deg = degree[c];
		for(int j=0; j<deg; j++){
			int k = l[j];
			if(id[k] == i)
				continue;
			Q->append(k, r+1);
			id[k] = i;
		}
	}
	delete Q;
	return index;
}

void ErrorEstimator::meanCurvatureNormalOriginal(int index, double Hn[])
{
	Hn[0] = Hn[1] = Hn[2] = 0;
	if(mesh->isBound[index] != INNER)
		return;

	double A = 0;
	int n = mesh->degree_v[index];
	int* link_v = mesh->vertex_link_v[index];

	float (*vertex)[3] = original;

	for(int i=0; i<n; i++){
		int t = link_v[i];
		int s = link_v[(i+1)%n];

		//vectors of edges of triangles
		float PQ1[3], PQ2[3], QQ[3];
		MeshData::VEC(PQ1, vertex[index], vertex[s]);
		MeshData::VEC(PQ2, vertex[index], vertex[t]);
		MeshData::VEC(QQ, vertex[s], vertex[t]);

		//normal vector of triangle
		double n[3];
		MeshData::CROSS(n, PQ1, PQ2); 

		//area of triangle
		double Ai = MeshData::AREA(vertex[index], vertex[s], vertex[t]);
		
		if((float)Ai != 0){ //> 0.001){
			A += 0.5*Ai;

			double dot1 = MeshData::DOT(PQ1, QQ);
			double dot2 = -MeshData::DOT(PQ2,QQ);

			double cot1 = dot2/Ai;
			double cot2 = dot1/Ai;

			Hn[0] += cot1*PQ1[0] + cot2*PQ2[0];
			Hn[1] += cot1*PQ1[1] + cot2*PQ2[1];
			Hn[2] += cot1*PQ1[2] + cot2*PQ2[2];
		}
	}
	/*
	if((float)A != 0){ //> 0.006){
		Hn[0] /= A;
		Hn[1] /= A;
		Hn[2] /= A;
	}
	else{
		Hn[0] = 0;
		Hn[1] = 0;
		Hn[2] = 0;
	}
	*/
}