// Copyright 2002 Rensselaer Polytechnic Institute

// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Lauren Foutz
//           Scott Hill

/*
  This file implements the functions

  template <class VertexListGraph, class DistanceMatrix,
    class P, class T, class R>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(
    const VertexListGraph& g, DistanceMatrix& d,
    const bgl_named_params<P, T, R>& params)

  AND

  template <class VertexAndEdgeListGraph, class DistanceMatrix,
    class P, class T, class R>
  bool floyd_warshall_all_pairs_shortest_paths(
    const VertexAndEdgeListGraph& g, DistanceMatrix& d,
    const bgl_named_params<P, T, R>& params)
*/

#ifndef BOOST_GRAPH_FLOYD_WARSHALL_HPP
#define BOOST_GRAPH_FLOYD_WARSHALL_HPP

#include <boost/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/relax.hpp>
#include <algorithm> // for std::min and std::max

namespace boost
{
  namespace detail
  {

    template<typename VertexListGraph, typename DistanceMatrix, typename BinaryPredicate, typename BinaryFunction, typename Infinity, typename Zero>
	bool floyd_warshall_dispatch(const VertexListGraph& g,  DistanceMatrix& d, const BinaryPredicate &compare, const BinaryFunction &combine,
						const Infinity& inf, const Zero& zero)
    {
      BOOST_USING_STD_MIN();

      typename graph_traits<VertexListGraph>::vertex_iteratorN
        i, lasti, j, lastj, k, lastk;

      /* main Floyd Warshall algorithm */
      for (tie(k, lastk) = vertices(g); k != lastk; k++)
        for (tie(i, lasti) = vertices(g); i != lasti; i++)
          for (tie(j, lastj) = vertices(g); j != lastj; j++)
          {
            d[*i][*j] = min BOOST_PREVENT_MACRO_SUBSTITUTION(d[*i][*j], combine(d[*i][*k], d[*k][*j]));
          }

		/* checks for negative weight cycle */
      for (tie(i, lasti) = vertices(g); i != lasti; i++)
      	if (compare(d[*i][*i], zero))
          return false;

	  return true;
    }

/* with predecessor map */
    template<typename VertexListGraph, typename DistanceMatrix, typename PredecessorMatrix, typename BinaryPredicate, typename BinaryFunction, typename Infinity, typename Zero>
	bool floyd_warshall_dispatch2(const VertexListGraph& g,  DistanceMatrix& d, PredecessorMatrix& p, const BinaryPredicate &compare, const BinaryFunction &combine,
						const Infinity& inf, const Zero& zero)
    {
      BOOST_USING_STD_MIN();

      typename graph_traits<VertexListGraph>::vertex_iteratorN i, lasti, j, lastj, k, lastk;

      // main Floyd Warshall algorithm
      for (tie(k, lastk) = vertices(g); k != lastk; k++)
        for (tie(i, lasti) = vertices(g); i != lasti; i++)
          for (tie(j, lastj) = vertices(g); j != lastj; j++)
          {
			if(d[*i][*j] > combine(d[*i][*k], d[*k][*j]))
				p[*i][*j] = p[*k][*j];

			d[*i][*j] = min BOOST_PREVENT_MACRO_SUBSTITUTION(d[*i][*j], combine(d[*i][*k], d[*k][*j]));
          }

		// checks for negative weight cycle
      for (tie(i, lasti) = vertices(g); i != lasti; i++)
      	if (compare(d[*i][*i], zero))
          return false;

	  return true;
    }
  }

  template <typename VertexListGraph, typename DistanceMatrix, typename BinaryPredicate, typename BinaryFunction,typename Infinity, typename Zero>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(const VertexListGraph& g, DistanceMatrix& d, const BinaryPredicate& compare,
				    const BinaryFunction& combine, const Infinity& inf, const Zero& zero)
  {
    function_requires<VertexListGraphConcept<VertexListGraph> >();

    return detail::floyd_warshall_dispatch(g, d, compare, combine, inf, zero);
  }

  template <typename VertexAndEdgeListGraph, typename DistanceMatrix, typename WeightMap, typename BinaryPredicate, typename BinaryFunction, typename Infinity, typename Zero>
  bool floyd_warshall_all_pairs_shortest_paths( const VertexAndEdgeListGraph& g, DistanceMatrix& d, const WeightMap& w,
  					 const BinaryPredicate& compare, const BinaryFunction& combine, const Infinity& inf, const Zero& zero)
  {
    BOOST_USING_STD_MIN();

    function_requires<VertexListGraphConcept<VertexAndEdgeListGraph> >();
    function_requires<EdgeListGraphConcept<VertexAndEdgeListGraph> >();
    function_requires<IncidenceGraphConcept<VertexAndEdgeListGraph> >();

    typename graph_traits<VertexAndEdgeListGraph>::vertex_iteratorN 	firstv, lastv, firstv2, lastv2;
    typename graph_traits<VertexAndEdgeListGraph>::edge_iteratorN 	first, last;

    for(tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
      for(tie(firstv2, lastv2) = vertices(g); firstv2 != lastv2; firstv2++)
	    d[*firstv][*firstv2] = inf;

    for(tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
      d[*firstv][*firstv] = 0;

    for(tie(first, last) = edges(g); first != last; first++)
    {
      if (d[source(*first, g)][target(*first, g)] != inf)
        d[source(*first, g)][target(*first, g)] = min BOOST_PREVENT_MACRO_SUBSTITUTION(get(w, *first), d[source(*first, g)][target(*first, g)]);
      else
        d[source(*first, g)][target(*first, g)] = get(w, *first);
    }

    bool is_undirected = is_same<typename graph_traits<VertexAndEdgeListGraph>::directed_category, undirected_tag>::value;
    if (is_undirected)
    {
      for(tie(first, last) = edges(g); first != last; first++)
      {
        if (d[target(*first, g)][source(*first, g)] != inf)
          d[target(*first, g)][source(*first, g)] = min BOOST_PREVENT_MACRO_SUBSTITUTION(get(w, *first), d[target(*first, g)][source(*first, g)]);
        else
          d[target(*first, g)][source(*first, g)] = get(w, *first);
      }
    }

    return detail::floyd_warshall_dispatch(g, d, compare, combine, inf, zero);
  }

/* with predecessor map */
  template <typename VertexAndEdgeListGraph, typename DistanceMatrix, typename PredecessorMatrix, typename WeightMap, typename BinaryPredicate, typename BinaryFunction, typename Infinity, typename Zero>
  bool floyd_warshall_all_pairs_shortest_paths2( const VertexAndEdgeListGraph& g, DistanceMatrix& d, PredecessorMatrix& p, const WeightMap& w,
  					 const BinaryPredicate& compare, const BinaryFunction& combine, const Infinity& inf, const Zero& zero)
  {
    BOOST_USING_STD_MIN();

    function_requires<VertexListGraphConcept<VertexAndEdgeListGraph> >();
    function_requires<EdgeListGraphConcept<VertexAndEdgeListGraph> >();
    function_requires<IncidenceGraphConcept<VertexAndEdgeListGraph> >();

    typename graph_traits<VertexAndEdgeListGraph>::vertex_iteratorN 	firstv, lastv, firstv2, lastv2;
    typename graph_traits<VertexAndEdgeListGraph>::edge_iteratorN 	first, last;

    // initialize matrix: distance infinity
    for(tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
      for(tie(firstv2, lastv2) = vertices(g); firstv2 != lastv2; firstv2++)
      {
	  	d[*firstv][*firstv2] = inf;
		p[*firstv][*firstv2] = *firstv;
		//cerr << "[FW::Initialization]  p: " << *firstv << " = " << p[*firstv][*firstv2] << endl;
	  }

    // initialize matrix: distance to itself i zero
    for(tie(firstv, lastv) = vertices(g); firstv != lastv; firstv++)
      d[*firstv][*firstv] = 0;

    for(tie(first, last) = edges(g); first != last; first++)
    {
      if (d[source(*first, g)][target(*first, g)] != inf)
	  {
	  	cerr << "[Floyd-Warshall] ERROR Predecessor map " << endl;
        d[source(*first, g)][target(*first, g)] = min BOOST_PREVENT_MACRO_SUBSTITUTION(get(w, *first), d[source(*first, g)][target(*first, g)]);
	  }
      else
	  {
        d[source(*first, g)][target(*first, g)] = get(w, *first);
		p[source(*first, g)][target(*first, g)] = source(*first, g); // enter predecessor map here
	    //cerr << "[FW::Initialization] (" << source(*first, g) << "," << target(*first, g) << ") Parent map: " << p[source(*first, g)][target(*first, g)];
		//cerr << " Weight Map: " << d[source(*first, g)][target(*first, g)] << endl;
	  }
    }

    bool is_undirected = is_same<typename graph_traits<VertexAndEdgeListGraph>::directed_category, undirected_tag>::value;
    if (is_undirected)
    {
      for(tie(first, last) = edges(g); first != last; first++)
      {
        if (d[target(*first, g)][source(*first, g)] != inf)
		{
		  cerr << "[Floyd-Warshall] Undirected: ERROR Predecessor map " << endl;
          d[target(*first, g)][source(*first, g)] = min BOOST_PREVENT_MACRO_SUBSTITUTION(get(w, *first), d[target(*first, g)][source(*first, g)]);
		}
        else
		{
          d[target(*first, g)][source(*first, g)] = get(w, *first);
		  p[target(*first, g)][source(*first, g)] = target(*first, g); // enter predecessor map here
		  //cerr << "[FW::Initialization] (" << target(*first, g) << "," << source(*first, g) << ") UndParent map: " << p[target(*first, g)][source(*first, g)];
		  //cerr << " Weight Map: " << d[target(*first, g)][source(*first, g)] << endl;
		}
      }
    }

    return detail::floyd_warshall_dispatch2(g, d, p, compare, combine, inf, zero);
  }

  namespace detail {
    template <class VertexListGraph, class DistanceMatrix, class WeightMap, class P, class T, class R>
    bool floyd_warshall_init_dispatch(const VertexListGraph& g, DistanceMatrix& d, WeightMap w, const bgl_named_params<P, T, R>& params)
    {
      typedef typename property_traits<WeightMap>::value_type WM;

      return floyd_warshall_initialized_all_pairs_shortest_paths(g, d,
        choose_param(get_param(params, distance_compare_t()),
          std::less<WM>()),
        choose_param(get_param(params, distance_combine_t()),
          closed_plus<WM>()),
        choose_param(get_param(params, distance_inf_t()),
          std::numeric_limits<WM>::max BOOST_PREVENT_MACRO_SUBSTITUTION()),
        choose_param(get_param(params, distance_zero_t()),
          WM()));
    }

    template <class VertexAndEdgeListGraph, class DistanceMatrix, class WeightMap, class P, class T, class R>
    bool floyd_warshall_noninit_dispatch(const VertexAndEdgeListGraph& g, DistanceMatrix& d, WeightMap w, const bgl_named_params<P, T, R>& params)
    {
      typedef typename property_traits<WeightMap>::value_type WM;

      return floyd_warshall_all_pairs_shortest_paths(g, d, w,
        choose_param(get_param(params, distance_compare_t()),
          std::less<WM>()),
        choose_param(get_param(params, distance_combine_t()),
          closed_plus<WM>()),
        choose_param(get_param(params, distance_inf_t()),
          std::numeric_limits<WM>::max BOOST_PREVENT_MACRO_SUBSTITUTION()),
        choose_param(get_param(params, distance_zero_t()),
          WM()));
    }

/* With predecessor map */
    template <class VertexAndEdgeListGraph, class DistanceMatrix, class PredecessorMatrix, class WeightMap, class P, class T, class R>
    bool floyd_warshall_noninit_dispatch2(const VertexAndEdgeListGraph& g, DistanceMatrix& d, PredecessorMatrix& p, WeightMap w, const bgl_named_params<P, T, R>& params)
    {
      typedef typename property_traits<WeightMap>::value_type WM;

      return floyd_warshall_all_pairs_shortest_paths2(g, d, p, w,
        choose_param(get_param(params, distance_compare_t()),
          std::less<WM>()),
        choose_param(get_param(params, distance_combine_t()),
          closed_plus<WM>()),
        choose_param(get_param(params, distance_inf_t()),
          std::numeric_limits<WM>::max BOOST_PREVENT_MACRO_SUBSTITUTION()),
        choose_param(get_param(params, distance_zero_t()),
          WM()));
    }

  }   // namespace detail

/* VertexListGraphs:   */
  template <class VertexListGraph, class DistanceMatrix, class P, class T, class R>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(const VertexListGraph& g, DistanceMatrix& d, const bgl_named_params<P, T, R>& params)
  {
    return detail::floyd_warshall_init_dispatch(g, d, choose_const_pmap(get_param(params, edge_weight), g, edge_weight), params);
  }

  template <class VertexListGraph, class DistanceMatrix>
  bool floyd_warshall_initialized_all_pairs_shortest_paths(const VertexListGraph& g, DistanceMatrix& d)
  {
    bgl_named_params<int,int> params(0);
    return detail::floyd_warshall_init_dispatch(g, d, get(edge_weight, g), params);
  }

/* VertexAndEdgeListGraphs:   */
  template <class VertexAndEdgeListGraph, class DistanceMatrix, class P, class T, class R>
  bool floyd_warshall_all_pairs_shortest_paths(const VertexAndEdgeListGraph& g, DistanceMatrix& d, const bgl_named_params<P, T, R>& params)
  {
    return detail::floyd_warshall_noninit_dispatch(g, d, choose_const_pmap(get_param(params, edge_weight), g, edge_weight), params);
  }

  template <class VertexAndEdgeListGraph, class DistanceMatrix>
  bool floyd_warshall_all_pairs_shortest_paths(const VertexAndEdgeListGraph& g, DistanceMatrix& d)
  {
    bgl_named_params<int,int> params(0);
    return detail::floyd_warshall_noninit_dispatch(g, d, get(edge_weight, g), params);
  }

/* With predecessor map */
  template <class VertexAndEdgeListGraph, class DistanceMatrix, class PredecessorMatrix, class P, class T, class R>
  bool floyd_warshall_all_pairs_shortest_paths2(const VertexAndEdgeListGraph& g, DistanceMatrix& d, PredecessorMatrix& p, const bgl_named_params<P, T, R>& params)
  {
    return detail::floyd_warshall_noninit_dispatch2(g, d, p, choose_const_pmap(get_param(params, edge_weight), g, edge_weight), params);
    //return detail::floyd_warshall_noninit_dispatch(g, d, choose_const_pmap(get_param(params, edge_weight), g, edge_weight), params);
  }

} // namespace boost

#endif


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <sophus/se3.hpp>
#include <sophus/so3.hpp>
#include <chrono>

using namespace std;
using namespace cv;

// void find_feature_matches(
//   const Mat &img_1, const Mat &img_2,
//   std::vector<KeyPoint> &keypoints_1,
//   std::vector<KeyPoint> &keypoints_2,
//   std::vector<DMatch> &matches);

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);

  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);

