#include <string>
#include <vector>
#include <math.h>
#include <conio.h>
#include <vtkAutoInit.h>
VTK_MODULE_INIT(vtkRenderingOpenGL2);
VTK_MODULE_INIT(vtkInteractionStyle);
VTK_MODULE_INIT(vtkRenderingFreeType);
#include "RVLVTK.h"
#include "opencv2\opencv.hpp"
#include "RVLCore2.h"
#include "Display.h"
#include "Graph.h"
#include "NormalEst.h"
using namespace std;
using namespace cv;



NormalEst::NormalEst()
{
	poljeNormala = NULL;
	poljeP0 = NULL;
	poljeZaFilter = NULL;
}


NormalEst::~NormalEst()
{
	if(poljeNormala)
		delete[] poljeNormala;
	if (poljeP0)
		delete[] poljeP0;
	if (poljeZaFilter)
		delete[] poljeZaFilter;
	if (poljeOptimiziranihNormala)
		delete[] poljeZaFilter;
}


void NormalEst::Apply(Graph &G)
{
	::Node *pNode;
	::Node *pNodeHelp;
	int NumOfNodes = G.NoOfNodes();
	numNodes = NumOfNodes;
	Vec3f P0, P1, P2, Vector1, Vector2, Suma;
	float LenghtOfVector;
	if (poljeNormala)
		delete[] poljeNormala;
	if (poljeP0)
		delete[] poljeP0;
	if (poljeZaFilter)
		delete[] poljeZaFilter;
	poljeNormala = new Vec3f[NumOfNodes];
	poljeP0 = new Vec3f[NumOfNodes];
	poljeZaFilter = new Vec3f[NumOfNodes];
	int k = 0;

	for (int i = 0; i < NumOfNodes; i++) {
		pNode = G.GetNode(i);
	//	Vec3f a = *(poljeZaFilter + i);


		P0 = Vec3f(pNode->P);
		*(poljeP0 + i) = P0;
		Suma = 0;
		for (int j = 0; j < 4; j++) {
			k++;
			if (j == 3)
				k = 0;
			if (pNode->neighbor[j] > 0 && pNode->neighbor[k] > 0) {
				pNodeHelp = G.GetNode(pNode->neighbor[j]);
				P1 = Vec3f(pNodeHelp->P);
				Vector1 = P1 - P0;
				pNodeHelp = G.GetNode(pNode->neighbor[k]);
				P2 = Vec3f(pNodeHelp->P);
				Vector2 = P2 - P0;
				Suma = Suma + Vector1.cross(Vector2);
			}
		}

		LenghtOfVector = norm(Suma, NORM_L2);
		if (LenghtOfVector != 0) {
			*(poljeZaFilter + i) = Suma;
			Suma = Suma / LenghtOfVector;
			*(poljeNormala + i) = Suma;
		}
		else {
			*(poljeNormala + i) = Vec3f(0, 0, 0);
			*(poljeZaFilter + i) = Vec3f(0, 0, 0);
		}
	}

}


void NormalEst::CreateLinesFromNormals(std::vector<Line> &lines)
{
	Line line;
	float scale = 0.1;

	for(int i = 0; i < numNodes; i++)
	{
		line.P1 = *(poljeP0+i);
		line.P2 = *(poljeNormala+i)*scale + *(poljeP0+i);
		lines.push_back(line);
	}



}


void NormalEst::MjerniSumNormale(Graph &G)
{
	::Node *pNode;
	float LenghtOfVector, gamma = 0.5;
	Vec3f Suma;
	int k;

	poljeOptimiziranihNormala = new Vec3f[numNodes];

	for (int i = 0; i < numNodes; i++)
	{
		Suma = 0;
		pNode = G.GetNode(i);
		Vec3f a = *(poljeZaFilter+i);
		for (int j = 0; j < 4; j++) {
			if (pNode->neighbor[j] > 0) {
				Suma = Suma + *(poljeZaFilter + pNode->neighbor[j]);
			}
		}
		Suma = Suma * gamma;
		Suma = Suma + a;
		LenghtOfVector = norm(Suma, NORM_L2);
		*(poljeOptimiziranihNormala + i) = Suma / LenghtOfVector;


	}

}



void NormalEst::CreateLinesFromOptNormals(std::vector<Line> &lines)
{
	Line line;
	float scale = 0.1;

	for (int i = 0; i < numNodes; i++)
	{
		line.P1 = *(poljeP0 + i);
		line.P2 = *(poljeOptimiziranihNormala + i)*scale + *(poljeP0 + i);
		lines.push_back(line);
	}



}