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

  Program:   Insight Segmentation & Registration Toolkit
  Module:    $RCSfile: itkAnytimeImageToImageMetric2.txx,v $
  Language:  C++
  Date:      $Date: 2007/09/28 21:38:43 $
  Version:   $Revision: 1.32 $

  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 _itkAnytimeImageToImageMetric2_txx
#define _itkAnytimeImageToImageMetric2_txx

#include "itkAnytimeImageToImageMetric2.h"
#include "itkEuler2DTransform.h"
#include "itkAffineTransform.h"
#include "../transforms/itkTransformFacade.h"
#include "../special/itkFastOrientedImage.h"


namespace itk
{

/**
 * Compute image derivatives using a central difference function
 * if we are not using a BSplineInterpolator, which includes
 * derivatives.
 */
//const FixedImageIndexType& fixedIndex,
//		const FixedImagePointType& fixedPoint,
#define FIXGRADIENT \
	if(fastOrientedImage) { \
		gradient=fastOrientedImage->TransformImageGradientToSpatialGradient(gradient); \
	}


	template < class TFixedImage, class TMovingImage >
		void
		AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
		::ComputeImageDerivatives(   typename FixedImageSpatialSampleContainer::const_iterator fiter,
		const MovingImagePointType& mappedPoint, 
		ParametersType& parametergradient, const MovingImageContinuousIndexType& tempIndex) const
	{
	    //const TransformJacobianType & jacobian; 
		GradientPixelType gradient;
	    const unsigned int ParametersDimension=this->m_Transform->GetNumberOfParameters();	
		const unsigned int ImageDimension = FixedImageType::ImageDimension;
		const FastOrientedImage<FixedImagePixelType,FixedImageType::ImageDimension > * fastOrientedImage=
			dynamic_cast<const FastOrientedImage<FixedImagePixelType,FixedImageType::ImageDimension > *>(this->m_FixedImage.GetPointer());
	
		switch (m_GradientMethod) {
		case GradientImage: 
			{
			// Get the gradient by NearestNeighboorInterpolation: 
			// which is equivalent to round up the point components.


			typename MovingImageType::IndexType mappedIndex; 
			for( unsigned int j = 0; j < MovingImageType::ImageDimension; j++ )
			{
				mappedIndex[j] = static_cast<long>( vnl_math_rnd( tempIndex[j] ) );
			}
			gradient = this->GetGradientImage()->GetPixel( mappedIndex );
			FIXGRADIENT;
		    const TransformJacobianType & jacobian = this->m_Transform->GetJacobian( fiter->FixedImagePointValue ); 
		    for(unsigned int par=0; par<ParametersDimension; par++)
               {
               parametergradient[par] = NumericTraits< RealType >::Zero;
               for(unsigned int dim=0; dim<ImageDimension; dim++)
                    {
                    parametergradient[par] += jacobian( dim, par ) * gradient[dim];
                    }
               }
			}

			break;

		case BSpline:
			{
			//  if( m_InterpolatorIsBSpline )
			//  {
			// Computed moving image gradient using derivative BSpline kernel.
			gradient = m_BSplineInterpolator->EvaluateDerivative( mappedPoint );
			FIXGRADIENT;
		    const TransformJacobianType & jacobian = this->m_Transform->GetJacobian( fiter->FixedImagePointValue ); 
		    for(unsigned int par=0; par<ParametersDimension; par++)
               {
               parametergradient[par] = NumericTraits< RealType >::Zero;
               for(unsigned int dim=0; dim<ImageDimension; dim++)
                    {
                    parametergradient[par] += jacobian( dim, par ) * gradient[dim];
                    }
               }
			}
		break;
		case Calculator:
			{   
			// For all generic interpolator use central differencing.
			gradient = m_DerivativeCalculator->Evaluate( mappedPoint );
			FIXGRADIENT;
   	        const TransformJacobianType & jacobian = this->m_Transform->GetJacobian( fiter->FixedImagePointValue ); 
		    for(unsigned int par=0; par<ParametersDimension; par++)
               {
               parametergradient[par] = NumericTraits< RealType >::Zero;
               for(unsigned int dim=0; dim<ImageDimension; dim++)
                    {
                    parametergradient[par] += jacobian( dim, par ) * gradient[dim];
                    }
               }
			}
			break;
		case InverseCompositional: // delibarate fallthrough
		case Inverse:
	    	{
			//MSG("Getting grad at "<<fixedIndex");
		    for(unsigned int par=0; par<ParametersDimension; par++)
               {
               parametergradient[par] = fiter->FixedImageParameterGradient[par];
			   }
			}
			break;

		case ESM:
			//itkExceptionMacro( "I dont know how to do the ESM method yet");
			if(m_ESMMode) {
				// This should be identical to inverse compositional
				for(unsigned int par=0; par<ParametersDimension; par++){
					parametergradient[par] = fiter->FixedImageParameterGradient[par];
				}
			} else {
				// this should be identical to normal, forward method
				// Get the gradient by NearestNeighboorInterpolation: 
				// which is equivalent to round up the point components.

	
				typename MovingImageType::IndexType mappedIndex; 
				for( unsigned int j = 0; j < MovingImageType::ImageDimension; j++ ){
					mappedIndex[j] = static_cast<long>( vnl_math_rnd( tempIndex[j] ) );
				}
				gradient = this->GetGradientImage()->GetPixel( mappedIndex );
				FIXGRADIENT;
				const TransformJacobianType & jacobian = this->m_Transform->GetJacobian( fiter->FixedImagePointValue ); 
				for(unsigned int par=0; par<ParametersDimension; par++){
					parametergradient[par] = NumericTraits< RealType >::Zero;
					for(unsigned int dim=0; dim<ImageDimension; dim++){
						parametergradient[par] += jacobian( dim, par ) * gradient[dim];
                    }
               }
			}
			m_ESMMode=!m_ESMMode;
			break;
		default:
			itkExceptionMacro( "Unknown method");

		}

	}


/*
 * Compute the gradient image and assign it to m_GradientImage.
 */
template <class TFixedImage, class TMovingImage> 
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::ComputeFixedGradient() 
{
  GradientImageFilterPointer gradientFilter
    = GradientImageFilterType::New();
  const unsigned int ImageDimension = FixedImageType::ImageDimension;

  gradientFilter->SetInput( this->m_FixedImage );

  const typename FixedImageType::SpacingType&
    spacing = this->m_FixedImage->GetSpacing();
  double maximumSpacing=0.0;
  for(unsigned int i=0; i<ImageDimension; i++)
    {
    if( spacing[i] > maximumSpacing )
      {
      maximumSpacing = spacing[i];
      }
    }
  gradientFilter->SetSigma( maximumSpacing );
  gradientFilter->SetNormalizeAcrossScale( true );
  
  gradientFilter->Update();
  
  this->m_FixedGradientImage = gradientFilter->GetOutput();
}


/**
 * Initialize
 */
template <class TFixedImage, class TMovingImage> 
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::Initialize(void) throw ( ExceptionObject )
{
        const unsigned int ImageDimension = FixedImageType::ImageDimension;
	if(!this->m_MovingImage) 
	{
		itkExceptionMacro(<<"MovingImage is not present");
	}

	// If the image is provided by a source, update the source.
	if( this->m_MovingImage->GetSource() )
	{
		this->m_MovingImage->GetSource()->Update();
	}
	// if any of the moving image dimensions are less than 4, we have to make
	// a new image that is a multiple stack of the original image so that
	// the interpolator and derivative will work.
	typedef typename MovingImageType::SizeType SizeType;
	typedef typename MovingImageType::SpacingType SpacingType;
	SizeType imagesize=this->m_MovingImage->GetBufferedRegion().GetSize();
	SizeType newsize=imagesize;
	SpacingType currentSpacing=this->m_MovingImage->GetSpacing();

	std::cout<<"image size: " <<imagesize<<std::endl;
	bool change=false;
	for(unsigned int i=0;i<ImageDimension;i++) {
		if(imagesize[i]<4) {
			// this dimension is too narrow to process
			// create new image same size, but 4 thick
			newsize[i]=4;
			currentSpacing[i]=currentSpacing[i]*((double)imagesize[i])/((double)newsize[i]);
			change=true;
		}
	}
	if(change) {
		std::cerr<<"Warning: the moving image is too thin in at least one dimension"<<std::endl;
		std::cerr<<"upsampling from "<<imagesize<<" to "<<newsize<<std::endl;
		typename itk::ResampleImageFilter<MovingImageType,MovingImageType>::Pointer resampler=
			itk::ResampleImageFilter<MovingImageType,MovingImageType>::New();
		resampler->SetInput(this->m_MovingImage);
		resampler->SetOutputSpacing(currentSpacing);
		resampler->SetOutputOrigin(this->m_MovingImage->GetOrigin());
		resampler->SetOutputDirection(this->m_MovingImage->GetDirection());
		resampler->SetSize(newsize);
		resampler->Update();
		//std::cerr<<"Old Image"<<std::endl;
		//std::cerr<<m_MovingImage<<std::endl;
		this->m_MovingImage=resampler->GetOutput();
		//std::cerr<<"New Image"<<std::endl;
		//std::cerr<<m_MovingImage<<std::endl;
	}
	if(m_GradientMethod==GradientImage||m_GradientMethod==ESM) {
		this->m_ComputeGradient=true;
	} else {
		this->m_ComputeGradient=false;
	}
	if(m_GradientMethod==BSpline) {
		m_InterpolatorIsBSpline = true;

		BSplineInterpolatorType * testPtr = dynamic_cast<BSplineInterpolatorType *>(
			this->m_Interpolator.GetPointer() );
		if ( !testPtr )
		{
			m_InterpolatorIsBSpline = false;
			m_GradientMethod=Calculator;
			std::cerr<<"Interpolator not BSpline, using calculator for derivative"<<std::endl;
			itkWarningMacro("Interpolator not BSpline, using calculator for derivative");
			m_BSplineInterpolator = NULL;
			itkDebugMacro( "Interpolator is not BSpline" );
		} 
		else
		{
			m_BSplineInterpolator = testPtr;
			itkDebugMacro( "Interpolator is BSpline" );
		}
	}

	if(m_GradientMethod==Calculator) {
		m_DerivativeCalculator = DerivativeFunctionType::New();
		m_DerivativeCalculator->SetInputImage( this->m_MovingImage );
	}
	if(m_GradientMethod==Inverse||m_GradientMethod==InverseCompositional||m_GradientMethod==ESM) {
		this->ComputeFixedGradient();
	//	TransformType* ptr=dynamic_cast<TransformType *>(this->m_Transform->CreateAnother().GetPointer());	
	//	this->m_IdentityTransform=TransformPointer (ptr);	
	//	this->m_IdentityTransform=(TransformPointer)this->m_Transform->CreateAnother();	
			this->m_IdentityTransform=(TransformType *)this->m_Transform->CreateAnother().GetPointer();	
			this->m_IdentityTransform->SetFixedParameters(this->m_Transform->GetFixedParameters());
			m_ESMMode=false;
			m_ICHessianComputed=false;
			//std::cout<<"id params : "<<this->m_IdentityTransform->GetParameters()<<std::endl;
//			this->m_IdentityTransform->SetIdentity();
	}
	
	
	this->Superclass::Superclass::Initialize();




	// Cache the number of transformation parameters
	this->m_NumberOfParameters = this->m_Transform->GetNumberOfParameters();
	this->dpsidphi.SetSize(this->m_NumberOfParameters,this->m_NumberOfParameters);
	this->m_CurrentChannel=0;
	//m_TotalPixelsProcessed=0;
	for (int i=0;i<NUMCHANNELS;i++) {
		this->m_nFixedImageSamples[i]=0;
		this->m_nSamples[i]=0;
		this->m_CachedParameters[i]=new ParametersType(this->m_NumberOfParameters);
		this->m_DerivativeCache[i]=new DerivativeType(this->m_NumberOfParameters);
	    this->m_ComputationLevel[i]=1.0;
		this->m_HessianCache[i]=new HessianType(this->m_NumberOfParameters,this->m_NumberOfParameters);
        this->m_HessianSumCache[i]=new HessianType(this->m_NumberOfParameters,this->m_NumberOfParameters);
        this->m_HessianSumCacheESM[i]=new HessianType(this->m_NumberOfParameters,this->m_NumberOfParameters);
 }
  /**
   * Allocate memory for the fixed image sample container.
   */
  //std::cout<<"Initializing space for "<< m_NumberOfSpatialSamples<< " samples...."<<std::endl;
  //std::cout<<"Samples are "<< sizeof(FixedImageSpatialSample) << " bytes. total is "<<sizeof(FixedImageSpatialSample)*m_NumberOfSpatialSamples<<std::endl;
  try {
  this->m_FixedImageSamples.resize( m_NumberOfSpatialSamples);
  } catch (std::exception e) {
    itkExceptionMacro( "Resize for " << this->m_NumberOfSpatialSamples<<" samples failed: ");
  }
  //std::cout<<"done"<<std::endl;
  /** 
   * Uniformly sample the fixed image (within the fixed image region)
   * to create the sample points list.
   */
  this->SampleFixedImageDomain( this->m_FixedImageSamples );
	if(m_GradientMethod==Inverse||m_GradientMethod==InverseCompositional||m_GradientMethod==ESM) {
		this->SampleJacobians(this->m_FixedImageSamples);
	}
}

/**
 * Compute the dpsidphi matrix
 */
template < class TFixedImage, class TMovingImage >
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::ComputeDpsidphi( ) const
{
	itk::TransformFacade<double,TFixedImage::ImageDimension> tf(this->m_Transform);
	tf.ComputeDpsidphi(dpsidphi);
}
/**
 * Compute the Jacobians of the fixed image pixels
 */
template < class TFixedImage, class TMovingImage >
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::SampleJacobians( FixedImageSpatialSampleContainer& samples )
{
  //ERRMSG("Computing Jacobians");
  typename FixedImageSpatialSampleContainer::iterator iter;
  typename FixedImageSpatialSampleContainer::const_iterator end=samples.end();
		const FastOrientedImage<FixedImagePixelType,FixedImageType::ImageDimension > * fastOrientedImage=
			dynamic_cast<const FastOrientedImage<FixedImagePixelType,FixedImageType::ImageDimension > *>(this->m_FixedImage.GetPointer());
  //TransformParametersType parameters=this->m_Transform->GetParameters();
  TransformParametersType identity=(dynamic_cast<TransformType *>(this->m_Transform->CreateAnother().GetPointer()))->GetParameters();
    //parameters.Fill(0);
  this->m_Transform->SetParameters(identity);
  const unsigned int ParametersDimension = this->GetNumberOfParameters();
  const unsigned int ImageDimension = FixedImageType::ImageDimension;
  GradientPixelType gradient;
  for( iter=samples.begin(); iter != end;++iter )
    {
		gradient=this->m_FixedGradientImage->GetPixel( iter->FixedImageIndex );
		FIXGRADIENT;
		const TransformJacobianType & jacobian = this->m_IdentityTransform->GetJacobian( iter->FixedImagePointValue ); 
	//	std::cout<<"point: "<<(*iter).FixedImagePointValue<<std::endl;
	//	std::cout<<"gradient: "<<gradient<<std::endl;
	//		std::cout<<"jacobian: "<<jacobian<<std::endl;

			//  (*iter).FixedImageJacobian=
	//this->m_Transform->GetJacobian( (*iter).FixedImagePointValue ); 
      //      std::cout<<"G: "<<gradient<<" P: "<<(*iter).FixedImagePointValue<<std::endl;
      (*iter).FixedImageParameterGradient=TransformParametersType(ParametersDimension);
      for(unsigned int par=0; par<ParametersDimension; par++)
	{
	  (*iter).FixedImageParameterGradient[par]=0.0;
	  for(unsigned int dim=0; dim<ImageDimension; dim++)
	    {
	   //   (*iter).FixedImageJacobian(dim,par) *= gradient[dim];
	   //   (*iter).FixedImageParameterGradient[par]+=
		//(*iter).FixedImageJacobian(dim,par);
	      //jacobian(dim,par) *= gradient[dim];
	      (*iter).FixedImageParameterGradient[par]+=
		jacobian(dim,par)*gradient[dim];
	    }
        }
 
 
	//		std::cout<<"pgradient: "<<(*iter).FixedImageParameterGradient<<std::endl;


    }
}
/**
 * Uniformly sample the fixed image domain using a random walk
 */
template < class TFixedImage, class TMovingImage >
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::SampleFixedImageDomain( FixedImageSpatialSampleContainer& samples )
{

  // Set up a random interator within the user specified fixed image region.
  RandomIterator randIter( this->m_FixedImage, this->GetFixedImageRegion() );

  randIter.SetNumberOfSamples( this->GetFixedImageRegion().GetNumberOfPixels() );
  randIter.ReinitializeSeed(m_RandomSeed);
  if(m_PriorityImage!=NULL)    randIter.SetPriorityImage(m_PriorityImage);
  randIter.GoToBegin();
  m_NumberOfAvailableSamples=0;
  typename FixedImageSpatialSampleContainer::iterator iter;
  typename FixedImageSpatialSampleContainer::const_iterator end=samples.end();
  //std::cout<<"Trying to get "<<m_NumberOfSpatialSamples<<" samples"<<std::endl;

  if( this->m_FixedImageMask )
    {

    typename Superclass::InputPointType inputPoint;

    iter=samples.begin();
    while( iter != end && !randIter.IsAtEnd())
      {
      // Get sampled index
      FixedImageIndexType index = randIter.GetIndex();
      // Check if the Index is inside the mask, translate index to point
      this->m_FixedImage->TransformIndexToPhysicalPoint( index, inputPoint );

      // If not inside the mask, ignore the point
      if( !this->m_FixedImageMask->IsInside( inputPoint ) )
        {
        ++randIter; // jump to another random position
        continue;
        }

      // Save index
      (*iter).FixedImageIndex = index;
      // Get sampled fixed image value
      (*iter).FixedImageValue = randIter.Get();
      // Translate index to point
      (*iter).FixedImagePointValue = inputPoint;

      // Jump to random position
      ++randIter;
      ++iter;
      ++m_NumberOfAvailableSamples;
      }
    }
  else
    {
    for( iter=samples.begin(); iter != end && !randIter.IsAtEnd(); ++iter,      ++m_NumberOfSpatialSamples )
      {
      // Get sampled index
      FixedImageIndexType index = randIter.GetIndex();
      // Save index
      (*iter).FixedImageIndex = index;
      // Get sampled fixed image value
      (*iter).FixedImageValue = randIter.Get();
      // Translate index to point
      this->m_FixedImage->TransformIndexToPhysicalPoint( index,
                                                   (*iter).FixedImagePointValue );
      // Jump to random position
      ++randIter;
      ++m_NumberOfAvailableSamples;

      }
    }
  m_FixedImageSamples.resize( m_NumberOfAvailableSamples);
  // m_NumberOfSpatialSamples=samples.size();
  //std::cout<<"Size of sample set"<<m_FixedImageSamples.size()<<std::endl;
}


///////////////////////////////////////////
/**
 * Get the match Measure
 */
template < class TFixedImage, class TMovingImage  >
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetHessian( const ParametersType& parameters, HessianType& Hessian ) const
{
  //this->ResetComputation(this->m_CurrentChannel);
  this->GetHessian(parameters,Hessian,this->m_CurrentChannel);
}


///////////////////////////////////////////
/**
 * Get the match Measure
 */
template < class TFixedImage, class TMovingImage  >
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetHessian( const ParametersType& parameters, HessianType& Hessian,const int channel, const double level) const
{
  this->SetComputationLevel(level,channel);
  //this->ResetComputation(this->m_CurrentChannel);
  this->GetHessian(parameters,Hessian,channel);
}


///////////////////////////////////////////
/**
 * Get the match Measure
 */
template < class TFixedImage, class TMovingImage  >
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetHessian( const ParametersType& parameters,
					 HessianType& Hessian, const double level) const
{
  this->SetComputationLevel(level,this->m_CurrentChannel);
  //this->ResetComputation(this->m_CurrentChannel);
  this->GetHessian(parameters,Hessian,this->m_CurrentChannel);
}

///////////////////////////////////////////
/**
 * Get the match Measure
 */
template < class TFixedImage, class TMovingImage  >
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetValueAndHessian( const ParametersType& parameters, MeasureType& Value,HessianType& Hessian ) const
{
  //this->ResetComputation(this->m_CurrentChannel);
  this->GetValueAndHessian(parameters, Value, Hessian,this->m_CurrentChannel);
}


///////////////////////////////////////////
/**
 * Get the match Measure
 */
template < class TFixedImage, class TMovingImage  >
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetValueAndHessian( const ParametersType& parameters, MeasureType& Value,
					 HessianType& Hessian,const int channel, const double level) const
{
  this->SetComputationLevel(level,channel);
  //this->ResetComputation(this->m_CurrentChannel);
  this->GetValueAndHessian(parameters,Value,Hessian,channel);
}


///////////////////////////////////////////
/**
 * Get the match Measure
 */
template < class TFixedImage, class TMovingImage  >
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetValueAndHessian( const ParametersType& parameters, MeasureType& Value, 
					 HessianType& Hessian, const double level) const
{
  this->SetComputationLevel(level,this->m_CurrentChannel);
  //this->ResetComputation(this->m_CurrentChannel);
  this->GetValueAndHessian(parameters,Value,Hessian,this->m_CurrentChannel);
}



/** Get the derivatives of the match measure. */
template < class TFixedImage, class TMovingImage  >
void AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetDerivativeAndHessian( const ParametersType & parameters,
		    DerivativeType & derivative, HessianType& Hessian, int channel,
		  double level ) const {
  this->SetComputationLevel(level,channel);
  this->GetDerivativeAndHessian(parameters,derivative,Hessian,channel);
}
/**  Get value and derivatives for multiple valued optimizers. */
template < class TFixedImage, class TMovingImage  >
void AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetValueAndDerivativeAndHessian( const ParametersType & parameters,
			    MeasureType& Value, DerivativeType& Derivative,HessianType& Hessian,
			    int channel,
			 double level ) const {
  this->SetComputationLevel(level,channel);
  this->GetValueAndDerivativeAndHessian(parameters,Value,Derivative,Hessian,channel);
}
 
/** Get the derivatives of the match measure. */
template < class TFixedImage, class TMovingImage  >
void AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetDerivativeAndHessian( const ParametersType & parameters,
		 DerivativeType & derivative, HessianType& Hessian,double level ) const {
  this->SetComputationLevel(level,this->m_CurrentChannel);
  this->GetDerivativeAndHessian(parameters,derivative,Hessian,this->m_CurrentChannel);
}
/**  Get value and derivatives for multiple valued optimizers. */
template < class TFixedImage, class TMovingImage  >
void AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetValueAndDerivativeAndHessian( const ParametersType & parameters,
			    MeasureType& Value, DerivativeType& Derivative, HessianType& Hessian,
			     double level ) const{
  this->SetComputationLevel(level,this->m_CurrentChannel);
  this->GetValueAndDerivativeAndHessian(parameters,Value,Derivative,Hessian,this->m_CurrentChannel);
}
 
template < class TFixedImage, class TMovingImage  >
void AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetDerivativeAndHessian( const ParametersType & parameters,
		 DerivativeType & derivative, HessianType& Hessian) const {
  this->GetDerivativeAndHessian(parameters,derivative,Hessian,this->m_CurrentChannel);
}
/**  Get value and derivatives for multiple valued optimizers. */
template < class TFixedImage, class TMovingImage  >
void AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetValueAndDerivativeAndHessian( const ParametersType & parameters,
			 MeasureType& Value, 
			 DerivativeType& Derivative,HessianType& Hessian) const {
  this->GetValueAndDerivativeAndHessian(parameters,Value,Derivative,Hessian,this->m_CurrentChannel);
}

/////////////////////////
			 /*
 * Get the match Measure
 */
template <class TFixedImage, class TMovingImage> 
typename AnytimeImageToImageMetric2<TFixedImage,TMovingImage>::MeasureType
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetValue( const ParametersType& parameters, const int channel) const
{
  MeasureType value;
  DerivativeType derivative(this->m_NumberOfParameters);
  HessianType H;
  m_NumberOfEvaluations++;
  m_NumberOfValueEvaluations++;
  // call the combined version
	if(this->m_FixedImageMask) {
		if(this->m_MovingImageMask) {
			this->GetValueAndDerivativeAndHessian_calc(true,false,false,true,true, parameters, value, derivative,H, channel );
		} else {
			this->GetValueAndDerivativeAndHessian_calc(true,false,false,true,false, parameters, value, derivative,H, channel );
		}
	}else {
		if(this->m_MovingImageMask) {
			this->GetValueAndDerivativeAndHessian_calc(true,false,false,false,true, parameters, value, derivative, H, channel );
		} else {
			this->GetValueAndDerivativeAndHessian_calc(true,false,false,false,false, parameters, value, derivative, H, channel );
		}
	}
  MetricEvaluationEvent me(value,parameters,true,false,false);
  this->InvokeEvent( me );
  return value;
}



/*
 * Get the Derivative Measure
 */
template < class TFixedImage, class TMovingImage> 
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetDerivative( const ParametersType& parameters, DerivativeType & derivative, int channel ) const
{
  MeasureType value;
  HessianType H;
  m_NumberOfEvaluations++;
  m_NumberOfDerivativeEvaluations++;
  // call the combined version
	if(this->m_FixedImageMask) {
		if(this->m_MovingImageMask) {
			this->GetValueAndDerivativeAndHessian_calc(false,true,false,true,true, parameters, value, derivative, H, channel );
		} else {
			this->GetValueAndDerivativeAndHessian_calc(false,true,false,true,false, parameters, value, derivative, H, channel );
		}
	}else {
		if(this->m_MovingImageMask) {
			this->GetValueAndDerivativeAndHessian_calc(false,true,false,false,true, parameters, value, derivative, H, channel );
		} else {
			this->GetValueAndDerivativeAndHessian_calc(false,true,false,false,false, parameters, value, derivative, H, channel );
		}
	}
  MetricEvaluationEvent me(value,parameters,false,true,false);
  this->InvokeEvent( me );
}

/*
 * Get both the match Measure and theDerivative Measure 
 */
template <class TFixedImage, class TMovingImage> 
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetValueAndDerivative(const ParametersType & parameters, 
                        MeasureType & value, DerivativeType  & derivative, 
			int channel) const
{
    m_NumberOfEvaluations++;
	m_NumberOfValueEvaluations++;
	m_NumberOfDerivativeEvaluations++;
	HessianType H;
	if(this->m_FixedImageMask) {
		if(this->m_MovingImageMask) {
			this->GetValueAndDerivativeAndHessian_calc(true,true,false,true,true, parameters, value, derivative, H, channel );
		} else {
			this->GetValueAndDerivativeAndHessian_calc(true,true,false,true,false, parameters, value, derivative, H,  channel );
		}
	}else {
		if(this->m_MovingImageMask) {
			this->GetValueAndDerivativeAndHessian_calc(true,true,false,false,true, parameters, value, derivative, H, channel );
		} else {
			this->GetValueAndDerivativeAndHessian_calc(true,true,false,false,false, parameters, value, derivative, H, channel );
		}
	}
  MetricEvaluationEvent me(value,parameters,true,true,false);
  this->InvokeEvent( me );
}
			 /*
 * Get the match hessian
 */
template <class TFixedImage, class TMovingImage> 
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetHessian( const ParametersType& parameters,HessianType& H, int channel) const
{
  MeasureType value;
  DerivativeType derivative(this->m_NumberOfParameters);
  m_NumberOfEvaluations++;
  m_NumberOfHessianEvaluations++;
  // call the combined version
	if(this->m_FixedImageMask) {
		if(this->m_MovingImageMask) {
			this->GetValueAndDerivativeAndHessian_calc(false,false,true,true,true, parameters, value, derivative,H, channel );
		} else {
			this->GetValueAndDerivativeAndHessian_calc(false,false,true,true,false, parameters, value, derivative,H, channel );
		}
	}else {
		if(this->m_MovingImageMask) {
			this->GetValueAndDerivativeAndHessian_calc(false,false,true,false,true, parameters, value, derivative, H, channel );
		} else {
			this->GetValueAndDerivativeAndHessian_calc(false,false,true,false,false, parameters, value, derivative, H, channel );
		}
	}
  MetricEvaluationEvent me(value,parameters,false,false,true);
  this->InvokeEvent( me );
}