  vector<DMatch> match;
  // BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);

  double min_dist = 10000, max_dist = 0;

  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }

  printf("-- Max dist : %f \n", max_dist);
  printf("-- Min dist : %f \n", min_dist);

  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}

Point2d pixel2cam(const Point2d &p, const Mat &K);

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

// BA by gauss-newton
void bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose
);

void pose_refinement_gauss_newton(const VecVector3d& points_3d, const VecVector2d& points_2d, const Eigen::Matrix3d& K, Sophus::SE3d& pose);

int main(int argc, char **argv) {
  // if (argc != 5) {
  //   cout << "usage: pose_estimation_3d2d img1 img2 depth1 depth2" << endl;
  //   return 1;
  // }
  string f1 = "../1.png"; //argv[1];
  string f2 = "../2.png"; //argv[2];
  string f3 = "../1_depth.png"; //argv[3];
  Mat img_1 = imread(f1, CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread(f2, CV_LOAD_IMAGE_COLOR);
  assert(img_1.data && img_2.data && "Can not load images!");

  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
  cout << "一共找到了" << matches.size() << "组匹配点" << endl;

  Mat d1 = imread(f3, CV_LOAD_IMAGE_UNCHANGED);
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  vector<Point3f> pts_3d;
  vector<Point2f> pts_2d;
  for (DMatch m:matches) {
    ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
    if (d == 0)   // bad depth
      continue;
    float dd = d / 5000.0;
    Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
    pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));
    pts_2d.push_back(keypoints_2[m.trainIdx].pt);
  }

  cout << "3d-2d pairs: " << pts_3d.size() << endl;

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  Mat r, t;
  solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false);
  Mat R;
  cv::Rodrigues(r, R);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;

  cout << "R=" << endl << R << endl;
  cout << "t=" << endl << t << endl;

  VecVector3d pts_3d_eigen;
  VecVector2d pts_2d_eigen;
  for (size_t i = 0; i < pts_3d.size(); ++i) {
    pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
    pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
  }

  cout << "calling bundle adjustment by gauss newton" << endl;
  Sophus::SE3d pose_gn;
  t1 = chrono::steady_clock::now();
  bundleAdjustmentGaussNewton(pts_3d_eigen, pts_2d_eigen, K, pose_gn);
  t2 = chrono::steady_clock::now();
  time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp by gauss newton cost time: " << time_used.count() << " seconds." << endl;

  Eigen::Vector3d t_gn = pose_gn.translation();
  Eigen::Matrix3d R_gn = pose_gn.so3().unit_quaternion().toRotationMatrix();
  cout << "R_gn = " << endl << R_gn << endl;
  cout << "t_gn = " << endl << t_gn << endl;

  // cout << "calling bundle adjustment by g2o" << endl;
  // Sophus::SE3d pose_g2o;
  // t1 = chrono::steady_clock::now();
  // bundleAdjustmentG2O(pts_3d_eigen, pts_2d_eigen, K, pose_g2o);
  // t2 = chrono::steady_clock::now();
  // time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  // cout << "solve pnp by g2o cost time: " << time_used.count() << " seconds." << endl;

  Eigen::Matrix3d K_eigen;
  K_eigen << 520.9, 0, 325.1,
             0, 521.0, 249.7,
             0, 0, 1;
  cout << "Custom Gauss-Newton" << endl;
  Sophus::SE3d pose_gn2(Sophus::SO3d(Eigen::Matrix3d::Identity()), Eigen::Vector3d::Zero());
  // Sophus::SE3d pose_gn2(Sophus::SO3d(Eigen::Matrix3d::Identity()), Eigen::Vector3d(0.5, -0.1, 0.2));
  t1 = chrono::steady_clock::now();
  pose_refinement_gauss_newton(pts_3d_eigen, pts_2d_eigen, K_eigen, pose_gn2);
  t2 = chrono::steady_clock::now();
  time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp with custom Gauss-Newton cost time: " << time_used.count() << " seconds." << endl;

  Eigen::Vector3d t_gn2 = pose_gn2.translation();
  Eigen::Matrix3d R_gn2 = pose_gn2.so3().unit_quaternion().toRotationMatrix();
  cout << "R_gn2 = " << endl << R_gn2 << endl;
  cout << "t_gn2 = " << endl << t_gn2 << endl;

  return 0;
}

void pose_refinement_gauss_newton(const VecVector3d& points_3d, const VecVector2d& points_2d, const Eigen::Matrix3d& K, Sophus::SE3d& pose)
{
  const int max_iter = 100;
  double fx = K(0, 0);
  double fy = K(1, 1);
  const int n = points_2d.size();
  double prev_cost = 0.0;
  for (int it = 0; it < max_iter; ++it)
  {
    Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
    Eigen::Matrix<double, 6, 1> b = Eigen::Matrix<double, 6, 1>::Zero();
    double cost = 0.0;
    for (int i = 0; i < n; ++i)
    {
      Eigen::Matrix<double, 2, 6> J = Eigen::Matrix<double, 2, 6>::Zero();
      const auto& X = points_3d[i];
      Eigen::Vector3d x = K * (pose * X);
      x /= x.z();
      Eigen::Vector2d err = points_2d[i] - x.head(2);
      cost += err.squaredNorm();
      double Z2 = std::pow(X.z(), 2);
      J(0, 0) = -fx / X.z();
      J(0, 2) = fx * X.x() / Z2;
      J(0, 3) = fx * X.x() * X.y() / Z2;
      J(0, 4) = -fx-fx * std::pow(X.x(), 2) / Z2;
      J(0, 5) = fx * X.y() / X.z();

      J(1, 1) = -fy / X.z();
      J(1, 2) = fy * X.y() / Z2;
      J(1, 3) = fy + fy * std::pow(X.y(), 2) / Z2;
      J(1, 4) = -fy * X.y() * X.x() / Z2;
      J(1, 5) = -fy * X.x() / X.z();

      H += J.transpose() * J;
      b += -J.transpose() * err;
    }

    std::cout << "iter " << it << ": cost = " << cost << "\n";
    Eigen::Matrix<double, 6, 1> delta_x = H.inverse() * b;
    // Eigen::Matrix<double, 6, 1> delta_x = H.ldlt().solve(b);

    if (it > 0 && prev_cost - cost < 1e-5 || delta_x.norm() < 1e-5)
    {
      break;
    }

    // update pose
    pose = Sophus::SE3d::exp(delta_x) * pose;
    prev_cost = cost;
  }

}

Point2d pixel2cam(const Point2d &p, const Mat &K) {
  return Point2d
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}

void bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose) {
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  const int iterations = 10;
  double cost = 0, lastCost = 0;
  double fx = K.at<double>(0, 0);
  double fy = K.at<double>(1, 1);
  double cx = K.at<double>(0, 2);
  double cy = K.at<double>(1, 2);

  for (int iter = 0; iter < iterations; iter++) {
    Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
    Vector6d b = Vector6d::Zero();

    cost = 0;
    // compute cost
    for (int i = 0; i < points_3d.size(); i++) {
      Eigen::Vector3d pc = pose * points_3d[i];
      double inv_z = 1.0 / pc[2];
      double inv_z2 = inv_z * inv_z;
      Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);

      Eigen::Vector2d e = points_2d[i] - proj;

      cost += e.squaredNorm();
      Eigen::Matrix<double, 2, 6> J;
      J << -fx * inv_z,
        0,
        fx * pc[0] * inv_z2,
        fx * pc[0] * pc[1] * inv_z2,
        -fx - fx * pc[0] * pc[0] * inv_z2,
        fx * pc[1] * inv_z,
        0,
        -fy * inv_z,
        fy * pc[1] * inv_z2,
        fy + fy * pc[1] * pc[1] * inv_z2,
        -fy * pc[0] * pc[1] * inv_z2,
        -fy * pc[0] * inv_z;

      H += J.transpose() * J;
      b += -J.transpose() * e;
    }

    Vector6d dx;
    dx = H.ldlt().solve(b);

    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }

    if (iter > 0 && cost >= lastCost) {
      // cost increase, update is not good
      cout << "cost: " << cost << ", last cost: " << lastCost << endl;
      break;
    }

    // update your estimation
    pose = Sophus::SE3d::exp(dx) * pose;
    lastCost = cost;

    cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << endl;
    if (dx.norm() < 1e-6) {
      // converge
      break;
    }
  }

  // cout << "pose by g-n: \n" << pose.matrix() << endl;
}


extern "C" {
#include <umfpack.h>
}
#include <iostream>
    #include <boost/numeric/bindings/traits/ublas_vector.hpp>
    #include <boost/numeric/bindings/traits/ublas_sparse.hpp>
    #include <boost/numeric/bindings/umfpack/umfpack.hpp>
    #include <boost/numeric/ublas/io.hpp>

    namespace ublas = boost::numeric::ublas;
    namespace umf = boost::numeric::bindings::umfpack;

    int main() {

      ublas::compressed_matrix<double, ublas::column_major, 0,
       ublas::unbounded_array<int>, ublas::unbounded_array<double> > A (5,5,12);
      ublas::vector<double> B (5), X (5);

      A(0,0) = 2.; A(0,1) = 3;
      A(1,0) = 3.; A(1,2) = 4.; A(1,4) = 6;
      A(2,1) = -1.; A(2,2) = -3.; A(2,3) = 2.;
      A(3,2) = 1.;
      A(4,1) = 4.; A(4,2) = 2.; A(4,4) = 1.;

      B(0) = 8.; B(1) = 45.; B(2) = -3.; B(3) = 3.; B(4) = 19.;

      umf::symbolic_type<double> Symbolic;
      umf::numeric_type<double> Numeric;

      umf::symbolic (A, Symbolic);
      umf::numeric (A, Symbolic, Numeric);
      umf::solve (A, X, B, Numeric);

      std::cout << X << std::endl;  // output: [5](1,2,3,4,5)
	  return 0;
    }


