#include "internal_angles_intrinsic.h"
#include "parallel_for.h"

template <typename DerivedL, typename DerivedK>
IGL_INLINE void igl::internal_angles_intrinsic(
  const Eigen::MatrixBase<DerivedL>& L_sq,
  Eigen::PlainObjectBase<DerivedK> & K)
{
  typedef typename DerivedL::Index Index;
  assert(L_sq.cols() == 3 && "Edge-lengths should come from triangles");
  const Index m = L_sq.rows();
  K.resize(m,3);
  parallel_for(
    m,
    [&L_sq,&K](const Index f)
    {
      for(size_t d = 0;d<3;d++)
      {
        const auto & s1 = L_sq(f,d);
        const auto & s2 = L_sq(f,(d+1)%3);
        const auto & s3 = L_sq(f,(d+2)%3);
        using Scalar = typename DerivedK::Scalar;
        K(f,d) = std::acos((s3 + s2 - s1)/(Scalar(2)*std::sqrt(s3*s2)));

        //// See https://github.com/libigl/libigl/issues/1463
        //// Kahan's method
        //auto c = sqrt(L_sq(f,d));
        //auto a = sqrt(L_sq(f,(d+1)%3));
        //auto b = sqrt(L_sq(f,(d+2)%3));
        //// If necessary, swap a and b so that a ≥ b . 
        //if(a<b){ std::swap(a,b); }
        //// Then perform two more comparisons to decide whether and how to
        //// compute an intermediate quantity µ thus:
        //using Scalar = typename DerivedK::Scalar;
        //Scalar mu;
        //if(b>=c && c>=0)
        //{
        //  mu = c-(a-b);
        //}else if(c>b && b>=0)
        //{
        //  mu = b-(a-c);
        //}else
        //{
        //  // the data are not side–lengths of a real triangle
        //  K(f,d) = std::numeric_limits<Scalar>::quiet_NaN();
        //  continue;
        //}
        //// mu has been computed, attempt to compute angle
        //K(f,d) = 2.0*atan(sqrt(((a-b)+c)*mu/((a+(b+c))*((a-c)+b)) ));

      }
    },
    1000l);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// generated by autoexplicit.sh
template void igl::internal_angles_intrinsic<Eigen::Matrix<float, -1, 1, 0, -1, 1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>>(Eigen::MatrixBase<Eigen::Matrix<float, -1, 1, 0, -1, 1>> const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, -1, 0, -1, -1>>&);
// generated by autoexplicit.sh
template void igl::internal_angles_intrinsic<Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 1, 0, -1, 1>> const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1>>&);
// generated by autoexplicit.sh
template void igl::internal_angles_intrinsic<Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1>>(Eigen::MatrixBase<Eigen::Matrix<float, -1, -1, 0, -1, -1>> const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, -1, 0, -1, -1>>&);
// generated by autoexplicit.sh
template void igl::internal_angles_intrinsic<Eigen::Matrix<float, -1, 3, 0, -1, 3>, Eigen::Matrix<float, -1, 3, 0, -1, 3>>(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 0, -1, 3>> const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 0, -1, 3>>&);
// generated by autoexplicit.sh
template void igl::internal_angles_intrinsic<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 3, 0, -1, 3>>(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1>> const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3>>&);
// generated by autoexplicit.sh
template void igl::internal_angles_intrinsic<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3>>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3>> const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3>>&);
// generated by autoexplicit.sh
template void igl::internal_angles_intrinsic<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, -1, 0, -1, -1>>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3>> const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1>>&);
// generated by autoexplicit.sh
template void igl::internal_angles_intrinsic<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::internal_angles_intrinsic<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
#endif