#include "vtkDijkstraImageData.h"
 
#include "vtkObjectFactory.h"
#include "vtkIntArray.h"
#include "vtkPointData.h"
#include "vtkPriorityQueue.h"
#include "vtkIdList.h"
#include "vtkImageData.h"
#include "vtkFloatArray.h"
 #include <math.h>
#include <stdlib.h>

vtkCxxSetObjectMacro(vtkDijkstraImageData,BoundaryScalars,vtkDataArray);
 
 //----------------------------------------------------------------------------
 vtkDijkstraImageData* vtkDijkstraImageData::New()
 {

	   


   // First try to create the object from the vtkObjectFactory
   vtkObject* ret = vtkObjectFactory::CreateInstance("vtkDijkstraImageData");
   if(ret)
    {
     return (vtkDijkstraImageData*)ret;
     }
   // If the factory was unable to create the object, then create it here.
   return new vtkDijkstraImageData;
 }
 
 


//----------------------------------------------------------------------------
// Constructor sets default values
vtkDijkstraImageData::vtkDijkstraImageData()
{
  this->SourceID = 0;
  this->SinkID = 0;
  this->NumberOfInputPoints = 0;
  this->PathPointer = -1;
  this->StopWhenEndReached = 1;
 
  this->ShortestPathIdList = NULL;
  this->Parent        = NULL;
  this->Visited          = NULL;
  this->PQ = NULL;
  this->BoundaryScalars = NULL;

  this->UseInverseDistance = 0;
  this->UseInverseSquaredDistance = 0;
  this->UseInverseExponentialDistance = 1;
  this->UseSquaredDistance = 0;

  this->puntos = NULL;
  this->lineas = NULL;


  this->logger = NULL;

	//if ((this->logger = freopen("c:\\creatis\\maracas\\dijkstra.txt","w", stdout)) == NULL)
	//	exit(-1);
    
}


//------------------------------------------------------------------------------
vtkDijkstraImageData::~vtkDijkstraImageData()
{


  if (this->ShortestPathIdList)
    this->ShortestPathIdList->Delete();
  if (this->Parent)
    this->Parent->Delete();
  if(this->BoundaryScalars) 
    this->BoundaryScalars->Delete();
  //if (this->Visited)
  // this->Visited->Delete();
  //if (this->PQ)
  // this->PQ->Delete();
  //if (this->Heap)
  //this->Heap->Delete();
  //if (this->p)
  //this->p->Delete();
  //if(this->BoundaryScalars)
  //this->BoundaryScalars->Delete();
  //DeleteGraph();
  //if (this->CumulativeWeightFromSource)
  //this->CumulativeWeightFromSource->Delete();
  
}

//------------------------------------------------------------------------------
unsigned long vtkDijkstraImageData::GetMTime()
{
  unsigned long mTime=this->MTime.GetMTime();
  
  return mTime;
}


void vtkDijkstraImageData::SetInput(vtkImageData* data){
	this->vtkProcessObject::SetNthInput(0, data);
}

vtkImageData* vtkDijkstraImageData::GetInput(){
	return (vtkImageData *)(this->Inputs[0]);
}

//------------------------------------------------------------------------------
void vtkDijkstraImageData::init(vtkImageData *inData)
{

  if (this->ShortestPathIdList)
    this->ShortestPathIdList->Delete();
  
  if (this->Parent)
    this->Parent->Delete();
  if (this->Visited)
    this->Visited->Delete();
  if (this->PQ)
  this->PQ->Delete();

  this->ShortestPathIdList = vtkIdList::New();
  this->Parent        = vtkIntArray::New();
  this->Visited          = vtkIntArray::New();
  this->PQ = vtkPriorityQueue::New();
  
  
  CreateGraph(inData);     
  

  int numPoints = inData->GetNumberOfPoints();

  this->Parent->SetNumberOfComponents(1);
  this->Parent->SetNumberOfTuples(numPoints);
  this->Visited->SetNumberOfComponents(1);
  this->Visited->SetNumberOfTuples(numPoints);
 
}