#ifndef CORE_QUAT_HPP
#define CORE_QUAT_HPP

#include <core/real.hpp>

#include <Eigen/Geometry>

// a unit quaternion
template<class U>
class quaternion : public Eigen::Quaternion<U, Eigen::DontAlign> {
  using base = typename quaternion::Quaternion;
public:

  using base::Quaternion::Quaternion;

  quaternion() {
    base::setIdentity();
  }

  static constexpr U epsilon = std::numeric_limits<U>::epsilon();

  static quaternion exp(const vector<3, U>& omega) {
    const U theta = omega.norm();

    quaternion res;

    if(theta < epsilon) {
      res.vec() = omega;
      res.normalize();
    } else {
      const U half_theta = theta / 2;
      res.w() = std::cos(half_theta);
      res.vec() = (std::sin(half_theta) / theta) * omega;
    }

    return res;
  }

  vector<3, U> log() const {
    quaternion q;

    if(this->w() < 0) {
      q = - *this;
    } else {
      q = *this;
    }

    const U half_theta = std::acos( std::min(q.w(), 1.0) );

    if(half_theta < epsilon) {
      return (q / q.w()).vec();
    } else {
      const U theta = 2 * theta;
      return q.vec() * ( theta / std::sin(half_theta));
    }

  }

};

using quat = quaternion<real>;

#endif


#include "solv_plug.hpp"
#include <Eigen/SparseCholesky>
#include <Eigen/Geometry>
#include <set>
#include "mesh/meshinfo.hpp"
#include "tetelem.hpp"

struct Triplet      // for Eigen interface
{
    int Row;
    int Col;
    real Value;

    int row() const { return Row; }
    int col() const { return Col; }
    real value() const { return Value; }
};

static void ApplyBc(const MeshInfo* mesh,
                    const SolvParams& params,
                    SpMat& mat,
                    SpVec& rhs);
static std::string SolvStatusToStr(const Eigen::ComputationInfo st);

bool SolvPlugin::Solve_(std::string& log_buf)
{
    const auto node_lst = solv_info_->Mesh->NodeLst;
    const auto elem_lst = solv_info_->Mesh->ElemLst;
    const auto node_num = solv_info_->Mesh->NodeNum;
    const auto elem_num = solv_info_->Mesh->ElemNum;

    const auto dof_count = 3 * node_num;

    TetElem tet_el(log_buf);
    Mat K_loc;
    Mat node_mat = Mat::Zero(4, 3);

    const size_t K_lst_size = elem_num * 4 * 4 * 3 * 3;

    std::vector<Triplet> K_lst;
    try
    {
        K_lst.resize(K_lst_size);
    }
    catch (const std::bad_alloc&)
    {
        log_buf = "Cannot allocate memory for stiffness matrix (" +
                  std::to_string(K_lst_size * sizeof(Triplet)) + " bytes required).";
        return false;
    }

    for (uint i_el = 0, i_k_el = 0; i_el < elem_num; ++i_el)
    {
        // build local stiffness matrices

        for (int i_node = 0; i_node < 4; ++i_node)
            for(int i_coord = 0; i_coord < 3; ++i_coord)
                node_mat(i_node, i_coord) = node_lst[3*(elem_lst[4*i_el + i_node] - 1) + i_coord];

        if(!tet_el.Init(i_el, node_mat, solv_params_->Young, solv_params_->Poisn))
            return false;

        K_loc = tet_el.BuildStifMat();
        for (int i_node = 0; i_node < 4; ++i_node)
        {
            const int z1 = 3 * (elem_lst[4*i_el + i_node] - 1);
            for (int j_node = 0; j_node < 4; ++j_node)
            {
                const int z2 = 3 * (elem_lst[4*i_el + j_node] - 1);
                for (int i = 0; i < 3; ++i)
                    for (int j = 0; j < 3; ++j)
                    {
                        K_lst[i_k_el].Row = z1 + i;
                        K_lst[i_k_el].Col = z2 + j;
                        K_lst[i_k_el].Value = K_loc(i_node*3 + i, j_node*3 + j);
                        ++i_k_el;
                    }
            }
        }
    }

    SpMat K(dof_count, dof_count);
    K.setFromTriplets(K_lst.cbegin(), K_lst.cend());
    K_lst.clear();

    stats_buf += "Number of DOF: " + std::to_string(dof_count) + '\n';

    SpVec F(dof_count);

    ApplyBc(solv_info_->Mesh, *solv_params_, K, F);

    Eigen::SimplicialLDLT<SpMat> solver;

    solver.compute(K);
    if(auto st = solver.info(); st != Eigen::ComputationInfo::Success)
    {
        log_buf += "Could not decompose stiffness matrix: " + SolvStatusToStr(st);
        return false;
    }

    Vec sol = solver.solve(F);
    if(auto st = solver.info(); st != Eigen::ComputationInfo::Success)
    {
        log_buf += "Could not solve linear system: " + SolvStatusToStr(st);
        return false;
    }

    const auto siz = sol.size();
    for(int i = 0; i < siz; ++i)
        solv_info_->DispLst[i] = sol(i);

    return true;
}

void ApplyBc(const MeshInfo* mesh,
             const SolvParams& params,
             SpMat& mat,
             SpVec& rhs)
{
    const real big_num = 1.0e+28;
    std::set<MeshInfo::id_t> used_node_lst;

    for(auto i_face = 0u; i_face < mesh->ElemFaceNum; ++i_face)
    {
        for(const auto& bc: params.BcLst)
        {
            if(bc.first != mesh->ElemFaceMarkerLst[i_face])
                continue;

            if(bc.second.BcLst.size() == 1 && bc.second.BcLst.front().Tag[0] == 'p')
            {
                const MeshInfo::id_t nodes[] = { mesh->ElemFaceLst[3*i_face + 0] - 1,
                                                 mesh->ElemFaceLst[3*i_face + 1] - 1,
                                                 mesh->ElemFaceLst[3*i_face + 2] - 1 };
                const Vec3 n1_vec(mesh->NodeLst[3*nodes[0]+0], mesh->NodeLst[3*nodes[0]+1], mesh->NodeLst[3*nodes[0]+2]);
                const Vec3 n2_vec(mesh->NodeLst[3*nodes[1]+0], mesh->NodeLst[3*nodes[1]+1], mesh->NodeLst[3*nodes[1]+2]);
                const Vec3 n3_vec(mesh->NodeLst[3*nodes[2]+0], mesh->NodeLst[3*nodes[2]+1], mesh->NodeLst[3*nodes[2]+2]);
                const Vec3 surf_v = (n2_vec - n1_vec).cross(n3_vec - n1_vec);
                const Vec3 pr_vec = (bc.second.BcLst.front().Value/6) * surf_v;

                for(int i = 0; i < 3; ++i)
                    for(int j = 0; j < 3; ++j)
                    {
                        rhs.coeffRef(3*nodes[i]+j) += pr_vec(j);
                        //used_node_lst.insert(nodes[i]+j);  - do we have to do this???
                    }
            }
            else
            {
                for(int i_n = 0; i_n < 3; ++i_n)
                {
                    const auto node = mesh->ElemFaceLst[3*i_face + i_n] - 1;
                    for(const auto& b: bc.second.BcLst)
                    {
                        for(int i_ax = 0; i_ax < 3; ++i_ax)
                            if(b.Tag[i_ax+1] && !used_node_lst.count(3*node+i_ax))
                            {
                                if(b.Tag[0] == 'u')
                                    mat.coeffRef(3*node+i_ax, 3*node+i_ax) = big_num;
                                rhs.coeffRef(3*node+i_ax) = (b.Tag[0] == 'u' ? big_num : 1.0) * b.Value;
                                used_node_lst.insert(3*node+i_ax);
                            }
                    }
                }
            }

            break;
        }
    }
}

std::string SolvStatusToStr(const Eigen::ComputationInfo st)
{
    switch (st)
    {
    case Eigen::ComputationInfo::Success:       return "success";
    case Eigen::ComputationInfo::InvalidInput:  return "the inputs are invalid, or the algorithm has been improperly called";
    case Eigen::ComputationInfo::NoConvergence: return "iterative procedure did not converge";
    case Eigen::ComputationInfo::NumericalIssue:return "the provided data did not satisfy the prerequisites";
    }

    return "";
}


#ifndef STAN_MATH_FWD_SCAL_FUN_LOG_FALLING_FACTORIAL_HPP
#define STAN_MATH_FWD_SCAL_FUN_LOG_FALLING_FACTORIAL_HPP

#include <stan/math/fwd/meta.hpp>
#include <stan/math/fwd/core.hpp>

#include <stan/math/prim/scal/fun/log_falling_factorial.hpp>
#include <boost/math/special_functions/digamma.hpp>

namespace stan {
namespace math {

template <typename T>
inline fvar<T> log_falling_factorial(const fvar<T>& x, const fvar<T>& n) {
  using boost::math::digamma;

  return fvar<T>(log_falling_factorial(x.val_, n.val_),
                 (digamma(x.val_ + 1) - digamma(x.val_ - n.val_ + 1)) * x.d_
                     + digamma(x.val_ - n.val_ + 1) * n.d_);
}

template <typename T>
inline fvar<T> log_falling_factorial(double x, const fvar<T>& n) {
  using boost::math::digamma;

  return fvar<T>(log_falling_factorial(x, n.val_),
                 digamma(x - n.val_ + 1) * n.d_);
}

template <typename T>
inline fvar<T> log_falling_factorial(const fvar<T>& x, double n) {
  using boost::math::digamma;

  return fvar<T>(log_falling_factorial(x.val_, n),
                 (digamma(x.val_ + 1) - digamma(x.val_ - n + 1)) * x.d_);
}
}  // namespace math
}  // namespace stan
#endif


// Copyright Stephan T. Lavavej, http://nuwen.net .
// Distributed under the Boost Software License, Version 1.0.
// See accompanying file LICENSE_1_0.txt or copy at
// http://boost.org/LICENSE_1_0.txt .

#ifndef PHAM_FIBONACCI_HH
#define PHAM_FIBONACCI_HH

#include "compiler.hh"

#ifdef NUWEN_PLATFORM_MSVC
    #pragma once
#endif

#include "static_assert_private.hh"
#include "typedef.hh"
#include "vector.hh"

#include "external_begin.hh"
    #include <stdexcept>
    #include <vector>
    #include <boost/type_traits.hpp>
#include "external_end.hh"

namespace nuwen {
    template <typename C> vuc_t fibonacci_encode(const C& c);
    template <typename T> std::vector<T> fibonacci_decode(const vuc_t& v);
}

namespace pham {
    template <typename T> const std::vector<T>& fib_seq() {
        PHAM_STATIC_ASSERT((
               boost::is_same<T, nuwen::uc_t >::value
            || boost::is_same<T, nuwen::us_t >::value
            || boost::is_same<T, nuwen::ul_t >::value
            || boost::is_same<T, nuwen::ull_t>::value));

        static std::vector<T> s_fib;

        if (s_fib.empty()) {
            for (T a = 0, b = 1, c = 1; c >= b; a = b, b = c, c = static_cast<T>(a + b)) {
                s_fib.push_back(c);
            }
        }

        return s_fib;
    }

