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

  Program:   Visualization Toolkit
  Module:    vtkImageTransform.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/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 notice for more information.

=========================================================================*/
#include "vtkImageTransform.h"

#include "vtkCellData.h"
#include "vtkDataArray.h"
#include "vtkImageData.h"
#include "vtkMatrix3x3.h"
#include "vtkMatrix4x4.h"
#include "vtkNew.h"
#include "vtkObjectFactory.h"
#include "vtkPointData.h"
#include "vtkPointSet.h"
#include "vtkSMPTools.h"

vtkStandardNewMacro(vtkImageTransform);

//============================================================================
//------------------------------------------------------------------------------
// Functors to support threaded execution
namespace
{ // anonymous

template <typename T>
struct InPlaceTranslatePoints
{
  T* Points;
  double* Translation;

  InPlaceTranslatePoints(double t[3], T* pts)
    : Points(pts)
    , Translation(t)
  {
  }

  void operator()(vtkIdType ptId, vtkIdType endPtId)
  {
    T* pIn = this->Points + 3 * ptId;
    T* pOut = pIn;

    for (; ptId < endPtId; ++ptId)
    {
      *pIn++ = *pOut++ + this->Translation[0];
      *pIn++ = *pOut++ + this->Translation[1];
      *pIn++ = *pOut++ + this->Translation[2];
    }
  }

  // Interface to vtkSMPTools
  static void Execute(double t[3], vtkIdType num, T* pts)
  {
    InPlaceTranslatePoints<T> translate(t, pts);
    vtkSMPTools::For(0, num, translate);
  }
}; // InPlaceTransformPoints

template <typename T>
struct InPlaceTransformPoints
{
  T* Points;
  vtkMatrix4x4* M4;

  InPlaceTransformPoints(vtkMatrix4x4* m4, T* pts)
    : Points(pts)
    , M4(m4)
  {
  }

  void operator()(vtkIdType ptId, vtkIdType endPtId)
  {
    T* pIn = this->Points + 3 * ptId;
    T tmp[3] = { 0, 0, 0 };

    for (; ptId < endPtId; ++ptId)
    {
      tmp[0] = M4->GetElement(0, 0) * pIn[0] + M4->GetElement(0, 1) * pIn[1] +
        M4->GetElement(0, 2) * pIn[2] + M4->GetElement(0, 3);
      tmp[1] = M4->GetElement(1, 0) * pIn[0] + M4->GetElement(1, 1) * pIn[1] +
        M4->GetElement(1, 2) * pIn[2] + M4->GetElement(1, 3);
      tmp[2] = M4->GetElement(2, 0) * pIn[0] + M4->GetElement(2, 1) * pIn[1] +
        M4->GetElement(2, 2) * pIn[2] + M4->GetElement(2, 3);
      *pIn++ = tmp[0];
      *pIn++ = tmp[1];
      *pIn++ = tmp[2];
    }
  }

  // Interface to vtkSMPTools
  static void Execute(vtkMatrix4x4* m4, vtkIdType num, T* pts)
  {
    InPlaceTransformPoints<T> transform(m4, pts);
    vtkSMPTools::For(0, num, transform);
  }
}; // InPlaceTransformPoints

template <typename T>
struct InPlaceTransformNormals
{
  T* Normals;
  vtkMatrix3x3* M3;
  double Determinant;
  double* Spacing;

  InPlaceTransformNormals(vtkMatrix3x3* m3, double* spacing, T* n)
    : Normals(n)
    , M3(m3)
    , Determinant(m3->Determinant())
    , Spacing(spacing)
  {
  }

