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

#include "itkDeformableSimplexMesh3DGradientConstraintForceFilter.h"
#include "itkNumericTraits.h"
#include "itkMath.h"
#include "itkMath.h"

#include <set>

namespace itk
{
/* Constructore  */
template <typename TInputMesh, typename TOutputMesh>
DeformableSimplexMesh3DGradientConstraintForceFilter<TInputMesh, TOutputMesh>::
  DeformableSimplexMesh3DGradientConstraintForceFilter()
{
  m_Range = 1;
  m_StartVoxel = nullptr;
}

template <typename TInputMesh, typename TOutputMesh>
DeformableSimplexMesh3DGradientConstraintForceFilter<TInputMesh, TOutputMesh>::
  ~DeformableSimplexMesh3DGradientConstraintForceFilter()
{
  this->Clear();
}

template <typename TInputMesh, typename TOutputMesh>
void
DeformableSimplexMesh3DGradientConstraintForceFilter<TInputMesh, TOutputMesh>::PrintSelf(std::ostream & os,
                                                                                         Indent         indent) const
{
  Superclass::PrintSelf(os, indent);
  os << indent << "Range = " << m_Range << std::endl;
  os << indent << "Image = " << m_Image << std::endl;
}

template <typename TInputMesh, typename TOutputMesh>
void
DeformableSimplexMesh3DGradientConstraintForceFilter<TInputMesh, TOutputMesh>::Clear()
{
  delete m_StartVoxel;
  m_StartVoxel = nullptr;
  std::vector<ImageVoxel *>::iterator it;
  for (it = m_Positive.begin(); it != m_Positive.end(); it++)
  {
    delete *it;
  }
  m_Positive.erase(m_Positive.begin(), m_Positive.end());
  for (it = m_Negative.begin(); it != m_Negative.end(); it++)
  {
    delete *it;
  }
  m_Negative.erase(m_Negative.begin(), m_Negative.end());
}

template <typename TInputMesh, typename TOutputMesh>
int
DeformableSimplexMesh3DGradientConstraintForceFilter<TInputMesh, TOutputMesh>::Signi(double a)
{
  if (a < 0)
  {
    return -1;
  }
  else if (a > 0)
  {
    return 1;
  }
  else
  {
    return 0;
  }
}

template <typename TInputMesh, typename TOutputMesh>
double
DeformableSimplexMesh3DGradientConstraintForceFilter<TInputMesh, TOutputMesh>::NextVoxel(const double * pp,
                                                                                         int *          ic,
                                                                                         double *       x,
                                                                                         double *       y,
                                                                                         double *       z)
{
  double d, dlx, dly, dlz;
  double dp[3];

  dp[0] = pp[0];
  dp[1] = pp[1];
  dp[2] = pp[2];

  if (dp[0] >= 0.0)
  {
    dlx = ((ic[0] + 1) * 1 - *x) / dp[0];
    if (dlx <= 0)
    {
      dlx = ((ic[0] + 2) * 1 - *x) / dp[0];
    }
  }
  else
  {
    dlx = (ic[0] * 1 - *x) / dp[0];
    if (dlx <= 0)
    {
      dlx = ((ic[0] - 1) * 1 - *x) / dp[0];
    }
  }

  if (dp[1] >= 0.0)
  {
    dly = ((ic[1] + 1) * 1 - *y) / dp[1];
    if (dly <= 0)
    {
      dly = ((ic[1] + 2) * 1 - *y) / dp[1];
    }
  }
  else
  {
    dly = (ic[1] * 1 - *y) / dp[1];
    if (dly <= 0)
    {
      dly = ((ic[1] - 1) * 1 - *y) / dp[1];
    }
  }

  if (dp[2] >= 0.0)
  {
    dlz = ((ic[2] + 1) * 1 - *z) / dp[2];
    if (dlz <= 0)
    {
      dlz = ((ic[2] + 2) * 1 - *z) / dp[2];
    }
  }
  else
  {
    dlz = (ic[2] * 1 - *z) / dp[2];
    if (dlz <= 0)
    {
      dlz = ((ic[2] - 1) * 1 - *z) / dp[2];
    }
  }

  if ((dlx < dly) && (dlx < dlz))
  {
    d = dlx;
    ic[0] += this->Signi(dp[0]);
  }
  else if ((dly < dlz) && (dly < dlx))
  {
    d = dly;
    ic[1] += this->Signi(dp[1]);
  }
  else
  {
    d = dlz;
    ic[2] += this->Signi(dp[2]);
  }

  *x += pp[0] * d;
  *y += pp[1] * d;
  *z += pp[2] * d;

  return d;
}

template <typename TInputMesh, typename TOutputMesh>
void
DeformableSimplexMesh3DGradientConstraintForceFilter<TInputMesh, TOutputMesh>::ComputeExternalForce(
  SimplexMeshGeometry *     data,
  const GradientImageType * gradientImage)
{
  PointType vec_for;

  // image coordinate
  int               ic[3];
  GradientIndexType coord;
  double            x, y, z;

  coord[0] = static_cast<ImageIndexValueType>(data->pos[0]);
  coord[1] = static_cast<ImageIndexValueType>(data->pos[1]);
  coord[2] = static_cast<ImageIndexValueType>(data->pos[2]);

  const OriginalImageType::PointType &   orgn = m_Image->GetOrigin();
  const OriginalImageType::SpacingType & sp = m_Image->GetSpacing();

  if (m_StartVoxel)
  {
    this->Clear();
  }

  OriginalImageIndexType index;
  // ***keep in mind to add direction cosines in here once we have them
  x = data->pos[0] - orgn[0];
  y = data->pos[1] - orgn[1];
  z = data->pos[2] - orgn[2];

  // need to divide by image spacing if it was not one
  ic[0] = static_cast<ImageIndexValueType>(x / sp[0]);
  ic[1] = static_cast<ImageIndexValueType>(y / sp[1]);
  ic[2] = static_cast<ImageIndexValueType>(z / sp[2]);

  if (ic[0] >= 0 && ic[0] < this->m_ImageWidth && ic[1] >= 0 && ic[1] < this->m_ImageHeight && ic[2] >= 0 &&
      ic[2] < this->m_ImageDepth)
  {
    bool     stop;
    SIDEEnum side = SIDEEnum::BOTH; // make sure you can set half segment as well but for noe
                                    // we just set it to full segment
    int    vpos[3];
    int    ii;
    double dist;
    double dp[3];
    double pos[3];

    ImageVoxel * current;
    vpos[0] = ic[0];
    vpos[1] = ic[1];
    vpos[2] = ic[2];

    if (Math::AlmostEquals(
          data->normal[0],
          itk::NumericTraits<itk::NumericTraits<SimplexMeshGeometry::CovariantVectorType>::ValueType>::ZeroValue()))
    {
      dp[0] = 1e-6;
    }
    else
    {
      dp[0] = data->normal[0];
    }

    if (Math::AlmostEquals(
          data->normal[1],
          itk::NumericTraits<itk::NumericTraits<SimplexMeshGeometry::CovariantVectorType>::ValueType>::ZeroValue()))
    {
      dp[1] = 1e-6;
    }
    else
    {
      dp[1] = data->normal[1];
    }

    if (Math::AlmostEquals(
          data->normal[2],
          itk::NumericTraits<itk::NumericTraits<SimplexMeshGeometry::CovariantVectorType>::ValueType>::ZeroValue()))
    {
      dp[2] = 1e-6;
    }
    else
    {
      dp[2] = data->normal[2];
    }

    index[0] = ic[0];
    index[1] = ic[1];
    index[2] = ic[2];
    pos[0] = data->pos[0];
    pos[1] = data->pos[1];
    pos[2] = data->pos[2];

    m_StartVoxel = new ImageVoxel(vpos, pos, (double)m_Image->GetPixel(index), 0.0, 0);
    current = new ImageVoxel(vpos, pos, (double)m_Image->GetPixel(index), 0.0, 0);
    m_Positive.push_back(current);

    // scan normal side
    if (side == SIDEEnum::NORMAL || side == SIDEEnum::BOTH)
    {
      int i = 0;
      stop = false;
      dist = 0.0;

      while (!stop)
      {
        double a = NextVoxel(dp, ic, &x, &y, &z);

        pos[0] += a * dp[0];
        pos[1] += a * dp[1];
        pos[2] += a * dp[2];
        dist += a;

        stop = ((dist > m_Range) || !(ic[0] >= 0 && ic[0] < this->m_ImageWidth && ic[1] >= 0 &&
                                      ic[1] < this->m_ImageHeight && ic[2] >= 0 && ic[2] < this->m_ImageDepth));

        if (!stop)
        {
          vpos[0] = ic[0];
          vpos[1] = ic[1];
          vpos[2] = ic[2];
          index[0] = ic[0];
          index[1] = ic[1];
          index[2] = ic[2];

          current = new ImageVoxel(vpos, pos, (double)m_Image->GetPixel(index), dist, ++i);
          m_Positive.push_back(current);
          if (current->GetDistance() > m_Range)
          {
            stop = true;
          }
          else
          {
            stop = false;
          }
        }
      }
    }

    // scan inverse normal side
    if (side == SIDEEnum::INVERSE || side == SIDEEnum::BOTH)
    {
      pos[0] = data->pos[0];
      pos[1] = data->pos[1];
      pos[2] = data->pos[2];
      // ***keep in mind to add direction cosines in here once we have them
      x = data->pos[0] - orgn[0];
      y = data->pos[1] - orgn[1];
      z = data->pos[2] - orgn[2];
      // need to divide by image spacing if it was not one
      ic[0] = static_cast<ImageIndexValueType>(x / sp[0]);
      ic[1] = static_cast<ImageIndexValueType>(y / sp[1]);
      ic[2] = static_cast<ImageIndexValueType>(z / sp[2]);

      dp[0] *= -1.0;
      dp[1] *= -1.0;
      dp[2] *= -1.0;
      ii = 0;
      stop = false;
      dist = 0.0;

      while (!stop)
      {
        double a = NextVoxel(dp, ic, &x, &y, &z);

        pos[0] += a * dp[0];
        pos[1] += a * dp[1];
        pos[2] += a * dp[2];
        dist += a;

        stop = ((dist > m_Range) || !(ic[0] >= 0 && ic[0] < this->m_ImageWidth && ic[1] >= 0 &&
                                      ic[1] < this->m_ImageHeight && ic[2] >= 0 && ic[2] < this->m_ImageDepth));

        if (!stop)
        {
          vpos[0] = ic[0];
          vpos[1] = ic[1];
          vpos[2] = ic[2];
          index[0] = ic[0];
          index[1] = ic[1];
          index[2] = ic[2];

          current = new ImageVoxel(vpos, pos, (double)m_Image->GetPixel(index), dist, --ii);
          m_Negative.push_back(current);
          if (current->GetDistance() > m_Range)
          {
            stop = true;
          }
          else
          {
            stop = false;
          }
        }
      }
    }
  }
  else
  {
    m_StartVoxel = nullptr;
  }

  // now fun begins try to use all the above
  std::vector<ImageVoxel *>::iterator it;
  double                              mag, max = 0;
  GradientIndexType                   coord3, coord2;
  coord2.Fill(0);
  for (it = m_Positive.begin(); it != m_Positive.end(); it++)
  {
    coord3[0] = static_cast<GradientIndexValueType>((*it)->GetX());
    coord3[1] = static_cast<GradientIndexValueType>((*it)->GetY());
    coord3[2] = static_cast<GradientIndexValueType>((*it)->GetZ());

    const GradientType & gradient3 = gradientImage->GetPixel(coord3);

    vec_for[0] = gradient3[0];
    vec_for[1] = gradient3[1];
    vec_for[2] = gradient3[2];
    // check magnitude

    mag = std::sqrt(dot_product(vec_for.GetVnlVector(), vec_for.GetVnlVector()));
    if (mag > max)
    {
      max = mag;
      coord2 = coord3;
    }
  }

  const GradientType & gradient0 = gradientImage->GetPixel(coord);
  const GradientType & gradient2 = gradientImage->GetPixel(coord2);

  vec_for[0] = gradient2[0];
  vec_for[1] = gradient2[1];
  vec_for[2] = gradient2[2];

  // now check highest gradient magnitude direction
  mag = dot_product(vec_for.GetVnlVector(), data->normal.GetVnlVector());

  if (mag > 0)
  {
    vec_for[0] -= gradient0[0];
    vec_for[1] -= gradient0[1];
    vec_for[2] -= gradient0[2];
  }
  else
  {
    vec_for.Fill(0);
  }

  data->externalForce[0] = vec_for[0];
  data->externalForce[1] = vec_for[1];
  data->externalForce[2] = vec_for[2];
}
} // namespace itk

#endif