    template <typename T> void fib_encode(const std::vector<T>& fib, nuwen::pack::packed_bits& pb, T n) {
        if (n == 0) {
            throw std::logic_error("LOGIC ERROR: pham::fib_encode() - Cannot encode a zero.");
        }

        nuwen::db_t d(1, 1);

        typename std::vector<T>::const_iterator j = fib.end() - 1;

        while (*j > n) {
            --j;
        }

        while (true) {
            if (n >= *j) {
                n = static_cast<T>(n - *j);
                d.push_front(1);
            } else {
                d.push_front(0);
            }

            if (j == fib.begin()) {
                break;
            } else {
                --j;
            }
        }

        for (nuwen::db_ci_t i = d.begin(); i != d.end(); ++i) {
            pb.push_back(*i);
        }
    }
}

template <typename C> nuwen::vuc_t nuwen::fibonacci_encode(const C& c) {
    typedef typename C::value_type T;

    const std::vector<T>& fib = pham::fib_seq<T>();
    pack::packed_bits pb;

    for (typename C::const_iterator i = c.begin(); i != c.end(); ++i) {
        pham::fib_encode(fib, pb, *i);
    }

    return pb.vuc();
}

template <typename T> std::vector<T> nuwen::fibonacci_decode(const vuc_t& v) {
    const std::vector<T>& fib = pham::fib_seq<T>();

    std::vector<T> ret;
    T n = 0;
    typename std::vector<T>::const_iterator j = fib.begin();

    for (ull_t i = 0; i < v.size() * 8; ++i) {
        if (n != 0 && bit_from_vuc(v, i - 1) == 1 && bit_from_vuc(v, i) == 1) {
            ret.push_back(n);
            n = 0;
            j = fib.begin();
        } else if (j == fib.end()) {
            throw std::runtime_error("RUNTIME ERROR: nuwen::fibonacci_decode() - Codeword contains too many bits.");
        } else if (bit_from_vuc(v, i) == 1) {
            const T t = static_cast<T>(n + *j);

            if (t < n) {
                throw std::runtime_error("RUNTIME ERROR: nuwen::fibonacci_decode() - Codeword overflows T.");
            }

            n = t;
            ++j;
        } else {
            ++j;
        }
    }

    if (n != 0) {
        throw std::runtime_error("RUNTIME ERROR: nuwen::fibonacci_decode() - Padding contains ones.");
    }

    if (j - fib.begin() > 7) {
        throw std::runtime_error("RUNTIME ERROR: nuwen::fibonacci_decode() - Padding contains too many zeros.");
    }

    return ret;
}

#endif // Idempotency


// Algolab BGL Tutorial 2 (Max flow, by taubnert@ethz.ch)
// Flow example demonstrating how to use push_relabel_max_flow using a custom edge adder
// to manage the interior graph properties required for flow algorithms
#include <iostream>

// BGL include
#include <boost/graph/adjacency_list.hpp>

// BGL flow include *NEW*
#include <boost/graph/push_relabel_max_flow.hpp>

// Graph Type with nested interior edge properties for flow algorithms
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor>>>> graph;

typedef traits::vertex_descriptor vertex_desc;
typedef traits::edge_descriptor edge_desc;

using namespace std;