  void operator()(vtkIdType ptId, vtkIdType endPtId)
  {
    T* nIn = this->Normals + 3 * ptId;
    T tmp[3] = { 0, 0, 0 };
    T toUnit = 0;

    for (; ptId < endPtId; ++ptId)
    {
      nIn[0] = nIn[0] / this->Spacing[0];
      nIn[1] = nIn[1] / this->Spacing[1];
      nIn[2] = nIn[2] / this->Spacing[2];
      tmp[0] = M3->GetElement(0, 0) * nIn[0] + M3->GetElement(0, 1) * nIn[1] +
        M3->GetElement(0, 2) * nIn[2];
      tmp[1] = M3->GetElement(1, 0) * nIn[0] + M3->GetElement(1, 1) * nIn[1] +
        M3->GetElement(1, 2) * nIn[2];
      tmp[2] = M3->GetElement(2, 0) * nIn[0] + M3->GetElement(2, 1) * nIn[1] +
        M3->GetElement(2, 2) * nIn[2];
      tmp[0] *= this->Determinant;
      tmp[1] *= this->Determinant;
      tmp[2] *= this->Determinant;
      toUnit = 1 / sqrt(tmp[0] * tmp[0] + tmp[1] * tmp[1] + tmp[2] * tmp[2]);
      *nIn++ = tmp[0] * toUnit;
      *nIn++ = tmp[1] * toUnit;
      *nIn++ = tmp[2] * toUnit;
    }
  }

  // Interface to vtkSMPTools
  static void Execute(vtkMatrix3x3* m3, double* spacing, vtkIdType num, T* n)
  {
    InPlaceTransformNormals<T> transform(m3, spacing, n);
    vtkSMPTools::For(0, num, transform);
  }
}; // InPlaceTransformNormals

template <typename T>
struct InPlaceTransformVectors
{
  T* Vectors;
  vtkMatrix3x3* M3;
  double* Spacing;

  InPlaceTransformVectors(vtkMatrix3x3* m3, double* spacing, T* v)
    : Vectors(v)
    , M3(m3)
    , Spacing(spacing)
  {
  }

  void operator()(vtkIdType ptId, vtkIdType endPtId)
  {
    T* nIn = this->Vectors + 3 * ptId;
    T tmp[3] = { 0, 0, 0 };

    for (; ptId < endPtId; ++ptId)
    {
      nIn[0] = nIn[0] / this->Spacing[0];
      nIn[1] = nIn[1] / this->Spacing[1];
      nIn[2] = nIn[2] / this->Spacing[2];
      tmp[0] = M3->GetElement(0, 0) * nIn[0] + M3->GetElement(0, 1) * nIn[1] +
        M3->GetElement(0, 2) * nIn[2];
      tmp[1] = M3->GetElement(1, 0) * nIn[0] + M3->GetElement(1, 1) * nIn[1] +
        M3->GetElement(1, 2) * nIn[2];
      tmp[2] = M3->GetElement(2, 0) * nIn[0] + M3->GetElement(2, 1) * nIn[1] +
        M3->GetElement(2, 2) * nIn[2];
      *nIn++ = tmp[0];
      *nIn++ = tmp[1];
      *nIn++ = tmp[2];
    }
  }

  // Interface to vtkSMPTools
  static void Execute(vtkMatrix3x3* m3, double* spacing, vtkIdType num, T* v)
  {
    InPlaceTransformVectors<T> transform(m3, spacing, v);
    vtkSMPTools::For(0, num, transform);
  }
}; // InPlaceTransformVectors

} // anonymous namespace

//============================================================================
//------------------------------------------------------------------------------
// Here is the VTK class proper.
//------------------------------------------------------------------------------
// A convenience function to transform points (in the point set) as well as
// associated normals and vectors.
void vtkImageTransform::TransformPointSet(vtkImageData* im, vtkPointSet* ps)
{
  vtkImageTransform::TransformPointSet(im, ps, true, true);
}

