/*******************************************************************
 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 _itkLocalGrayscaleDistanceMapImageFilter_txx
#define _itkLocalGrayscaleDistanceMapImageFilter_txx
#include "itkLocalGrayscaleDistanceMapImageFilter.h"

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

namespace itk
{

template<class TEdge1,class TEdge2, class TOutputImage>

LocalGrayscaleDistanceMapImageFilter<TEdge1,TEdge2,TOutputImage>
::LocalGrayscaleDistanceMapImageFilter(){
/** this filter requires two input images*/
  this->SetNumberOfRequiredInputs( 2 );
}


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

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

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

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



template<class TEdge1,class TEdge2, class TOutputImage>
void 
LocalGrayscaleDistanceMapImageFilter<TEdge1,TEdge2,TOutputImage>
::BeforeThreadedGenerateData()
{

  typename  Edge1Type::ConstPointer edge1  = this->GetInput1();

  /**Create search index for NN search, using iterator for N-dimensionality*/

  typename Edge1Type::SpacingType spacing1 = edge1->GetSpacing();


  // A radius of 1 in all axial directions gives a 3x3x3x3x... neighborhood.
  typename NeighborhoodIteratorType::RadiusType radius;
  for (unsigned int i = 0; i < OutputImageDimension; ++i) radius[i] = m_MaxDef;

  // Initializes the iterators on the input & output image regions
  NeighborhoodIteratorType it(radius, edge1, 
                                  edge1->GetRequestedRegion());


  // Set location away from boundary
  typename Edge1Type::IndexType itIdx;
  for (unsigned int i = 0; i < OutputImageDimension; ++i) itIdx[i] = m_MaxDef+2;
  
  it.SetLocation(itIdx);

  //iterate over neighborhood, creating search index
  for (unsigned int i = 0; i < it.Size(); ++i)
  {    
    coord coord1;
    double distance=0.0;
    typename NeighborhoodIteratorType::OffsetType idx = it.GetOffset(i);

    for(int x=0;x<OutputImageDimension;x++){
      coord1.coords.push_back(idx[x]);  
      distance = distance+spacing1[x]*double(idx[x]*idx[x]);
    }
    distance=sqrt(distance);
    coord1.dist=distance;
    coord_list.push_back(coord1);
  }
      
  std::sort(coord_list.begin(), coord_list.end());
          




  /**Create Grayscale Images to use for search *****/
   //Create the grayscale images
  typename CountFilterType::Pointer filter = CountFilterType::New();
  typename CountFilterType::Pointer filter2 = CountFilterType::New();

  typename Edge1Type::SizeType indexRadius;
   for(int x=0;x<OutputImageDimension;x++)
     indexRadius[x]=m_Radius;
  
  filter->SetRadius( indexRadius );
  filter2->SetRadius( indexRadius );

  filter->SetInput( this->GetInput1() );
  filter2->SetInput( this->GetInput2());
  //filter->SetNumberOfThreads(atoi(argv[7]));
  //filter2->SetNumberOfThreads(atoi(argv[7]));
  
  filter->Update();
  filter2->Update();

  //mask grayscale images
  typename MaskFilter::Pointer masker = MaskFilter::New();
  masker->SetInput1(filter->GetOutput());
  masker->SetInput2(this->GetInput1());
  //masker->SetNumberOfThreads(atoi(argv[7]));
  masker->Update();
  m_gray1= masker->GetOutput();

  typename MaskFilter::Pointer masker2 = MaskFilter::New();
  masker2->SetInput1(filter2->GetOutput());
  masker2->SetInput2(this->GetInput2());
  //masker2->SetNumberOfThreads(atoi(argv[7]));
  masker2->Update();
  m_gray2= masker2->GetOutput();

}


template<class TEdge1,class TEdge2, class TOutputImage>
void
LocalGrayscaleDistanceMapImageFilter<TEdge1,TEdge2,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 Edge1Type::Pointer edge1 = m_gray1;
  typename Edge2Type::Pointer edge2 = m_gray2;

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

  
  std::vector<double> hdVect;

  /**create iterators for the 3 images (2 distance transforms, output image)*/
  typename NeighborhoodIteratorType::RadiusType radius;
  radius.Fill(m_MaxDef);
  NeighborhoodIteratorType Niter( radius, edge1,
                               outputRegionForThread );
   NeighborhoodIteratorType NiterM(radius, edge2, outputRegionForThread);
   IteratorType iterLD(outputImage, outputRegionForThread);
  
  /** Iterate through image creating grayscale local distance map. Each pixel in distance map is set to:
  * |1A(x)-1B(x)| x max(d(x_g,A),d(x_g,B)), where 1A(X) is  afunction which has value 1 if A(X) is non-zero
  * and 0 otherwise.  d(x_g,A) is the minimum distance from point x to the nearest point in A which has grayscale
  * value within some tolerance of g. */

  for(NiterM.GoToBegin(),Niter.GoToBegin(), iterLD.GoToBegin(); !Niter.IsAtEnd();++NiterM,++Niter, ++iterLD){
    float val1 = NiterM.GetCenterPixel();
    float val2 = Niter.GetCenterPixel();
	iterLD.Set(0);
    if(val1||val2){
      double min_dist = -100;
      int found = 0;
      for(std::vector<coord>::const_iterator cI=coord_list.begin();cI!=coord_list.end(), coord(*cI).dist<=m_MaxDef;cI++){
	 coord coord1 = *cI;
	 typename NeighborhoodIteratorType::OffsetType offset;
	      
         std::vector<short> coordinates=coord1.coords;
    	 for(int x=0;x<OutputImageDimension;x++)
           offset[x]=coordinates[x];	
   
      if(val1>1){
	float val_s = Niter.GetPixel(offset);
	if(val_s>=(val1-m_Tol)&&val_s<=(val1+m_Tol)){
	min_dist = coord1.dist;
        found = 1;
	break;
        }
      } 
      if(val2>1){
        float val_s = NiterM.GetPixel(offset);
        if(val_s>=(val2-m_Tol)&&val_s<=(val2+m_Tol)){
          min_dist = coord1.dist;
	  found = 1;
	  break;
        }
      }  

    }
    /**If distance is greater than the maximum deformation search, set value to impossible value*/
    if(min_dist>m_MaxDef){
      min_dist = -100;
      found=0;
    }
    if(found)
      hdVect.push_back(min_dist);
    else
      min_dist = -100;
    iterLD.Set(min_dist);
	
    }
    progress.CompletedPixel();
  }

}



/**
 * Standard "PrintSelf" method
 */
template<class TEdge1,class TEdge2, class TOutputImage>
void
LocalGrayscaleDistanceMapImageFilter<TEdge1,TEdge2,TOutputImage>
::PrintSelf(
  std::ostream& os, 
  Indent indent) const
{
  Superclass::PrintSelf( os, indent );
  os << indent << "Max Deformation: " <<m_MaxDef<< std::endl;

}

} // end namespace itk

#endif