// Custom edge adder class, highly recommended
class edge_adder {
  graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}

  void add_edge(int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const auto e = boost::add_edge(from, to, G).first;
    const auto rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

void make_it_flow() {
  int n, m;
  cin >> n; cin >> m;
  graph G(n + m);
  edge_adder adder(G);

  // Add special vertices source and sink
  const vertex_desc v_source = boost::add_vertex(G);
  const vertex_desc v_sink = boost::add_vertex(G);

  for(int i = 0; i < m; i++) {
    adder.add_edge(v_source, i, 1); // from, to, capacity
    int a, b, c;
    cin >> a; cin >> b; cin >> c;
    if(c == 1) {
      adder.add_edge(i, m + a, 1); // from, to, capacity
    } else if(c == 2) {
      adder.add_edge(i, m + b, 1); // from, to, capacity
    } else {
      adder.add_edge(i, m + a, 1); // from, to, capacity
      adder.add_edge(i, m + b, 1); // from, to, capacity
    }
  }

  int sum_s = 0;
  for(int i = 0; i < n; i++) {
    int si; cin >> si;
    adder.add_edge(m + i, v_sink, si);
    sum_s += si;
  }

  // Calculate flow from source to sink
  // The flow algorithm uses the interior properties (managed in the edge adder)
  // - edge_capacity, edge_reverse (read access),
  // - edge_residual_capacity (read and write access).
  long flow = boost::push_relabel_max_flow(G, v_source, v_sink);

  if(flow == sum_s && flow == m) {
    std::cout << "yes" << "\n";
  } else {
    std::cout << "no" << "\n";
  }
}

int main() {
  std::ios_base::sync_with_stdio(false); // Always!
  int t; cin >> t;
  while(t--) make_it_flow();
  return 0;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MOD_N_INCLUDE
#define MTL_MOD_N_INCLUDE

#include <iostream>
#include <boost/operators.hpp>
#include <cassert>

#include <boost/config/concept_macros.hpp>
#ifdef __GXX_CONCEPTS__
#  include <bits/concepts.h>
#endif

#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/linear_algebra/is_invertible.hpp>
#include <boost/numeric/linear_algebra/inverse.hpp>
#include <boost/numeric/linear_algebra/operators.hpp>
#include <boost/numeric/linear_algebra/concepts.hpp>
#include <boost/numeric/meta_math/is_prime.hpp>

namespace mtl {

template<typename T, T N>
//  where std::Integral<T>
class mod_n_t
  : boost::totally_ordered< mod_n_t<T, N> >,
    boost::arithmetic< mod_n_t<T, N> >
{
    // or BOOST_STATIC_ASSERT((IS_INTEGRAL))

    T                 value;
 public:
    typedef T         value_type;
    typedef mod_n_t   self;

    static T const modulo= N;

    mod_n_t() : value(0) {}

    explicit mod_n_t(T const& v)
    {
	value= v >= 0 ? v%modulo : modulo - -v%modulo;
    }

    // modulo of negative numbers can be bizarre
    // better use constructor for T
    explicit mod_n_t(int v)
    {
	value= v >= 0 ? v%modulo : modulo - -v%modulo;
    }

    // copy constructor
    mod_n_t(const mod_n_t<T, N>& m): value(m.get()) {}

    // assignment
    mod_n_t<T, N>& operator= (const mod_n_t<T, N>& m)
    {
	value= m.value;
	return *this;
    }

    mod_n_t<T, N>& operator= (T const& v)
    {
	value= v >= 0 ? v%modulo : modulo - -v%modulo;
	return *this;
    }

    // conversion from other moduli must be called explicitly
    template<T OtherN>
    mod_n_t<T, N>& convert(const mod_n_t<T, OtherN>& m)
    {
	value= m.value >= 0 ? m.value%modulo : modulo - -m.value%modulo;
	return *this;
    }

    T get() const
    {
	return value;
    }

    bool operator==(self const& y) const
    {
	check(*this); check(y);
	return this->value == y.value;
    }

    bool operator<(self const& y) const
    {
	check(*this); check(y);
	return this->value < y.value;
    }

    self& operator+= (self const& y)
    {
	check(*this); check(y);
	this->value += y.value;
	this->value %= modulo;
	return *this;
    }

    self& operator-= (self const& y)
    {
	check(*this); check(y);
	// add n to avoid negative numbers esp. if T is unsigned
	this->value += modulo;
	this->value -= y.value;
	this->value %= modulo;
	return *this;
    }

    self& operator*= (self const& y)
    {
	check(*this); check(y);
	this->value *= y.value;
	this->value %= modulo;
	return *this;
    }

    self& operator/= (self const& y);

};

template<typename T, T N>
inline void check(const mod_n_t<T, N>& x)
{
    assert(x.get() >= 0 && x.get() < N);
}

template<typename T, T N>
inline std::ostream& operator<< (std::ostream& stream, const mod_n_t<T, N>& a)
{
    check(a);
    return stream << a.get();
}

// Extended Euclidian algorithm in vector notation
    // uu = (u1, u2, u3) := (1, 0, u)
    // vv = (v1, v2, v3) := (0, 1, v)
    // while (u3 % v3 != 0) {
    //   q= u3 / v3
    //   rr= uu - q * vv
    //   uu= vv
    //   vv= rr }
    //
    // with u = N and v = y
    // --> v2 * v mod u == gcd(u, v)
    // --> v2 * y mod N == 1
    // --> x * v2 == x / y
    // v1, u1, and r1 not used
template<typename T, T N>
inline mod_n_t<T, N>& mod_n_t<T, N>::operator/= (const mod_n_t<T, N>& y)
{
    check(*this); check(y);
    if (y.get() == 0) throw "Division by 0";

    // Goes wrong with unsigned b/c some values will be negative (even if the result isn't)
    // Something like remove_sign<T>::type would be cute
    int u= N, v= y.get(), /* u1= 1, */  u2= 0, /* v1= 0, */  v2= 1, q, r, /* r1, */  r2;

    while (u % v != 0) {
	q= u / v;

	r= u % v; /* r1= u1 - q * v1; */ r2= u2 - q * v2;
	u= v; /* u1= v1; */ u2= v2;
	v= r; /* v1= r1; */ v2= r2;
    }

    return *this *= mod_n_t<T, N>(v2);
}

inline int gcd(int u, int v)
{
    int r;
    while ((r= u % v) != 0) {
	u= v; v= r;
    }
    return v;
}

} // namespace mtl

namespace math {

    using mtl::mod_n_t;

    template<typename T, T N>
    struct identity_t< add< mod_n_t<T, N> >, mod_n_t<T, N> >
    {
	typedef mod_n_t<T, N> mod_t;
	mod_t operator() (add<mod_t> const&, mod_t const& v) const
	{
	    return mod_t(0);
	}
    };

    // Reverse definition, a little more efficient if / uses inverse
    template<typename T, T N>
    struct inverse_t< add< mod_n_t<T, N> >, mod_n_t<T, N> >
    {
	typedef mod_n_t<T, N> mod_t;
	mod_t operator() (add<mod_t> const& op, mod_t const& v) const
	{
	    return identity(op, v) - v;
	}
    };

    template<typename T, T N>
    struct is_invertible_t< add< mod_n_t<T, N> >, mod_n_t<T, N> >
    {
	typedef mod_n_t<T, N> mod_t;
	bool operator() (add<mod_t> const&, mod_t const& v) const
	{ return true; }
    };

    template<typename T, T N>
    struct identity_t< mult< mod_n_t<T, N> >, mod_n_t<T, N> >
    {
	typedef mod_n_t<T, N> mod_t;
	mod_t operator() (mult<mod_t> const&, mod_t const& v) const
	{
	    return mod_t(1);
	}
    };

    // Reverse definition, a little more efficient if / uses inverse
    template<typename T, T N>
    struct inverse_t< mult< mod_n_t<T, N> >, mod_n_t<T, N> >
    {
	typedef mod_n_t<T, N> mod_t;
	mod_t operator() (mult<mod_t> const&, mod_t const& v) const
	{
	    return mod_t(1) / v;
	}
    };

    template<typename T, T N>
    struct is_invertible_t< mult< mod_n_t<T, N> >, mod_n_t<T, N> >
    {
	typedef mod_n_t<T, N> mod_t;
	bool operator() (mult<mod_t> const&, mod_t const& v) const
	{
#           ifdef MTL_TRACE_MOD_N_INVERTIBILITY_DISPATCHING
                std::cout << "[slow mod n inversion test] ";
#           endif
	    T value = v.get();
	    return value != 0 && mtl::gcd(N, value) == 1;
	}
    };

# ifdef __GXX_CONCEPTS__

    // With Concept we can provide a faster invertibility test for prime numbers:
    //  only 0 is not invertible and gcd doesn't need to be called

    template<typename T, T N>
        where meta_math::Prime<N>
    struct is_invertible_t< mult< mod_n_t<T, N> >, mod_n_t<T, N> >
    {
	typedef mod_n_t<T, N> mod_t;
	bool operator() (mult<mod_t> const&, mod_t const& v) const
	{
#           ifdef MTL_TRACE_MOD_N_INVERTIBILITY_DISPATCHING
                std::cout << "[fast mod n inversion test] ";
#           endif
 	    return v.get() != 0;
	}
    };

// Concept mapping
// All modulo sets are commutative rings with identity
// but only if N is prime it is also a field
// Due to some mapping nesting trouble we define normally derived maps

template <typename T, T N>
concept_map CommutativeRingWithIdentity< mod_n_t<T, N> >
{
    // Why do we need the typedefs???

    typedef mod_n_t<T, N>& plus_assign_result_type;
    typedef mod_n_t<T, N>  addition_result_type;
    typedef mod_n_t<T, N>  unary_result_type;
    typedef mod_n_t<T, N>& minus_assign_result_type;
    typedef mod_n_t<T, N>  subtraction_result_type;

    typedef mod_n_t<T, N>& mult_assign_result_type;
    typedef mod_n_t<T, N>  mult_result_type;
    typedef mod_n_t<T, N>& divide_assign_result_type;
    typedef mod_n_t<T, N>  division_result_type;

    typedef mod_n_t<T, N>  inverse_result_type;
    typedef mod_n_t<T, N>  identity_result_type;
    typedef bool           is_invertible_result_type;
}

template <typename T, T N>
concept_map MultiplicativePartiallyInvertibleMonoid< mod_n_t<T, N> >
{
    // Why do we need the typedefs???

    typedef mod_n_t<T, N>& mult_assign_result_type;
    typedef mod_n_t<T, N>  mult_result_type;
    typedef mod_n_t<T, N>& divide_assign_result_type;
    typedef mod_n_t<T, N>  division_result_type;

    typedef mod_n_t<T, N>  inverse_result_type;
    typedef mod_n_t<T, N>  identity_result_type;
    typedef bool           is_invertible_result_type;
}

template <typename T, T N>
    where meta_math::Prime<N>
concept_map Field< mod_n_t<T, N> >
{
    // Why do we need the typedefs???

    typedef mod_n_t<T, N>& plus_assign_result_type;
    typedef mod_n_t<T, N>  addition_result_type;
    typedef mod_n_t<T, N>  unary_result_type;
    typedef mod_n_t<T, N>& minus_assign_result_type;
    typedef mod_n_t<T, N>  subtraction_result_type;

    typedef mod_n_t<T, N>& mult_assign_result_type;
    typedef mod_n_t<T, N>  mult_result_type;
    typedef mod_n_t<T, N>& divide_assign_result_type;
    typedef mod_n_t<T, N>  division_result_type;

    typedef mod_n_t<T, N>  inverse_result_type;
    typedef mod_n_t<T, N>  identity_result_type;
    typedef bool           is_invertible_result_type;
}

# endif // __GXX_CONCEPTS__

} // namespace math

#endif // MTL_MOD_N_INCLUDE


#include <mex.h>
#include <math.h>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>
#include <vector>
#include <time.h>
#include <tbb/tbb.h>

using namespace Eigen;
using namespace std;
using namespace tbb;

double *J_index, *JT_index, *J_value, *JT_value, *JTJ_info, *pu, *wu, *bu, *max_iter, *step_size;
double *b, *lambda, *pi, *Jx_in, *p_mod, *dummy;

int J_row;
int J_col;
int row_spy;
int col_spy;
int max_it;
double step;

class spmv_mul_add
{

public:
	spmv_mul_add(double* A_index, double* A_value, double* input, double* addition, double* output, int spy)
	{

		this->A_index = A_index;
		this->A_value = A_value;
		this->input = input;
        this->addition = addition;
		this->output = output;
		this->spy = spy;

	};

	~spmv_mul_add() {};

	void operator() (const blocked_range<int>& r) const
	{
		for (int i = r.begin(); i != r.end(); i++)
		{
			output[i] = 0;

			for (int j = 0; j < spy; j++)
			{
				int index = i * spy + j;

                if(A_index[index] >= 0)
                {
                    int idx = A_index[index];
                    output[i] += A_value[index] * input[idx];
                }
			}

            output[i] += addition[i];
		}
	};

	double* A_index;
	double* A_value;
	double* input;
    double* addition;
	double* output;
	int spy;

};

class jacobi_update
{

public:
	jacobi_update(double* dof, double* update, double* divisor, double alpha)
	{
		this->dof = dof;
		this->update = update;
		this->divisor = divisor;
        this->alpha = alpha;
	};

	~jacobi_update() {};

	void operator() (const blocked_range<int>& r) const
	{
		for (int i = r.begin(); i != r.end(); i++)
		{
            dof[i] = fmax(dof[i] - alpha * update[i] / divisor[i], 0.0);
			//dof[i] = dof[i] - alpha * update[i] / divisor[i];
		}
	};

	double* dof;
	double* update;
	double* divisor;
    double alpha;

};

class set_zero
{

public:
	set_zero(double* data)
	{
		this->data = data;
	};

	~set_zero() {};

	void operator() (const blocked_range<int>& r) const
	{
		for (int i = r.begin(); i != r.end(); i++)
		{
			data[i] = 0;
		}
	};

	double* data;

};

double get_Fischer_Burmeister(double *x, double *pj, int num)
{

    double fb = 0;

    for (int i = 0; i < num; i++)
    {
        double ent = x[i] + pj[i] - sqrt(x[i] * x[i] + pj[i] * pj[i]);
        fb += ent * ent;
    }

    return sqrt(fb);

}

void damped_Jacobi()
{

    task_scheduler_init init(8);

    parallel_for(blocked_range<int>(0, J_col), set_zero(lambda));

    parallel_for(blocked_range<int>(0, J_row), set_zero(dummy));

    parallel_for(blocked_range<int>(0, J_col), spmv_mul_add(JT_index, JT_value, pu, bu, b, col_spy));

    double pre_FB, post_FB;

    parallel_for(blocked_range<int>(0, J_row), spmv_mul_add(J_index, J_value, lambda, dummy, Jx_in, row_spy));

    parallel_for(blocked_range<int>(0, J_col), spmv_mul_add(JT_index, JT_value, Jx_in, b, pi, col_spy));

    post_FB = get_Fischer_Burmeister(lambda, pi, J_col);

    int iteration = -1;

    for(int outer = 0; outer < max_it; outer++)
    {
        iteration = outer;
        pre_FB = post_FB;

        parallel_for(blocked_range<int>(0, J_col), jacobi_update(lambda, pi, wu, step));

        ///////////////////////////////////////////////////////////////////////////////

        parallel_for(blocked_range<int>(0, J_row), spmv_mul_add(J_index, J_value, lambda, dummy, Jx_in, row_spy));

        parallel_for(blocked_range<int>(0, J_col), spmv_mul_add(JT_index, JT_value, Jx_in, b, pi, col_spy));

        ///////////////////////////////////////////////////////////////////////////////

        post_FB = get_Fischer_Burmeister(lambda, pi, J_col);

        if(abs(pre_FB) < 1e-6)
		{
			break;
		}

        if((pre_FB - post_FB) / pre_FB < 1e-3)
        {
            break;
        }

        /*if(abs(pre_FB - post_FB) < 1e-3 * abs(pre_FB))
        {
            break;
        }*/

    }

    //mexPrintf("%d\n", iteration);

    parallel_for(blocked_range<int>(0, J_row), spmv_mul_add(J_index, J_value, lambda, dummy, p_mod, row_spy));

}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{

    mxArray *output_mex;

    J_index = mxGetPr(prhs[0]);
    JT_index = mxGetPr(prhs[1]);
    J_value = mxGetPr(prhs[2]);
    JT_value = mxGetPr(prhs[3]);
    JTJ_info = mxGetPr(prhs[4]);
    pu = mxGetPr(prhs[5]);
    wu = mxGetPr(prhs[6]);
    bu = mxGetPr(prhs[7]);
    max_iter = mxGetPr(prhs[8]);
    step_size = mxGetPr(prhs[9]);

    // JTJ_info : dof_n, max_valence, tri_n, 3 * 2

    J_row = JTJ_info[0];
    J_col = JTJ_info[2];
    row_spy = JTJ_info[1];
    col_spy = JTJ_info[3];
    max_it = max_iter[0];
    step = step_size[0];

    b = (double*)malloc(J_col * sizeof(double));
    lambda = (double*)malloc(J_col * sizeof(double));
    pi = (double*)malloc(J_col * sizeof(double));
    Jx_in = (double*)malloc(J_row * sizeof(double));
    dummy = (double*)malloc(J_row * sizeof(double));

    output_mex = plhs[0] = mxCreateDoubleMatrix(J_row, 1, mxREAL);
    p_mod = mxGetPr(output_mex);

    damped_Jacobi();

    free(b);
    free(lambda);
    free(pi);
    free(Jx_in);
    free(dummy);

    return;

}


#include <stdio.h>
#include <stdlib.h>

#include <Eigen/Core>

#include "../libfovis/absolute_orientation_horn.hpp"

using namespace std;

static int
rand_int_range(int min, int max)
{
  return rand() % (max - min) + min;
}

static double
rand_double_range(double min, double max)
{
  double v = rand() / (double)RAND_MAX;
  return v * (max - min) + min;
}

void
rpy_to_quat (const double rpy[3], double q[4])
{
  double roll = rpy[0], pitch = rpy[1], yaw = rpy[2];

  double halfroll = roll / 2;
  double halfpitch = pitch / 2;
  double halfyaw = yaw / 2;

  double sin_r2 = sin (halfroll);
  double sin_p2 = sin (halfpitch);
  double sin_y2 = sin (halfyaw);

  double cos_r2 = cos (halfroll);
  double cos_p2 = cos (halfpitch);
  double cos_y2 = cos (halfyaw);

  q[0] = cos_r2 * cos_p2 * cos_y2 + sin_r2 * sin_p2 * sin_y2;
  q[1] = sin_r2 * cos_p2 * cos_y2 - cos_r2 * sin_p2 * sin_y2;
  q[2] = cos_r2 * sin_p2 * cos_y2 + sin_r2 * cos_p2 * sin_y2;
  q[3] = cos_r2 * cos_p2 * sin_y2 - sin_r2 * sin_p2 * cos_y2;
}

int main(int argc, char** argv)
{
  int num_trials = 1000;

  for(int trial=0; trial<num_trials; trial++) {

    // generate a random set of points
    int num_points = rand_int_range(5, 1000);

    Eigen::Matrix3Xd points(3, num_points);

    for(int col=0; col<num_points; col++) {
      points(0, col) = rand_double_range(-10, 10);
      points(1, col) = rand_double_range(-10, 10);
      points(2, col) = rand_double_range(-10, 10);
    }

    // generate a random transformation
    Eigen::Vector3d translation(rand_double_range(-10, 10),
        rand_double_range(-10, 10),
        rand_double_range(-10, 10));

    double rpy[3] = {
      rand_double_range(-M_PI, M_PI),
      rand_double_range(-M_PI, M_PI),
      rand_double_range(-M_PI, M_PI) };
    double rot_quat[4];
    rpy_to_quat(rpy, rot_quat);

    Eigen::Quaterniond rotation(rot_quat[0], rot_quat[1], rot_quat[2], rot_quat[3]);

    Eigen::Isometry3d trans;
    trans.setIdentity();
    trans.translate(translation);
    trans.rotate(rotation);

    // apply transformation to original random point set
    Eigen::Matrix3Xd transformed = trans * points;

    Eigen::Isometry3d estimated_transform;
    absolute_orientation_horn(points, transformed, &estimated_transform);

    // reproject points using estimated transformation
    Eigen::Matrix3Xd reprojected = estimated_transform * points;

    // compute reprojection error
    Eigen::Matrix3Xd reproject_err = reprojected - transformed;

    Eigen::Vector3d mean_err = reproject_err.rowwise().sum() / num_points;
    printf("%4d (%4d): mean reprojection error: %6.3f, %6.3f, %6.3f\n", trial, num_points, mean_err(0), mean_err(1), mean_err(2));
    if(fabs(mean_err(0)) > 1e-9 || fabs(mean_err(1)) > 1e-9 || fabs(mean_err(1)) > 1e-9) {
      fprintf(stderr, "FAIL!\n");
      exit(1);
    }
  }

  fprintf(stderr, "OK\n");
  return 0;
}


#include "intersection.hpp"

#include <Eigen/Geometry>

#include <igl/predicates/predicates.h>
#include <ipc/friction/closest_point.hpp>

#include <utils/is_zero.hpp>

namespace ipc::rigid {

bool is_point_along_edge(
    const VectorMax3I& p, const VectorMax3I& e0, const VectorMax3I& e1)
{
    VectorMax3I e = e1 - e0;

    Interval alpha = point_edge_closest_point(p, e0, e1);
    // Check this in case empty intervals are not allowed
    if (!overlap(alpha, Interval(0, 1))) {
        return false;
    }
    Interval valid_alpha = boost::numeric::intersect(alpha, Interval(0, 1));

    // Check the distance to the closest point is small
    VectorMax3I edge_to_point = e0 + valid_alpha * e - p;

    return is_zero(edge_to_point);
}

bool are_edges_intersecting(
    const Vector3I& ea0,
    const Vector3I& ea1,
    const Vector3I& eb0,
    const Vector3I& eb1)
{
    // Check if the origin is withing the 3D difference of edge points
    // WARNING: This is a very converative estimate.
    Vector3I ea_alpha = (ea1 - ea0) * Interval(0, 1) + ea0;
    Vector3I eb_alpha = (eb1 - eb0) * Interval(0, 1) + eb0;
    return is_zero(Vector3I(ea_alpha - eb_alpha));
}

inline bool are_points_on_same_side_of_edge(
    const Vector3I& p1,
    const Vector3I& p2,
    const Vector3I& a,
    const Vector3I& b)
{
    Vector3I cp1 = (b - a).cross(p1 - a);
    Vector3I cp2 = (b - a).cross(p2 - a);
    return cp1.dot(cp2).upper() >= 0;
}

bool is_point_inside_triangle(
    const Vector3I& point,
    const Vector3I& triangle_vertex0,
    const Vector3I& triangle_vertex1,
    const Vector3I& triangle_vertex2)
{
    return are_points_on_same_side_of_edge(
               point, triangle_vertex0, triangle_vertex1, triangle_vertex2)
        && are_points_on_same_side_of_edge(
               point, triangle_vertex1, triangle_vertex0, triangle_vertex2)
        && are_points_on_same_side_of_edge(
               point, triangle_vertex2, triangle_vertex0, triangle_vertex1);
}

} // namespace ipc::rigid


// Copyright (C) 2004 Jeremy Siek <jsiek@cs.indiana.edu>
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/transitive_closure.hpp>
#include <iostream>
using namespace std;

using namespace boost;
typedef adjacency_list<> graph_t;

int main(int argc, char* argv[])
{
    graph_t g(5), g_TC;

    add_edge(0, 2, g);
    add_edge(1, 0, g);
    add_edge(1, 2, g);
    add_edge(1, 4, g);
    add_edge(3, 0, g);
    add_edge(3, 2, g);
    add_edge(4, 2, g);
    add_edge(4, 3, g);

    transitive_closure(g, g_TC);

    cout << "original graph: 0->2, 1->0, 1->2, 1->4, 3->0, 3->2, 4->2, 4->3"
         << endl;
    cout << "transitive closure: ";
    graph_t::edge_iterator i, iend;
    for (boost::tie(i, iend) = edges(g_TC); i != iend; ++i)
    {
        cout << source(*i, g_TC) << "->" << target(*i, g_TC) << " ";
    }
    cout << endl;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestAccumulate
#include <boost/test/unit_test.hpp>

#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/accumulate.hpp>
#include <boost/compute/algorithm/iota.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/counting_iterator.hpp>

#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(sum_int)
{
    int data[] = { 2, 4, 6, 8 };
    boost::compute::vector<int> vector(data, data + 4, queue);
    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(vector.begin(), vector.end(), 0, queue),
        20
    );

    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(vector.begin(), vector.end(), -10, queue),
        10
    );

    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(vector.begin(), vector.end(), 5, queue),
        25
    );
}

