// Copyright (c) 1998
// Utrecht University (The Netherlands),
// ETH Zurich (Switzerland),
// INRIA Sophia-Antipolis (France),
// Max-Planck-Institute Saarbruecken (Germany),
// and Tel-Aviv University (Israel).  All rights reserved.
//
// This file is part of CGAL (www.cgal.org)
//
// $URL: https://github.com/CGAL/cgal/blob/v6.1/Distance_3/include/CGAL/Distance_3/internal/squared_distance_utils_3.h $
// $Id: include/CGAL/Distance_3/internal/squared_distance_utils_3.h b26b07a1242 $
// SPDX-License-Identifier: LGPL-3.0-or-later OR LicenseRef-Commercial
//
//
// Author(s)     : Geert-Jan Giezeman

#ifndef CGAL_SQUARED_DISTANCE_UTILS_3_H
#define CGAL_SQUARED_DISTANCE_UTILS_3_H

#include <CGAL/determinant.h>
#include <CGAL/enum.h>
#include <CGAL/number_utils.h>
#include <CGAL/Rational_traits.h>
#include <CGAL/wmult.h>

#include <type_traits>

namespace CGAL {
namespace internal {

inline
Comparison_result smaller_of(const Comparison_result& a, const Comparison_result& b)
{
  return (std::min)(a,b);
}

inline
Uncertain<Comparison_result> smaller_of(const Uncertain<Comparison_result>& a, const Uncertain<Comparison_result>& b)
{
  return Uncertain((std::min)(a.inf(),b.inf()), (std::min)(a.sup(),b.sup()));
}

inline
Uncertain<Comparison_result> smaller_of(const Uncertain<Comparison_result>& a, const Comparison_result& b)
{
  return smaller_of(a,make_uncertain(b));
}

inline
Uncertain<Comparison_result> smaller_of(const Comparison_result& a, const Uncertain<Comparison_result>& b)
{
  return smaller_of(make_uncertain(a),b);
}

template <class K>
bool is_null(const typename K::Vector_3 &v, const K&)
{
  typedef typename K::RT RT;
  return v.hx() == RT(0) && v.hy() == RT(0) && v.hz() == RT(0);
}

template <class K>
typename K::RT
wdot(const typename K::Vector_3 &u,
     const typename K::Vector_3 &v,
     const K&)
{
  if constexpr(std::is_same_v<typename K::RT, double>)
    return std::fma(u.hx(), v.hx(), std::fma(u.hy(), v.hy(), u.hz()*v.hz()));
  else
    return  (u.hx()*v.hx() + u.hy()*v.hy() + u.hz()*v.hz());
}

template <class K>
typename K::RT
wdot_tag(const typename K::Point_3 &p,
         const typename K::Point_3 &q,
         const typename K::Point_3 &r,
         const K&,
         const Cartesian_tag&)
{
  return  ( (p.x() - q.x()) * (r.x()- q.x())
            + (p.y()- q.y())* (r.y()- q.y())
            + (p.z()- q.z()) * (r.z() - q.z()) );
}

template <class K>
typename K::RT
wdot_tag(const typename K::Point_3 &p,
         const typename K::Point_3 &q,
         const typename K::Point_3 &r,
         const K&,
         const Homogeneous_tag&)
{
  return  ( (p.hx() * q.hw() - q.hx() * p.hw())
            * (r.hx() * q.hw() - q.hx() * r.hw())
            + (p.hy() * q.hw() - q.hy() * p.hw())
            * (r.hy() * q.hw() - q.hy() * r.hw())
            + (p.hz() * q.hw() - q.hz() * p.hw())
            * (r.hz() * q.hw() - q.hz() * r.hw()));
}

template <class K>
inline
typename K::RT
wdot(const typename K::Point_3 &p,
     const typename K::Point_3 &q,
     const typename K::Point_3 &r,
     const K& k)
{
  typedef typename K::Kernel_tag Tag;
  Tag tag;
  return wdot_tag(p, q, r, k, tag);
}

inline
double diff_of_products(const double a, const double b, const double c, const double d)
{
// Kahan method, less numerical error
#if 1
  double w = d * c;
  double e = std::fma(c, -d, w);
  double f = std::fma(a, b, -w);
  return f + e;
#else
  return std::fma(a, b, -c*d);
#endif
}

template <typename OFT>
OFT diff_of_products(const OFT& a, const OFT& b, const OFT& c, const OFT& d, std::enable_if_t<!std::is_same_v<double,OFT>>* = 0)
{
  return a*b - c*d;
}

template <class K>
typename K::Vector_3
wcross(const typename K::Vector_3 &u,
       const typename K::Vector_3 &v,
       const K&)
{
  typedef typename K::Vector_3 Vector_3;
  return Vector_3(
        diff_of_products(u.hy(),v.hz(),u.hz(),v.hy()),
        diff_of_products(u.hz(),v.hx(),u.hx(),v.hz()),
        diff_of_products(u.hx(),v.hy(),u.hy(),v.hx()));
}

template <class K>
inline
bool
is_acute_angle(const typename K::Vector_3 &u,
               const typename K::Vector_3 &v,
               const K& k)
{
  typedef typename K::RT RT;
  return RT(wdot(u, v, k)) > RT(0) ;
}

template <class K>
inline
bool
is_straight_angle(const typename K::Vector_3 &u,
                  const typename K::Vector_3 &v,
                  const K& k)
{
  typedef typename K::RT RT;
  return RT(wdot(u, v, k)) == RT(0) ;
}

template <class K>
inline
bool
is_obtuse_angle(const typename K::Vector_3 &u,
                const typename K::Vector_3 &v,
                const K& k)
{
  typedef typename K::RT RT;
  return RT(wdot(u, v, k)) < RT(0) ;
}

template <class K>
inline
bool
is_acute_angle(const typename K::Point_3 &p,
               const typename K::Point_3 &q,
               const typename K::Point_3 &r,
               const K& k)
{
  typedef typename K::RT RT;
  return RT(wdot(p, q, r, k)) > RT(0) ;
}

template <class K>
inline
bool
is_straight_angle(const typename K::Point_3 &p,
                  const typename K::Point_3 &q,
                  const typename K::Point_3 &r,
                  const K& k)
{
  typedef typename K::RT RT;
  return RT(wdot(p, q, r, k)) == RT(0) ;
}

template <class K>
inline
bool
is_obtuse_angle(const typename K::Point_3 &p,
                const typename K::Point_3 &q,
                const typename K::Point_3 &r,
                const K& k)
{
  typedef typename K::RT RT;
  return RT(wdot(p, q, r, k)) < RT(0) ;
}
template <class K>
void
squared_distance_to_plane_RT(const typename K::Vector_3& normal,
                             const typename K::Vector_3& diff,
                             typename K::RT& num,
                             typename K::RT& den,
                             const K& k)
{
  typedef typename K::RT RT;
  RT dot, squared_length;
  dot = wdot(normal, diff, k);
  squared_length = wdot(normal, normal, k);
  num = square(dot);
  den = wmult((K*)0, squared_length, diff.hw(), diff.hw());
}

template <class K>
typename K::FT
squared_distance_to_plane(const typename K::Vector_3& normal,
                          const typename K::Vector_3& diff,
                          const K& k)
{
  typedef typename K::RT RT;
  typedef typename K::FT FT;
  RT num, den;
  squared_distance_to_plane_RT(normal, diff, num, den, k);
  return Rational_traits<FT>().make_rational(num, den);
}

template <class K>
void
squared_distance_to_line_RT(const typename K::Vector_3& dir,
                            const typename K::Vector_3& diff,
                            typename K::RT& num,
                            typename K::RT& den,
                            const K& k)
{
  typedef typename K::Vector_3 Vector_3;
  Vector_3 wcr = wcross(dir, diff, k);
  num = wdot(wcr, wcr, k);
  den = wmult((K*)0, wdot(dir, dir, k), diff.hw(), diff.hw());
}

template <class K>
typename K::FT
squared_distance_to_line(const typename K::Vector_3& dir,
                         const typename K::Vector_3& diff,
                         const K& k)
{
  typedef typename K::RT RT;
  typedef typename K::FT FT;
  RT num, den;
  squared_distance_to_line_RT(dir, diff, num, den, k);
  return Rational_traits<FT>().make_rational(num, den);
}

template <class K>
typename K::Comparison_result
compare_squared_distance_to_line(const typename K::Vector_3& dir,
                                 const typename K::Vector_3& diff,
                                 const K& k,
                                 const typename K::FT &d2)
{
  return compare(squared_distance_to_line(dir,diff,k),d2);
}

template <class K>
inline
bool
same_direction_tag(const typename K::Vector_3 &u,
                   const typename K::Vector_3 &v,
                   const K&,
                   const Cartesian_tag&)
{
  typedef typename K::FT FT;
  const FT& ux = u.x();
  const FT& uy = u.y();
  const FT& uz = u.z();
  if (CGAL_NTS abs(ux) > CGAL_NTS abs(uy)) {
    if (CGAL_NTS abs(ux) > CGAL_NTS abs(uz)) {
      return CGAL_NTS sign(ux) == CGAL_NTS sign(v.x());
    } else {
      return CGAL_NTS sign(uz) == CGAL_NTS sign(v.z());
    }
  } else {
    if (CGAL_NTS abs(uy) > CGAL_NTS abs(uz)) {
      return CGAL_NTS sign(uy) == CGAL_NTS sign(v.y());
    } else {
      return CGAL_NTS sign(uz) == CGAL_NTS sign(v.z());
    }
  }
}

template <class K>
inline
bool
same_direction_tag(const typename K::Vector_3 &u,
                   const typename K::Vector_3 &v,
                   const K&,
                   const Homogeneous_tag&)
{
  typedef typename K::RT RT;
  const RT& uhx = u.hx();
  const RT& uhy = u.hy();
  const RT& uhz = u.hz();
  if (CGAL_NTS abs(uhx) > CGAL_NTS abs(uhy)) {
    if (CGAL_NTS abs(uhx) > CGAL_NTS abs(uhz)) {
      return CGAL_NTS sign(uhx) == CGAL_NTS sign(v.hx());
    } else {
      return CGAL_NTS sign(uhz) == CGAL_NTS sign(v.hz());
    }
  } else {
    if (CGAL_NTS abs(uhy) > CGAL_NTS abs(uhz)) {
      return CGAL_NTS sign(uhy) == CGAL_NTS sign(v.hy());
    } else {
      return CGAL_NTS sign(uhz) == CGAL_NTS sign(v.hz());
    }
  }
}

template <class K>
inline
bool
same_direction(const typename K::Vector_3 &u,
               const typename K::Vector_3 &v,
               const K& k)
{
  typedef typename K::Kernel_tag Tag;
  Tag tag;
  return same_direction_tag(u, v, k, tag);
}

template <class K>
inline
typename K::RT
distance_measure_sub(typename K::RT startwdist, typename K::RT endwdist,
                     const typename K::Vector_3 &start,
                     const typename K::Vector_3 &end,
                     const K&)
{
  return CGAL_NTS abs(wmult((K*)0, startwdist, end.hw())) -
         CGAL_NTS abs(wmult((K*)0, endwdist, start.hw()));
}

} // namespace internal
} // namespace CGAL

#endif // CGAL_SQUARED_DISTANCE_UTILS_3_H

