bool inVector(int x, Eigen::VectorXi &vec){
  for(int i=0; i<vec.rows(); ++i){
    if(vec(i) == x) return true;
  }
  return false;
}

Eigen::Vector3d tangentBase(Eigen::Vector3d vertex, Eigen::Vector3d normal, Eigen::Vector3d neighbor){
  return (neighbor - normal.dot(neighbor - vertex)*normal) - vertex;
}

void prefactor(){
  int fixed = handle_vertices.rows();
  int free = V.rows()-fixed;
  
  // Reshuffle matrices to have constrained verts last.
  Vs = V_cp;
  Fp = F;
  reorder.resize(V.rows());
  for(int i=0; i<reorder.rows(); i++)
    reorder(i) = i;
  
  int dst=free;
  for(int i=0; i<fixed; ++i){
    int src = handle_vertices(i);
    // Skip over handles already in the proper range.
    if(free <= src) continue;
    // Find next index in the fixed range that is not already fixed.
    while(inVector(dst, handle_vertices))
      ++dst;
    // Swap the rows
    Vs.row(dst) = V_cp.row(src);
    Vs.row(src) = V_cp.row(dst);
    Fp.row(dst) = F.row(src);
    Fp.row(src) = F.row(dst);
    reorder(dst) = src;
    reorder(src) = dst;
    ++dst;
  }
  // Fixup face indices
  for(int i=0; i<Fp.rows(); ++i){
    for(int j=0; j<Fp.cols(); ++j){
      Fp(i, j) = reorder(Fp(i, j));
    }
  }
  
  // Compute cotan and mass matrix, construct A matrix.
  Eigen::SparseMatrix<double> L, M, A, Aff;
  igl::cotmatrix(Vs, Fp, L);
  igl::massmatrix(Vs, Fp, igl::MASSMATRIX_TYPE_DEFAULT, M);
  Eigen::SparseMatrix<double> I(M.rows(), M.cols());
  I.setIdentity();
  A = L*I.cwiseQuotient(M)*L;
  Aff = A.topLeftCorner(free, free);
  Afc = A.topRightCorner(free, fixed);
  
  // Solve the system, giving us the smoothed vertex positions.
  Eigen::MatrixXd rhs = -Afc * Vs.bottomRows(fixed);
  solver.compute(Aff);
  Vs.topRows(free) = solver.solve(rhs);
  
  // Compute detail bases
  Eigen::MatrixXd Displacement = V - Vs;
  Eigen::MatrixXd N;
  std::vector<std::vector<int>> neighbors;
  igl::per_vertex_normals(Vs, Fp, N);
  igl::adjacency_list(Fp, neighbors);
  d.resize(Vs.rows(), 3);
  Neighbor.resize(Vs.rows());
  for(int i=0; i<Vs.rows(); ++i){
    Eigen::Vector3d vertex = Vs.row(i);
    Eigen::Vector3d normal = N.row(i);
    // Find maximal tangent vector in neighborhood
    float max = 0;
    Eigen::Vector3d tangent;
    for(int j=0; j<neighbors[i].size(); ++j){
      Eigen::Vector3d neighbor = Vs.row(neighbors[i][j]);
      Eigen::Vector3d ctangent = tangentBase(vertex, normal, neighbor);
      if(max < ctangent.squaredNorm()){
        max = ctangent.squaredNorm();
        tangent = ctangent;
        Neighbor(i) = neighbors[i][j];
      }
    }
    // Compute displacement factors
    tangent.normalize();
    Eigen::Vector3d base = normal.cross(tangent);
    d(i, 0) = Displacement.row(i).dot(tangent);
    d(i, 1) = Displacement.row(i).dot(base);
    d(i, 2) = Displacement.row(i).dot(normal);
  }
}

void show(Viewer& viewer){
  // Fill the vertices back in, respecting reordering.
  for(int i=0; i<V.rows(); ++i){
    int dst = reorder(i);
    switch(mode){
    case 0: V.row(i) = V_cp.row(i); break;
    case 1: V.row(i) = Vs.row(dst); break;
    case 2: V.row(i) = Vsp.row(dst); break;
    case 3: V.row(i) = Vsp.row(dst) + Delta.row(dst); break;
    }
  }
  
  Eigen::MatrixXd N;
  igl::per_vertex_normals(V, F, N);
  viewer.data().set_normals(N);
}

bool solve(Viewer& viewer){
  // Dumb
  prefactor();
  
  int fixed = handle_vertices.rows();
  int free = V.rows()-fixed;
  
  // Solve the system again, with updated handle positions
  Vsp.resize(V.rows(), V.cols());
  for(int i=0; i<fixed; ++i)
    Vsp.row(reorder(handle_vertices(i))) = handle_vertex_positions.row(i);
  
  Eigen::MatrixXd rhs = -Afc * Vsp.bottomRows(fixed);
  Vsp.topRows(free) = solver.solve(rhs);
  
  // Transfer details
  Eigen::MatrixXd N;
  igl::per_vertex_normals(Vsp, Fp, N);
  Delta.resize(Vsp.rows(), 3);
  for(int i=0; i<Vsp.rows(); ++i){
    Eigen::Vector3d vertex = Vsp.row(i);
    Eigen::Vector3d normal = N.row(i);
    Eigen::Vector3d neighbor = Vsp.row(Neighbor(i));
    Eigen::Vector3d tangent = tangentBase(vertex, normal, neighbor).normalized();
    Eigen::Vector3d base = normal.cross(tangent);
    Delta.row(i) = d(i, 0)*tangent + d(i, 1)*base + d(i, 2)*normal;
  }
  
  show(viewer);
  return true;
}