#include "gLocalize.h"

#include <qeventloop.h>
#include <qapplication.h>

gLocalize::gLocalize(){

	abortSignal=false;
}


void gLocalize::localize(vtkImageData* vol, 
	int march_tresh,
	double thrDown_D,
	double thrUp_D,
	double thr_HAUSD,
	double thr_S,
	QList <bool> selectedFilters){

	abortSignal=false;
	int i=0;
	int progress=0;
	  //variabili
  	vtkSmartPointer<vtkImageData> fixOutput = vtkSmartPointer<vtkImageData>::New();

	vtkSmartPointer<vtkPolyDataConnectivityFilter> filter = vtkSmartPointer<vtkPolyDataConnectivityFilter>::New();
	vtkSmartPointer<vtkMarchingCubes> marchingcubes = vtkSmartPointer<vtkMarchingCubes>::New() ;
	vtkSmartPointer<vtkWindowedSincPolyDataFilter> smoother = vtkSmartPointer<vtkWindowedSincPolyDataFilter>::New();
    vtkSmartPointer<vtkImageData> fixed = vtkSmartPointer<vtkImageData> :: New();
  




 marchingcubes->SetInput(vol);
 
  
 //The algorithm starts with a marching cube at a given intensity level threshold
 marchingcubes->SetValue(0,march_tresh); // Surface #0, iso-value=1 
 marchingcubes->SetComputeNormals(0);
 if (marchingcubes.GetPointer()->GetNumberOfInputConnections(0)==0) 
 {return;
 //EMIT FAIL QUALCOSA
 //return -1;
 }
  marchingcubes->Update();
   std::cout<< "inizio run" <<std::endl;
  QString item= item.number(marchingcubes->GetNumberOfContours());
  if(item.toInt()<=0){
    throw("CRITICAL: No surfaces found with given treshold");
    QList <surf> dummylist;
 //   emit sendrenderMarkerList (dummylist);
    }

// Smoothing filter and normals calculation on marching cubes output data
  smoother->SetInput(marchingcubes->GetOutput());
  smoother->SetNumberOfIterations(20);
  smoother->NormalizeCoordinatesOn();
  smoother->SetPassBand(0.01);
  smoother->BoundarySmoothingOn();
  smoother->Update(); 
  
  
  vtkSmartPointer<vtkPolyDataNormals> normals=vtkSmartPointer<vtkPolyDataNormals>::New();
  normals->SetInput(smoother->GetOutput());
  vtkSmartPointer<vtkActor> normalact=vtkSmartPointer<vtkActor>::New();
  vtkSmartPointer<vtkPolyDataMapper> normalmap=vtkSmartPointer<vtkPolyDataMapper>::New();
  normalmap->SetInput( normals->GetOutput());
  normalact->SetMapper(normalmap);

// Start of marker recognitioning procedure

  filter->AddInput(normals->GetOutput());
  filter->SetExtractionModeToAllRegions();
  filter->SetColorRegions(0);
  filter->Update();
  QString regionnumber = regionnumber.number(filter->GetNumberOfExtractedRegions());
  filter->SetExtractionModeToSpecifiedRegions();
  filter->Update();

  //Some data allocation:
  //surflist: list of surfaces where the recognized marker are added
  //tempsurf: one surface used as temporary variable during geometrical filtering
  QList <surf> surflist;
  surflist.clear();
  surf tempsurf;
  
	
  //Double check on number of surface in ConnectivityFilter output
  if(regionnumber.toInt()==0){
	  std::cout<<"ok"<<std::endl;
    throw("CRITICAL: No surfaces in ConnectivityFilter output");
    QList <surf> dummylist;
	
   // emit sendrenderMarkerList (dummylist);
    };
  if(regionnumber.toInt()>3000){
    cout<<"ho trovato troppe superfici: "<<regionnumber.toInt()<<endl;
	};
  int ok;
   cout<<regionnumber.toDouble()<<endl;
   
   
  //progress bar initialization    
  //emit recognitionProgress (regionnumber.toInt(),0);

  int OKd=0;
  int OKh=0;
  int OKs=0;
  
  for(double idouble=0;idouble<regionnumber.toDouble() ;idouble++)
  {
	  
	QApplication::processEvents();
	progress++;

	emit localizationProgress( (double)progress/(double)regionnumber.toDouble() * 100 );
  
    if(abortSignal==true){ 
		abortSignal=false;
		emit localizationAborted();
		return;
    }

    //flag need for good filtering driving
    ok=1; 

    //Add one surface at once and check for geometrical constraints
    filter->AddSpecifiedRegion(idouble);
    filter->Update();

    // Filter on bounding box diagonal lenght
	if(selectedFilters.at(0))
		if(ok==1) {
			tempsurf.lenght=filter->GetOutput()->GetLength();
			if(tempsurf.lenght<thrDown_D || tempsurf.lenght>thrUp_D ) 
				ok=0;
		}

    //Hausdorf reads from tempsurf the bounding box center and use it as reference center
			if(selectedFilters.at(1))
    if(ok==1) {    
		OKd++;
	    filter->GetOutput()->GetCenter(tempsurf.center);
		tempsurf.hausdorff=hausdorff(filter,tempsurf);
		if(tempsurf.hausdorff> (thr_HAUSD) )  
			ok=0;

    }

//     Filter based on sides difference
		if(selectedFilters.at(2))
    if(ok==1) {
		OKh++;
		filter->GetOutput()->GetBounds(tempsurf.bounds);
		tempsurf.confronto_lati=lati(tempsurf.bounds);
		if(tempsurf.confronto_lati>thr_S) 
			ok=0;
    }

    //if all filters are verified, add a surface index and append it to the surflist
    if(ok==1) {
		OKs++;
		tempsurf.sorting=idouble; //indexing
		surflist.append(tempsurf);
	}

    // MANDATORY filter cleaning
    filter->DeleteSpecifiedRegion(idouble);

}

cout<<"Nsurfaces"<<regionnumber.toInt() <<endl;
cout<<"OKD "<< OKd<<" OKH "<<OKh<<" OKS "<<OKs<<endl; 

  
  // marker center calculation
  // final_list: surf list returned through emit
  // tmp_p: surf used as temporary variable
  QList<surf> final_list;
  surf tmp_p;
  
  if(surflist.count()!=0){   
    for(i=0;i<surflist.count();i++){
      tmp_p=surflist.at(i);
      filter->AddSpecifiedRegion(surflist.at(i).sorting);
      filter->Update();
      
      //Centroid calculation needs mesh vertex
      vtkSmartPointer<vtkExtractEdges> edge_extractor =vtkSmartPointer<vtkExtractEdges>::New();
      edge_extractor->RemoveAllInputs();
      edge_extractor->SetInput(filter->GetOutput());
      edge_extractor->Update();

      vtkSmartPointer<vtkPolyData> tmp = vtkSmartPointer<vtkPolyData>::New();
      tmp=edge_extractor->GetOutput();
      tmp->Update();
      tmp->BuildCells();
      tmp->BuildLinks();

      QList <point> punti;
      point ptemp;

      for (int id=0;id< (int) tmp->GetNumberOfPoints();id++){
	ptemp.x=tmp->GetPoint(id)[0];
	ptemp.y=tmp->GetPoint(id)[1];
	ptemp.z=tmp->GetPoint(id)[2];
	/*cout<<filter->GetOutput()->GetPoint(id)[0]<<" "<<filter->GetOutput()->GetPoint(id)[1]<<" "<<filter->GetOutput()->GetPoint(id)[2]<<endl;*/
	punti.append(ptemp);
	};
      //Call centroid calculation
      tmp_p.centroid=centroid(punti);
      punti.clear();

      // MANDATORY filter cleaning
      filter->DeleteSpecifiedRegion(surflist.at(i).sorting);

      final_list.append(tmp_p);
    };

  };
  
  
  for(int ii=0;ii<final_list.size();ii++)
  {
   std::cout<<1000*final_list.at(ii).centroid.x<<
    " "<<1000*final_list.at(ii).centroid.y<<
     " "<<1000*final_list.at(ii).centroid.z<<std::endl;
  }

  emit localizationEnd(final_list);


}