void vtkDijkstraImageData::DeleteGraph()
{
  
  /*const int npoints = this->GetNumberOfInputPoints();
  
    if (this->Neighbors)
    {
    for (int i = 0; i < npoints; i++)
    {
    if(this->Neighbors[i])
    this->Neighbors[i]->Delete();
    }
    delete [] this->Neighbors;
    }
    this->Neighbors = NULL;
  */
}

//------------------------------------------------------------------------------
void vtkDijkstraImageData::CreateGraph(vtkImageData *inData) {

  
  //DeleteGraph();
  //delete old arrays in case we are re-executing this filter
  int numPoints = inData->GetNumberOfPoints(); 
  this->SetNumberOfInputPoints(numPoints);
  
  // initialization
  int *dim = inData->GetDimensions();
  vtkDataArray *scalars = inData->GetPointData()->GetScalars();
  vtkIdList *graphNodes = vtkIdList::New();
  int graphSize = 0;
  // create the graph
  for(int k = 0; k <dim[2]; k++) {
    this->UpdateProgress ((float) k / (2 * ((float) dim[2] - 1)));
    for(int j = 0; j <dim[1]; j++) {
      for(int i = 0; i <dim[0]; i++) {
    
    int id = k*(dim[1]*dim[0]) + j*dim[0] + i;
    float maskValue = scalars->GetTuple1(id);
    // only add neighbor if it is in the graph
    
    if(maskValue > 0) {    
      // add to graph
      graphNodes->InsertNextId(id);
      graphSize++;
    }
      }
    }
  }

  this->SetNumberOfGraphNodes(graphSize);
  //printf("graph size %i \n ",graphSize);
  
  // fill the PQ
  PQ->Allocate(graphSize);
  for(int i=0; i<graphSize;i++) {
    PQ->Insert(VTK_LARGE_FLOAT,graphNodes->GetId(i));
  }
  // free some memory
  graphNodes->Delete();
  
}


