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

  Program:   Visualization Toolkit
  Module:    vtkSMPTransform.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 "vtkSMPTransform.h"
#include "vtkDataArray.h"
#include "vtkMath.h"
#include "vtkMatrix4x4.h"
#include "vtkObjectFactory.h"
#include "vtkPoints.h"

#include "vtkSMPTools.h"

#include <cstdlib>

vtkStandardNewMacro(vtkSMPTransform);

vtkSMPTransform::vtkSMPTransform()
{
  VTK_LEGACY_BODY(vtkSMPTransform::vtkSMPTransform, "VTK 8.1");
}

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

//------------------------------------------------------------------------
template <class T1, class T2, class T3>
inline void vtkSMPTransformPoint(T1 matrix[4][4], T2 in[3], T3 out[3])
{
  T3 x = static_cast<T3>(
    matrix[0][0] * in[0] + matrix[0][1] * in[1] + matrix[0][2] * in[2] + matrix[0][3]);
  T3 y = static_cast<T3>(
    matrix[1][0] * in[0] + matrix[1][1] * in[1] + matrix[1][2] * in[2] + matrix[1][3]);
  T3 z = static_cast<T3>(
    matrix[2][0] * in[0] + matrix[2][1] * in[1] + matrix[2][2] * in[2] + matrix[2][3]);

  out[0] = x;
  out[1] = y;
  out[2] = z;
}

//------------------------------------------------------------------------
template <class T1, class T2, class T3, class T4>
inline void vtkSMPTransformDerivative(T1 matrix[4][4], T2 in[3], T3 out[3], T4 derivative[3][3])
{
  vtkSMPTransformPoint(matrix, in, out);

  for (int i = 0; i < 3; i++)
  {
    derivative[0][i] = static_cast<T4>(matrix[0][i]);
    derivative[1][i] = static_cast<T4>(matrix[1][i]);
    derivative[2][i] = static_cast<T4>(matrix[2][i]);
  }
}

//------------------------------------------------------------------------
template <class T1, class T2, class T3>
inline void vtkSMPTransformVector(T1 matrix[4][4], T2 in[3], T3 out[3])
{
  T3 x = static_cast<T3>(matrix[0][0] * in[0] + matrix[0][1] * in[1] + matrix[0][2] * in[2]);
  T3 y = static_cast<T3>(matrix[1][0] * in[0] + matrix[1][1] * in[1] + matrix[1][2] * in[2]);
  T3 z = static_cast<T3>(matrix[2][0] * in[0] + matrix[2][1] * in[1] + matrix[2][2] * in[2]);

  out[0] = x;
  out[1] = y;
  out[2] = z;
}

//------------------------------------------------------------------------
template <class T1, class T2, class T3>
inline void vtkSMPTransformNormal(T1 mat[4][4], T2 in[3], T3 out[3])
{
  // to transform the normal, multiply by the transposed inverse matrix
  T1 matrix[4][4];
  memcpy(*matrix, *mat, 16 * sizeof(T1));
  vtkMatrix4x4::Invert(*matrix, *matrix);
  vtkMatrix4x4::Transpose(*matrix, *matrix);

  vtkSMPTransformVector(matrix, in, out);

  vtkMath::Normalize(out);
}

//----------------------------------------------------------------------------
// Transform the normals and vectors using the derivative of the
// transformation.  Either inNms or inVrs can be set to nullptr.
// Normals are multiplied by the inverse transpose of the transform
// derivative, while vectors are simply multiplied by the derivative.
// Note that the derivative of the inverse transform is simply the
// inverse of the derivative of the forward transform.
class TransformAllFunctor
{
public:
  vtkPoints* inPts;
  vtkPoints* outPts;
  vtkDataArray* inNms;
  vtkDataArray* outNms;
  vtkDataArray* inVcs;
  vtkDataArray* outVcs;
  int nOptionalVectors;
  vtkDataArray** inVrsArr;
  vtkDataArray** outVrsArr;
  double (*matrix)[4];
  double (*matrixInvTr)[4];
  void operator()(vtkIdType begin, vtkIdType end) const
  {
    for (vtkIdType id = begin; id < end; id++)
    {
      double point[3];
      inPts->GetPoint(id, point);
      vtkSMPTransformPoint(matrix, point, point);
      outPts->SetPoint(id, point);
      if (inVcs)
      {
        inVcs->GetTuple(id, point);
        vtkSMPTransformVector(matrix, point, point);
        outVcs->SetTuple(id, point);
      }
      if (inVrsArr)
      {
        for (int iArr = 0; iArr < nOptionalVectors; iArr++)
        {
          inVrsArr[iArr]->GetTuple(id, point);
          vtkSMPTransformVector(matrix, point, point);
          outVrsArr[iArr]->SetTuple(id, point);
        }
      }
      if (inNms)
      {
        inNms->GetTuple(id, point);
        vtkSMPTransformVector(matrixInvTr, point, point);
        vtkMath::Normalize(point);
        outNms->SetTuple(id, point);
      }
    }
  }
};

