/// \file Planes.h
/// \author Johannes de Fine Licht (johannes.definelicht@cern.ch)

#ifndef VECGEOM_VOLUMES_PLANES_H_
#define VECGEOM_VOLUMES_PLANES_H_

#include "VecGeom/base/Global.h"

#include "VecGeom/base/AlignedBase.h"
#include "VecGeom/base/Array.h"
#include "VecGeom/base/SOA3D.h"
#include "VecGeom/volumes/kernel/GenericKernels.h"

#if defined(VECGEOM_VC) && defined(VECGEOM_QUADRILATERALS_VC)
#include <Vc/Vc>
typedef Vc::Vector<vecgeom::Precision> VcPrecision;
typedef Vc::Vector<vecgeom::Precision>::Mask VcBool;
constexpr int kVectorSize = VcPrecision::Size;
#endif

namespace vecgeom {

VECGEOM_DEVICE_FORWARD_DECLARE(class Planes;);
VECGEOM_DEVICE_DECLARE_CONV(class, Planes);

inline namespace VECGEOM_IMPL_NAMESPACE {

class Planes : public AlignedBase {

private:
  SOA3D<Precision> fNormals;   ///< Normalized normals of the planes.
  Array<Precision> fDistances; ///< Distance from plane to origin (0, 0, 0).

public:
  VECCORE_ATT_HOST_DEVICE
  Planes(int size);

  VECCORE_ATT_HOST_DEVICE
  ~Planes();

  VECCORE_ATT_HOST_DEVICE
  Planes &operator=(Planes const &rhs);

  VECCORE_ATT_HOST_DEVICE
  VECGEOM_FORCE_INLINE
  Precision const *operator[](int index) const;

  VECCORE_ATT_HOST_DEVICE
  VECGEOM_FORCE_INLINE
  int size() const;

  VECCORE_ATT_HOST_DEVICE
  VECGEOM_FORCE_INLINE
  void reserve(size_t newsize)
  {
    fNormals.reserve(newsize);
    fDistances.Allocate(newsize);
  }

  VECCORE_ATT_HOST_DEVICE
  VECGEOM_FORCE_INLINE
  SOA3D<Precision> const &GetNormals() const;

  VECCORE_ATT_HOST_DEVICE
  VECGEOM_FORCE_INLINE
  Vector3D<Precision> GetNormal(int i) const;

  VECCORE_ATT_HOST_DEVICE
  VECGEOM_FORCE_INLINE
  Array<Precision> const &GetDistances() const;

  VECCORE_ATT_HOST_DEVICE
  VECGEOM_FORCE_INLINE
  Precision GetDistance(int i) const;

  VECCORE_ATT_HOST_DEVICE
  void Set(int index, Vector3D<Precision> const &normal, Vector3D<Precision> const &origin);

  VECCORE_ATT_HOST_DEVICE
  void Set(int index, Vector3D<Precision> const &normal, Precision distance);

  /// Flip the sign of the normal and distance at the specified index
  VECCORE_ATT_HOST_DEVICE
  void FlipSign(int index);

  template <typename Real_v>
  VECGEOM_FORCE_INLINE
  VECCORE_ATT_HOST_DEVICE
  vecCore::Mask_v<Real_v> Contains(Vector3D<Real_v> const &point) const;

  template <typename Real_v, typename Inside_v>
  VECGEOM_FORCE_INLINE
  VECCORE_ATT_HOST_DEVICE
  Inside_v Inside(Vector3D<Real_v> const &point) const;

  template <typename Real_v, typename Inside_v>
  VECGEOM_FORCE_INLINE
  VECCORE_ATT_HOST_DEVICE
  Inside_v Inside(Vector3D<Real_v> const &point, int i) const;

  template <typename Real_v>
  VECGEOM_FORCE_INLINE
  VECCORE_ATT_HOST_DEVICE
  Real_v Distance(Vector3D<Real_v> const &point, Vector3D<Real_v> const &direction) const;

  template <bool pointInsideT, typename Real_v>
  VECGEOM_FORCE_INLINE
  VECCORE_ATT_HOST_DEVICE
  Real_v Distance(Vector3D<Real_v> const &point) const;
};

std::ostream &operator<<(std::ostream &os, Planes const &planes);

VECCORE_ATT_HOST_DEVICE
Precision const *Planes::operator[](int i) const
{
  if (i == 0) return fNormals.x();
  if (i == 1) return fNormals.y();
  if (i == 2) return fNormals.z();
  if (i == 3) return &fDistances[0];
  return NULL;
}

VECCORE_ATT_HOST_DEVICE
int Planes::size() const
{
  return fNormals.size();
}

VECCORE_ATT_HOST_DEVICE
SOA3D<Precision> const &Planes::GetNormals() const
{
  return fNormals;
}

VECCORE_ATT_HOST_DEVICE
Vector3D<Precision> Planes::GetNormal(int i) const
{
  return fNormals[i];
}

VECCORE_ATT_HOST_DEVICE
Array<Precision> const &Planes::GetDistances() const
{
  return fDistances;
}

VECCORE_ATT_HOST_DEVICE
Precision Planes::GetDistance(int i) const
{
  return fDistances[i];
}

namespace {

template <typename Real_v>
VECGEOM_FORCE_INLINE
VECCORE_ATT_HOST_DEVICE
void AcceleratedContains(int & /*i*/, const int /*n*/, SOA3D<Precision> const & /*normals*/,
                         Array<Precision> const & /*distances*/, Vector3D<Real_v> const & /*point*/,
                         vecCore::Mask_v<Real_v> & /*result*/)
{
  return;
}

#if defined(VECGEOM_VC) && defined(VECGEOM_QUADRILATERALS_VC)
template <>
VECGEOM_FORCE_INLINE
VECCORE_ATT_HOST_DEVICE
void AcceleratedContains<Precision>(int &i, const int n, SOA3D<Precision> const &normals,
                                    Array<Precision> const &distances, Vector3D<Precision> const &point,
                                    vecCore::Mask_v<double> &result)
{
  for (; i < n - kVectorSize; i += kVectorSize) {
    VcBool valid = VcPrecision(normals.x() + i) * point[0] + VcPrecision(normals.y() + i) * point[1] +
                       VcPrecision(normals.z() + i) * point[2] + VcPrecision(&distances[0] + i) <
                   0;
    result = vecCore::MaskFull(valid);
    if (!result) {
      i = n;
      break;
    }
  }
}
#endif

} // End anonymous namespace

template <typename Real_v>
VECGEOM_FORCE_INLINE
VECCORE_ATT_HOST_DEVICE
vecCore::Mask_v<Real_v> Planes::Contains(Vector3D<Real_v> const &point) const
{

  vecCore::Mask_v<Real_v> result = vecCore::Mask_v<Real_v>(true);

  int i       = 0;
  const int n = size();
  AcceleratedContains<Real_v>(i, n, fNormals, fDistances, point, result);
  for (; i < n; ++i) {
    result &= point.Dot(fNormals[i]) + fDistances[i] < 0;
  }

  return result;
}

namespace {

template <typename Real_v, typename Inside_v>
VECGEOM_FORCE_INLINE
VECCORE_ATT_HOST_DEVICE
void AcceleratedInside(int & /*i*/, const int /*n*/, SOA3D<Precision> const & /*normals*/,
                       Array<Precision> const & /*distances*/, Vector3D<Real_v> const & /*point*/,
                       Inside_v & /*result*/)
{
  return;
}

#if defined(VECGEOM_VC) and defined(VECGEOM_QUADRILATERALS_VC)
template <>
VECGEOM_FORCE_INLINE
VECCORE_ATT_HOST_DEVICE
void AcceleratedInside<Precision, Inside_t>(int &i, const int n, SOA3D<Precision> const &normals,
                                            Array<Precision> const &distances, Vector3D<Precision> const &point,
                                            Inside_t &result)
{
  for (; i < n - kVectorSize; i += kVectorSize) {
    VcPrecision distance = VcPrecision(normals.x() + i) * point[0] + VcPrecision(normals.y() + i) * point[1] +
                           VcPrecision(normals.z() + i) * point[2] + VcPrecision(&distances[0] + i);
    // If point is outside tolerance of any plane, it is safe to return
    if (!vecCore::MaskEmpty(distance > kTolerance)) {
      result = EInside::kOutside;
      i      = n;
      break;
    }
    // If point is inside tolerance of all planes, keep looking
    if (vecCore::MaskFull(distance < -kTolerance)) continue;
    // Otherwise point must be on a surface, but could still be outside
    result = EInside::kSurface;
  }
}
#endif

} // End anonymous namespace

template <typename Real_v, typename Inside_v>
VECGEOM_FORCE_INLINE
VECCORE_ATT_HOST_DEVICE
Inside_v Planes::Inside(Vector3D<Real_v> const &point) const
{
  Inside_v result = Inside_v(EInside::kInside);

  int i       = 0;
  const int n = size();
  AcceleratedInside<Real_v, Inside_v>(i, n, fNormals, fDistances, point, result);
  for (; i < n; ++i) {
    Real_v distanceResult =
        fNormals.x(i) * point[0] + fNormals.y(i) * point[1] + fNormals.z(i) * point[2] + fDistances[i];
    vecCore::MaskedAssign(result, distanceResult > Real_v(kHalfTolerance), EInside::kOutside);
    vecCore::MaskedAssign(result, result == Inside_v(EInside::kInside) && distanceResult > Real_v(-kHalfTolerance),
                          Inside_v(EInside::kSurface));
    if (vecCore::MaskFull(result == Inside_v(EInside::kOutside))) break;
  }

  return result;
}

template <typename Real_v, typename Inside_v>
VECGEOM_FORCE_INLINE
VECCORE_ATT_HOST_DEVICE
Inside_v Planes::Inside(Vector3D<Real_v> const &point, int i) const
{

  Inside_v result = Inside_v(EInside::kInside);
  Real_v distanceResult =
      fNormals.x(i) * point[0] + fNormals.y(i) * point[1] + fNormals.z(i) * point[2] + fDistances[i];
  vecCore::MaskedAssign(result, distanceResult > Real_v(kHalfTolerance), Inside_v(EInside::kOutside));
  vecCore::MaskedAssign(result, result == Inside_v(EInside::kInside) && distanceResult > Real_v(-kHalfTolerance),
                        Inside_v(EInside::kSurface));

  return result;
}

template <typename Real_v>
VECGEOM_FORCE_INLINE
VECCORE_ATT_HOST_DEVICE
Real_v Planes::Distance(Vector3D<Real_v> const &point, Vector3D<Real_v> const &direction) const
{

  Real_v bestDistance = InfinityLength<Real_v>();
  for (int i = 0, iMax = size(); i < iMax; ++i) {
    Vector3D<Precision> normal = fNormals[i];
    Real_v distance            = -(point.Dot(normal) + fDistances[i]) / direction.Dot(normal);
    vecCore::MaskedAssign(bestDistance, distance >= Real_v(0) && distance < bestDistance, distance);
  }

  return bestDistance;
}

template <bool pointInsideT, typename Real_v>
VECGEOM_FORCE_INLINE
VECCORE_ATT_HOST_DEVICE
Real_v Planes::Distance(Vector3D<Real_v> const &point) const
{

  Real_v bestDistance = InfinityLength<Real_v>();
  for (int i = 0, iMax = size(); i < iMax; ++i) {
    Real_v distance = Flip<!pointInsideT>::FlipSign(point.Dot(fNormals[i]) + fDistances[i]);
    vecCore::MaskedAssign(bestDistance, distance >= Real_v(0) && distance < bestDistance, distance);
  }
  return bestDistance;
}

} // End inline namespace

} // End global namespace

#endif // VECGEOM_VOLUMES_PLANES_H_