//
//
///**
// * Centroid calculation of a given points cloud
// */
point gLocalize::centroid(QList< point > punti)
{
 int i;
 point centroide;
 centroide.x=0,centroide.y=0,centroide.z=0;
  for ( i=0;i<punti.size();i++){
    centroide.x+=punti.at(i).x/1000;
    centroide.y+=punti.at(i).y/1000;
    centroide.z+=punti.at(i).z/1000;
  };

  centroide.x=centroide.x/punti.size();
  centroide.y=centroide.y/punti.size();
  centroide.z=centroide.z/punti.size();

  return centroide;
}
//
//
///**
// * performs bounding box side difference and return the larger one
// */
double gLocalize::lati(double p[6])
{
  double x=p[1]-p[0];double y=p[3]-p[2];double z=p[5]-p[4];
  double d1=(x-y); double d2=(x-z); double  d3=(y-z);
  if(d1<0) d1=d1*-1;if(d2<0) d2=d2*-1;if(d3<0) d3=d3*-1;

  if(d1>d2 && d1>d3) return d1;
  if(d2>d1 && d2>d3) return d2;
  if(d3>d2 && d3>d1) return d3;
  return 9999;
}


int gLocalize::get_byteorder()
{
    union {
                long l;
                char c[4];
        } test;
        test.l = 1;
        if( test.c[3] && !test.c[2] && !test.c[1] && !test.c[0] )
                return ORDER_BIGENDIAN;

        if( !test.c[3] && !test.c[2] && !test.c[1] && test.c[0] )
                return ORDER_LITTLEENDIAN;

        return ORDER_UNKNOWN;
}