			 /*
 * Get the match Measure
 */
template <class TFixedImage, class TMovingImage> 
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetValueAndHessian( const ParametersType& parameters,MeasureType& value,HessianType& H, int channel) const
{
  DerivativeType derivative(this->m_NumberOfParameters);
  m_NumberOfEvaluations++;
  m_NumberOfValueEvaluations++;
  m_NumberOfHessianEvaluations++;
  // call the combined version
	if(this->m_FixedImageMask) {
		if(this->m_MovingImageMask) {
			this->GetValueAndDerivativeAndHessian_calc(true,false,true,true,true, parameters, value, derivative,H, channel );
		} else {
			this->GetValueAndDerivativeAndHessian_calc(true,false,true,true,false, parameters, value, derivative,H, channel );
		}
	}else {
		if(this->m_MovingImageMask) {
			this->GetValueAndDerivativeAndHessian_calc(true,false,true,false,true, parameters, value, derivative, H, channel );
		} else {
			this->GetValueAndDerivativeAndHessian_calc(true,false,true,false,false, parameters, value, derivative, H, channel );
		}
	}
  MetricEvaluationEvent me(value,parameters,true,false,true);
  this->InvokeEvent( me );
}


/*
 * Get the Derivative Measure
 */
template < class TFixedImage, class TMovingImage> 
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetDerivativeAndHessian( const ParametersType& parameters, DerivativeType & derivative, HessianType& H,int channel ) const
{
  MeasureType value;
  m_NumberOfEvaluations++;
  m_NumberOfDerivativeEvaluations++;
  m_NumberOfHessianEvaluations++;
  // call the combined version
	if(this->m_FixedImageMask) {
		if(this->m_MovingImageMask) {
			this->GetValueAndDerivativeAndHessian_calc(false,true,true,true,true, parameters, value, derivative, H, channel );
		} else {
			this->GetValueAndDerivativeAndHessian_calc(false,true,true,true,false, parameters, value, derivative, H, channel );
		}
	}else {
		if(this->m_MovingImageMask) {
			this->GetValueAndDerivativeAndHessian_calc(false,true,true,false,true, parameters, value, derivative, H, channel );
		} else {
			this->GetValueAndDerivativeAndHessian_calc(false,true,true,false,false, parameters, value, derivative, H, channel );
		}
	}
  MetricEvaluationEvent me(value,parameters,false,true,true);
  this->InvokeEvent( me );
}

/*
 * Get both the match Measure and theDerivative Measure 
 */
template <class TFixedImage, class TMovingImage> 
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetValueAndDerivativeAndHessian(const ParametersType & parameters, 
                        MeasureType & value, DerivativeType  & derivative, HessianType & H,
			int channel) const
{
  m_NumberOfEvaluations++;
  m_NumberOfValueEvaluations++;
  m_NumberOfDerivativeEvaluations++;
  m_NumberOfHessianEvaluations++;
	if(this->m_FixedImageMask) {
		if(this->m_MovingImageMask) {
			this->GetValueAndDerivativeAndHessian_calc(true,true,true,true,true, parameters, value, derivative, H, channel );
		} else {
			this->GetValueAndDerivativeAndHessian_calc(true,true,true,true,false, parameters, value, derivative, H,  channel );
		}
	}else {
		if(this->m_MovingImageMask) {
			this->GetValueAndDerivativeAndHessian_calc(true,true,true,false,true, parameters, value, derivative, H, channel );
		} else {
			this->GetValueAndDerivativeAndHessian_calc(true,true,true,false,false, parameters, value, derivative, H, channel );
		}
	}
  MetricEvaluationEvent me(value,parameters,true,true,true);
  this->InvokeEvent( me );
}
	/*
 * Get both the match Measure and theDerivative Measure 
 */
template <class TFixedImage, class TMovingImage>
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetValueAndDerivativeAndHessian_calc(bool calcvalue,bool calcderivative,bool calchessian,
			bool fixedmask,bool movingmask,const ParametersType & parameters, 
							 MeasureType & value, DerivativeType  & derivative, HessianType & H,
							 int channel) const
{
	itkExceptionMacro("No punky, ya really have to override this one in the derived class.  Do not pass go, do not collect $200");
}
/*template <class TFixedImage, class TMovingImage>
template <	bool calcvalue,bool calcderivative,bool calchessian,
			bool fixedmask,bool movingmask> 
void
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetValueAndDerivativeAndHessian_calc(const ParametersType & parameters, 
							 MeasureType & value, DerivativeType  & derivative, HessianType & H,
							 int channel) const
{
	itkExceptionMacro("No punky, ya really have to override this one in the derived class.  Do not pass go, do not collect $200");
}
*/





template < class TFixedImage, class TMovingImage  >
void AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::ConvertDerivative( GradientCalculationMethod gradmethod, ParametersType& derivative, ParametersType& derivativeESM ) const{
	if(gradmethod==Inverse) {
        if(!m_DPsiDPhiComputed) this->ComputeDpsidphi();
		DerivativeType oldderivative=derivative;
		//std::cout<<"Old deriviative:"<<std::endl;
		//std::cout<<oldderivative<<std::endl;
		for(unsigned int phi=0;phi<this->m_NumberOfParameters;phi++) {
			derivative[phi]=0.0;
		for(unsigned int psi=0;psi<this->m_NumberOfParameters;psi++) {
			derivative[phi]+=this->dpsidphi[psi][phi]*oldderivative[psi];
		}
		}
	}
	if(gradmethod==ESM) {
        if(!m_DPsiDPhiComputed) this->ComputeDpsidphi();
		//DerivativeType oldderivative=derivativeESM;
		//std::cout<<"Old deriviative:"<<std::endl;
		//std::cout<<oldderivative<<std::endl;
		for(unsigned int phi=0;phi<this->m_NumberOfParameters;phi++) {
	//		derivative[phi]=0.0;
		for(unsigned int psi=0;psi<this->m_NumberOfParameters;psi++) {
			derivative[phi]+=this->dpsidphi[psi][phi]*derivativeESM[psi];
		}
		derivative[phi]/=2.0;
		}

	}
	if(gradmethod==InverseCompositional) {
		for(unsigned int phi=0;phi<this->m_NumberOfParameters;phi++) {
			derivative[phi]*=-1.0;
		}
	}
}

  
  template < class TFixedImage, class TMovingImage  >
void AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::ConvertHessian( GradientCalculationMethod gradmethod, HessianType& Hessian ,HessianType& HessianESM) const{
	/*if((gradmethod==Inverse||gradmethod==InverseCompositional)&&m_ICHessianComputed) {
		// if its been computed, copy it over
				Hessian=m_ICHessian;
	} else	if((gradmethod==Inverse||gradmethod==InverseCompositional)&&!m_ICHessianComputed) {
		// if we get here, then we just computed it, so copy it into the cache
		m_ICHessian=Hessian;
		m_ICHessianComputed=true;

	}
	*/
	if(gradmethod==Inverse) {
		if(!m_DPsiDPhiComputed) this->ComputeDpsidphi();
		HessianType oldHessian=Hessian;
		//std::cout<<"Old deriviative:"<<std::endl;
		//std::cout<<oldderivative<<std::endl;
		for(unsigned int phi1=0;phi1<this->m_NumberOfParameters;phi1++) {
			for(unsigned int phi2=0;phi2<this->m_NumberOfParameters;phi2++) {
				Hessian[phi1][phi2]=0.0;
				for(unsigned int psi1=0;psi1<this->m_NumberOfParameters;psi1++) {
					for(unsigned int psi2=0;psi2<this->m_NumberOfParameters;psi2++) {
						Hessian[phi1][phi2]+=
							this->dpsidphi[psi1][phi1]*oldHessian[psi1][psi2]*
							this->dpsidphi[psi2][phi2];
					}
				}
			}
		}
	}
	if(gradmethod==ESM) {
		if(!m_DPsiDPhiComputed) this->ComputeDpsidphi();
		//HessianType oldHessian=Hessian;
		//std::cout<<"Old deriviative:"<<std::endl;
		//std::cout<<oldderivative<<std::endl;
		for(unsigned int phi1=0;phi1<this->m_NumberOfParameters;phi1++) {
			for(unsigned int phi2=0;phi2<this->m_NumberOfParameters;phi2++) {
	//			Hessian[phi1][phi2]=0.0;
				for(unsigned int psi1=0;psi1<this->m_NumberOfParameters;psi1++) {
					for(unsigned int psi2=0;psi2<this->m_NumberOfParameters;psi2++) {
						Hessian[phi1][phi2]+=
							this->dpsidphi[psi1][phi1]*HessianESM[psi1][psi2]*
							this->dpsidphi[psi2][phi2];
					}
				}
				Hessian[phi1][phi2]/=2.0;
			}
		}
	}

}
template <class TFixedImage, class TMovingImage> 
TransformBase::Pointer
AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetTransformCopy(void) const 
{
	TransformBase::Pointer rv=dynamic_cast<TransformBase *>
      (this->m_Transform->CreateAnother().GetPointer());
	rv->SetFixedParameters(this->m_Transform->GetFixedParameters());
	//std::cout<<"Transform is: "<<this->GetTransform()->GetNameOfClass()<<std::endl;
	return rv;
}

  template < class TFixedImage, class TMovingImage  >
void AnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::ConvertDerivativeAndHessian( GradientCalculationMethod gradmethod, ParametersType& derivative,HessianType& Hessian, ParametersType& derivativeESM,HessianType& HessianESM ) const{
	if(gradmethod==Inverse||gradmethod==ESM) {
       this->ComputeDpsidphi();
		m_DPsiDPhiComputed=true;
	}
	this->ConvertDerivative(gradmethod,derivative,derivativeESM);
	this->ConvertHessian(gradmethod,Hessian,HessianESM);

    m_DPsiDPhiComputed=false;
}



} // end namespace itk

#endif