#ifndef __errorStatistics_txx
#define __errorStatistics_txx

#include "errorStatistics.h"
#include <fstream>

//Constructors:
template <typename TInputImageType, typename TEdge1>
errorStatistics<TInputImageType,TEdge1>::errorStatistics(): 
  m_Percentage(.85), trim_bottom(0){
  
}

template <typename TInputImageType, typename TEdge1>
errorStatistics<TInputImageType,TEdge1>::errorStatistics(typename InputImageType::Pointer image1, typename Edge1Type::Pointer edge_image): 
  m_Image1(image1),
  m_EdgeImage1(edge_image),
  m_EdgeImage1Region(m_EdgeImage1->GetRequestedRegion()),
  m_Image1Region(m_Image1->GetRequestedRegion()),trim_bottom(0){	
 
}

template <typename TInputImageType, typename TEdge1>
errorStatistics<TInputImageType,TEdge1>::errorStatistics(typename InputImageType::Pointer image1,typename Edge1Type::Pointer edge_image, double percentage): 
  m_Image1(image1), 
  m_EdgeImage1(edge_image),
  m_Percentage(percentage),
  m_EdgeImage1Region(m_EdgeImage1->GetRequestedRegion()),
  m_Image1Region(m_Image1->GetRequestedRegion()),trim_bottom(0){
  
}


//Member Functions:

//Set Functions:
template <typename TInputImageType, typename TEdge1>
void errorStatistics<TInputImageType,TEdge1>::SetImage1(typename InputImageType::Pointer image1){
	m_Image1 = image1;
}
template <typename TInputImageType, typename TEdge1>
void errorStatistics<TInputImageType,TEdge1>::SetEdgeImage1(typename Edge1Type::Pointer image1){
	m_EdgeImage1 = image1;
}
template <typename TInputImageType, typename TEdge1>
void errorStatistics<TInputImageType,TEdge1>::SetPercentage(double percentage){
	m_Percentage = percentage;
}
template <typename TInputImageType, typename TEdge1>
void errorStatistics<TInputImageType,TEdge1>::SetImage1Region(typename InputImageType::RegionType region){
	m_Image1Region = region;
}
template <typename TInputImageType, typename TEdge1>
void errorStatistics<TInputImageType,TEdge1>::SetEdgeImage1Region(typename Edge1Type::RegionType region){
	m_EdgeImage1Region = region;
}



//ComputeValues()

//////////////////////////////
//Function to get distance metrics
template <typename TInputImageType, typename TEdge1>
void  errorStatistics<TInputImageType,TEdge1>::ComputeValues(){

  //initialize metrics
  double RMS = 0.0;
	

  //First put all distances in a vector
  std::vector<double> allDistancesAcc;
  ConstIteratorType Iter1(m_Image1, m_Image1Region);
  EdgeConstIteratorType eIter(m_EdgeImage1,m_EdgeImage1Region);

  for(Iter1.GoToBegin(),eIter.GoToBegin();!Iter1.IsAtEnd();++Iter1, ++eIter){
    if(eIter.Get()>0){
      double dist = 0;
      dist = Iter1.Get();
	if(dist >= 0){
      	  RMS+=dist*dist;
          allDistancesAcc.push_back(dist);
	}
    }
  }

 
  int i;

  double numMeasurements = allDistancesAcc.size();


  std::sort(allDistancesAcc.begin(),allDistancesAcc.end());

  // max
  double max = allDistancesAcc[(unsigned)(numMeasurements-1)];
	
  // Partial max metric
  double Pmax = allDistancesAcc[(unsigned)(numMeasurements*m_Percentage-1)];  

  double rec_perc = 1.-m_Percentage;
  double AvgDistance = 0.0;
  double LTSDistance = 0.0;
  double MDistance = 0.0;
  double mean = 0.0;

  //Get robust metrics
 for(std::vector<double>::const_iterator mI=allDistancesAcc.begin();
    mI!=allDistancesAcc.end();mI++)
    mean += *mI;
 mean /= numMeasurements;


  // IP-A(AVG) 
  // Trim the bottom and top of the measurement vector
  unsigned firstItem;
  firstItem = unsigned(numMeasurements*(1.-m_Percentage)/2.);
  for(std::vector<double>::const_iterator mI=allDistancesAcc.begin()+firstItem;
    mI!=allDistancesAcc.end()-firstItem;mI++)
    AvgDistance += *mI;
 
  AvgDistance /= numMeasurements*m_Percentage;

  // M-estimated Distance
  //
  double thresh = 0;
  if(trim_bottom){
    thresh = allDistancesAcc[unsigned(numMeasurements*(1.-m_Percentage))];
    for(std::vector<double>::const_iterator mI=allDistancesAcc.begin();
      mI!=allDistancesAcc.end();mI++){
        double distance = *mI;
        if(distance<thresh)
          MDistance += thresh;
        else
          MDistance += distance;
        }
    }
  else{
    thresh = allDistancesAcc[unsigned(numMeasurements*m_Percentage-1)];
    for(std::vector<double>::const_iterator mI=allDistancesAcc.begin();
      mI!=allDistancesAcc.end();mI++){
        double distance = *mI;
        if(distance>thresh)
          MDistance += thresh;
        else
          MDistance += distance;
     }
  }
  MDistance /= numMeasurements;

  //IP-LTS
  // Least Trimmed Squares
  //
  if(trim_bottom){
	unsigned lastItem;
  	lastItem = unsigned(numMeasurements*(1.-m_Percentage));
  	for(std::vector<double>::const_iterator mI=allDistancesAcc.begin()+lastItem;
	  mI!=allDistancesAcc.end();mI++){
    	    LTSDistance += *mI;
  	}
 
  LTSDistance /= numMeasurements*m_Percentage;
  }
  else{
	 unsigned lastItem;
  	lastItem = unsigned(numMeasurements*m_Percentage);
  	for(std::vector<double>::const_iterator mI=allDistancesAcc.begin();
    	  mI!=allDistancesAcc.begin()+lastItem;mI++){
    		LTSDistance += *mI;
  	}	
 
  LTSDistance /= numMeasurements*m_Percentage;
  }

  //LMS, least median squares
	double LMS=0.0;
	double min = 2000;
	
	unsigned endItem;
  	endItem = unsigned(numMeasurements*(.5));
	for(int x = 0;x<numMeasurements/2;x++){
		double residual = fabs(allDistancesAcc[x]-allDistancesAcc[x+endItem]);   	
		if(residual==min){
			//find midpoint
			double mid = (allDistancesAcc[x]+allDistancesAcc[x+endItem])/2.0;
			LMS +=mid;
			LMS/=2;
		}
		else if(residual < min){
			min = residual;
			LMS = (allDistancesAcc[x]+allDistancesAcc[x+endItem])/2.0;
		}

	}


  //Set private datamembers
  m_M = MDistance;
  m_LTS =LTSDistance;
  m_AVG = AvgDistance;
  m_Pmax = Pmax;
  m_max = max;
  m_RMS = RMS/numMeasurements;
  m_numMeasures = (long int) numMeasurements;
  m_LMS = LMS;
  m_mean = mean;
  return;
}

  

#endif