//------------------------------------------------------------------------------
void vtkDijkstraImageData::RunDijkstra(vtkDataArray *scalars,int startv, int endv)
{
  
	int i, u, v;
  
	printf("tipo de ponderacion de pesos : linear %i, squared %i, exponential %i\n",this->UseInverseDistance,this->UseInverseSquaredDistance,this->UseInverseExponentialDistance);
	
	
  
	//-------------------------------------------------------------------------------
	// INICIALIZACION. 
	//-------------------------------------------------------------------------------
	
	// 1. Se marcan todos los vertices del grafo como no visitados.
	InitSingleSource(startv);
	
	// 2. Se marca el punto de arranque como visitado
	this->Visited->SetValue(startv, 1);
	
	// 3. Se obtiene el tamanio inicial de los vertices no visitados
	int initialSize = PQ->GetNumberOfItems();
	int size = initialSize;
	int stop = 0;

	//Nota: PQ contiene los nodos del grafo no evaluados.
	//Segun Dijkstra, el algoritmo termina cuando todos los nodos tienen un valor de distancia minima.

	//------------------------------------------------------------------------
	// PARA TODOS LOS VERTICES EN PQ (SIN EVALUAR)
	//------------------------------------------------------------------------
	while ((PQ->GetNumberOfItems() > 0) && !stop)
	{
		
		this->UpdateProgress (0.5 + (float) (initialSize - size) / ((float) 2 * initialSize));
      	
	
		vtkFloatingPointType u_weight;
		
		//----------------------------------------------------------------------
		//1. Aqui obtiene el siguiente punto a evaluar. Al retirarlo de la cola
		// se asume que ya ha sido evaluado, se evaluaran sus vecinos.
		// u is now in S since the shortest path to u is determined
		//----------------------------------------------------------------------
		#if (VTK_MAJOR_VERSION == 4 && VTK_MINOR_VERSION == 0)      
		u = PQ->Pop(u_weight,0);
		#else
		u = PQ->Pop(0, u_weight);
		#endif
		//printf("Evaluando el nodo %i con prioridad %.2f\n",u,u_weight);
		//printPointData(u);


        //-----------------------------------------
		//2. Se marca "u" como ya visitado.
		//-----------------------------------------
		this->Visited->SetValue(u, 1);
      
		
		
		//------------------------------------------------------------------------------------
		//3. Si u es el nodo final el algoritmo se detiene. (despues de evaluar sus vecinos)
		//------------------------------------------------------------------------------------
		if (u == endv && StopWhenEndReached)
			stop = 1;
      
		// Update all vertices v neighbors to u
		// find the neighbors of u
		
		
		
		//-----------------------------------------
		//4. Encontrar los vecinos de u
		//-----------------------------------------
		vtkIdList *list = vtkIdList::New();
		this->FindNeighbors(list,u,scalars);
      
		//printf("%i tiene %i vecinos\n",u,list->GetNumberOfIds());
		
		//-----------------------------------------
		// 5. Para cada vecino ....
		//-----------------------------------------
		for (i = 0; i < list->GetNumberOfIds(); i++) 
		{

			//---------------------------------------------------------------------
			// 5.1 Se obtiene su ID (el indice es el orden en la lista de vecinos)
			//---------------------------------------------------------------------
			v = list->GetId(i);
			//printf("---> evaluando el vecino % i\n",v);
    
			// s is the set of vertices with determined shortest path...do not 
			// use them again

			//-----------------------------------------------------------------
			// 5.2 Si ya ha sido visitado se descarta...
			//-----------------------------------------------------------------
			if (this->Visited->GetValue(v) != 1)
			{
				//printf("---> ---> el vecino %i no ha sido visitado\n",v);
				// Only relax edges where the end is not in s and edge 
				// is in the front 
				//---------------------------------------------------
				// 5.2.1 Se obtiene el COSTO W
				//---------------------------------------------------
				float w = EdgeCost(scalars, u, v);
				
				//---------------------------------------------------
				// 5.2.2 
				//---------------------------------------------------
				float v_weight = this->PQ->GetPriority(v);
				if(v == endv) {
					//printf("we have found endv %i, its weight is %f, neighbor is %i , weight is %f, edge weight is %f\n",v,v_weight,u,u_weight,w);
					//stop=1;
					//break;
				}

				//printf("---> ---> el costo de %i es %.2f\n",v,(w+u_weight));
				//printf("---> ---> el costo previo de %i es %.2f\n",v,v_weight);
        
								
				//-------------------------------------------------------------------------------
				// 5.2.3 Si se encuentra un camino menor, se reajusta el costo (Node Relaxation)
				//-------------------------------------------------------------------------------
				if (v_weight > (u_weight + w))
				{
					// 5.2.3.1 Se elimina V de la cola de nodos por evaluar (PQ). Porque se encontro un costo mas bajo
					this->PQ->DeleteId(v);
					
					// 5.2.3.2 Se inserta nuevamente V en la cola de nodos por evaluar con el nuevo costo.
					this->PQ->Insert((u_weight + w),v);
					//printf("---> ---> reajustando el peso de %i a %.2f\n",v,u_weight + w);
					//printf("u_weight=%.2f\n",u_weight);
					
					// 5.2.3.3 se coloca V conectado con U (esto sirve para reconstruir el camino minimo)
					
					this->Parent->SetValue(v, u);
					//printf("setting parent of %i to be %i",v,u);
				}
			}
			else{
				//printf("---> el vecino %i ya se visito\n",v);
			}
		}

		//-------------------------------------------------------
		// 6. Liberar la memoria ocupada por la lista de vecinos
		//-------------------------------------------------------
		list->Delete();
		size--;
	}
	this->PQ->Delete();
	this->Visited->Delete();
}

//----------------------------------------------------------------------------
void vtkDijkstraImageData::InitSingleSource(int startv)
{
  for (int v = 0; v < this->GetNumberOfInputPoints(); v++)
    {
      this->Parent->SetValue(v, -1);
      this->Visited->SetValue(v, 0);
    }
  PQ->DeleteId(startv);
  // re-insert the source with priority 0
  PQ->Insert(0,startv);
  //printf("priority of startv %f",PQ->GetPriority(startv));
}


