/*=================================================================
 itkLocalDistanceMapImageFilter.h

 Authors: Eric Billet, Andriy Fedorov 
 PI: Nikos Chrisochoides

Copyright (c) 2008 College of William and Mary
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

   * Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
   * Redistributions in binary form must reproduce the above
copyright notice, this list of conditions and the following disclaimer
in the documentation and/or other materials provided with the
distribution.
   * Neither the name of the College of William and Mary nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
======================================================================*/
#ifndef _itkLocalDistanceMapSmoothingImageFilter_txx
#define _itkLocalDistanceMapSmoothingImageFilter_txx
#include "itkLocalDistanceMapSmoothingImageFilter.h"

#include "itkConstNeighborhoodIterator.h"
#include "itkNeighborhoodInnerProduct.h"
#include "itkImageRegionIterator.h"
#include "itkNeighborhoodAlgorithm.h"
#include "itkZeroFluxNeumannBoundaryCondition.h"
#include "itkOffset.h"
#include "itkProgressReporter.h"

namespace itk
{

template<class TEdge1,class TEdge2,class TDistance1, class TOutputImage>

LocalDistanceMapSmoothingImageFilter< TEdge1,TEdge2, TDistance1,  TOutputImage>
::LocalDistanceMapSmoothingImageFilter(){
// this filter requires three input images
  this->SetNumberOfRequiredInputs( 3 );
}


template<class TEdge1,class TEdge2,class TDistance1, class TOutputImage>
void
LocalDistanceMapSmoothingImageFilter< TEdge1,TEdge2, TDistance1,  TOutputImage>
::SetInput1( const TEdge1 * edge1 )
{
  this->SetNthInput(0, const_cast<TEdge1 *>( edge1 ) );      
}

template<class TEdge1,class TEdge2,class TDistance1, class TOutputImage>
void
LocalDistanceMapSmoothingImageFilter< TEdge1,TEdge2, TDistance1,  TOutputImage>
::SetInput2( const TEdge2 * edge2 )
{
  this->SetNthInput(1, const_cast<TEdge2 *>( edge2 ) );      
}

template<class TEdge1,class TEdge2,class TDistance1, class TOutputImage>
void
LocalDistanceMapSmoothingImageFilter< TEdge1,TEdge2, TDistance1,  TOutputImage>
::SetInput3( const TDistance1 * distance1 )
{
  this->SetNthInput(2, const_cast<TDistance1 *>( distance1 ) );      
}

template<class TEdge1,class TEdge2,class TDistance1, class TOutputImage>
const 
typename LocalDistanceMapSmoothingImageFilter< TEdge1,TEdge2, TDistance1,  TOutputImage>
::Edge1Type * LocalDistanceMapSmoothingImageFilter< TEdge1,TEdge2, TDistance1,  TOutputImage>::GetInput1()
{
  return static_cast< const TEdge1 * >
    (this->ProcessObject::GetInput(0));
}

template<class TEdge1,class TEdge2,class TDistance1, class TOutputImage>
const 
typename LocalDistanceMapSmoothingImageFilter< TEdge1,TEdge2, TDistance1,  TOutputImage>
::Edge2Type * LocalDistanceMapSmoothingImageFilter< TEdge1,TEdge2, TDistance1,  TOutputImage>::GetInput2()
{
  return static_cast< const TEdge2 * >
    (this->ProcessObject::GetInput(1));
}
template<class TEdge1,class TEdge2,class TDistance1, class TOutputImage>
const 
typename LocalDistanceMapSmoothingImageFilter< TEdge1,TEdge2, TDistance1,  TOutputImage>
::Distance1Type * LocalDistanceMapSmoothingImageFilter< TEdge1,TEdge2, TDistance1,  TOutputImage>::GetInput3()
{
  return static_cast< const TDistance1 * >
    (this->ProcessObject::GetInput(2));
}


template<class TEdge1,class TEdge2,class TDistance1, class TOutputImage>
void
LocalDistanceMapSmoothingImageFilter< TEdge1,TEdge2, TDistance1,  TOutputImage>
::ThreadedGenerateData(const OutputImageRegionType& outputRegionForThread,
                       int threadId)
{


  /** Allocate output*/
  typename OutputImageType::Pointer outputImage = this->GetOutput();

  typename  Edge1Type::ConstPointer edge1  = this->GetInput1();
  typename  Edge2Type::ConstPointer edge2  = this->GetInput2();
  typename  Distance1Type::ConstPointer distance1  = this->GetInput3();

  typename NeighborhoodIteratorType::RadiusType radius;
  radius.Fill(m_Radius);

  /**create iterators for the 3 images (2 distance transforms, output image)*/
  ConstNeighborhoodIterator<Edge1Type> EIt(radius,edge1, outputRegionForThread);
  ConstNeighborhoodIterator<Edge2Type> EIt2(radius,edge2, outputRegionForThread);
  ConstNeighborhoodIterator<Distance1Type> DistanceIt(radius,distance1, outputRegionForThread);
  ImageRegionIterator<OutputImageType> OutputIt(outputImage, outputRegionForThread);

  ProgressReporter progress(this, threadId, outputRegionForThread.GetNumberOfPixels());

  for(OutputIt.GoToBegin(),DistanceIt.GoToBegin(), EIt.GoToBegin(), EIt2.GoToBegin(); !OutputIt.IsAtEnd();++OutputIt,++DistanceIt, ++EIt, ++EIt2){
    OutputIt.Set(0);
    if(EIt.GetCenterPixel() || EIt2.GetCenterPixel()){
			
    /**put all neighborhood values into a vector*/
      std::vector<double> allDistances;
                        
      for(int i =0;i<DistanceIt.Size();i++){
                               
        if((EIt.GetPixel(i)>0 || EIt2.GetPixel(i)>0) && DistanceIt.GetPixel(i)>=0) //distances should all be positive
	   allDistances.push_back(DistanceIt.GetPixel(i));
				
      }
			
      if(allDistances.size()>=m_MinElements){
				
      /**get robust statistic of vector*/
        double pt = this->GetRobustStat(allDistances);
			
      /**set robustLDimage voxel = to neighborhood statistic*/
        OutputIt.Set(pt);
      }
      else
        OutputIt.Set(-100);
			
    }
	
    progress.CompletedPixel();
  }

}


template<class TEdge1,class TEdge2,class TDistance1, class TOutputImage>
double
LocalDistanceMapSmoothingImageFilter< TEdge1,TEdge2, TDistance1,  TOutputImage>
::GetRobustStat(std::vector<double> distances){
	double numMeasurements = distances.size();
	std::sort(distances.begin(),distances.end());

	/**Find the LTS */
	double midpoint;
	double AvgDistance=0.0;	
	unsigned firstItem = unsigned(numMeasurements*(1.-m_Percent));
	unsigned lastItem = unsigned(numMeasurements*(.99));

	
	for(std::vector<double>::const_iterator mI=distances.begin()+firstItem;mI!=distances.begin()+lastItem;mI++)
   		AvgDistance += *mI;
		
	
	AvgDistance/=(numMeasurements*m_Percent);	
	   midpoint = AvgDistance;

	
	return midpoint;
}


/**
 * Standard "PrintSelf" method
 */
template<class TEdge1,class TEdge2,class TDistance1, class TOutputImage>
void
LocalDistanceMapSmoothingImageFilter< TEdge1,TEdge2, TDistance1,  TOutputImage>
::PrintSelf(
  std::ostream& os, 
  Indent indent) const
{
  Superclass::PrintSelf( os, indent );
  os << indent << "Radius: " <<m_Radius<< std::endl;

}

} // end namespace itk

#endif