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

#include "stdafx.h"
#include "MeshEditor.h"
#include "RBF.h"
#include "math.h"
#include "PBCG.h"

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

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

RBF::RBF()
{
	support = NULL;
	sol = NULL;
	index = NULL;
}

RBF::~RBF()
{
	if(index == NULL)
		delete[] index;
	if(sol == NULL)
		delete[] sol;
	if(support == NULL)
		delete[] support;
}

void RBF::computeDualSupport(int size)
{
	int face_N = mesh->face_N;
	int (*link)[3] = mesh->face_link_E;
	int *visit = new int[face_N];
	mesh->computeCenter();
	float (*center)[3] = mesh->center;

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

	for(int i=0; i<face_N; i++)
		visit[i] = -1;

	for(i=0; i<face_N; i++){
		Node* Q = new Node;
		Q->append(i, 0);
		visit[i] = i;
		float max = 0;
		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;

			float d = (float)MeshData::DIST(center[i], center[c]);
			if(max < d)
				max = d;

			if(r == size)
				continue;

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

	delete[] visit;
}

void RBF::fittingDual(int size)
{
	this->computeDualSupport(size);
	this->generateDualIndex();

	int point_N = mesh->face_N;
	float (*normal)[3] = mesh->normal_f;
	float (*point)[3] = mesh->center;

	int total = point_N;
	for(int i=0; i<point_N; i++)
		total += index_N[i];

	PBCG solver;
	double *sa = solver.sa = new double[total+2];
	unsigned long *ija = solver.ija = new unsigned long[total+2];
	double *b = new double[point_N+1];
	if(sol != NULL)
		delete[] sol;
	sol = new double[point_N+1];
	ija[1]=point_N+2;
	int kk=point_N+1;
	for(i=0; i<point_N; i++){
		sa[i+1] = 1.0;
		b[i+1] = 0;
		sol[i+1] = 0.0;
		int* t = index[i];
		int m = index_N[i];
	
		for(int j=0; j<m; j++){
			int k = t[j];
			float* n = normal[k];
			float vx = point[i][0] - point[k][0];
			float vy = point[i][1] - point[k][1];
			float vz = point[i][2] - point[k][2];
			double w = weight(vx*vx + vy*vy + vz*vz, support[i]);
			if(w != 0.0){
				double dot = n[0]*vx + n[1]*vy + n[2]*vz;
				b[i+1] -= w*dot; //sqrt(K2*dot*dot+1.0);
				
				kk++;
				sa[kk] = w;
				ija[kk] = k+1;
			}
		}
		ija[i+2]=kk+1;
	}

	//sovle 
	int iter;
	double err;
	solver.linbcg(point_N, b, sol, 1, 1e-5, 1000, &iter, &err);

	delete[] b;
}

double RBF::weight(double d2, float support)
{
	double T2 = support*support;
	if(T2 < d2)
		return 0;
	double r = sqrt(d2/T2);
	return (1.0 - r)*(1.0 - r);
	//return pow(1.0 - r, 4)*(4.0*r + 1.0);
}

void RBF::generateDualIndex()
{
	int face_N = mesh->face_N;
	int (*link)[3] = mesh->face_link_E;
	int *visit = new int[face_N];
	float (*center)[3] = mesh->center;
	
	if(index != NULL){
		for(int i=0; i<face_N; i++){
			if(index_N[i] != 0)
				delete[] index[i];
		}
		delete[] index;
		delete[] index_N;
	}
	index = new int*[face_N];
	index_N = new int[face_N];

	for(int i=0; i<face_N; i++){
		index_N[i] = 0;
		visit[i] = -1;
	}

	for(i=0; i<face_N; i++){
		Node* Q = new Node;
		Q->append(i, 0);
		visit[i] = i;
		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;

			float d = (float)MeshData::DIST(center[i], center[c]);

			if(d > support[i])
				continue;
			index_N[c]++;

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

	for(i=0; i<face_N; i++){
		index[i] = new int[index_N[i]-1];
		index_N[i] = 0;
	}
	
	for(i=0; i<face_N; i++){
		Node* Q = new Node;
		Q->append(i, 0);
		visit[i] = i;
		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;

			float d = (float)MeshData::DIST(center[i], center[c]);

			if(d > support[i])
				continue;
			if(c != i){
				index[c][index_N[c]] = i;
				index_N[c]++;
			}

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

	for(i=0; i<face_N; i++){
		int count = index_N[i];
		int *ii = index[i];
		for(int j=0; j<count; j++){
			for(int k=j+1; k<count; k++){
				if(ii[j] > ii[k]){
					int t = ii[j];
					ii[j] = ii[k];
					ii[k] = t;
				}
			}
		}
	}
	delete[] visit;
}

void RBF::gradientF(float g[], int i)
{
	int size = index_N[i];
	int j;
	int* table = index[i];
	double f = 0;
	float x = mesh->center[i][0];
	float y = mesh->center[i][1];
	float z = mesh->center[i][2];
	float vx, vy, vz;
	double dot, d2;
	float *c, *n;
	float gw[3];
	double w;
	double g1[3];
	g1[0] = -mesh->normal_f[i][0];
	g1[1] = -mesh->normal_f[i][1];
	g1[2] = -mesh->normal_f[i][2];
	//double f1, f2;
	for(int k=0; k<size; k++){
		j = table[k];
		c = mesh->center[j];
		n = mesh->normal_f[j];
		vx = x - c[0];
		vy = y - c[1];
		vz = z - c[2];
		dot = n[0]*vx + n[1]*vy + n[2]*vz;
		d2 = vx*vx + vy*vy + vz*vz;
		weightD(gw, d2, vx, vy, vz, support[j]);
		w = weight(vx*vx + vy*vy + vz*vz, support[j]);

		//f1 = dot/sqrt(K2*dot*dot+1.0);
		//f2 = (K2*dot*dot-K2*dot+1.0)/pow(K2*dot*dot+1.0, 1.5);

		//g1[0] -= (f1+sol[j+1])*gw[0] + f2*n[0]*w;
		//g1[1] -= (f1+sol[j+1])*gw[1] + f2*n[1]*w;
		//g1[2] -= (f1+sol[j+1])*gw[2] + f2*n[2]*w;
		g1[0] -= (dot+sol[j+1])*gw[0] + n[0]*w;
		g1[1] -= (dot+sol[j+1])*gw[1] + n[1]*w;
		g1[2] -= (dot+sol[j+1])*gw[2] + n[2]*w;
	}
	g[0] = (float)g1[0];
	g[1] = (float)g1[1];
	g[2] = (float)g1[2];
}

inline void RBF::weightD(float g[], double d2, float vx, float vy, float vz, float support)
{
	double T2 = support;
	if(T2 < d2 || d2 == 0.0){
		g[0] = g[1] = g[2] = 0;
	}
	double r = sqrt(d2/T2);
	if((float)r == 0){
		g[0] = g[1] = g[2] = 0;
		return;
	}
	double t = -2.0*(1.0-r)/r;
	//double t = -20.0*pow(1.0-r, 3.0);
	g[0] = (float)(vx * t);
	g[1] = (float)(vy * t);
	g[2] = (float)(vz * t);
}

void RBF::setNormalAsGradient()
{
	int face_N = mesh->face_N;
	mesh->computeCenter();
	float (*center)[3] = mesh->center;
	mesh->computeFaceNormal();
	float (*normal)[3] = mesh->normal_f;
	for(int i=0; i<face_N; i++){
		float g[3];
		gradientF(g, i);
		double len = MeshData::LENGTH(g);
		if((float)len != 0){
			g[0] = (float)(g[0]/len);
			g[1] = (float)(g[1]/len);
			g[2] = (float)(g[2]/len);
		}
		normal[i][0] = -g[0];
		normal[i][1] = -g[1];
		normal[i][2] = -g[2];
	}
}

float RBF::valueF(int i)
{
	int size = index_N[i];
	int j;
	int* table = index[i];
	float x = mesh->center[i][0];
	float y = mesh->center[i][1];
	float z = mesh->center[i][2];
	double f = 0;
	float vx, vy, vz;
	double d;
	float *c, *n;
	for(int k=0; k<size; k++){
		j = table[k];
		c = mesh->center[j];
		n = mesh->normal_f[j];
		vx = x - c[0];
		vy = y - c[1];
		vz = z - c[2];
		d = n[0]*vx + n[1]*vy + n[2]*vz;
	
		//f += (d/sqrt(K2*d*d+1.0) + sol[j+1])*weight(vx*vx + vy*vy + vz*vz);
		f += (d + sol[j+1])*weight(vx*vx + vy*vy + vz*vz, support[j]);
	}
	return -(float)f;
}