BOOST_AUTO_TEST_CASE(product_int)
{
    int data[] = { 2, 4, 6, 8 };
    boost::compute::vector<int> vector(data, data + 4);
    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(
            vector.begin(), vector.end(), 1, boost::compute::multiplies<int>()),
        384
    );

    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(
            vector.begin(), vector.end(), -1, boost::compute::multiplies<int>()),
        -384
    );

    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(
            vector.begin(), vector.end(), 2, boost::compute::multiplies<int>()),
        768
    );
}

BOOST_AUTO_TEST_CASE(quotient_int)
{
    int data[] = { 2, 8, 16 };
    boost::compute::vector<int> vector(data, data + 3, queue);
    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(
            vector.begin(),
            vector.end(),
            1024,
            boost::compute::divides<int>(),
            queue
        ),
        4
    );
}

BOOST_AUTO_TEST_CASE(sum_counting_iterator)
{
    // sum 0 -> 9
    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(
            boost::compute::make_counting_iterator(0),
            boost::compute::make_counting_iterator(10),
            0,
            boost::compute::plus<int>(),
            queue
        ),
        45
    );

    // sum 0 -> 9 + 7
    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(
            boost::compute::make_counting_iterator(0),
            boost::compute::make_counting_iterator(10),
            7,
            boost::compute::plus<int>(),
            queue
        ),
        52
    );

    // sum 15 -> 24
    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(
            boost::compute::make_counting_iterator(15),
            boost::compute::make_counting_iterator(25),
            0,
            boost::compute::plus<int>(),
            queue
        ),
        195
    );

    // sum -5 -> 10
    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(
            boost::compute::make_counting_iterator(-5),
            boost::compute::make_counting_iterator(10),
            0,
            boost::compute::plus<int>(),
            queue
        ),
        30
    );

    // sum -5 -> 10 - 2
    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(
            boost::compute::make_counting_iterator(-5),
            boost::compute::make_counting_iterator(10),
            -2,
            boost::compute::plus<int>(),
            queue
        ),
        28
    );
}

BOOST_AUTO_TEST_CASE(sum_iota)
{
    // size 0
    boost::compute::vector<int> vector(0, context);

    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(vector.begin(), vector.end(), 0, queue),
        0
    );

    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(vector.begin(), vector.end(), 4, queue),
        4
    );

    // size 50
    vector.resize(50);
    boost::compute::iota(vector.begin(), vector.end(), 0, queue);

    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(vector.begin(), vector.end(), 0, queue),
        1225
    );

    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(vector.begin(), vector.end(), 11, queue),
        1236
    );

    // size 1000
    vector.resize(1000);
    boost::compute::iota(vector.begin(), vector.end(), 0, queue);

    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(vector.begin(), vector.end(), 0, queue),
        499500
    );

    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(vector.begin(), vector.end(), -45, queue),
        499455
    );

    // size 1025
    vector.resize(1025);
    boost::compute::iota(vector.begin(), vector.end(), 0, queue);

    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(vector.begin(), vector.end(), 0, queue),
        524800
    );

    BOOST_CHECK_EQUAL(
        boost::compute::accumulate(vector.begin(), vector.end(), 2, queue),
        524802
    );
}

BOOST_AUTO_TEST_CASE(min_and_max)
{
    using boost::compute::int2_;

    int data[] = { 5, 3, 1, 6, 4, 2 };
    boost::compute::vector<int> vector(data, data + 6, queue);

    BOOST_COMPUTE_FUNCTION(int2_, min_and_max, (int2_ accumulator, const int value),
    {
        return (int2)(min(accumulator.x, value), max(accumulator.y, value));
    });

    int2_ result = boost::compute::accumulate(
        vector.begin(), vector.end(), int2_(100, -100), min_and_max, queue
    );
    BOOST_CHECK_EQUAL(result[0], 1);
    BOOST_CHECK_EQUAL(result[1], 6);
}

BOOST_AUTO_TEST_CASE(min_max)
{
    float data[] = { 1.2f, 5.5f, 0.1f, 9.6f, 4.2f, 6.7f, 9.0f, 3.4f };
    boost::compute::vector<float> vec(data, data + 8, queue);

    using ::boost::compute::min;
    using ::boost::compute::max;

    float min_value = boost::compute::accumulate(
        vec.begin(), vec.end(), std::numeric_limits<float>::max(), min<float>(), queue
    );
    BOOST_CHECK_EQUAL(min_value, 0.1f);

    float max_value = boost::compute::accumulate(
        vec.begin(), vec.end(), std::numeric_limits<float>::min(), max<float>(), queue
    );
    BOOST_CHECK_EQUAL(max_value, 9.6f);

    // find min with init less than any value in the array
    min_value = boost::compute::accumulate(
        vec.begin(), vec.end(), -1.f, min<float>(), queue
    );
    BOOST_CHECK_EQUAL(min_value, -1.f);

    // find max with init greater than any value in the array
    max_value = boost::compute::accumulate(
        vec.begin(), vec.end(), 10.f, max<float>(), queue
    );
    BOOST_CHECK_EQUAL(max_value, 10.f);
}

BOOST_AUTO_TEST_SUITE_END()


/*
   Copyright (C) 2015-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

     http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/

// Critical temperature of triangular lattice Ising model

// reference: J. Stephenson, J. of Math. Phys. 11, 420 (1970)

#pragma once

#include <cmath>
#include <stdexcept>
#include <boost/math/differentiation/autodiff.hpp>
#include <standards/newton.hpp>

namespace {

template<typename T>
struct func {
  func(T Ja, T Jb, T Jc) : Ja_(Ja), Jb_(Jb), Jc_(Jc) {}
  auto operator()(T beta) const -> decltype(boost::math::differentiation::make_fvar<T, 1>(beta)) {
    using std::exp;
    auto beta_fvar = boost::math::differentiation::make_fvar<T, 1>(beta);
    auto za = exp(-2 * beta_fvar * Ja_);
    auto zb = exp(-2 * beta_fvar * Jb_);
    auto zc = exp(-2 * beta_fvar * Jc_);
    return za * zb + zb * zc + zc * za - 1;
  }
  T Ja_, Jb_, Jc_;
};

}

namespace ising {
namespace tc {

template<typename T>
inline T triangular(T Ja, T Jb, T Jc) {
  if (Ja * Jb * Jc <= 0) throw(std::invalid_argument("Ja * Jb * Jc should be positive"));
  auto result = standards::newton_1d(func<T>(Ja, Jb, Jc), 1 / (2 * (Ja + Jb + Jc)));
  if (!result.second) throw(std::runtime_error("convergence error"));
  return 1 / result.first;
}

} // end namespace tc
} // end namespace ising


/*****************************************************************************
 *
 * File:    sssp_gold.cpp
 * Author:  Alex Stivala
 * Created: February 2011
 *
 * $Id: sssp_gold.cpp 668 2011-09-08 04:40:08Z astivala $
 *
 * Get single-source shortest path on CPU for verification.
 * Uses Dijkstra's algorithm from the Boost Graph Library.
 * (Based on the example/dijkstra-example.cpp code from Boost Graph Library
 * manual
 *  http://www.boost.org/doc/libs/1_38_0/libs/graph/doc/dijkstra_shortest_paths.html)
 *
 ****************************************************************************/

#include <assert.h>

#include <cutil_inline.h>      /* CUDA SDK */

#include <boost/config.hpp>
#include <iostream>
#include <fstream>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

#include "sssp_gold.h"

using namespace boost;

/*
 * sssp_gold() -  use Dijkstra's algorithim on CPU (using Boost Graph Library)
 *                to solve single-source shortest path problem for checking.
 *
 * Parameters:
 *     adjlist - adjacnecy list (array of (node1,node2,cost) structs)
 *     num_nodes  - number of nodes
 *     num_arcs - number of edges (entries in adjlist)
 *     source - source node
 *     distances (OUT) - array of shortest costs from source to each node
 *     predecessors (OUT) - predecessor node on minimum spanning tree for
 *                          each node.
 * Return value:
 *     time spent running dijkstra_shortest_paths() in milliseconds
 */
