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

  Program:   Visualization Toolkit
  Module:    vtkSphericalTransform.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 "vtkSphericalTransform.h"
#include "vtkMath.h"
#include "vtkObjectFactory.h"
#include <cmath>
#include <cstdlib>

vtkStandardNewMacro(vtkSphericalTransform);

//------------------------------------------------------------------------------
vtkSphericalTransform::vtkSphericalTransform() = default;

vtkSphericalTransform::~vtkSphericalTransform() = default;

void vtkSphericalTransform::PrintSelf(ostream& os, vtkIndent indent)
{
  vtkWarpTransform::PrintSelf(os, indent);
}

void vtkSphericalTransform::InternalDeepCopy(vtkAbstractTransform* transform)
{
  vtkSphericalTransform* sphericalTransform = static_cast<vtkSphericalTransform*>(transform);

  // copy these even though they aren't used
  this->SetInverseTolerance(sphericalTransform->InverseTolerance);
  this->SetInverseIterations(sphericalTransform->InverseIterations);

  // copy the inverse flag, which is used
  if (this->InverseFlag != sphericalTransform->InverseFlag)
  {
    this->InverseFlag = sphericalTransform->InverseFlag;
    this->Modified();
  }
}

vtkAbstractTransform* vtkSphericalTransform::MakeTransform()
{
  return vtkSphericalTransform::New();
}

template <class T>
void vtkSphericalToRectangular(const T inPoint[3], T outPoint[3], T derivative[3][3])
{
  T r = inPoint[0];
  T sinphi = sin(inPoint[1]);
  T cosphi = cos(inPoint[1]);
  T sintheta = sin(inPoint[2]);
  T costheta = cos(inPoint[2]);

  outPoint[0] = r * sinphi * costheta;
  outPoint[1] = r * sinphi * sintheta;
  outPoint[2] = r * cosphi;

  if (derivative)
  {
    derivative[0][0] = sinphi * costheta;
    derivative[0][1] = r * cosphi * costheta;
    derivative[0][2] = -r * sinphi * sintheta;

    derivative[1][0] = sinphi * sintheta;
    derivative[1][1] = r * cosphi * sintheta;
    derivative[1][2] = r * sinphi * costheta;

    derivative[2][0] = cosphi;
    derivative[2][1] = -r * sinphi;
    derivative[2][2] = 0;
  }
}

template <class T>
void vtkRectangularToSpherical(const T inPoint[3], T outPoint[3])
{
  T x = inPoint[0];
  T y = inPoint[1];
  T z = inPoint[2];

  T RR = x * x + y * y;
  T r = sqrt(RR + z * z);

  outPoint[0] = r;
  if (r == 0)
  {
    outPoint[1] = 0;
  }
  else
  {
    outPoint[1] = acos(z / r);
  }
  if (RR == 0)
  {
    outPoint[2] = 0;
  }
  else
  {
    // Change range to [0, 2*Pi], otherwise the same as atan2(y, x)
    outPoint[2] = T(vtkMath::Pi()) + atan2(-y, -x);
  }
}

void vtkSphericalTransform::ForwardTransformPoint(const float inPoint[3], float outPoint[3])
{
  vtkSphericalToRectangular(inPoint, outPoint, static_cast<float(*)[3]>(nullptr));
}

void vtkSphericalTransform::ForwardTransformPoint(const double inPoint[3], double outPoint[3])
{
  vtkSphericalToRectangular(inPoint, outPoint, static_cast<double(*)[3]>(nullptr));
}

void vtkSphericalTransform::ForwardTransformDerivative(
  const float inPoint[3], float outPoint[3], float derivative[3][3])
{
  vtkSphericalToRectangular(inPoint, outPoint, derivative);
}

void vtkSphericalTransform::ForwardTransformDerivative(
  const double inPoint[3], double outPoint[3], double derivative[3][3])
{
  vtkSphericalToRectangular(inPoint, outPoint, derivative);
}

void vtkSphericalTransform::InverseTransformPoint(const float inPoint[3], float outPoint[3])
{
  vtkRectangularToSpherical(inPoint, outPoint);
}

void vtkSphericalTransform::InverseTransformPoint(const double inPoint[3], double outPoint[3])
{
  vtkRectangularToSpherical(inPoint, outPoint);
}

void vtkSphericalTransform::InverseTransformDerivative(
  const float inPoint[3], float outPoint[3], float derivative[3][3])
{
  float tmp[3];
  vtkRectangularToSpherical(inPoint, outPoint);
  vtkSphericalToRectangular(outPoint, tmp, derivative);
}

void vtkSphericalTransform::InverseTransformDerivative(
  const double inPoint[3], double outPoint[3], double derivative[3][3])
{
  double tmp[3];
  vtkRectangularToSpherical(inPoint, outPoint);
  vtkSphericalToRectangular(outPoint, tmp, derivative);
}