/*=========================================================================
 *
 *  Copyright NumFOCUS
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/
#ifndef itkRecursiveGaussianImageFilter_hxx
#define itkRecursiveGaussianImageFilter_hxx

#include "itkRecursiveGaussianImageFilter.h"
#include "itkObjectFactory.h"
#include "itkImageLinearIteratorWithIndex.h"
#include <new>

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

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

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

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

/**
 *   Compute filter for Gaussian kernel.
 */
template <typename TInputImage, typename TOutputImage>
void
RecursiveGaussianImageFilter<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;


  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 GaussianOrderEnum::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 GaussianOrderEnum::FirstOrder:
    {
      if (this->GetNormalizeAcrossScale())
      {
        across_scale_normalization = m_Sigma;
      }
      // 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 GaussianOrderEnum::SecondOrder:
    {
      if (this->GetNormalizeAcrossScale())
      {
        across_scale_normalization = itk::Math::sqr(m_Sigma);
      }
      // 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
RecursiveGaussianImageFilter<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 = std::sin(W1 / sigmad);
  ScalarRealType Sin2 = std::sin(W2 / sigmad);
  ScalarRealType Cos1 = std::cos(W1 / sigmad);
  ScalarRealType Cos2 = std::cos(W2 / sigmad);
  ScalarRealType Exp1 = std::exp(L1 / sigmad);
  ScalarRealType Exp2 = std::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
RecursiveGaussianImageFilter<TInputImage, TOutputImage>::ComputeDCoefficients(ScalarRealType   sigmad,
                                                                              ScalarRealType   W1,
                                                                              ScalarRealType   L1,
                                                                              ScalarRealType   W2,
                                                                              ScalarRealType   L2,
                                                                              ScalarRealType & SD,
                                                                              ScalarRealType & DD,
                                                                              ScalarRealType & ED)
{
  //  const ScalarRealType Sin1 = std::sin(W1 / sigmad);
  //  const ScalarRealType Sin2 = std::sin(W2 / sigmad);
  const ScalarRealType Cos1 = std::cos(W1 / sigmad);
  const ScalarRealType Cos2 = std::cos(W2 / sigmad);
  const ScalarRealType Exp1 = std::exp(L1 / sigmad);
  const ScalarRealType Exp2 = std::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
RecursiveGaussianImageFilter<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
RecursiveGaussianImageFilter<TInputImage, TOutputImage>::VerifyPreconditions() ITKv5_CONST
{
  this->Superclass::VerifyPreconditions();

  if (this->m_Sigma <= 0.0)
  {
    itkExceptionMacro("Sigma must be greater than zero.");
  }
}

template <typename TInputImage, typename TOutputImage>
void
RecursiveGaussianImageFilter<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