//------------------------------------------------------------------------------
// A convenience method to transform a point set, with the ability to control
// whether normals and vectors are transformed as well.
void vtkImageTransform::TransformPointSet(
  vtkImageData* im, vtkPointSet* ps, bool transformNormals, bool transformVectors)
{
  // Check input
  if (im == nullptr || ps == nullptr)
  {
    return;
  }

  // Nothing to do if the direction matrix is the identity
  vtkMatrix4x4* m4 = im->GetIndexToPhysicalMatrix();
  if (m4->IsIdentity())
  {
    return;
  }

  // Make sure points are available
  vtkIdType numPts = ps->GetNumberOfPoints();
  if (numPts < 1)
  {
    return;
  }

  // Grab the points-related-data and process as appropriate
  vtkDataArray* pts = ps->GetPoints()->GetData();
  vtkMatrix3x3* m3 = im->GetDirectionMatrix();
  double* ar = im->GetSpacing();

  // If there is no rotation or spacing, only translate
  if (m3->IsIdentity() && ar[0] == 1 && ar[1] == 1 && ar[2] == 1)
  {
    vtkImageTransform::TranslatePoints(im->GetOrigin(), pts);
    return;
  }

  // Otherwise, need to transform points and optionally
  // vectors and normals.
  vtkImageTransform::TransformPoints(m4, pts);

  if (transformNormals)
  {
    vtkNew<vtkMatrix3x3> m3n;
    vtkMatrix3x3::Transpose(m3, m3n);
    m3n->Invert();

    vtkDataArray* normals = ps->GetPointData()->GetNormals();
    if (normals != nullptr)
    {
      vtkImageTransform::TransformNormals(m3n, ar, normals);
    }
    normals = ps->GetCellData()->GetNormals();
    if (normals != nullptr)
    {
      vtkImageTransform::TransformNormals(m3n, ar, normals);
    }
  }

  if (transformVectors)
  {
    vtkDataArray* vectors = ps->GetPointData()->GetVectors();
    if (vectors != nullptr)
    {
      vtkImageTransform::TransformVectors(m3, ar, vectors);
    }
    vectors = ps->GetCellData()->GetVectors();
    if (vectors != nullptr)
    {
      vtkImageTransform::TransformVectors(m3, ar, vectors);
    }
  }
}

//------------------------------------------------------------------------------
void vtkImageTransform::TranslatePoints(double* t, vtkDataArray* da)
{
  void* pts = da->GetVoidPointer(0);
  vtkIdType num = da->GetNumberOfTuples();

  switch (da->GetDataType())
  {
    vtkTemplateMacro(InPlaceTranslatePoints<VTK_TT>::Execute(t, num, (VTK_TT*)pts));
  }
}

//------------------------------------------------------------------------------
void vtkImageTransform::TransformPoints(vtkMatrix4x4* m4, vtkDataArray* da)
{
  void* pts = da->GetVoidPointer(0);
  vtkIdType num = da->GetNumberOfTuples();

  switch (da->GetDataType())
  {
    vtkTemplateMacro(InPlaceTransformPoints<VTK_TT>::Execute(m4, num, (VTK_TT*)pts));
  }
}

//------------------------------------------------------------------------------
void vtkImageTransform::TransformNormals(vtkMatrix3x3* m3, double spacing[3], vtkDataArray* da)
{
  void* n = da->GetVoidPointer(0);
  vtkIdType num = da->GetNumberOfTuples();

  switch (da->GetDataType())
  {
    vtkTemplateMacro(InPlaceTransformNormals<VTK_TT>::Execute(m3, spacing, num, (VTK_TT*)n));
  }
}

//------------------------------------------------------------------------------
void vtkImageTransform::TransformVectors(vtkMatrix3x3* m3, double spacing[3], vtkDataArray* da)
{
  void* v = da->GetVoidPointer(0);
  vtkIdType num = da->GetNumberOfTuples();

  switch (da->GetDataType())
  {
    vtkTemplateMacro(InPlaceTransformVectors<VTK_TT>::Execute(m3, spacing, num, (VTK_TT*)v));
  }
}

//------------------------------------------------------------------------------
void vtkImageTransform::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
}