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

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

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

#include "itkMeanSquaresAnytimeImageToImageMetric2.h"
#include "itkImageRegionConstIteratorWithIndex.h"

namespace itk
{

/*
 * Constructor
 */
template <class TFixedImage, class TMovingImage> 
MeanSquaresAnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::MeanSquaresAnytimeImageToImageMetric2()
{
  itkDebugMacro("Constructor");
}

/*
 * Get the match Measure
 */
/*template <class TFixedImage, class TMovingImage> 
typename MeanSquaresAnytimeImageToImageMetric2<TFixedImage,TMovingImage>::MeasureType
MeanSquaresAnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetValue( const TransformParametersType & parameters, const int channel ) const
{
  MeasureType value;
  DerivativeType derivative(this->m_NumberOfParameters);
  HessianType Hessian;
  // call the combined version
  int dummy=channel;
  this->GetValueAndDerivativeAndHessian( parameters, value, derivative, Hessian,dummy );
  return value;
}

template <class TFixedImage, class TMovingImage> 
void
MeanSquaresAnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetHessian( const TransformParametersType & parameters, HessianType& Hessian, const int channel ) const
{
  MeasureType value;
  DerivativeType derivative(this->m_NumberOfParameters);
  // call the combined version
  int dummy=channel;
  this->GetValueAndDerivativeAndHessian( parameters, value, derivative, Hessian,dummy );
}

template <class TFixedImage, class TMovingImage> 
void
MeanSquaresAnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetValueAndHessian( const TransformParametersType & parameters, MeasureType& value, 
					 HessianType& Hessian, const int channel ) const
{
  DerivativeType derivative(this->m_NumberOfParameters);
  // call the combined version
  int dummy=channel;
  this->GetValueAndDerivativeAndHessian( parameters, value, derivative, Hessian,dummy );
}

*/


/*
 * Get the Derivative Measure
 */
/*template < class TFixedImage, class TMovingImage> 
void
MeanSquaresAnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetDerivative( const TransformParametersType & parameters,
                 DerivativeType & derivative, int channel  ) const
{
  // Do Both and return one
  MeasureType value;
  HessianType Hessian;
  // call the combined version
  this->GetValueAndDerivativeAndHessian( parameters, value, derivative,Hessian, channel );

}
*/
/*
 * Get the Derivative Measure
 */
/*template < class TFixedImage, class TMovingImage> 
void
MeanSquaresAnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetDerivativeAndHessian( const TransformParametersType & parameters,
                 DerivativeType & derivative, HessianType& Hessian, int channel  ) const
{
  // Do Both and return one
  MeasureType value;
  // call the combined version
  this->GetValueAndDerivativeAndHessian( parameters, value, derivative,Hessian, channel );

}
*/
/*
 * Get both the match Measure and theDerivative Measure 
 */
/*template <class TFixedImage, class TMovingImage> 
void
MeanSquaresAnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetValueAndDerivative(const TransformParametersType & parameters, 
                        MeasureType & value, DerivativeType  & derivative, int channel) const
{
  HessianType Hessian;
  // call the combined version
  this->GetValueAndDerivativeAndHessian( parameters, value, derivative,Hessian, channel );
}
*/

/*
 * Get both the match Measure and theDerivative Measure 
 */

template <class TFixedImage, class TMovingImage> 
void
MeanSquaresAnytimeImageToImageMetric2<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
{
	if(calcvalue) {
		if(calcderivative) {
			if(calchessian) {
				if(fixedmask) {
					if(movingmask) {
						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(movingmask) {
						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);
					}
				}
			} else {
				if(fixedmask) {
					if(movingmask) {
						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(movingmask) {
						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);
					}
				}
			}
		} else {
			if(calchessian) {
				if(fixedmask) {
					if(movingmask) {
						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(movingmask) {
						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);
					}
				}
			} else {
				if(fixedmask) {
					if(movingmask) {
						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(movingmask) {
						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);
					}
				}
			}
		}
	} else {
		if(calcderivative) {
			if(calchessian) {
				if(fixedmask) {
					if(movingmask) {
						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(movingmask) {
						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);
					}
				}
			} else {
				if(fixedmask) {
					if(movingmask) {
						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(movingmask) {
						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);
					}
				}
			}
		} else {
			if(calchessian) {
				if(fixedmask) {
					if(movingmask) {
						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(movingmask) {
						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);
					}
				}
			} else {
				if(fixedmask) {
					if(movingmask) {
						this->GetValueAndDerivativeAndHessian_calc<false,false,false,true,true>(parameters,value,derivative,H,channel);
					} else {
						this->GetValueAndDerivativeAndHessian_calc<false,false,false,true,false>(parameters,value,derivative,H,channel);
					}
				} else {
					if(movingmask) {
						this->GetValueAndDerivativeAndHessian_calc<false,false,false,false,true>(parameters,value,derivative,H,channel);
					} else {
						this->GetValueAndDerivativeAndHessian_calc<false,false,false,false,false>(parameters,value,derivative,H,channel);
					}
				}
			}
		}
	}

}


template <class TFixedImage, class TMovingImage> 
template <	bool calcvalue,bool calcderivative,bool calchessian,
			bool fixedmask,bool movingmask> 
void
MeanSquaresAnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::GetValueAndDerivativeAndHessian_calc(const TransformParametersType & parameters, 
                        MeasureType & value, DerivativeType  & derivative,HessianType & Hessian, int channel) const
{
  //std::cout<<"GetValueAndDerivative called"<<std::endl;
  // ANYTIME BLOCK:
  bool isEqual=1;
  for (int i=0;i<this->m_NumberOfParameters&&isEqual;i++) {
    //std::cout<<(*m_CachedParameters[channel])[i]<<" "<<parameters[i]<<std::endl;
    isEqual=((*this->m_CachedParameters[channel])[i]==parameters[i]);
    }

  if(!isEqual) {
    this->ResetComputation(channel);
    m_SumCache[channel]=0.0;
  
    m_DerivativeSumCache[channel]->
      Fill( NumericTraits<ITK_TYPENAME DerivativeType::ValueType>::Zero );
    this->m_HessianSumCache[channel]->
      Fill( NumericTraits<ITK_TYPENAME DerivativeType::ValueType>::Zero );
	if(this->m_GradientMethod==Superclass::ESM) {
    m_DerivativeSumCacheESM[channel]->
      Fill( NumericTraits<ITK_TYPENAME DerivativeType::ValueType>::Zero );
    this->m_HessianSumCacheESM[channel]->
      Fill( NumericTraits<ITK_TYPENAME DerivativeType::ValueType>::Zero );
	}
  } else {
    if(((double)this->m_nFixedImageSamples[channel])/this->m_NumberOfAvailableSamples
       >= this->m_ComputationLevel[channel]) {
      //        std::cout<<"Metric: Using Cached value"<<m_ValueCache[channel]<<std::endl;
	 value = this->m_ValueCache[channel];
	 derivative = (*this->m_DerivativeCache[channel]);
	 Hessian = (*this->m_HessianCache[channel]);
	 return;
    }
  }

///////////////////////////////////////////////////
  //was itkDebugMacro
  //ERRMSG("GetValueAndDerivative( " << parameters << " ) ");

 // if( !this->GetGradientImage() )
 //   {
 //   itkExceptionMacro(<<"The gradient image is null, maybe you forgot to call Initialize()");
 //   }
 // bool ReallyDoHessian=calchessian ;//&& !((this->m_GradientMethod==Inverse||
						//this->m_GradientMethod==InverseCompositional)&&m_ICHessianComputed); 
 // m_ICHessianComputed=false;


  FixedImageConstPointer fixedImage = this->m_FixedImage;

  if( !fixedImage ) 
    {
    itkExceptionMacro( << "Fixed image has not been assigned" );
    }
  //{typename FixedImageType::IndexType index;
  //index.Fill(40);
  //MSG("testpixel: " <<m_GradientImage->GetPixel( index ));
  // }
  const unsigned int ImageDimension = FixedImageType::ImageDimension;

  //typedef  itk::ImageRegionConstIteratorWithIndex<
  //  FixedImageType> FixedIteratorType;

  //typedef  itk::ImageRegionConstIteratorWithIndex<
  //  ITK_TYPENAME Superclass::GradientImageType> GradientIteratorType;

  // Declare iterators for iteration over the sample container
  typename FixedImageSpatialSampleContainer::const_iterator fiter;
  typename FixedImageSpatialSampleContainer::const_iterator fend = 
    this->m_FixedImageSamples.end();
  if(this->m_nFixedImageSamples[channel]==0) {
    fiter=this->m_FixedImageSamples.begin();
  } else {
    fiter=this->m_Index[channel];
  }

  //FixedIteratorType ti( fixedImage, this->GetFixedImageRegion() );

//  typename FixedImageType::IndexType index;

  this->m_NumberOfPixelsCounted = 0;

  this->SetTransformParameters( parameters );

  const unsigned int ParametersDimension = this->GetNumberOfParameters();
  ParametersType parametergradient=ParametersType( ParametersDimension);

  HessianType HessianESM;
  DerivativeType derivativeESM(ParametersDimension);

  if(calcderivative) {
	derivative = DerivativeType( ParametersDimension );
	derivative.Fill( NumericTraits<ITK_TYPENAME DerivativeType::ValueType>::Zero );
	if(this->m_GradientMethod==ESM) {
		derivativeESM.Fill(NumericTraits<ITK_TYPENAME DerivativeType::ValueType>::Zero );
	}
  }
  if(calchessian) {
	 // if(ReallyDoHessian) {
	Hessian.SetSize(ParametersDimension,ParametersDimension);
	if(this->m_GradientMethod==Superclass::ESM) {
		HessianESM.SetSize(ParametersDimension,ParametersDimension);
	}
	//  }
	//Hessian.set_identity();	
  }

  while((((double)this->m_nFixedImageSamples[channel]/this->m_NumberOfAvailableSamples)
	  <this->m_ComputationLevel[channel] || this->m_nSamples[channel]<10 )&& fiter != fend ) {

    ++(this->m_nFixedImageSamples[channel]);

 	if(fixedmask) {
		if(!this->m_FixedImageMask->IsInside( (*fiter).FixedImagePointValue ) )
		{
		++fiter;
	      //      std::cout<<"Rejecting point, not inside fixed mask"<<std::endl;
		  continue;
		}
	}

    typename Superclass::OutputPointType transformedPoint = this->m_Transform->TransformPoint( (*fiter).FixedImagePointValue );

	if(movingmask) {
		if( !this->m_MovingImageMask->IsInside( transformedPoint ) )
		{
		++fiter;
		//std::cout<<"Rejecting point, not inside moving mask"<<std::endl;
		continue;
		}
	}
	MovingImageContinuousIndexType tempIndex;
	this->m_MovingImage->TransformPhysicalPointToContinuousIndex( transformedPoint, tempIndex );

	if( this->m_Interpolator->IsInsideBuffer( tempIndex ) ) {
		const RealType movingValue  = this->m_Interpolator->EvaluateAtContinuousIndex( tempIndex );
		const RealType fixedValue     = (*fiter).FixedImageValue; //ti->Value();
	//	this->m_NumberOfPixelsCounted++;


		const RealType diff = movingValue - fixedValue; 
		if(calcvalue) {
			m_SumCache[channel] += diff * diff;
		}
		++(this->m_nSamples[channel]);

		if(calcderivative || calchessian) {
			//this->ComputeImageDerivatives((*fiter).FixedImageIndex,(*fiter).FixedImagePointValue, 
			//transformedPoint,parametergradient,tempIndex);
			this->ComputeImageDerivatives(fiter, 
			transformedPoint,parametergradient,tempIndex);
      
			for(unsigned int par=0; par<ParametersDimension; par++)
			{    
				if(calcderivative){
				   (*m_DerivativeSumCache[channel])[par] += 2.0*diff*parametergradient[par];
			    }
				if(calchessian) {
					//if(ReallyDoHessian) {
				   for(unsigned int par2=par; par2<ParametersDimension; par2++) {
					(*this->m_HessianSumCache[channel])[par][par2] += parametergradient[par]*parametergradient[par2];
				   }
					//}
				}
			}
			if(this->m_GradientMethod==Superclass::ESM) {
			this->ComputeImageDerivatives(fiter, 
			transformedPoint,parametergradient,tempIndex);
      
			for(unsigned int par=0; par<ParametersDimension; par++)
			{    
				if(calcderivative){
				   (*m_DerivativeSumCacheESM[channel])[par] += 2.0*diff*parametergradient[par];
			    }
				if(calchessian) {
					//if(ReallyDoHessian) {
				   for(unsigned int par2=par; par2<ParametersDimension; par2++) {
					(*this->m_HessianSumCacheESM[channel])[par][par2] += parametergradient[par]*parametergradient[par2];
				   }
					//}
				}
			}
			}
		}
	} else {
		//std::cout<<"Rejecting point, not inside buffer"<<std::endl;
		//std::cout<<"In point: "<<(*fiter).FixedImagePointValue <<" Outpoint: "<<transformedPoint<<std::endl;
	}
	// std::cout<<derivative<<std::endl;
	++fiter;
	}
	this->m_NumberOfPixelsCounted = this->m_nSamples[channel];

	if( !this->m_NumberOfPixelsCounted )
	{
		itkExceptionMacro(<<"All the points mapped to outside of the moving image");
	}
	else
	{
		if(calcderivative) {
			for(unsigned int i=0; i<ParametersDimension; i++){
				derivative[i] =(*this->m_DerivativeSumCache[channel])[i]/ this->m_NumberOfPixelsCounted;
			}
			if(this->m_GradientMethod==Superclass::ESM) {
			for(unsigned int i=0; i<ParametersDimension; i++){
				derivativeESM[i] =(*this->m_DerivativeSumCacheESM[channel])[i]/ this->m_NumberOfPixelsCounted;
			}
			}
		}
	if(calchessian) {
		//if(ReallyDoHessian) {
		if(this->m_HessianMethod==Superclass::Fast) {
		for(unsigned int par=0; par<ParametersDimension; par++){
			Hessian[par][par] =(*this->m_HessianSumCache[channel])[par][par]/ (this->m_NumberOfPixelsCounted);
			for(unsigned int par2=(par+1); par2<ParametersDimension; par2++){
				Hessian[par][par2] =(*this->m_HessianSumCache[channel])[par][par2]/ (this->m_NumberOfPixelsCounted);
				Hessian[par2][par] =Hessian[par][par2];
			}
		}
		if(this->m_GradientMethod==Superclass::ESM) {
		for(unsigned int par=0; par<ParametersDimension; par++){
			HessianESM[par][par] =(*this->m_HessianSumCacheESM[channel])[par][par]/ (this->m_NumberOfPixelsCounted);
			for(unsigned int par2=(par+1); par2<ParametersDimension; par2++){
				HessianESM[par][par2] =(*this->m_HessianSumCacheESM[channel])[par][par2]/ (this->m_NumberOfPixelsCounted);
				HessianESM[par2][par] =HessianESM[par][par2];
			}
		}
		}
		} else {
		for(unsigned int par=0; par<ParametersDimension; par++){
			Hessian[par][par] =(*this->m_HessianSumCache[channel])[par][par];
			for(unsigned int par2=(par+1); par2<ParametersDimension; par2++){
				Hessian[par][par2] =(*this->m_HessianSumCache[channel])[par][par2];
				Hessian[par2][par] =Hessian[par][par2];
			}
		}
		if(this->m_GradientMethod==Superclass::ESM) {
		for(unsigned int par=0; par<ParametersDimension; par++){
			HessianESM[par][par] =(*this->m_HessianSumCacheESM[channel])[par][par];
			for(unsigned int par2=(par+1); par2<ParametersDimension; par2++){
				HessianESM[par][par2] =(*this->m_HessianSumCacheESM[channel])[par][par2];
				HessianESM[par2][par] =HessianESM[par][par2];
			}
		}
		}
		}
		//}
	}
		if(calcvalue) {
			value=this->m_SumCache[channel] / this->m_NumberOfPixelsCounted;
		}
	}


	//ANYTIME
	this->m_Index[channel]=fiter;
	itkDebugMacro( "Ratio of voxels mapping into moving image buffer: " 
		<< this->m_nSamples[channel] << " / " 
		<< this->m_nFixedImageSamples[channel] << std::endl );

	if( this->m_nSamples[channel] < this->m_nFixedImageSamples[channel] / 4 )
	{
		//    itkExceptionMacro( "Too many samples map outside moving image buffer: "
		//               << m_nSamples[channel] << " / " 
		//	       << m_nFixedImageSamples[channel] << std::endl );
		//  std::cout<<"Warning!"<<std::endl;
		itkWarningMacro( "Too many samples map outside moving image buffer: "
			<< this->m_nSamples[channel] << " / " 
			<< this->m_nFixedImageSamples[channel] << std::endl );
	}
	if(calcvalue) {
	this->m_ValueCache[channel]=value;
	}
  if(calcderivative && calchessian) {
	  this->ConvertDerivativeAndHessian(this->m_GradientMethod,derivative,Hessian,derivativeESM,HessianESM);
	  (*this->m_DerivativeCache[channel])=derivative;
	    (*this->m_HessianCache[channel])=Hessian;
  } else if(calcderivative) {
	  this->ConvertDerivative(this->m_GradientMethod,derivative,derivativeESM);
	  (*this->m_DerivativeCache[channel])=derivative;
  } else if(calchessian) {
		this->ConvertHessian(this->m_GradientMethod,Hessian,HessianESM);
	    (*this->m_HessianCache[channel])=Hessian;
  }
	(*this->m_CachedParameters[channel])=parameters;
	//MSG("Value: "<<value<<" Derivative: "<<derivative);
	//std::cout <<"Channel "<<channel<<" "<<m_nFixedImageSamples[channel]<<" "<<lastValue-m_ValueCache[channel]<<std::endl;
    this->m_TotalPixelsProcessed+=this->m_nSamples[channel];

}
template <class TFixedImage, class TMovingImage> 
void
MeanSquaresAnytimeImageToImageMetric2<TFixedImage,TMovingImage>
::Initialize(void) throw (ExceptionObject)
{
 		if(this->m_HessianMethod==Superclass::True) {
			itkExceptionMacro("Cannot do true hessian for MSD");
		}
		if(this->m_HessianMethod==Superclass::Diagonal) {
			itkExceptionMacro("Cannot do diagonal hessian for MSD");
		}
		if(this->m_HessianMethod==Superclass::Full) {
			itkExceptionMacro("Cannot do full hessian for MSD");
		}
 //std::cout<<"Initializing meansquared metric"<<std::endl;
  this->Superclass::Initialize();
  //MSG("Init: "<<*(this->GetGradientImage()));
  for (int i=0;i<NUMCHANNELS;i++) {
    m_DerivativeSumCache[i]=new DerivativeType(this->m_NumberOfParameters);
    m_DerivativeSumCacheESM[i]=new DerivativeType(this->m_NumberOfParameters);
    //m_HessianSumCache[i]=new HessianType(this->m_NumberOfParameters,this->m_NumberOfParameters);
  }
  // ti=new FixedIteratorType( this->m_FixedImage, this->GetFixedImageRegion() );
  // ti->SetNumberOfSamples(m_NumberOfSpatialSamples);
}


} // end namespace itk


#endif