#include "compute_vertex_quadrics.h"

void compute_vertex_quadrics(
  const Eigen::MatrixXd & V,
  const Eigen::MatrixXi & F,
  const Eigen::MatrixXi & EMAP,
  const Eigen::MatrixXi & EF,
  const Eigen::MatrixXi & EI,
  std::vector<
    std::tuple<Eigen::MatrixXd,Eigen::RowVectorXd,double> > & quadrics)
{
  using namespace std;
  using namespace igl;
  typedef std::tuple<Eigen::MatrixXd,Eigen::RowVectorXd,double> Quadric;
  const int dim = V.cols();
  //// Quadrics per face
  //std::vector<Quadric> face_quadrics(F.rows());
  // Initialize each vertex quadric to zeros
  quadrics.resize(
    V.rows(),
    // gcc <=4.8 can't handle initializer lists correctly
    Quadric{Eigen::MatrixXd::Zero(dim,dim),Eigen::RowVectorXd::Zero(dim),0});
  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dim,dim);
  // Rather initial with zeros, initial with a small amount of energy pull
  // toward original vertex position
  const double w = 1e-10;
  for(int v = 0;v<V.rows();v++)
  {
    std::get<0>(quadrics[v]) = w*I;
    Eigen::RowVectorXd Vv = V.row(v);
    std::get<1>(quadrics[v]) = w*-Vv;
    std::get<2>(quadrics[v]) = w*Vv.dot(Vv);
  }
  // Generic nD qslim from "Simplifying Surfaces with Color and Texture
  // using Quadric Error Metric" (follow up to original QSlim)
  for(int f = 0;f<F.rows();f++)
  {
    int infinite_corner = -1;
    for(int c = 0;c<3;c++)
    {
      if(
         std::isinf(V(F(f,c),0)) || 
         std::isinf(V(F(f,c),1)) || 
         std::isinf(V(F(f,c),2)))
      {
        assert(infinite_corner == -1 && "Should only be one infinite corner");
        infinite_corner = c;
      }
    }
    // Inputs:
    //   p  1 by n row point on the subspace 
    //   S  m by n matrix where rows coorespond to orthonormal spanning
    //     vectors of the subspace to which we're measuring distance (usually
    //     a plane, m=2)
    //   weight  scalar weight
    // Returns quadric triple {A,b,c} so that A-2*b+c measures the quadric
    const auto subspace_quadric = [&I](
      const Eigen::RowVectorXd & p,
      const Eigen::MatrixXd & S,
      const double  weight)->Quadric
    {
      // Dimension of subspace
      const int m = S.rows();
      // Weight face's quadric (v'*A*v + 2*b'*v + c) by area
      // e1 and e2 should be perpendicular
      Eigen::MatrixXd A = I;
      Eigen::RowVectorXd b = -p;
      double c = p.dot(p);
      for(int i = 0;i<m;i++)
      {
        Eigen::RowVectorXd ei = S.row(i);
        for(int j = 0;j<i;j++) assert(std::abs(S.row(j).dot(ei)) < 1e-10);
        A += -ei.transpose()*ei;
        b += p.dot(ei)*ei;
        c += -pow(p.dot(ei),2);
      }
      // gcc <=4.8 can't handle initializer lists correctly: needs explicit
      // cast
      return Quadric{ weight*A, weight*b, weight*c };
    };
    if(infinite_corner == -1)
    {
      // Finite (non-boundary) face
      Eigen::RowVectorXd p = V.row(F(f,0));
      Eigen::RowVectorXd q = V.row(F(f,1));
      Eigen::RowVectorXd r = V.row(F(f,2));
      Eigen::RowVectorXd pq = q-p;
      Eigen::RowVectorXd pr = r-p;
      // Gram Determinant = squared area of parallelogram 
      double area = sqrt(pq.squaredNorm()*pr.squaredNorm()-pow(pr.dot(pq),2));
      Eigen::RowVectorXd e1 = pq.normalized();
      Eigen::RowVectorXd e2 = (pr-e1.dot(pr)*e1).normalized();
      Eigen::MatrixXd S(2,V.cols());
      S<<e1,e2;
      Quadric face_quadric = subspace_quadric(p,S,area);
      // Throw at each corner
      for(int c = 0;c<3;c++)
      {
        quadrics[F(f,c)] = quadrics[F(f,c)] + face_quadric;
        // quadrics[F(f,c)] = quadricPlus(quadrics[F(f,c)], face_quadric);
        // Quadric sum;
        // std::get<0>(sum) = (std::get<0>(quadrics[F(f,c)]) + std::get<0>(face_quadric)).eval();
        // std::get<1>(sum) = (std::get<1>(quadrics[F(f,c)]) + std::get<1>(face_quadric)).eval();
        // std::get<2>(sum) = (std::get<2>(quadrics[F(f,c)]) + std::get<2>(face_quadric));
        // quadrics[F(f,c)] = sum;
      }
    }else
    {
      // cth corner is infinite --> edge opposite cth corner is boundary
      // Boundary edge vector
      const Eigen::RowVectorXd p = V.row(F(f,(infinite_corner+1)%3));
      Eigen::RowVectorXd ev = V.row(F(f,(infinite_corner+2)%3)) - p;
      const double length = ev.norm();
      ev /= length;
      // Face neighbor across boundary edge
      int e = EMAP(f+F.rows()*infinite_corner);
      int opp = EF(e,0) == f ? 1 : 0;
      int n =  EF(e,opp);
      int nc = EI(e,opp);
      assert(
        ((F(f,(infinite_corner+1)%3) == F(n,(nc+1)%3) && 
          F(f,(infinite_corner+2)%3) == F(n,(nc+2)%3)) || 
          (F(f,(infinite_corner+1)%3) == F(n,(nc+2)%3) 
          && F(f,(infinite_corner+2)%3) == F(n,(nc+1)%3))) && 
        "Edge flaps not agreeing on shared edge");
      // Edge vector on opposite face
      const Eigen::RowVectorXd eu = V.row(F(n,nc)) - p;
      assert(!std::isinf(eu(0)));
      // Matrix with vectors spanning plane as columns
      Eigen::Vector3d evT = ev.transpose();
      Eigen::Vector3d euT = eu.transpose();
      Eigen::Vector3d boundaryFaceNormal = evT.cross(euT);
      boundaryFaceNormal = boundaryFaceNormal / boundaryFaceNormal.norm();
      euT = euT / euT.norm();
      Eigen::MatrixXd A(ev.size(),2);
      A<<ev.transpose(),eu.transpose();
      // A<<evT,boundaryFaceNormal+0.5*euT;
      // Use QR decomposition to find basis for orthogonal space
      Eigen::HouseholderQR<Eigen::MatrixXd> qr(A);
      const Eigen::MatrixXd Q = qr.householderQ();
      const Eigen::MatrixXd N = 
        Q.topRightCorner(ev.size(),ev.size()-2).transpose();
      assert(N.cols() == ev.size());
      assert(N.rows() == ev.size()-2);
      Eigen::MatrixXd S(N.rows()+1,ev.size());
      S<<ev,N;
      // Quadric boundary_edge_quadric = subspace_quadric(p,S,length);
      Quadric boundary_edge_quadric = subspace_quadric(p,S,length * length / 100);
      for(int c = 0;c<3;c++)
      {
        if(c != infinite_corner)
        {
          quadrics[F(f,c)] = quadrics[F(f,c)] + boundary_edge_quadric;
          // quadrics[F(f,c)] = quadricPlus(quadrics[F(f,c)], boundary_edge_quadric);
          // Quadric sum;
          // std::get<0>(sum) = (std::get<0>(quadrics[F(f,c)]) + std::get<0>(boundary_edge_quadric)).eval();
          // std::get<1>(sum) = (std::get<1>(quadrics[F(f,c)]) + std::get<1>(boundary_edge_quadric)).eval();
          // std::get<2>(sum) = (std::get<2>(quadrics[F(f,c)]) + std::get<2>(boundary_edge_quadric));
          // quadrics[F(f,c)] = sum;
        }
      }
    }
  }
}