double sssp_gold(adjlist_entry_t adjlist[], long num_nodes, long num_arcs,
               long source,
               double distances[], long predecessors[])
{
  unsigned int hTimer;
  typedef adjacency_list < listS, vecS, directedS,
    no_property, property < edge_weight_t, double > > graph_t;
  typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
  typedef graph_traits < graph_t >::edge_descriptor edge_descriptor;
  typedef std::pair<long, long> Edge;

  Edge *edge_array = new Edge[num_arcs];
  double *weights = new double[num_arcs];
  for (long i = 0; i < num_arcs; i++)
  {
    edge_array[i] =  Edge(adjlist[i].from, adjlist[i].to);
    weights[i] = adjlist[i].cost;
  }

  graph_t g(edge_array, edge_array + num_arcs, weights, num_nodes);
  property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);

  std::vector<vertex_descriptor> p(num_vertices(g));
  std::vector<double> d(num_vertices(g));
  vertex_descriptor s = vertex(source, g);

  cutilCheckError( cutCreateTimer(&hTimer) );
  cutilCheckError( cutResetTimer(hTimer) );
  cutilCheckError( cutStartTimer(hTimer) );

  dijkstra_shortest_paths(g, s, predecessor_map(&p[0]).distance_map(&d[0]));

  cutilCheckError( cutStopTimer(hTimer) );
  double runtime = cutGetTimerValue(hTimer);

  graph_traits < graph_t >::vertex_iterator vi, vend;
  long i = 0;
  for (tie(vi, vend) = vertices(g); vi != vend; ++vi)
  {
    assert(i < num_nodes);
    distances[i] = d[*vi];
    predecessors[i] = p[*vi];
    i++;
  }

  delete[] edge_array;
  delete[] weights;

  return runtime;
}


#include <armadillo>
#include <cassert>
#include <cstdio>
#include <iostream>

void alter(arma::vec &vref) {

    vref *= -4.0;

    return;
}

int main() {

    // arma::arma_rng::set_seed_random();
    arma::arma_rng::set_seed(5489);

    size_t lr = 4;
    size_t lc = 3;

    arma::mat Am(lr, lc, arma::fill::randn);

    Am.print("Am");

    arma::vec ones(lr, arma::fill::ones);
    Am.col(2) = ones;

    Am.print("Am");

    arma::vec wrapper(Am.colptr(2), lr, false, false);
    wrapper *= -4.0;

    Am.print("Am");

    alter(wrapper);

    Am.print("Am");

    return 0;

}


#ifndef IMAGEDISTANCE_HPP
#define IMAGEDISTANCE_HPP

#include <Eigen/Core>
#include <array>
#include <functional>

namespace imagedistance
{
    using Histogram = Eigen::VectorXd;

    class HistogramManager
    {
    public:
        HistogramManager(const Eigen::MatrixXd&                                        r_channel,
                         const Eigen::MatrixXd&                                        g_channel,
                         const Eigen::MatrixXd&                                        b_channel,
                         const std::function<Eigen::Vector3d(const Eigen::Vector3d&)>& rgb_to_hsl_converter,
                         const int                                                     num_bins = 30);

        std::array<Histogram, 3> m_rgb_histograms;
        std::array<Histogram, 3> m_hsl_histograms;
        Histogram                m_intensity_histogram;
        std::array<Histogram, 2> m_edge_histograms;

        double m_size;
        double m_aspect;
    };

    // This function returns a 38-dimensional vector, calculated based on Kapoor et al.'s paper [2014]
    Eigen::VectorXd CalcDistances(const HistogramManager& a, const HistogramManager& b);

    double CalcL2Distance(const Histogram& a, const Histogram& b);
    double CalcSmoothedL2Distance(const Histogram& a, const Histogram& b);
    double CalcSymmetricKlDivergenceDistance(const Histogram& a, const Histogram& b);
    double CalcEntropyDistance(const Histogram& a, const Histogram& b);
} // namespace imagedistance

#endif /* IMAGEDISTANCE_HPP */


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/AlgorithmUtils.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <vector>

namespace fluid {
namespace algorithm {

class NMF
{

public:
  // pass iteration number; returns true if able to continue (i.e. not
  // cancelled)
  using ProgressCallback = std::function<bool(index)>;

  static void estimate(const RealMatrixView W, const RealMatrixView H,
                       index idx, RealMatrixView V)
  {
    using namespace Eigen;
    using namespace _impl;

    MatrixXd W1 = asEigen<Matrix>(W).transpose();
    MatrixXd H1 = asEigen<Matrix>(H).transpose();
    MatrixXd result = (W1.col(idx) * H1.row(idx)).transpose();
    V <<= asFluid(result);
  }

  // processFrame computes activations of a dictionary W in a given frame
  void processFrame(const RealVectorView x, const RealMatrixView W0,
                    RealVectorView out, index nIterations = 10,
                    RealVectorView v = RealVectorView(nullptr, 0, 0))
  {
    using namespace Eigen;
    using namespace _impl;
    index    rank = W0.extent(0);
    MatrixXd W = asEigen<Matrix>(W0).transpose();
    VectorXd h =
        MatrixXd::Random(rank, 1) * 0.5 + MatrixXd::Constant(rank, 1, 0.5);
    VectorXd v0 = asEigen<Matrix>(x);
    W = W.array().max(epsilon).matrix();
    h = h.array().max(epsilon).matrix();
    v0 = v0.array().max(epsilon).matrix();

    MatrixXd WT = W.transpose();
    W.colwise().normalize();
    VectorXd ones = VectorXd::Ones(x.extent(0));
    while (nIterations--)
    {
      ArrayXd  v1 = (W * h).array().max(epsilon);
      ArrayXXd hNum = (WT * (v0.array() / v1).matrix()).array();
      ArrayXXd hDen = (WT * ones).array();
      h = (h.array() * hNum / hDen.max(epsilon)).matrix();
      // VectorXd r = W * h;
      // double divergence = (v.cwiseProduct(v.cwiseQuotient(r)) - v + r).sum();
      // std::cout<<"Divergence "<<divergence<<std::endl;
    }
    out <<= asFluid(h);
    if (v.extent(0) > 0)
    {
      ArrayXd v2 = (W * h).array();
      v <<= asFluid(v2);
    }
  }

  void process(const RealMatrixView X, RealMatrixView W1, RealMatrixView H1,
               RealMatrixView V1, index rank, index nIterations, bool updateW,
               bool           updateH = false,
               RealMatrixView W0 = RealMatrixView(nullptr, 0, 0, 0),
               RealMatrixView H0 = RealMatrixView(nullptr, 0, 0, 0))
  {
    using namespace Eigen;
    using namespace _impl;
    index    nFrames = X.extent(0);
    index    nBins = X.extent(1);
    MatrixXd W;
    if (W0.extent(0) == 0 && W0.extent(1) == 0)
    {
      W = MatrixXd::Random(nBins, rank) * 0.5 +
          MatrixXd::Constant(nBins, rank, 0.5);
    }
    else
    {
      assert(W0.extent(0) == rank);
      assert(W0.extent(1) == nBins);
      W = asEigen<Matrix>(W0).transpose();
    }
    MatrixXd H;
    if (H0.extent(0) == 0 && H0.extent(1) == 0)
    {
      H = MatrixXd::Random(rank, nFrames) * 0.5 +
          MatrixXd::Constant(rank, nFrames, 0.5);
    }
    else
    {
      assert(H0.extent(0) == nFrames);
      assert(H0.extent(1) == rank);
      H = asEigen<Matrix>(H0).transpose();
    }
    MatrixXd V = asEigen<Matrix>(X).transpose();
    multiplicativeUpdates(V, W, H, nIterations, updateW, updateH);
    MatrixXd VT = V.transpose();
    MatrixXd WT = W.transpose();
    MatrixXd HT = H.transpose();

    V1 <<= asFluid(VT);
    W1 <<= asFluid(WT);
    H1 <<= asFluid(HT);
  }

  void addProgressCallback(ProgressCallback&& callback)
  {
    mCallbacks.emplace_back(std::move(callback));
  }

private:
  using MatrixXd = Eigen::MatrixXd;

  void multiplicativeUpdates(Eigen::Ref<MatrixXd> V, Eigen::Ref<MatrixXd> W,
                             Eigen::Ref<MatrixXd> H, index nIterations,
                             bool updateW, bool updateH)
  {
    using namespace Eigen;
    MatrixXd ones = MatrixXd::Ones(V.rows(), V.cols());
    H = H.array().max(epsilon).matrix();
    W = W.array().max(epsilon).matrix();
    W.colwise().normalize();
    H.rowwise().normalize();
    for (auto i = 0; i < nIterations; ++i)
    {
      if (updateW)
      {
        ArrayXXd V1 = (W * H).array().max(epsilon);
        ArrayXXd wnum = ((V.array() / V1).matrix() * H.transpose()).array();
        ArrayXXd wden = (ones * H.transpose()).array();
        W = (W.array() * wnum / wden.max(epsilon)).matrix();
        if (W.maxCoeff() > epsilon) W.colwise().normalize();
        assert(W.allFinite());
      }
      ArrayXXd V2 = (W * H).array().max(epsilon);
      if (updateH)
      {
        ArrayXXd hnum = (W.transpose() * (V.array() / V2).matrix()).array();
        ArrayXXd hden = (W.transpose() * ones).array();
        H = (H.array() * hnum / hden.max(epsilon)).matrix();
        assert(H.allFinite());
      }
      MatrixXd R = W * H;
      R = R.cwiseMax(epsilon);
      for (auto& cb : mCallbacks)
        if (!cb(i + 1)) return;
      // double divergence = (V.cwiseProduct(V.cwiseQuotient(R)) - V + R).sum();
      // divergenceCurve.push_back(divergence);
      // divergenceCurve(mIterations);
      // std::cout << "Divergence " << divergence << "\n";
    }
    V = W * H;
  }

  std::vector<ProgressCallback> mCallbacks;
};
} // namespace algorithm
} // namespace fluid


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE SinogramCreatorTest
#include <boost/test/unit_test.hpp>

#include "SinogramCreatorTools.h"

BOOST_AUTO_TEST_SUITE(FirstSuite)

BOOST_AUTO_TEST_CASE(roundToNearesMultiplicity_test)
{
  BOOST_REQUIRE_EQUAL(SinogramCreatorTools::roundToNearesMultiplicity(0.0f, 1.f), 0u);
  BOOST_REQUIRE_EQUAL(SinogramCreatorTools::roundToNearesMultiplicity(0.4f, 1.f), 0u);
  BOOST_REQUIRE_EQUAL(SinogramCreatorTools::roundToNearesMultiplicity(0.5f, 1.f), 1u);
  BOOST_REQUIRE_EQUAL(SinogramCreatorTools::roundToNearesMultiplicity(30.f, 1.f), 30u);
  BOOST_REQUIRE_EQUAL(SinogramCreatorTools::roundToNearesMultiplicity(0.00f, 0.01f), 0u);
  BOOST_REQUIRE_EQUAL(SinogramCreatorTools::roundToNearesMultiplicity(0.01f, 0.01f), 1u);
  BOOST_REQUIRE_EQUAL(SinogramCreatorTools::roundToNearesMultiplicity(0.02f, 0.01f), 2u);
}

BOOST_AUTO_TEST_CASE(test_angle_middle)
{
  for (int i = 0; i < 180; i++) {
    float x = std::cos(i / M_PI);
    float y = std::sin(i / M_PI);
    BOOST_REQUIRE_EQUAL(SinogramCreatorTools::calculateAngle(x, y, -x, -y), i);
  }
}

BOOST_AUTO_TEST_CASE(test_angle)
{
  BOOST_REQUIRE_EQUAL(SinogramCreatorTools::calculateAngle(0.f, 0.f, 0.f, 0.f), 0);
  BOOST_REQUIRE_EQUAL(SinogramCreatorTools::calculateAngle(0.f, 0.f, -1.f, 0.f), 0);
  BOOST_REQUIRE_EQUAL(SinogramCreatorTools::calculateAngle(-1.f, 0.f, 1.f, 0.f), 0);
  BOOST_REQUIRE_EQUAL(SinogramCreatorTools::calculateAngle(1.f, 0.f, -1.f, 0.f), 0);
  BOOST_REQUIRE_EQUAL(SinogramCreatorTools::calculateAngle(0.f, 1.f, 0.f, -1.f), 90);
  BOOST_REQUIRE_EQUAL(SinogramCreatorTools::calculateAngle(0.f, -1.f, 0.f, 1.f), 90);
}