void vtkSMPTransform::TransformPointsNormalsVectors(vtkPoints* inPts, vtkPoints* outPts,
  vtkDataArray* inNms, vtkDataArray* outNms, vtkDataArray* inVrs, vtkDataArray* outVrs,
  int nOptionalVectors, vtkDataArray** inVrsArr, vtkDataArray** outVrsArr)
{
  vtkIdType n = inPts->GetNumberOfPoints();
  double matrix[4][4];
  this->Update();

  TransformAllFunctor functor;
  functor.inPts = inPts;
  functor.outPts = outPts;
  functor.inNms = inNms;
  functor.outNms = outNms;
  functor.inVcs = inVrs;
  functor.outVcs = outVrs;
  functor.nOptionalVectors = nOptionalVectors;
  functor.inVrsArr = inVrsArr;
  functor.outVrsArr = outVrsArr;
  functor.matrix = this->Matrix->Element;
  if (inNms)
  {
    vtkMatrix4x4::DeepCopy(*matrix, this->Matrix);
    vtkMatrix4x4::Invert(*matrix, *matrix);
    vtkMatrix4x4::Transpose(*matrix, *matrix);
    functor.matrixInvTr = matrix;
  }

  vtkSMPTools::For(0, n, functor);
}

//----------------------------------------------------------------------------
class TransformPointsFunctor
{
public:
  vtkPoints* inPts;
  vtkPoints* outPts;
  double (*matrix)[4];
  void operator()(vtkIdType begin, vtkIdType end) const
  {
    for (vtkIdType id = begin; id < end; id++)
    {
      double point[3];
      inPts->GetPoint(id, point);
      vtkSMPTransformPoint(matrix, point, point);
      outPts->SetPoint(id, point);
    }
  }
};

void vtkSMPTransform::TransformPoints(vtkPoints* inPts, vtkPoints* outPts)
{
  vtkIdType n = inPts->GetNumberOfPoints();
  this->Update();

  TransformPointsFunctor functor;
  functor.inPts = inPts;
  functor.outPts = outPts;
  functor.matrix = this->Matrix->Element;

  vtkSMPTools::For(0, n, functor);
}

//----------------------------------------------------------------------------
class TransformNormalsFunctor
{
public:
  vtkDataArray* inNms;
  vtkDataArray* outNms;
  double (*matrix)[4];
  void operator()(vtkIdType begin, vtkIdType end) const
  {
    for (vtkIdType id = begin; id < end; id++)
    {
      double norm[3];
      inNms->GetTuple(id, norm);
      vtkSMPTransformVector(matrix, norm, norm);
      vtkMath::Normalize(norm);
      outNms->SetTuple(id, norm);
    }
  }
};

void vtkSMPTransform::TransformNormals(vtkDataArray* inNms, vtkDataArray* outNms)
{
  vtkIdType n = inNms->GetNumberOfTuples();
  double matrix[4][4];

  this->Update();

  // to transform the normal, multiply by the transposed inverse matrix
  vtkMatrix4x4::DeepCopy(*matrix, this->Matrix);
  vtkMatrix4x4::Invert(*matrix, *matrix);
  vtkMatrix4x4::Transpose(*matrix, *matrix);

  TransformNormalsFunctor functor;
  functor.inNms = inNms;
  functor.outNms = outNms;
  functor.matrix = matrix;

  vtkSMPTools::For(0, n, functor);
}

//----------------------------------------------------------------------------
class TransformVectorsFunctor
{
public:
  vtkDataArray* inVcs;
  vtkDataArray* outVcs;
  double (*matrix)[4];
  void operator()(vtkIdType begin, vtkIdType end) const
  {
    for (vtkIdType id = begin; id < end; id++)
    {
      double vec[3];
      inVcs->GetTuple(id, vec);
      vtkSMPTransformVector(matrix, vec, vec);
      outVcs->SetTuple(id, vec);
    }
  }
};

void vtkSMPTransform::TransformVectors(vtkDataArray* inNms, vtkDataArray* outNms)
{
  vtkIdType n = inNms->GetNumberOfTuples();
  this->Update();

  TransformVectorsFunctor functor;
  functor.inVcs = inNms;
  functor.outVcs = outNms;
  functor.matrix = this->Matrix->Element;

  vtkSMPTools::For(0, n, functor);
}
