/*=========================================================================
 *
 *  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
 *
 *         https://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 itkFrustumSpatialFunction_hxx
#define itkFrustumSpatialFunction_hxx


namespace itk
{
template <unsigned int VDimension, typename TInput>
auto
FrustumSpatialFunction<VDimension, TInput>::Evaluate(const InputType & position) const -> OutputType
{
  using PointType = InputType;
  using VectorType = typename PointType::VectorType;
  static_assert(VDimension > 2, "VDimension must be greater than 2");
  static_assert(PointType::PointDimension > 2, "PointDimension of TInput must be greater than 2");

  VectorType   relativePosition = position - m_Apex;
  const double distanceToApex = relativePosition.GetNorm();

  // Check Top and Bottom planes. If the angle is negative, the top plane
  // value may be less than the bottom plane, which is still correct.
  if (m_TopPlane <= m_BottomPlane)
  {
    if (distanceToApex < m_TopPlane || distanceToApex > m_BottomPlane)
    {
      return 0;
    }
  }
  else
  {
    if (distanceToApex > m_TopPlane || distanceToApex < m_BottomPlane)
    {
      return 0;
    }
  }

  if (m_RotationPlane == RotationPlaneEnum::RotateInXZPlane)
  {
    const double dx = relativePosition[0];
    const double dy = relativePosition[1];
    const double dz = relativePosition[2];

    const double distanceXZ = std::sqrt(dx * dx + dz * dz);

    const double deg2rad = std::atan(1.0) / 45.0;

    // Check planes along Y
    const double angleY = std::atan2(dy, distanceXZ);
    if (itk::Math::abs(angleY) > m_ApertureAngleY * deg2rad)
    {
      return 0;
    }

    // Check planes along X
    const double angleX = std::atan2(dx, dz);

    if (std::cos(angleX + (180.0 + m_AngleZ) * deg2rad) < std::cos(deg2rad * m_ApertureAngleX))
    {
      return 0;
    }

    return 1;
  }
  else if (m_RotationPlane == RotationPlaneEnum::RotateInYZPlane)
  {
    const double dx = relativePosition[0];
    const double dy = relativePosition[1];
    const double dz = relativePosition[2];

    const double distanceYZ = std::sqrt(dy * dy + dz * dz);

    const double deg2rad = std::atan(1.0) / 45.0;

    // Check planes along X
    const double angleX = std::atan2(dx, distanceYZ);
    if (itk::Math::abs(angleX) > m_ApertureAngleX * deg2rad)
    {
      return 0;
    }

    // Check planes along Y
    const double angleY = std::atan2(dy, dz);

    if (std::cos(angleY + (180.0 + m_AngleZ) * deg2rad) < std::cos(deg2rad * m_ApertureAngleY))
    {
      return 0;
    }

    return 1;
  }
  else
  {
    itkExceptionMacro("Rotation plane not set or set to an unsupported value!");
  }
}

template <unsigned int VDimension, typename TInput>
void
FrustumSpatialFunction<VDimension, TInput>::PrintSelf(std::ostream & os, Indent indent) const
{
  Superclass::PrintSelf(os, indent);

  os << indent << "Apex: " << m_Apex << std::endl;
  os << indent << "AngleZ: " << m_AngleZ << std::endl;
  os << indent << "ApertureAngleX: " << m_ApertureAngleX << std::endl;
  os << indent << "ApertureAngleY: " << m_ApertureAngleY << std::endl;
  os << indent << "TopPlane: " << m_TopPlane << std::endl;
  os << indent << "BottomPlane: " << m_BottomPlane << std::endl;
  os << indent << "RotationPlane: " << m_RotationPlane << std::endl;
}

} // end namespace itk

#endif