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

  Program:   Insight Segmentation & Registration Toolkit
  Module:    $RCSfile: itkRecursiveGaussianImageFilterMod.txx,v $
  Language:  C++
  Date:      $Date: 2006/03/19 04:36:56 $
  Version:   $Revision: 1.38 $

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

#include "itkRecursiveGaussianImageFilterMod.h"
#include "itkObjectFactory.h"
#include "itkImageLinearIterator.h"
#include <new>


namespace itk
{
  
template <typename TInputImage, typename TOutputImage>
RecursiveGaussianImageFilterMod<TInputImage,TOutputImage>
::RecursiveGaussianImageFilterMod()
{
  m_Sigma = 1.0;
  m_NormalizeAcrossScale = false;
  m_Order = ZeroOrder;
}

/**
 *   Explicitly set a zeroth order derivative.
 */
template <typename TInputImage, typename TOutputImage>
void
RecursiveGaussianImageFilterMod<TInputImage,TOutputImage>
::SetZeroOrder()
{
  this->SetOrder( ZeroOrder );
}

/**
 *   Explicitly set a first order derivative.
 */
template <typename TInputImage, typename TOutputImage>
void
RecursiveGaussianImageFilterMod<TInputImage,TOutputImage>
::SetFirstOrder()
{
  this->SetOrder( FirstOrder );
}

/**
 *   Explicitly set a second order derivative.
 */
template <typename TInputImage, typename TOutputImage>
void
RecursiveGaussianImageFilterMod<TInputImage,TOutputImage>
::SetSecondOrder()
{
  this->SetOrder( SecondOrder );
}


/**
 *   Compute filter for Gaussian kernel.
 */
template <typename TInputImage, typename TOutputImage>
void
RecursiveGaussianImageFilterMod<TInputImage,TOutputImage>
::SetUp(ScalarRealType spacing)
{ 
  const ScalarRealType spacingTolerance = 1e-8;
  
  /**  Parameters of exponential series. */
  ScalarRealType A1[3];
  ScalarRealType B1[3];
  ScalarRealType W1;
  ScalarRealType L1;
  ScalarRealType A2[3];
  ScalarRealType B2[3];
  ScalarRealType W2;
  ScalarRealType L2;

  ScalarRealType direction = 1.0;
  if( spacing < 0.0 )
    {
    direction = -1.0;
    spacing = -spacing;
    }

  if( spacing < spacingTolerance )
    {
    itkExceptionMacro(<<"The spacing " << spacing << "is suspiciosly small in this image");
    } 
  
  const ScalarRealType sigmad = m_Sigma/spacing;
  ScalarRealType across_scale_normalization = 1.0;
  if( this->GetNormalizeAcrossScale() )
    {
    across_scale_normalization = sigmad;
    }

  A1[0] = static_cast<ScalarRealType>(  1.3530 );
  B1[0] = static_cast<ScalarRealType>(  1.8151 );
  W1    = static_cast<ScalarRealType>(  0.6681 );
  L1    = static_cast<ScalarRealType>( -1.3932 );
  A2[0] = static_cast<ScalarRealType>( -0.3531 );
  B2[0] = static_cast<ScalarRealType>(  0.0902 );
  W2    = static_cast<ScalarRealType>(  2.0787 );
  L2    = static_cast<ScalarRealType>( -1.3732 );

  A1[1] = static_cast<ScalarRealType>( -0.6724 );
  B1[1] = static_cast<ScalarRealType>( -3.4327 );
  A2[1] = static_cast<ScalarRealType>(  0.6724 );
  B2[1] = static_cast<ScalarRealType>(  0.6100 );

  A1[2] = static_cast<ScalarRealType>( -1.3563 );
  B1[2] = static_cast<ScalarRealType>(  5.2318 );
  A2[2] = static_cast<ScalarRealType>(  0.3446 );
  B2[2] = static_cast<ScalarRealType>( -2.2355 );

  ScalarRealType SD, DD, ED;
  this->ComputeDCoefficients(sigmad, W1, L1, W2, L2, SD, DD, ED);
  ScalarRealType SN, DN, EN;
  
  
  switch( m_Order ) 
    {
    case ZeroOrder: // Approximation of convolution with a gaussian.
      {
      ComputeNCoefficients(sigmad,
         A1[0], B1[0], W1, L1,
         A2[0], B2[0], W2, L2,
         this->m_N0, 
         this->m_N1, 
         this->m_N2, 
         this->m_N3,
         SN, DN, EN);

      ScalarRealType alpha0 = 2 * SN / SD - this->m_N0;
      this->m_N0 *= across_scale_normalization / alpha0;
      this->m_N1 *= across_scale_normalization / alpha0;
      this->m_N2 *= across_scale_normalization / alpha0;
      this->m_N3 *= across_scale_normalization / alpha0;
      const bool symmetric = true;
      this->ComputeRemainingCoefficients(symmetric);
      break;
      }
    case FirstOrder: // Approximation of convolution with 
                     // the first derivative of a Gaussian.
      {
      ComputeNCoefficients(sigmad,
         A1[1], B1[1], W1, L1,
         A2[1], B2[1], W2, L2,
         this->m_N0, this->m_N1, this->m_N2, this->m_N3,
         SN, DN, EN);

      ScalarRealType alpha1 = 2 * (SN*DD - DN*SD) / (SD*SD);
      // If negative spacing, negate the first derivative response.
      alpha1 *= direction;
      
      this->m_N0 *= across_scale_normalization / alpha1;
      this->m_N1 *= across_scale_normalization / alpha1;
      this->m_N2 *= across_scale_normalization / alpha1;
      this->m_N3 *= across_scale_normalization / alpha1;
      
      const bool symmetric = false;
      this->ComputeRemainingCoefficients(symmetric);
      break;
      }
    case SecondOrder: // Approximation of convolution with 
                      // the second derivative of a Gaussian.
      {
      ScalarRealType N0_0, N1_0, N2_0, N3_0;
      ScalarRealType N0_2, N1_2, N2_2, N3_2;
      ScalarRealType SN0, DN0, EN0;
      ScalarRealType SN2, DN2, EN2;
      ComputeNCoefficients(sigmad,
         A1[0], B1[0], W1, L1,
         A2[0], B2[0], W2, L2,
         N0_0, N1_0, N2_0, N3_0,
         SN0, DN0, EN0);
      ComputeNCoefficients(sigmad,
         A1[2], B1[2], W1, L1,
         A2[2], B2[2], W2, L2,
         N0_2, N1_2, N2_2, N3_2,
         SN2, DN2, EN2);

      ScalarRealType beta = -(2*SN2 - SD*N0_2) / (2*SN0 - SD*N0_0);
      this->m_N0 = N0_2 + beta * N0_0;
      this->m_N1 = N1_2 + beta * N1_0;
      this->m_N2 = N2_2 + beta * N2_0;
      this->m_N3 = N3_2 + beta * N3_0;
      SN = SN2 + beta * SN0;
      DN = DN2 + beta * DN0;
      EN = EN2 + beta * EN0;

      ScalarRealType alpha2;
      alpha2  = EN*SD*SD - ED*SN*SD - 2*DN*DD*SD + 2*DD*DD*SN;
      alpha2 /= SD*SD*SD;
      
      this->m_N0 *= across_scale_normalization / alpha2;
      this->m_N1 *= across_scale_normalization / alpha2;
      this->m_N2 *= across_scale_normalization / alpha2;
      this->m_N3 *= across_scale_normalization / alpha2;
      
      const bool symmetric = true;
      this->ComputeRemainingCoefficients(symmetric);
      break;
      }
    default:
      {
      itkExceptionMacro(<<"Unknown Order");
      return;
      }
    }
}


/**
 * Compute the N coefficients.
 */
template <typename TInputImage, typename TOutputImage>
void
RecursiveGaussianImageFilterMod<TInputImage,TOutputImage>
::ComputeNCoefficients(ScalarRealType sigmad,
           ScalarRealType A1, ScalarRealType B1, ScalarRealType W1, ScalarRealType L1,
           ScalarRealType A2, ScalarRealType B2, ScalarRealType W2, ScalarRealType L2,
           ScalarRealType& N0, ScalarRealType& N1, ScalarRealType& N2, ScalarRealType& N3,
           ScalarRealType& SN, ScalarRealType& DN, ScalarRealType& EN)
{
  ScalarRealType Sin1 = vcl_sin(W1 / sigmad);
  ScalarRealType Sin2 = vcl_sin(W2 / sigmad);
  ScalarRealType Cos1 = vcl_cos(W1 / sigmad);
  ScalarRealType Cos2 = vcl_cos(W2 / sigmad);
  ScalarRealType Exp1 = vcl_exp(L1 / sigmad);
  ScalarRealType Exp2 = vcl_exp(L2 / sigmad);
  
  N0  = A1 + A2;
  N1  = Exp2 * (B2*Sin2 - (A2+2*A1) * Cos2); 
  N1 += Exp1 * (B1*Sin1 - (A1+2*A2) * Cos1); 
  N2  = (A1+A2) * Cos2*Cos1;
  N2 -= B1*Cos2*Sin1 + B2*Cos1*Sin2;
  N2 *= 2*Exp1*Exp2;
  N2 += A2*Exp1*Exp1 + A1*Exp2*Exp2;
  N3  = Exp2*Exp1*Exp1 * (B2*Sin2 - A2*Cos2);
  N3 += Exp1*Exp2*Exp2 * (B1*Sin1 - A1*Cos1);

  SN = N0 + N1 + N2 + N3;
  DN = N1 + 2*N2 + 3*N3;
  EN = N1 + 4*N2 + 9*N3;
}


/**
 * Compute the D coefficients.
 */
template <typename TInputImage, typename TOutputImage>
void
RecursiveGaussianImageFilterMod<TInputImage,TOutputImage>
::ComputeDCoefficients(ScalarRealType sigmad,
           ScalarRealType W1, ScalarRealType L1, ScalarRealType W2, ScalarRealType L2,
           ScalarRealType& SD, ScalarRealType& DD, ScalarRealType& ED) 
{
  //  const ScalarRealType Sin1 = vcl_sin(W1 / sigmad);
  //  const ScalarRealType Sin2 = vcl_sin(W2 / sigmad);
  const ScalarRealType Cos1 = vcl_cos(W1 / sigmad);
  const ScalarRealType Cos2 = vcl_cos(W2 / sigmad);
  const ScalarRealType Exp1 = vcl_exp(L1 / sigmad);
  const ScalarRealType Exp2 = vcl_exp(L2 / sigmad);
  
  this->m_D4  =     Exp1*Exp1*Exp2*Exp2;
  this->m_D3  =  -2*Cos1*Exp1*Exp2*Exp2;
  this->m_D3 +=  -2*Cos2*Exp2*Exp1*Exp1;
  this->m_D2  =   4*Cos2*Cos1*Exp1*Exp2;
  this->m_D2 +=     Exp1*Exp1 + Exp2*Exp2;
  this->m_D1  = -2*(Exp2*Cos2 + Exp1*Cos1);

  SD = 1.0 + this->m_D1 + this->m_D2 + this->m_D3 + this->m_D4;
  DD = this->m_D1 + 2*this->m_D2 + 3*this->m_D3 + 4*this->m_D4;
  ED = this->m_D1 + 4*this->m_D2 + 9*this->m_D3 + 16*this->m_D4;
}


/**
 * Compute the M coefficients and the boundary coefficients.
 */
template <typename TInputImage, typename TOutputImage>
void
RecursiveGaussianImageFilterMod<TInputImage,TOutputImage>
::ComputeRemainingCoefficients(bool symmetric) 
{
  if( symmetric )
    {
    this->m_M1 = this->m_N1 - this->m_D1 * this->m_N0;
    this->m_M2 = this->m_N2 - this->m_D2 * this->m_N0;
    this->m_M3 = this->m_N3 - this->m_D3 * this->m_N0;
    this->m_M4 =            - this->m_D4 * this->m_N0;
    }
  else
    {
    this->m_M1 = -( this->m_N1 - this->m_D1 * this->m_N0 );
    this->m_M2 = -( this->m_N2 - this->m_D2 * this->m_N0 );
    this->m_M3 = -( this->m_N3 - this->m_D3 * this->m_N0 );
    this->m_M4 =                 this->m_D4 * this->m_N0;
    }

  // Compute coefficients to be used at the boundaries
  // in order to simulate edge extension boundary conditions.
  const ScalarRealType SN = this->m_N0 + this->m_N1 + this->m_N2 + this->m_N3;
  const ScalarRealType SM = this->m_M1 + this->m_M2 + this->m_M3 + this->m_M4;
  const ScalarRealType SD = 1.0 + this->m_D1 + this->m_D2 + this->m_D3 + this->m_D4;

  this->m_BN1 = this->m_D1 * SN / SD;
  this->m_BN2 = this->m_D2 * SN / SD;
  this->m_BN3 = this->m_D3 * SN / SD;
  this->m_BN4 = this->m_D4 * SN / SD;

  this->m_BM1 = this->m_D1 * SM / SD;
  this->m_BM2 = this->m_D2 * SM / SD;
  this->m_BM3 = this->m_D3 * SM / SD;
  this->m_BM4 = this->m_D4 * SM / SD;
}


template <typename TInputImage, typename TOutputImage>
void
RecursiveGaussianImageFilterMod<TInputImage,TOutputImage>
::PrintSelf(std::ostream& os, Indent indent) const
{
  Superclass::PrintSelf(os,indent);

  os << "Sigma: " << m_Sigma << std::endl; 
  os << "Order: " << m_Order << std::endl; 
  os << "NormalizeAcrossScale: " << m_NormalizeAcrossScale << std::endl;
}

} // end namespace itk

#endif