//----------------------------------------------------------------------------
void vtkDijkstraImageData::FindNeighbors(vtkIdList *list,int id, vtkDataArray *scalars) {
  
  // find i, j, k for that node
  vtkImageData *input = this->GetInput();

  int *dim = input->GetDimensions();
  int numPts = dim[0] * dim[1] * dim[2];
  
  //printf("vecinos de %i :(",id);
  for(int vk = -1; vk<2; vk++) {
    for(int vj = -1; vj<2; vj++) {
      for(int vi = -1; vi<2; vi++) {         
    
		int tmpID = id + (vk * dim[1]*dim[0]) + (vj * dim[0]) + vi;
		// check we are in bounds (for volume faces)     
		if( tmpID >= 0 && tmpID < numPts && tmpID != 0) {
			float mask = scalars->GetTuple1(tmpID);
			// only add neighbor if it is in the graph
			if(mask > 0 && tmpID != id) {        
				list->InsertUniqueId(tmpID);
				//printf("%i,",tmpID);
				//printPointData(tmpID);
			}
		}
      }
    }
  }
  //printf(")\n");
}



float vtkDijkstraImageData::fuerzaAtraccion(int u, float w)
{
	//1. Identificar las coordenadas del punto de inicio y del punto final (polos)
	double coordsSource[3];
	double coordsSink[3];
	double coordsPoint[3];

	this->GetInput()->GetPoint(this->GetSourceID(), coordsSource);
	this->GetInput()->GetPoint(this->GetSinkID(), coordsSink);
	
    
	//2. Identificar las coordenadas del punto al cual se le quiere sacar el potencial electrico
	this->GetInput()->GetPoint(u, coordsPoint);


	//3 Calcular r1 y r2

	float r1 = sqrt((coordsSource[0]-coordsPoint[0])*(coordsSource[0]-coordsPoint[0]) +
		(coordsSource[1]-coordsPoint[1])*(coordsSource[1]-coordsPoint[1]) +
		(coordsSource[2]-coordsPoint[2])*(coordsSource[2]-coordsPoint[2]));


	float r2 = sqrt((coordsSink[0]-coordsPoint[0])*(coordsSink[0]-coordsPoint[0]) +
		(coordsSink[1]-coordsPoint[1])*(coordsSink[1]-coordsPoint[1]) +
		(coordsSink[2]-coordsPoint[2])*(coordsSink[2]-coordsPoint[2]));

	float d = sqrt((coordsSink[0]-coordsSource[0])*(coordsSink[0]-coordsSource[0]) +
		(coordsSink[1]-coordsSource[1])*(coordsSink[1]-coordsSource[1]) +
		(coordsSink[2]-coordsSource[2])*(coordsSink[2]-coordsSource[2]));



   	if (r2 < d && r1 > d){
		return w/2;
	} else if ( r2 > d && r1 < d){
		return w/2;
	} else if (r1 < d && r2 < d){
		return w*w;
	} else return w/2;


	
}

//----------------------------------------------------------------------------
// The edge cost function should be implemented as a callback function to
// allow more advanced weighting
float vtkDijkstraImageData::EdgeCost(vtkDataArray *scalars, int u, int v)
{
  
  float w;
  
    float dist2 = scalars->GetTuple1(v);  //+ fuerzaAtraccion(v, scalars->GetTuple1(v));
    float dist = sqrt(dist2);
    if(this->UseInverseDistance)
      w = (1.0/dist);
    else if(this->UseInverseSquaredDistance)
      w = (1.0/(dist2));
    else if(this->UseInverseExponentialDistance)
      w = (1.0/exp(dist));
    else if(this->UseSquaredDistance)
      w = dist2;



  return w;
}