/**
 * Implementation of the Hausdorff distance from a reference 1 cm diameter sphere
 */
double gLocalize::hausdorff(vtkPolyDataConnectivityFilter* filter, surf tempsurf)
{
  vtkSmartPointer<vtkSphereSource> source_sphere = vtkSmartPointer<vtkSphereSource>::New();
  source_sphere->SetCenter(tempsurf.center[0],tempsurf.center[1],tempsurf.center[2]);
  source_sphere->SetRadius(5.0);
  source_sphere->Update();
  
  vtkSmartPointer<vtkPolyData> sphere = source_sphere->GetOutput();
  sphere->Update();
  sphere->BuildCells();
  
  vtkSmartPointer<vtkExtractEdges> edge_extractor =vtkSmartPointer<vtkExtractEdges>::New();
  edge_extractor->RemoveAllInputs();
  edge_extractor->SetInput(filter->GetOutput());
  edge_extractor->Update();

  vtkSmartPointer<vtkPolyData> tmp = vtkSmartPointer<vtkPolyData>::New();
  tmp=edge_extractor->GetOutput();
  tmp->Update();
  tmp->BuildCells();
  tmp->BuildLinks();
   
  QList <point> punti_sup;
  point ptemp;

  for (int id=0;id< (int) tmp->GetNumberOfPoints();id++){
    ptemp.x=tmp->GetPoint(id)[0];
    ptemp.y=tmp->GetPoint(id)[1];
    ptemp.z=tmp->GetPoint(id)[2];
    /*cout<<filter->GetOutput()->GetPoint(id)[0]<<" "<<filter->GetOutput()->GetPoint(id)[1]<<" "<<filter->GetOutput()->GetPoint(id)[2]<<endl;*/
    punti_sup.append(ptemp);
   };
   
   
  QList <point> punti_sphere;
  for (int id=0;id< (int) sphere->GetNumberOfPoints();id++){
   ptemp.x=sphere->GetPoint(id)[0];
   ptemp.y=sphere->GetPoint(id)[1];
   ptemp.z=sphere->GetPoint(id)[2];
   /*cout<<filter->GetOutput()->GetPoint(id)[0]<<" "<<filter->GetOutput()->GetPoint(id)[1]<<" "<<filter->GetOutput()->GetPoint(id)[2]<<endl;*/
   punti_sphere.append(ptemp);
  };
   
  int nr_source_points = punti_sup.count();
  int nr_target_points = punti_sphere.count();
  
  double summax = 0;
  double summin = 0;
  double maxmin = -1e9;
  double maxmax = -1e9;
  
  for(int i=0; i < nr_source_points; i++) {
    double smallest = 1e9;
    double largest = 0;

    double x1 = punti_sup.at(i).x;
    double y1 = punti_sup.at(i).y;
    double z1 = punti_sup.at(i).z;

    for(int j=0; j < nr_target_points; j++) {
      double x2 = punti_sphere.at(j).x;
      double y2 = punti_sphere.at(j).y;
      double z2 = punti_sphere.at(j).z;

      double dx = x1 - x2;
      double dy = y1 - y2;
      double dz = z1 - z2;

      double dist = dx * dx + dy * dy + dz*dz;
            
      if (dist < smallest) smallest = dist;
      if (dist > largest) largest = dist;
    }

    summin += smallest;
    summax += largest;
    
    if (smallest > maxmin) maxmin = smallest;  // so this is max min
    if (largest > maxmax) maxmax = largest;    // and this is max max
  }
  
punti_sphere.clear();
punti_sup.clear();
return maxmin;  // Hausdorff distance  
}