/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    $RCSfile: itkCountImageFilter.txx,v $
  Language:  C++
  Date:      $Date: 2006/01/11 19:43:31 $
  Version:   $Revision: 1.14 $

  Copyright (c) Insight Software Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even 
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
#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