#include "triangle_triangle_intersect_shared_edge.h"
#include "PI.h"
#include <Eigen/Geometry>
#include <type_traits>

//#define IGL_TRIANGLE_TRIANGLE_INTERSECT_SHARED_EDGE_DEBUG
#ifdef IGL_TRIANGLE_TRIANGLE_INTERSECT_SHARED_EDGE_DEBUG
// CGAL::Epeck
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#warning "🐌🐌🐌🐌🐌🐌🐌🐌 Slow debug mode for igl::triangle_triangle_intersect"
#endif

template <
  typename DerivedV,
  typename DerivedF,
  typename Derivedp>
IGL_INLINE bool igl::triangle_triangle_intersect_shared_edge(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedF> & F,
  const int f,
  const int c,
  const Eigen::MatrixBase<Derivedp> & p,
  const int g,
  const typename DerivedV::Scalar epsilon)
{
  constexpr bool stinker = false;
  static_assert(
    std::is_same<typename DerivedV::Scalar,typename Derivedp::Scalar>::value, 
    "V and p should have same Scalar type");
  assert(V.cols() == 3);
  assert(p.cols() == 3);
#ifdef IGL_TRIANGLE_TRIANGLE_INTERSECT_DEBUG
  using Kernel = CGAL::Epeck;
  typedef CGAL::Point_3<Kernel>    Point_3;
  typedef CGAL::Segment_3<Kernel>  Segment_3;
  typedef CGAL::Triangle_3<Kernel> Triangle_3;
    bool cgal_found_intersection = false;
    Point_3 Vg[3];
    Point_3 Vf[3];
    for(int i = 0;i<3;i++)
    {
      Vg[i] = Point_3(V(F(g,i),0),V(F(g,i),1),V(F(g,i),2));
      if(i == c)
      {
        Vf[i] = Point_3(p(0),p(1),p(2));
      }else
      {
        Vf[i] = Point_3(V(F(f,i),0),V(F(f,i),1),V(F(f,i),2));
      }
    }
    Triangle_3 Tg(Vg[0],Vg[1],Vg[2]);
    Triangle_3 Tf(Vf[0],Vf[1],Vf[2]);
#endif
  bool found_intersection = false;
  // Only intersects if the dihedral angle is zero (precondition: no zero
  // area triangles before or after collapse)

  // normal of g
  const auto vg10 = (V.row(F(g,1))-V.row(F(g,0))).template head<3>();
  const auto vg20 = (V.row(F(g,2))-V.row(F(g,0))).template head<3>();
  const auto ng = vg10.cross(vg20);

  // normal of f (with p replaced)
  const auto vf1p = (V.row(F(f,(c+1)%3))-p).template head<3>();
  const auto vf2p = (V.row(F(f,(c+2)%3))-p).template head<3>();
  const auto nf = vf1p.cross(vf2p);

  // normalized edge vector
  const auto o_vec_un = (V.row(F(f,(c+2)%3))-V.row(F(f,(c+1)%3))).template head<3>();
  const auto o_vec = o_vec_un.stableNormalized();
  const auto theta = std::abs(std::atan2(o_vec.dot(ng.cross(nf)),ng.dot(nf)));
  if(stinker){ printf(" theta: %g\n",theta); }
  // This magic number should be a parameter
  if(theta < igl::PI - epsilon)
  {
    return false;
  }
  // Triangles really really might intersect.
  found_intersection = true;
  // Find intersection
#ifdef IGL_TRIANGLE_TRIANGLE_INTERSECT_SHARED_EDGE_DEBUG
  if(CGAL::do_intersect(Tg,Tf))
  {
    CGAL::Object obj = CGAL::intersection(Tg,Tf);
    if(const Segment_3 *iseg = CGAL::object_cast<Segment_3 >(&obj))
    {
      printf("  ❌ segment is just the edge.\n");
    }else if(const Point_3 *ipoint = CGAL::object_cast<Point_3 >(&obj))
    {
      printf("  🤔 how just a single point?\n");
    } else if(const Triangle_3 *itri = CGAL::object_cast<Triangle_3 >(&obj))
    {
      cgal_found_intersection = true;
      printf("  ✅ sure it's a triangle\n");
    } else if(const std::vector<Point_3 > *polyp =
        CGAL::object_cast< std::vector<Point_3 > >(&obj))
    {
      printf("  ✅ polygon\n");
    }else {
      printf("  🤔 da fuke?\n");
    }
  }
  assert(found_intersection == cgal_found_intersection);
#endif
  return found_intersection;
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// generated by autoexplicit.sh
template bool igl::triangle_triangle_intersect_shared_edge<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, int, int, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false> > const&, int, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar);
// generated by autoexplicit.sh
template bool igl::triangle_triangle_intersect_shared_edge<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 1, -1, 1, 1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, int, int, Eigen::MatrixBase<Eigen::Matrix<double, 1, -1, 1, 1, -1> > const&, int, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar);
// generated by autoexplicit.sh
template bool igl::triangle_triangle_intersect_shared_edge<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, 1, -1, false> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, int, int, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, 1, -1, false> > const&, int, Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar);
#endif