//----------------------------------------------------------------------------
void vtkDijkstraImageData::BuildShortestPath(int start,int end)
{

	int p = end;
	int next = 0;
	int i = 0;
	double punto[3];

	this->puntos = vtkPoints::New();
	this->lineas = vtkCellArray::New();

	
	
	//printf("======================camino minimo========================\n");
	//printf("Punto Inicial = %i,  Punto Final = %i\n",start, end);

	while (p != start && p > 0)
    {
		printPointData(p);
       
		this->GetInput()->GetPoint(p,punto);
		this->puntos->InsertPoint(i,punto); //puntos y lineas usados como resultado (vtkPolyData)
	   
		next = this->Parent->GetValue(p);

		this->GetInput()->GetPoint(next,punto);
		this->puntos->InsertPoint(i+1,punto);
		
		this->lineas->InsertNextCell(2);
		this->lineas->InsertCellPoint(i);
		this->lineas->InsertCellPoint(i+1);

		this->ShortestPathIdList->InsertNextId(p);
		p = next;
		i++;
    }
	
	this->ShortestPathIdList->InsertNextId(p);
	printPointData(p);
	//printf("===========================================================\n");
	
	

	this->GetOutput()->SetPoints (this->puntos);
	this->GetOutput()->SetLines(this->lineas);
	this->GetOutput()->Modified();
    //fclose(logger);

}

vtkIdList* vtkDijkstraImageData::GetShortestPathIdList()
{
	return this->ShortestPathIdList;
}



void vtkDijkstraImageData::printPointData(int pointID)
{
	double coords[3];

	this->GetInput()->GetPoint(pointID, coords);
	double scalar = this->GetInput()->GetPointData()->GetScalars()->GetTuple1(pointID);

	printf("Punto ID: %i ",pointID);
	printf("Coords[%.0f, %.0f, %.0f] ", coords[0], coords[1], coords[2]);
	printf("Scalar: %.2f\n", scalar);

}



//----------------------------------------------------------------------------
// ITERATOR PART 

void vtkDijkstraImageData::InitTraversePath(){
  this->PathPointer = -1;
}

//----------------------------------------------------------------------------
int vtkDijkstraImageData::GetNumberOfPathNodes(){
  return this->ShortestPathIdList->GetNumberOfIds();
}

//----------------------------------------------------------------------------
int vtkDijkstraImageData::GetNextPathNode(){
  this->PathPointer = this->PathPointer + 1;
  
  if(this->PathPointer < this->GetNumberOfPathNodes()) {
    //printf("this->GetPathNode(this->PathPointer) %i",this->GetPathNode(this->PathPointer));
    return this->ShortestPathIdList->GetId(this->PathPointer);
  } else {
    return -1;
  }
}