BOOST_AUTO_TEST_CASE(test_distance)
{
  const float EPSILON = 0.00001f;
  BOOST_REQUIRE_CLOSE(SinogramCreatorTools::calculateDistance(0.f, 0.f, 0.f, 0.f), 0, EPSILON);
  BOOST_REQUIRE_CLOSE(SinogramCreatorTools::calculateDistance(0.f, 0.f, -1.f, 0.f), -0, EPSILON);
  BOOST_REQUIRE_CLOSE(SinogramCreatorTools::calculateDistance(0.f, 0.f, 1.f, 0.f), 0, EPSILON);
  BOOST_REQUIRE_CLOSE(SinogramCreatorTools::calculateDistance(1.f, 1.f, -1.f, 1.f), 1, EPSILON);
  BOOST_REQUIRE_CLOSE(SinogramCreatorTools::calculateDistance(-1.f, 1.f, 1.f, 1.f), 1, EPSILON);
  BOOST_REQUIRE_CLOSE(SinogramCreatorTools::calculateDistance(0.f, 1.f, 1.f, 0.f), std::sqrt(0.5f), EPSILON);
  BOOST_REQUIRE_CLOSE(SinogramCreatorTools::calculateDistance(1.f, 0.f, 0.f, 1.f), std::sqrt(0.5f), EPSILON);
  BOOST_REQUIRE_CLOSE(SinogramCreatorTools::calculateDistance(-1.f, 0.f, 1.f, 0.f), 0, EPSILON);
  BOOST_REQUIRE_CLOSE(SinogramCreatorTools::calculateDistance(1.f, 0.f, -1.f, 0.f), 0, EPSILON);
  BOOST_REQUIRE_CLOSE(SinogramCreatorTools::calculateDistance(0.f, 1.f, 0.f, -1.f), 0, EPSILON);
  BOOST_REQUIRE_CLOSE(SinogramCreatorTools::calculateDistance(0.f, -1.f, 0.f, 1.f), 0, EPSILON);
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * @file upwindquadrature_main.cc
 * @brief NPDE homework template main
 * @author Philippe Peter
 * @date June 2020
 * @copyright Developed at SAM, ETH Zurich
 */
#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/fe/fe.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <cmath>
#include <memory>

#include "../../../lecturecodes/ConvectionDiffusion/convection_emp.h"
#include "upwindquadrature.h"

int main() {
  // PARAMETERS
  // mesh specification (number of cells in both sides of the tensor-product
  // triangular mesh)
  int M = 49;

  // coefficient functions:
  // Dirichlet functor
  const auto g = [](const Eigen::Vector2d &x) {
    return x(1) == 0 ? 0.5 - std::abs(x(0) - 0.5) : 0.0;
  };
  lf::mesh::utils::MeshFunctionGlobal mf_g{g};

  // velocity field
  const auto v = [](const Eigen::Vector2d &x) {
    return (Eigen::Vector2d() << -x(1), x(0)).finished();
  };

  // diffusion coefficient
  const double eps = 1e-4;
  lf::mesh::utils::MeshFunctionConstant mf_eps{eps};

  // MESH CONSTRUCTION
  // construct a triangular tensor product mesh on the unit square
  std::unique_ptr<lf::mesh::MeshFactory> mesh_factory_ptr =
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::mesh::utils::TPTriagMeshBuilder builder(std::move(mesh_factory_ptr));
  builder.setBottomLeftCorner(Eigen::Vector2d{0.0, 0.0})
      .setTopRightCorner(Eigen::Vector2d{1.0, 1.0})
      .setNumXCells(M)
      .setNumYCells(M);
  std::shared_ptr<lf::mesh::Mesh> mesh_p = builder.Build();

  // DOF HANDLER & FINITE ELEMENT SPACE
  // Construct dofhanlder for linear finite elements on the mesh.
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  const lf::assemble::DofHandler &dofh{fe_space->LocGlobMap()};

  // PREPARING DATA TO IMPOSE DIRICHLET CONDITIONS
  // Obtain specification for shape functions on edges
  const lf::fe::ScalarReferenceFiniteElement<double> *rsf_edge_p =
      fe_space->ShapeFunctionLayout(lf::base::RefEl::kSegment());

  // Create a dataset of boolean flags indicating edges on the boundary of the
  // mesh
  auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(mesh_p, 1)};

  // Fetch flags and values for degrees of freedom located on Dirichlet
  // boundary.
  auto ess_bdc_flags_values{
      lf::fe::InitEssentialConditionFromFunction(*fe_space, bd_flags, mf_g)};

  //============================================================================
  // SOLVE LAPLACIAN WITH NON-HOMOGENEOUS DIRICHLET BC (STANDARD: UNSTABLE)
  //============================================================================
  // Matrix in triplet format holding Galerkin matrix, zero initially.
  lf::assemble::COOMatrix<double> A(dofh.NumDofs(), dofh.NumDofs());

  // ASSEMBLE GALERKIN MATRIX
  // First the part corresponding to the laplacian
  lf::uscalfe::ReactionDiffusionElementMatrixProvider laplacian_provider(
      fe_space, mf_eps, lf::mesh::utils::MeshFunctionConstant(0.0));
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, laplacian_provider, A);

  // Next part corresponding to the convection term:
  ConvectionDiffusion::ConvectionElementMatrixProvider convection_provider(v);
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, convection_provider, A);

  // RIGHT-HAND SIDE VECTOR
  Eigen::VectorXd phi(dofh.NumDofs());
  phi.setZero();

  // IMPOSE DIRICHLET CONDITIONS:
  // Eliminate Dirichlet dofs from linear system
  lf::assemble::FixFlaggedSolutionComponents<double>(
      [&ess_bdc_flags_values](lf::uscalfe::glb_idx_t gdof_idx) {
        return ess_bdc_flags_values[gdof_idx];
      },
      A, phi);

  // SOLVE LINEAR SYSTEM
  Eigen::SparseMatrix A_crs = A.makeSparse();
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(A_crs);
  Eigen::VectorXd sol_vec = solver.solve(phi);

  // OUTPUT RESULTS TO VTK FILe
  // construct mesh function representing finite element solution
  lf::fe::MeshFunctionFE mf_sol(fe_space, sol_vec);
  // construct vtk writer
  lf::io::VtkWriter vtk_writer(
      mesh_p, CURRENT_BINARY_DIR "/upwind_quadrature_solution_unstable.vtk");
  // output data
  vtk_writer.WritePointData("upwind_quadrature_solution_unstable", mf_sol);

  //============================================================================
  // SOLVE LAPLACIAN WITH NON-HOMOGENEOUS DIRICHLET BC (UPWIND: STABLE)
  //============================================================================
  // Matrix in triplet format holding Galerkin matrix, zero initially.
  lf::assemble::COOMatrix<double> A_stable(dofh.NumDofs(), dofh.NumDofs());

  // ASSEMBLE GALERKIN MATRIX
  // First the part corresponding to the laplacian, computed using standard
  // Galerkin approach
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, laplacian_provider,
                                      A_stable);

  // Next part corresponding to the convection term, computed using upwind
  // quadrature:
  UpwindQuadrature::UpwindConvectionElementMatrixProvider
      convection_provider_stable(v, UpwindQuadrature::initializeMasses(mesh_p));
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, convection_provider_stable,
                                      A_stable);

  // RIGHT-HAND SIDE VECTOR
  Eigen::VectorXd phi_stable(dofh.NumDofs());
  phi_stable.setZero();

  // IMPOSE DIRICHLET CONDITIONS:
  // Eliminate Dirichlet dofs from linear system
  lf::assemble::FixFlaggedSolutionComponents<double>(
      [&ess_bdc_flags_values](lf::uscalfe::glb_idx_t gdof_idx) {
        return ess_bdc_flags_values[gdof_idx];
      },
      A_stable, phi_stable);

  // SOLVE LINEAR SYSTEM
  Eigen::SparseMatrix A_stable_crs = A_stable.makeSparse();
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver_stable;
  solver_stable.compute(A_stable_crs);
  Eigen::VectorXd sol_vec_stable = solver_stable.solve(phi_stable);

  // OUTPUT RESULTS TO VTK FILe
  // construct mesh function representing finite element solution
  lf::fe::MeshFunctionFE mf_sol_stable(fe_space, sol_vec_stable);
  // construct vtk writer
  lf::io::VtkWriter vtk_writer_stable(
      mesh_p, CURRENT_BINARY_DIR "/upwind_quadrature_solution_stable.vtk");
  // output data
  vtk_writer_stable.WritePointData("upwind_quadrature_solution_stable",
                                   mf_sol_stable);

  return 0;
}


// Local private PANACEA includes
#include "matrix_eigen.hpp"

// Third party includes
#include <Eigen/Dense>

// Standard includes
#include <cassert>
#include <cmath>
#include <iostream>
#include <memory>

namespace panacea {

MatrixEigen::MatrixEigen() { matrix_ = std::make_unique<Eigen::MatrixXd>(); }

const MatrixType MatrixEigen::type() const { return MatrixType::Eigen; }

double MatrixEigen::getDeterminant() const {
  assert(matrix_->rows() > 0 || matrix_->cols() > 0);
  return matrix_->determinant();
}

bool MatrixEigen::isZero(const double threshold) const noexcept {
  assert(threshold > 0.0);
  // Because it is symmetric only need to check one half
  for (int i = 0; i < matrix_->rows(); ++i) {
    for (int j = i; j < matrix_->cols(); ++j) {
      if (std::fabs(matrix_->operator()(i, j)) > threshold)
        return false;
    }
  }
  return true;
}

void MatrixEigen::resize(const int rows, const int cols) {
  assert(rows >= 0);
  assert(cols >= 0);
  matrix_->resize(rows, cols);
}

MatrixEigen &MatrixEigen::operator=(const MatrixEigen &mat) {
  this->resize(mat.rows(), mat.cols());
  for (int row = 0; row < mat.rows(); ++row) {
    for (int col = 0; col < mat.cols(); ++col) {
      this->operator()(row, col) = mat(row, col);
    }
  }
  return *this;
}

MatrixEigen &MatrixEigen::operator=(const Matrix &mat) {
  this->resize(mat.rows(), mat.cols());
  for (int row = 0; row < mat.rows(); ++row) {
    for (int col = 0; col < mat.cols(); ++col) {
      this->operator()(row, col) = mat(row, col);
    }
  }
  return *this;
}

double &MatrixEigen::operator()(const int row, const int col) {
  assert(row >= 0);
  assert(col >= 0);
  assert(row < matrix_->rows());
  assert(col < matrix_->cols());
  return (*matrix_)(row, col);
}

double MatrixEigen::operator()(const int row, const int col) const {
  assert(row >= 0);
  assert(col >= 0);
  assert(row < matrix_->rows());
  assert(col < matrix_->cols());
  return (*matrix_)(row, col);
}

void MatrixEigen::makeIdentity() {
  for (int row = 0; row < matrix_->rows(); ++row) {
    this->operator()(row, row) = 1.0;
    for (int col = row + 1; col < matrix_->cols(); ++col) {
      this->operator()(row, col) = 0.0;
      this->operator()(col, row) = 0.0;
    }
  }
}

void MatrixEigen::setZero() { matrix_->setZero(); }

int MatrixEigen::rows() const { return matrix_->rows(); }

int MatrixEigen::cols() const { return matrix_->cols(); }

void MatrixEigen::print() const { std::cout << *matrix_ << std::endl; }

Eigen::MatrixXd MatrixEigen::pseudoInverse() const {
  return matrix_->completeOrthogonalDecomposition().pseudoInverse();
}

void pseudoInverse(Matrix &return_mat, const MatrixEigen &mat) {
  assert(return_mat.rows() == mat.rows());
  assert(return_mat.cols() == mat.cols());

  auto temp_mat = mat.pseudoInverse();
  for (int row = 0; row < temp_mat.rows(); ++row) {
    for (int col = 0; col < temp_mat.cols(); ++col) {
      return_mat(row, col) = temp_mat(row, col);
    }
  }
}
} // namespace panacea