//----------------------------------------------------------------------------
// find closest scalar to id that is non-zero
int vtkDijkstraImageData::findClosestPointInGraph(vtkDataArray *scalars,int id,int dim0,int dim1, int dim2) {

  
  int kFactor = dim0 * dim1;
  int jFactor = dim0;
  
  int numPoints = kFactor * dim2;
  vtkIdList* Q = vtkIdList::New();
  Q->InsertNextId(id);
  
  int pointer = 0;
  int foundID = -1;
  
  while(Q->GetNumberOfIds() != 0) {
    
    int current = Q->GetId(pointer);
    pointer = pointer + 1;
    //printf("we are looking at id %i \n",current);
    
    // check to see if we found something in the graph
    if(scalars->GetTuple1(current) >0) {
      //printf("before return");
      return current;
    } else {
      // set it to -1 to show that we already looked at it
      scalars->SetTuple1(current,-1);
      // put the neighbors on the stack
      // top
      if (current + kFactor <numPoints) {
    if(scalars->GetTuple1(current + kFactor) != -1){
      //printf("expand k+1 %i",current + kFactor);
      Q->InsertNextId(current+kFactor);
    }
      }
      // bottom
       if (current - kFactor >= 0){
     if(scalars->GetTuple1(current - kFactor) != -1){
       //printf("expand k-1 %i", current - kFactor);
       Q->InsertNextId(current-kFactor);
     }
       }
       // front
       if (current + jFactor < numPoints) {
     if(scalars->GetTuple1(current + jFactor) != -1){
       //printf("expand j+1 %i",current + jFactor);
       Q->InsertNextId(current + jFactor);
     }
       }
       // back
       if (current - jFactor >= 0) {
     if(scalars->GetTuple1(current - jFactor) != -1){
       //printf("expand j-1 %i",current - jFactor);
       Q->InsertNextId(current - jFactor);    
     }
       }
       // left
       if (current+1 <numPoints){
     if(scalars->GetTuple1(current + 1) != -1){
       //printf("expand i+1 %i",current+1);
       Q->InsertNextId(current + 1);    
     }
       }
       // right
       if (current -1 >= 0) {
     if(scalars->GetTuple1(current - 1) != -1){
       //printf("expand i-1 %i",current - 1);
       Q->InsertNextId(current - 1);    
     }
       }
     }
   }
   Q->Delete();
   return foundID;
 }
 
 
 
 //----------------------------------------------------------------------------
 // This method is passed a input and output Data, and executes the filter
 // algorithm to fill the output from the input.
 // It just executes a switch statement to call the correct function for
 // the Datas data types.
 // -- sp 2002-09-05 updated for vtk4
 void vtkDijkstraImageData::Execute()
 {
     
   vtkImageData *inData = this->GetInput();
   vtkPolyData  *outData = this->GetOutput();

   
    
  
   // Components turned into x, y and z
   if (inData->GetNumberOfScalarComponents() > 3)
     {
     vtkErrorMacro("This filter can handle upto 3 components");
     return;
     }
   

   //printf("*************** vtkDijkstraImageData Execute **************\n\n\n");
   this->init(inData);
   //printf("*************** after init ****************\n\n\n");
   
   int *dim = inData->GetDimensions();
   vtkDataArray *scalars = inData->GetPointData()->GetScalars();
   
    // find closest point in graph to source and sink if their value is not 0
   printf("source ID %i value is %f\n",this->GetSourceID(),scalars->GetTuple1(this->GetSourceID()));
   if(scalars->GetTuple1(this->GetSourceID()) == 0) {
     
     vtkFloatArray *copyScalars = vtkFloatArray::New();
     copyScalars->DeepCopy(inData->GetPointData()->GetScalars());
     
     this->SetSourceID(this->findClosestPointInGraph(copyScalars,this->GetSourceID(),dim[0],dim[1],dim[2]));
     
     copyScalars->Delete();
     printf("NEW source ID %i value is %f\n",this->GetSourceID(),scalars->GetTuple1(this->GetSourceID()));
 
   }
 
   printf("sink ID %i value is %f\n",this->GetSinkID(),scalars->GetTuple1(this->GetSinkID()));
   if(scalars->GetTuple1(this->GetSinkID()) == 0) {
     vtkFloatArray *copyScalars2 = vtkFloatArray::New();
     copyScalars2->DeepCopy(inData->GetPointData()->GetScalars());
     this->SetSinkID(this->findClosestPointInGraph(copyScalars2,this->GetSinkID(),dim[0],dim[1],dim[2]));
     copyScalars2->Delete();
     
     printf("NEW sink ID %i value is %f\n",this->GetSinkID(),scalars->GetTuple1(this->GetSinkID()));
   }
       
   
   this->RunDijkstra(scalars,this->GetSourceID(),this->GetSinkID());
   
   this->BuildShortestPath(this->GetSourceID(),this->GetSinkID());
   
   
   
     
 }
 
 
 //----------------------------------------------------------------------------
 void vtkDijkstraImageData::PrintSelf(ostream& os, vtkIndent indent)
 {
   Superclass::PrintSelf(os,indent);
 
   os << indent << "Source ID: ( "
      << this->GetSourceID() << " )\n";
 
   os << indent << "Sink ID: ( "
      << this->GetSinkID() << " )\n";
 }