/* statistic_tests.cpp file
 *
 * Copyright Jens Maurer 2000, 2002
 * 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)
 *
 * $Id$
 *
 * Revision history
 */

#include <iostream>
#include <iomanip>
#include <string>
#include <functional>
#include <vector>
#include <set>
#include <algorithm>

#include <boost/cstdint.hpp>
#include <boost/random.hpp>

#include <boost/math/special_functions/gamma.hpp>

#include <boost/math/distributions/uniform.hpp>
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/triangular.hpp>
#include <boost/math/distributions/cauchy.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/distributions/exponential.hpp>
#include <boost/math/distributions/lognormal.hpp>

#include "statistic_tests.hpp"
#include "integrate.hpp"

class test_environment;

class test_base
{
protected:
  explicit test_base(test_environment & env) : environment(env) { }
  void check_(double val) const;
private:
  test_environment & environment;
};

class equidistribution_test : test_base
{
public:
  equidistribution_test(test_environment & env, unsigned int classes,
                        unsigned int high_classes)
    : test_base(env), classes(classes),
      test_distrib_chi_square(boost::math::chi_squared(classes-1), high_classes)
  { }

  template<class RNG>
  void run(RNG & rng, int n1, int n2)
  {
    using namespace boost;
    std::cout << "equidistribution: " << std::flush;
    equidistribution_experiment equi(classes);
    variate_generator<RNG&, uniform_smallint<> > uint_linear(rng, uniform_smallint<>(0, classes-1));
    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(equi, uint_linear, n1), n2));
    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(equi, uint_linear, n1), 2*n2));

    std::cout << "  2D: " << std::flush;
    equidistribution_2d_experiment equi_2d(classes);
    unsigned int root = static_cast<unsigned int>(std::sqrt(double(classes)));
    assert(root * root == classes);
    variate_generator<RNG&, uniform_smallint<> > uint_square(rng, uniform_smallint<>(0, root-1));
    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(equi_2d, uint_square, n1), n2));
    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(equi_2d, uint_square, n1), 2*n2));
    std::cout << std::endl;
  }
private:
  unsigned int classes;
  distribution_experiment test_distrib_chi_square;
};

class ks_distribution_test : test_base
{
public:
  ks_distribution_test(test_environment & env, unsigned int classes)
    : test_base(env),
      test_distrib_chi_square(kolmogorov_smirnov_probability(5000),
                              classes)
  { }

  template<class RNG>
  void run(RNG & rng, int n1, int n2)
  {
    boost::math::uniform ud(static_cast<double>((rng.min)()), static_cast<double>((rng.max)()));
    run(rng, ud, n1, n2);
  }
  template<class RNG, class Dist>
  void run(RNG & rng, const Dist& dist, int n1, int n2)
  {
    using namespace boost;
    std::cout << "KS: " << std::flush;
    kolmogorov_experiment ks(n1);
    check_(run_experiment(test_distrib_chi_square,
                         ks_experiment_generator(ks, rng, dist), n2));
    check_(run_experiment(test_distrib_chi_square,
                         ks_experiment_generator(ks, rng, dist), 2*n2));
    std::cout << std::endl;
  }
private:
  distribution_experiment test_distrib_chi_square;
};

class runs_test : test_base
{
public:
  runs_test(test_environment & env, unsigned int classes,
            unsigned int high_classes)
    : test_base(env), classes(classes),
      test_distrib_chi_square(boost::math::chi_squared(classes-1), high_classes)
  { }

  template<class RNG>
  void run(RNG & rng, int n1, int n2)
  {
    using namespace boost;
    std::cout << "runs: up: " << std::flush;
    runs_experiment<true> r_up(classes);

    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(r_up, rng, n1), n2));
    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(r_up, rng, n1), 2*n2));

    std::cout << "  down: " << std::flush;
    runs_experiment<false> r_down(classes);
    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(r_down, rng, n1), n2));
    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(r_down, rng, n1), 2*n2));

    std::cout << std::endl;
  }
private:
  unsigned int classes;
  distribution_experiment test_distrib_chi_square;
};

class gap_test : test_base
{
public:
  gap_test(test_environment & env, unsigned int classes,
            unsigned int high_classes)
    : test_base(env), classes(classes),
      test_distrib_chi_square(boost::math::chi_squared(classes-1), high_classes)
  { }

  template<class RNG>
  void run(RNG & rng, int n1, int n2)
  {
    boost::math::uniform ud(
      static_cast<double>((rng.min)()),
      static_cast<double>((rng.max)()) +
        (std::numeric_limits<typename RNG::result_type>::is_integer? 0.0 : 1.0));
    run(rng, ud, n1, n2);
  }

  template<class RNG, class Dist>
  void run(RNG & rng, const Dist& dist, int n1, int n2)
  {
    using namespace boost;
    std::cout << "gaps: " << std::flush;
    gap_experiment gap(classes, dist, 0.2, 0.8);

    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(gap, rng, n1), n2));
    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(gap, rng, n1), 2*n2));

    std::cout << std::endl;
  }
private:
  unsigned int classes;
  distribution_experiment test_distrib_chi_square;
};

class poker_test : test_base
{
public:
  poker_test(test_environment & env, unsigned int classes,
             unsigned int high_classes)
    : test_base(env), classes(classes),
      test_distrib_chi_square(boost::math::chi_squared(classes-1), high_classes)
  { }

  template<class RNG>
  void run(RNG & rng, int n1, int n2)
  {
    using namespace boost;
    std::cout << "poker: " << std::flush;
    poker_experiment poker(8, classes);
    variate_generator<RNG&, uniform_smallint<> > usmall(rng, uniform_smallint<>(0, 7));
    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(poker, usmall, n1), n2));
    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(poker, usmall, n1), 2*n2));
    std::cout << std::endl;
  }
private:
  unsigned int classes;
  distribution_experiment test_distrib_chi_square;
};

class coupon_collector_test : test_base
{
public:
  coupon_collector_test(test_environment & env, unsigned int classes,
                        unsigned int high_classes)
    : test_base(env), classes(classes),
      test_distrib_chi_square(boost::math::chi_squared(classes-1), high_classes)
  { }

  template<class RNG>
  void run(RNG & rng, int n1, int n2)
  {
    using namespace boost;
    std::cout << "coupon collector: " << std::flush;
    coupon_collector_experiment coupon(5, classes);

    variate_generator<RNG&, uniform_smallint<> > usmall(rng, uniform_smallint<>(0, 4));
    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(coupon, usmall, n1), n2));
    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(coupon, usmall, n1), 2*n2));
    std::cout << std::endl;
  }
private:
  unsigned int classes;
  distribution_experiment test_distrib_chi_square;
};

class permutation_test : test_base
{
public:
  permutation_test(test_environment & env, unsigned int classes,
                   unsigned int high_classes)
    : test_base(env), classes(classes),
      test_distrib_chi_square(boost::math::chi_squared(fac<int>(classes)-1),
                              high_classes)
  { }

  template<class RNG>
  void run(RNG & rng, int n1, int n2)
  {
    using namespace boost;
    std::cout << "permutation: " << std::flush;
    permutation_experiment perm(classes);

    // generator_reference_t<RNG> gen_ref(rng);
    RNG& gen_ref(rng);
    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(perm, gen_ref, n1), n2));
    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(perm, gen_ref, n1), 2*n2));
    std::cout << std::endl;
  }
private:
  unsigned int classes;
  distribution_experiment test_distrib_chi_square;
};

class maximum_test : test_base
{
public:
  maximum_test(test_environment & env, unsigned int high_classes)
    : test_base(env),
      test_distrib_chi_square(kolmogorov_smirnov_probability(1000),
                              high_classes)
  { }

  template<class RNG>
  void run(RNG & rng, int n1, int n2)
  {
    using namespace boost;
    std::cout << "maximum-of-t: " << std::flush;
    maximum_experiment<RNG> mx(rng, n1, 5);
    check_(run_experiment(test_distrib_chi_square, mx, n2));
    check_(run_experiment(test_distrib_chi_square, mx, 2*n2));
    std::cout << std::endl;
  }
private:
  distribution_experiment test_distrib_chi_square;
};

class birthday_test : test_base
{
public:
  birthday_test(test_environment & env, unsigned int high_classes)
    : test_base(env),
      test_distrib_chi_square(boost::math::chi_squared(4-1), high_classes)
  { }

  template<class RNG>
  void run(RNG & rng, int n1, int n2)
  {
    using namespace boost;
    std::cout << "birthday spacing: " << std::flush;
    boost::variate_generator<RNG&, boost::uniform_int<> > uni(rng, boost::uniform_int<>(0, (1<<25)-1));
    birthday_spacing_experiment bsp(4, 512, (1<<25));
    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(bsp, uni, n1), n2));
    check_(run_experiment(test_distrib_chi_square,
                         experiment_generator(bsp, uni, n1), 2*n2));
    std::cout << std::endl;
  }
private:
  distribution_experiment test_distrib_chi_square;
};

#ifdef BOOST_MSVC
#pragma warning(disable:4355)
#endif

class test_environment
{
public:
  static const int classes = 20;
  explicit test_environment(double confid)
    : confidence(confid),
    confidence_chi_square_quantil(quantile(boost::math::chi_squared(classes-1), confidence)),
      test_distrib_chi_square6(boost::math::chi_squared(7-1), classes),
      ksdist_test(*this, classes),
      equi_test(*this, 100, classes),
      rns_test(*this, 7, classes),
      gp_test(*this, 7, classes),
      pk_test(*this, 5, classes),
      cpn_test(*this, 15, classes),
      perm_test(*this, 5, classes),
      max_test(*this, classes),
      bday_test(*this, classes)
  {
    std::cout << "Confidence level: " << confid
              << "; 1-alpha = " << (1-confid)
              << "; chi_square(" << (classes-1)
              << ", " << confidence_chi_square_quantil
              << ") = "
              << cdf(boost::math::chi_squared(classes-1), confidence_chi_square_quantil)
              << std::endl;
  }

  bool check_confidence(double val, double chi_square_conf) const
  {
    std::cout << val;
    bool result = (val <= chi_square_conf);
    if(!result) {
      std::cout << "* [";
      double prob = (val > 10*chi_square_conf ? 1 :
                     cdf(boost::math::chi_squared(classes-1), val));
      std::cout << (1-prob) << "]";
    }
    std::cout << " " << std::flush;
    return result;
  }

  bool check_(double chi_square_value) const
  {
    return check_confidence(chi_square_value, confidence_chi_square_quantil);
  }

  template<class RNG>
  void run_test(const std::string & name)
  {
    using namespace boost;

    std::cout << "Running tests on " << name << std::endl;

    RNG rng(1234567);

    ksdist_test.run(rng, 5000, 250);
    equi_test.run(rng, 5000, 250);
    rns_test.run(rng, 100000, 250);
    gp_test.run(rng, 10000, 250);
    pk_test.run(rng, 5000, 250);
    cpn_test.run(rng, 500, 250);
    perm_test.run(rng, 1200, 250);
    max_test.run(rng, 1000, 250);
    bday_test.run(rng, 1000, 150);

    std::cout << std::endl;
  }

  template<class RNG, class Dist, class ExpectedDist>
  void run_test(const std::string & name, const Dist & dist, const ExpectedDist & expected_dist)
  {
    using namespace boost;

    std::cout << "Running tests on " << name << std::endl;

    RNG rng;
    variate_generator<RNG&, Dist> vgen(rng, dist);

    ksdist_test.run(vgen, expected_dist, 5000, 250);
    rns_test.run(vgen, 100000, 250);
    gp_test.run(vgen, expected_dist, 10000, 250);
    perm_test.run(vgen, 1200, 250);

    std::cout << std::endl;
  }

private:
  double confidence;
  double confidence_chi_square_quantil;
  distribution_experiment test_distrib_chi_square6;
  ks_distribution_test ksdist_test;
  equidistribution_test equi_test;
  runs_test rns_test;
  gap_test gp_test;
  poker_test pk_test;
  coupon_collector_test cpn_test;
  permutation_test perm_test;
  maximum_test max_test;
  birthday_test bday_test;
};

void test_base::check_(double val) const
{
  environment.check_(val);
}

class program_args
{
public:
  program_args(int argc, char** argv)
  {
    if(argc > 0) {
      names.insert(argv + 1, argv + argc);
    }
  }
  bool check_(const std::string & test_name) const
  {
    return(names.empty() || names.find(test_name) != names.end());
  }
private:
  std::set<std::string> names;
};

int main(int argc, char* argv[])
{
  program_args args(argc, argv);
  test_environment env(0.99);

#define TEST(name)                      \
  if(args.check_(#name))                 \
    env.run_test<boost::name>(#name)

  TEST(minstd_rand0);
  TEST(minstd_rand);
  TEST(rand48);
  TEST(ecuyer1988);
  TEST(kreutzer1986);
  TEST(taus88);
  TEST(hellekalek1995);
  TEST(mt11213b);
  TEST(mt19937);
  TEST(lagged_fibonacci607);
  TEST(lagged_fibonacci1279);
  TEST(lagged_fibonacci2281);
  TEST(lagged_fibonacci3217);
  TEST(lagged_fibonacci4423);
  TEST(lagged_fibonacci9689);
  TEST(lagged_fibonacci19937);
  TEST(lagged_fibonacci23209);
  TEST(lagged_fibonacci44497);
  TEST(ranlux3);
  TEST(ranlux4);

#if !defined(BOOST_NO_INT64_T) && !defined(BOOST_NO_INTEGRAL_INT64_T)
  TEST(ranlux64_3);
  TEST(ranlux64_4);
#endif

  TEST(ranlux3_01);
  TEST(ranlux4_01);
  TEST(ranlux64_3_01);
  TEST(ranlux64_4_01);

  if(args.check_("normal"))
    env.run_test<boost::mt19937>("normal", boost::normal_distribution<>(), boost::math::normal());
  if(args.check_("triangle"))
    env.run_test<boost::mt19937>("triangle", boost::triangle_distribution<>(0, 1, 3), boost::math::triangular(0, 1, 3));
  if(args.check_("cauchy"))
    env.run_test<boost::mt19937>("cauchy", boost::cauchy_distribution<>(), boost::math::cauchy());
  if(args.check_("gamma"))
    env.run_test<boost::mt19937>("gamma", boost::gamma_distribution<>(1), boost::math::gamma_distribution<>(1));
  if(args.check_("exponential"))
    env.run_test<boost::mt19937>("exponential", boost::exponential_distribution<>(), boost::math::exponential());
  if(args.check_("lognormal"))
    env.run_test<boost::mt19937>("lognormal", boost::lognormal_distribution<>(1, 1),
      boost::math::lognormal(std::log(1.0/std::sqrt(2.0)), std::sqrt(std::log(2.0))));
}


#include <vector>
#include <iostream>
#include <string>
#include <tuple>
#include <deque>
#include <map>
#include <set>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <pcl/common/common_headers.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
#include <pcl/sample_consensus/sac_model_normal_plane.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/surface/gp3.h>
#include <pcl/surface/mls.h>
#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/sac_model_plane.h>

#include <pcl/filters/passthrough.h>
#include <vtkPlaneSource.h>
#include <vtkImplicitPlaneWidget.h>

#include "../../../core/format_helper.h"
#include "../../../core/math_helper.h"

cv::Mat img_left, img_disp;
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals(new pcl::PointCloud<pcl::PointNormal>);
std::map<std::tuple<int,int>, int> pts_map;
std::vector<std::tuple<int,int>> road;

void createPolyFromPlane(const pcl::ModelCoefficients::Ptr coefficients,
                         pcl::PointCloud<pcl::PointXYZ>::Ptr poly_cloud)
{
  double a = coefficients->values[0];
  double b = coefficients->values[1];
  double c = coefficients->values[2];
  double d = coefficients->values[3];

  double x[4] = {-20, 20, 20, -20};
  double z[4] = {  0,  0, 100, 100};
  for(int i = 0; i < 4; i++) {
    double y = (a*x[i] + c*z[i] + d) / (-b);
    pcl::PointXYZ pt(x[i], y, z[i]);
    poly_cloud->points.push_back(pt);
  }
}

void triangulate(const double (&cam_params)[5], double x, double y, double disp, cv::Mat& pt3d)
{
  double f = cam_params[0];
  double cx = cam_params[2];
  double cy = cam_params[3];
  double b = cam_params[4];

  // add - in front for x and y just to set the positive y to skies
  pt3d.at<double>(0) = -(x - cx) * b / disp;
  pt3d.at<double>(1) = -(y - cy) * b / disp;
  pt3d.at<double>(2) = f * b / disp;
  pt3d.at<double>(3) = 1.0;
}

void getSignedDistancesToModel(const Eigen::Vector4f& plane, const pcl::PointCloud<pcl::PointXYZ>::Ptr pc,
                               std::vector<double>& distances)
{
  distances.assign(pc->points.size(), 0.0);
  double a = plane[0];
  double b = plane[1];
  double c = plane[2];
  double d = plane[3];
  double norm = std::sqrt(a*a + b*b + c*c);
  for(size_t i = 0; i < pc->points.size(); i++) {
    pcl::PointXYZ pt = pc->points[i];
    double p = a*pt.x + b*pt.y + c*pt.z + d;
    distances[i] = p / norm;
  }
}

void estimateFreeSpace(const std::vector<std::string>& imagelist, const std::vector<std::string>& disp_imglist,
                      const std::string& gt_file, const std::string& cparams_file,
                      const std::string& source_folder, const std::string& disp_folder, const std::string& output_folder)
{
  double cam_params[5];
  core::FormatHelper::readCameraParams(cparams_file, cam_params);

  // KITTI 07
  // meadow on right
  size_t frame_num = 350;
  //size_t frame_num = 395;
  //size_t frame_num = 363;
  //size_t frame_num = 371;
  //size_t frame_num = 510;
  // car in front
  //size_t frame_num = 995;
  // traffic sign
  //size_t frame_num = 442;
  //size_t frame_num = 620;

  // KITTI 02
  //size_t frame_num = 1820;
  //size_t frame_num = 184;

  bool done_looping = false;
  while(!done_looping) {

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr road_patch_pc(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
  std::vector<std::tuple<int,int>> pc2img;
  std::vector<std::tuple<int,int>> patch2img;
  std::vector<int> small_obstacles;
  std::vector<int> super_obstacles;

  // clear the global variables
  cloud_normals->clear();
  pts_map.clear();
  road.clear();

  ifstream gt_fp(gt_file);
  cv::Mat Rt = cv::Mat::eye(4, 4, CV_64F);
  cv::Mat pt3d = cv::Mat::zeros(4, 1, CV_64F);
  //cv::Mat img_left;
  cv::Mat mat_disp;
  //double min_disp = 40.0;
  double min_disp = 1.0;
  //int img_step = 1;
  //for(size_t i = 0; i < end_frame; i++) {
  //for(size_t i = frame_num; i < end_frame; i++) {

  //std::map<std::tuple<int,int>, int> pts_map;

  std::cout << "Frame: " << frame_num << "\n";
  size_t pt_cnt = 0;
  for(size_t i = frame_num; i < frame_num+1; i++) {
    //core::FormatHelper::readNextMatrixKitti(gt_fp, Rt);
    //if(!(i % img_step == 0) || i < frame_num) continue;
    std::cout << "----------------\n" << source_folder + imagelist[2*i] << "\n";
    img_left = cv::imread(source_folder + imagelist[2*i], CV_LOAD_IMAGE_GRAYSCALE);
    //string fname = output_folder + imagelist[i].substr(9, imagelist[i].size()-13) + "_disp.png";
    //std::string fname = output_folder + disp_imglist[i];
    img_disp = cv::imread(disp_folder + "/img/" + disp_imglist[i], CV_LOAD_IMAGE_GRAYSCALE);
    std::string mat_fname = disp_folder + "mat/" + disp_imglist[i].substr(0,11) + ".yml";
    std::cout << "reading: " << mat_fname << "\n";
    cv::FileStorage mat_file(mat_fname, cv::FileStorage::READ);
    mat_file["disparity_matrix"] >> mat_disp;
    for(int y = 0; y < mat_disp.rows; y++) {
      for(int x = 0; x < mat_disp.cols; x++) {
        float disp = mat_disp.at<float>(y,x);
        if(disp > min_disp) {
          triangulate(cam_params, x, y, disp, pt3d);
          //pt3d = Rt * pt3d;
          //std::cout << pt3d << "\n\n";
          pcl::PointXYZ pt;
          pt.x = pt3d.at<double>(0);
          pt.y = pt3d.at<double>(1);
          pt.z = pt3d.at<double>(2);

          //if(pt.x < -3.0 || pt.x > 6.0) continue;
          //if(pt.y < -5.0 || pt.y > 0.0) continue;

          point_cloud->points.push_back(pt);
          pcl::PointXYZRGB point;
          point.x = pt3d.at<double>(0);
          point.y = pt3d.at<double>(1);
          point.z = pt3d.at<double>(2);
          point.r = img_left.at<uint8_t>(y,x);
          point.g = img_left.at<uint8_t>(y,x);
          point.b = img_left.at<uint8_t>(y,x);
          point_cloud_rgb->points.push_back(point);
          pc2img.push_back(std::make_tuple(x,y));
          pts_map.insert(std::make_pair(std::make_tuple(x,y), pt_cnt));
          pt_cnt++;
        }
      }
    }
    //imshow("disparity", img_disp);
    //waitKey(0);
  }
  std::cout << "Point cloud size: " << point_cloud->points.size() << "\n";
  //std::cout << "Point cloud size: " << point_cloud_rgb->points.size() << "\n";
  point_cloud->width = point_cloud->points.size();
  point_cloud->height = 1;
  point_cloud_rgb->width = point_cloud_rgb->points.size();
  point_cloud_rgb->height = 1;

  pcl::io::savePCDFile("pc_orig_rgb.pcd", *point_cloud_rgb);

  //// with smoothing - fuckes up the camera orientation
  //// Create a KD-Tree
  //pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  //// Output has the PointNormal type in order to store the normals calculated by MLS
  ////pcl::PointCloud<pcl::PointNormal> normals;
  ////pcl::PointCloud<pcl::PointNormal>::Ptr normals (new pcl::PointCloud<pcl::PointNormal>);
  //// Init object (second point type is for the normals, even if unused)
  //pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls;
  //mls.setComputeNormals(true);
  //// Set parameters
  //mls.setInputCloud(point_cloud);
  //mls.setPolynomialFit(true);
  //mls.setSearchMethod(tree);
  //mls.setSearchRadius(0.2);       // 0.15 -
  //// Reconstruct
  //mls.process(*cloud_normals);
  //for(int i = 0; i < cloud_normals->points.size(); i++) {
  //  point_cloud->points[i].x = cloud_normals->points[i].x;
  //  point_cloud->points[i].y = cloud_normals->points[i].y;
  //  point_cloud->points[i].z = cloud_normals->points[i].z;
  //  //point_cloud_rgb->points[i].x = cloud_normals->points[i].x;
  //  //point_cloud_rgb->points[i].y = cloud_normals->points[i].y;
  //  //point_cloud_rgb->points[i].z = cloud_normals->points[i].z;
  //}
  ////for(int i = 0; i < cloud_normals->points.size(); i++) {
  ////  for(int j = 0; j < 3; j++) {
  ////    cloud_normals->points[i].normal[j] = - cloud_normals->points[i].normal[j];
  ////  }
  ////}

  for(size_t i = 0; i < point_cloud->points.size(); i++) {
    pcl::PointXYZ pt = point_cloud->points[i];
    if(pt.x > -1.5 && pt.x < 3.5 && pt.z > 4.0 && pt.z < 20.0 && pt.y > -3.0 && pt.y < 0.0) {
      road_patch_pc->points.push_back(pt);
      patch2img.push_back(pc2img[i]);
    }
  }
  road_patch_pc->width = road_patch_pc->points.size();
  road_patch_pc->height = 1;
  pcl::io::savePCDFile("road_patch_pc.pcd", *road_patch_pc);

  //cv::Rect plane_patch(460, 300)
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients(true);
  // Mandatory
  seg.setModelType(pcl::SACMODEL_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setMaxIterations(100000);
  seg.setDistanceThreshold(0.1);
  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
  seg.setInputCloud(road_patch_pc->makeShared());
  seg.segment(*inliers, *coefficients);

  std::vector<double> distances;
  Eigen::Vector4f model_coeff;
  for(int i = 0; i < 4; i++)
    model_coeff[i] = coefficients->values[i];
  //pcl::SampleConsensusModelPlane<pcl::PointXYZ> plane_model(point_cloud);
  //plane_model.getDistancesToModel(model_coeff, distances);
  getSignedDistancesToModel(model_coeff, point_cloud, distances);
  // color the obstacles
  //for(size_t i = 0; i < point_cloud->points.size(); i++) {
  //  //if(distances[i] < -2.0)
  //  //  std::cout << distances[i] << " - dist\n";
  //  if(distances[i] < -0.3) {
  //    point_cloud_rgb->points[i].r = 255.0;
  //    point_cloud_rgb->points[i].g = 0.0;
  //    point_cloud_rgb->points[i].b = 0.0;
  //    super_obstacles.push_back(i);
  //  }
  //  else if(distances[i] < -0.05 && distances[i] >= -0.3) {
  //    point_cloud_rgb->points[i].r = 0.0;
  //    point_cloud_rgb->points[i].g = 255.0;
  //    point_cloud_rgb->points[i].b = 0.0;
  //    small_obstacles.push_back(i);
  //  }
  //  //double dist = getDistanceFromPlane(point_cloud->points[i], coefficients);
  //}

  // render point cloud
  pcl::visualization::PCLVisualizer viewer("3D Viewer");
  viewer.setBackgroundColor(0, 0, 0);
  //pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr point_cloud_ptr(&point_cloud);
  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> hcolor(point_cloud_rgb);
  viewer.addPointCloud<pcl::PointXYZRGB>(point_cloud_rgb, hcolor, "reconstruction");
  pcl::PointCloud<pcl::PointXYZ>::Ptr poly_cloud(new pcl::PointCloud<pcl::PointXYZ>);
  createPolyFromPlane(coefficients, poly_cloud);
  viewer.addPolygon<pcl::PointXYZ>(poly_cloud, 0, 0, 180, "poly1");
  //viewer.addPolygon<pcl::PointXYZ>(poly_cloud, 100, 100, 100, "poly1");
  viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
                                     pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, "poly1");
  viewer.addCoordinateSystem(1.0);
  viewer.initCameraParameters();
  //Main loop
  while (!viewer.wasStopped()) {
    //viewer.spinOnce(100);        // 100
    viewer.spin();        // 100
    //boost::this_thread::sleep(boost::posix_time::microseconds(10000));
  }
  viewer.close();

  //// we need placeholders for threading support
  cv::namedWindow("image");
  cv::namedWindow("patch_image");
  cv::namedWindow("free_space_image");
  //cv::namedWindow("road_image");
  //cv::imshow("image", img_disp);

  cv::imshow("image", img_left);
  cv::Mat img_patch = img_left.clone();
  for(int i = 0; i < patch2img.size(); i++) {
    int x = std::get<0>(patch2img[i]);
    int y = std::get<1>(patch2img[i]);
    img_patch.at<uint8_t>(y,x) = 0.0;
  }
  cv::imshow("patch_image", img_patch);

  //cv::Mat img_obstacles = img_left.clone();
  cv::Mat img_obstacles;
  cv::cvtColor(img_left, img_obstacles, cv::COLOR_GRAY2RGB);
  for(int i = 0; i < small_obstacles.size(); i++) {
    int x = std::get<0>(pc2img[small_obstacles[i]]);
    int y = std::get<1>(pc2img[small_obstacles[i]]);
    //img_obstacles.at<uint8_t>(y,x) = 0.0;
    img_obstacles.at<cv::Vec3b>(y,x)[0] = 0.0;
    img_obstacles.at<cv::Vec3b>(y,x)[1] = 0.0;
    img_obstacles.at<cv::Vec3b>(y,x)[2] = 255.0;
  }
  for(int i = 0; i < super_obstacles.size(); i++) {
    int x = std::get<0>(pc2img[super_obstacles[i]]);
    int y = std::get<1>(pc2img[super_obstacles[i]]);
    //img_obstacles.at<uint8_t>(y,x) = 0.0;
    img_obstacles.at<cv::Vec3b>(y,x)[0] = 0.0;
    img_obstacles.at<cv::Vec3b>(y,x)[1] = 255.0;
    img_obstacles.at<cv::Vec3b>(y,x)[2] = 0.0;
  }
  cv::imshow("free_space_image", img_obstacles);
  //std::stringstream strnum;
  //strnum << std::setw(5) << std::setfill('0') << frame_num;
  //cv::imwrite("out/image_" + strnum.str() + ".png", img_obstacles);
  //cv::waitKey(10);
  frame_num++;
  while(true) {
    int key = cv::waitKey(0);
    //std::cout << key << "\n";
    if(key == 27 || key == 1048603) {
      done_looping = true;
      break;
    }
    else if(key == 65362) {
      frame_num++;
      break;
    }
    else if(key == 65364) {
      frame_num--;
      break;
    }
  }

  } // end big loop
}

int main(int argc, char** argv)
{
  std::string config_file;
  std::string imagelistfn, disp_imagelistfn;
  std::string cam_params_file;
  std::string source_folder;
  std::string disp_folder, gt_file;
  std::string output_folder;

  try {
    po::options_description generic("Generic options");
    generic.add_options()
      ("help", "produce help message")
      ("config,c", po::value<std::string>(&config_file)->default_value("config.txt"), "config filename")
      ;
    po::options_description config("Config file options");
    config.add_options()
      ("camera_params,p", po::value<std::string>(&cam_params_file)->default_value("camera_params.txt"),
       "camera params file")
      ("source_folder,s", po::value<std::string>(&source_folder), "folder with source")
      ("output_folder,o", po::value<std::string>(&output_folder), "folder for output")
      ("disp_folder,d", po::value<std::string>(&disp_folder), "folder for output")
      ("groundtruth,g", po::value<std::string>(&gt_file), "groundtruth file")
      ("imglist,l", po::value<std::string>(&imagelistfn), "file with image list")
      ("disp_imglist,dl", po::value<std::string>(&disp_imagelistfn), "file with image list");

    po::options_description cmdline_options;
    cmdline_options.add(generic).add(config);

    po::options_description config_file_options;
    config_file_options.add(config);
    po::variables_map vm;
    //po::store(po::command_line_parser(argc, argv).options(cmdline_options).run(), vm);
    po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
    notify(vm);
    if(vm.count("help")) {
      std::cout << generic;
      std::cout << config;
      return 0;
    }

    ifstream ifs(config_file.c_str());
    if (!ifs) {
      std::cout << "can not open config file: " << config_file << "\n";
      std::cout << generic;
      std::cout << config;
      return 0;
    }
    else {
      po::store(parse_config_file(ifs, config_file_options), vm);
      notify(vm);
    }
    std::cout << "Configuring done, using:" << endl;

    if(vm.count("camera_params")) {
      std::cout << "Camera params: ";
      std::cout << cam_params_file << endl;
    }
    if(vm.count("source_folder")) {
      std::cout << "Source folder: ";
      std::cout << source_folder << endl;
    }
    if(vm.count("output_folder")) {
      std::cout << "Output folder: ";
      std::cout << output_folder << endl;
    }
    if(vm.count("imglist")) {
      std::cout << "Image list file: ";
      std::cout << imagelistfn << endl;
    }
  }
  catch(std::exception& e) {
    std::cout << e.what() << "\n";
    return 1;
  }

  if(imagelistfn == "")
  {
    std::cout << "error: no xml image list given." << endl;
    return -1;
  }
  if(source_folder == "")
  {
    std::cout << "error: no output or source folder given." << endl;
    return -1;
  }

  std::vector<std::string> imagelist, disp_imagelist;
  bool ok = core::FormatHelper::readStringList(imagelistfn, imagelist);
  if(!ok || imagelist.empty())
  {
    std::cout << "can not open " << imagelistfn << " or the string list is empty" << endl;
    return -1;
  }
  ok = core::FormatHelper::readStringList(disp_imagelistfn, disp_imagelist);

  estimateFreeSpace(imagelist, disp_imagelist, gt_file, cam_params_file, source_folder, disp_folder, output_folder);

  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.

// Written by Cornelius Steinhardt

#ifndef ITL_TFQMR_INCLUDE
#define ITL_TFQMR_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/linear_algebra/inverse.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/mtl/operation/resource.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

#include <boost/numeric/itl/krylov/base_solver.hpp>

namespace itl {

/// Transposed-free Quasi-minimal residual
template < typename Matrix, typename Vector,
	   typename LeftPreconditioner, typename RightPreconditioner, typename Iteration >
int tfqmr(const Matrix &A, Vector &x, const Vector &b, const LeftPreconditioner &L,
	  const RightPreconditioner &R, Iteration& iter)
{
    mtl::vampir_trace<7009> tracer;
    using math::reciprocal; using mtl::size;
    typedef typename mtl::Collection<Vector>::value_type Scalar;

    if (size(b) == 0) throw mtl::logic_error("empty rhs vector");

    const Scalar                zero= math::zero(Scalar()), one= math::one(Scalar());
    Scalar                      theta(zero), eta(zero), tau, rho, rhon, sigma, alpha, beta, c;
    Vector                      rt(b - A*Vector(solve(R, x))) /* shift x= R*x */, r(solve(L, rt)), u1(resource(x)), u2(resource(x)),
                                y1(resource(x)), y2(resource(x)), w(resource(x)), d(resource(x), zero), v(resource(x));

    if (iter.finished(rt))
	return iter;
    y1= w= r;
    rt= A * Vector(solve(R, y1));
    u1= v= solve(L,rt);
    tau= two_norm(r);
    rho= tau * tau;

    // TFQMR iteration
    while (! iter.finished(tau)) {
	++iter;
	sigma= dot(r,v);
        if (sigma == zero)
	    return iter.fail(1, "tfgmr breakdown, sigma=0 #1");
        alpha= rho / sigma;

        // inner loop
        for(int j=1; j < 3; j++) {
            if (j == 1) {
                w-= alpha * u1;
                d= y1+ (theta * theta * eta / alpha) * d;
	    } else {
                y2= y1 - alpha * v;
                rt= A * Vector(solve(R, y2));
                u2= solve(L, rt);
                w-= alpha * u2;
                d= y2 + (theta * theta * eta / alpha) * d;
            }
            theta= two_norm(w) / tau;
            c= reciprocal(sqrt(one + theta*theta));
            tau*= theta * c;
            eta= c * c * alpha;
            x+= eta * d;
        } // end inner loop
        if (rho == zero)
            return iter.fail(1, "tfgmr breakdown, rho=0 #2");
        rhon= dot(r,w);
        beta= rhon/rho;
        rho= rhon;
        y1= w + beta*y2;
        rt= A * Vector(solve(R, y1));
        u1= solve(L, rt);
        v= u1 + beta*(u2 + beta*v);
        rt= A * x - b;
    }
    //shift back
    x= solve(R, x);
    return iter;
}

/// Solver class for Transposed-free quasi-minimal residual method; right preconditioner ignored (prints warning if not identity)
/** Methods inherited from \ref base_solver. **/
template < typename LinearOperator, typename Preconditioner= pc::identity<LinearOperator>,
	   typename RightPreconditioner= pc::identity<LinearOperator> >
class tfqmr_solver
  : public base_solver< tfqmr_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator >
{
    typedef base_solver< tfqmr_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator > base;
  public:
    /// Construct solver from a linear operator; generate (left) preconditioner from it
    explicit tfqmr_solver(const LinearOperator& A) : base(A), L(A), R(A) {}

    /// Construct solver from a linear operator and left preconditioner
    tfqmr_solver(const LinearOperator& A, const Preconditioner& L) : base(A), L(L), R(A) {}

    /// Construct solver from a linear operator and left preconditioner
    tfqmr_solver(const LinearOperator& A, const Preconditioner& L, const RightPreconditioner& R)
      : base(A), L(L), R(R) {}

    /// Solve linear system approximately as specified by \p iter
    template < typename HilbertSpaceX, typename HilbertSpaceB, typename Iteration >
    int solve(HilbertSpaceX& x, const HilbertSpaceB& b, Iteration& iter) const
    {
	return tfqmr(this->A, x, b, L, R, iter);
    }

  private:
    Preconditioner        L;
    RightPreconditioner   R;
};

} // namespace itl

#endif // ITL_TFQMR_INCLUDE


#include <vector>
#include <cfloat>

#include <boost/foreach.hpp>

#include "types.hpp"
#include "track.hpp"

using namespace std;

/**
 * iterates over all possible wire_pos_ptr combinations
 * if next combination is not available returns false
 */
bool	next_combination( vector<wire_pos_ptr_t> &wire_pos_ptr, const vector<int> &wire_count )
{
	const int chambers_count = wire_count.size();
	int	i = 0;

	while(1 + wire_pos_ptr[i] == wire_count[i])
	{
		i++;

		if (i == chambers_count)
		{
			/* reached last combination */
			return false;
		}
	}

	for(int j = 0; j < i; j++)
	{
		wire_pos_ptr[j] = 0;
	}

	wire_pos_ptr[i]++;

	return true;
}

/**
 * Removes items from data that haven't got any wires left in them.
 * @param data - data[i] is a pointer to list of wires for i-th chamber
 * @return false if there is no enough non-empty chambers left
 */
bool	delete_empty_chambers( vector< vector<wire_pos_t>* > &data, vector<double> &normal_pos, vector<uint> &used_chambers )
{
	uint	empty_chambers_count = 0;
	auto	data_it = data.begin();
	auto	normal_pos_it = normal_pos.begin();
	auto	used_chambers_it = used_chambers.begin();

	// check if there is enough non-empty chambers
	while(data_it != data.end())
	{
		if ((*data_it)->empty())
		{
			empty_chambers_count++;

			if (data.size() <= MIN_TRACK_CHAMBERS)
			{
				return false;
			}

			data_it = data.erase(data_it);
			normal_pos_it = normal_pos.erase(normal_pos_it);
			used_chambers_it = used_chambers.erase(used_chambers_it);
		}
		else
		{
			data_it++;
			normal_pos_it++;
			used_chambers_it++;
		}
	}

	return true;
}

track_info_t	reconstruct_track( const vector< vector<wire_pos_t>* > &data, const vector<double> &normal_pos )
{
	const chamber_id_t	chambers_count = data.size();
	vector<wire_pos_ptr_t>	best_wire_pos_ptr(chambers_count);
	vector<wire_pos_ptr_t>	wire_pos_ptr(chambers_count);
	vector<int>		wire_count(chambers_count);

	// initialize variables
	{
		int chamber_id = 0;

		BOOST_FOREACH(auto chamber_data, data)
		{
			wire_pos_ptr[chamber_id] = 0;
			wire_count[chamber_id] = chamber_data->size();

			chamber_id++;
		}
	}

	double	best_c0, best_c1;
	double	best_sumsq, prev_best_sumsq = FLT_MAX;
	vector<double>	best_wires_pos;
	bool	first = true;

	do
	{
		vector<double>	wires;
		double	c0, c1, sumsq;
		int i = 0;

		// fill array with values to fit
		BOOST_FOREACH(auto chamber_data, data)
		{
			wire_pos_t	wire_id = (*chamber_data)[wire_pos_ptr[i]];

			wires.push_back(wire_id);

			i++;
		}

		// perform linear fit
		{
			double	m_x = 0;
			double	m_y = 0;
			double	m_dxdy = 0;
			double	m_dxdx = 0;
			int		N = normal_pos.size();

			BOOST_FOREACH(double x, normal_pos)
			{
				m_x += x;
			}
			m_x /= N;

			BOOST_FOREACH(double y, wires)
			{
				m_y += y;
			}
			m_y /= N;

			auto	xit = normal_pos.begin();
			auto	yit = wires.begin();
			while(xit != normal_pos.end())
			{
				m_dxdy += (*xit - m_x) * (*yit - m_y);
				m_dxdx += (*xit - m_x) * (*xit - m_x);
				xit++; yit++;
			}
			m_dxdy /= N;
			m_dxdx /= N;

			c1 = m_dxdy / m_dxdx;
			c0 = m_y - c1 * m_x;

			xit = normal_pos.begin();
			yit = wires.begin();
			sumsq = 0;
			while(xit != normal_pos.end())
			{
				double	v = *yit - (c0 + *xit * c1);
				sumsq += v * v;
				xit++; yit++;
			}
		}

		if (first || (best_sumsq > sumsq))
		{
			prev_best_sumsq = best_sumsq;
			best_sumsq = sumsq;
			best_c0 = c0;
			best_c1 = c1;
			best_wires_pos = wires;
			first = false;

			// save best wires array positions
			for(chamber_id_t chamber_id = 0; chamber_id < chambers_count; chamber_id++)
			{
				best_wire_pos_ptr[chamber_id] = wire_pos_ptr[chamber_id];
			}
		}
	}
	while(next_combination(wire_pos_ptr, wire_count));

	return track_info_t({
			best_c0, best_c1, best_sumsq, prev_best_sumsq, best_wires_pos, best_wire_pos_ptr
				});
}

/**
 * @warning This function deletes wires that lie on the reconstructd track.
 */
template<track_type_t track_type>
vector<track_info_t>	reconstruct_all_tracks( vector< vector<wire_pos_t>* > data, vector<double> normal_pos, double max_chisq )
{
	vector<uint>	used_chambers;
	vector<track_info_t>	result;

	for(uint i = 0; i < data.size(); i++)
	{
		used_chambers.push_back(i);
	}

	// check if there is enough chambers
	if (data.size() < MIN_TRACK_CHAMBERS)
	{
		throw "Not enough chambers in original data!";
	}

	while(delete_empty_chambers(data, normal_pos, used_chambers))
	{
		track_info_t	track = reconstruct_track(data, normal_pos);

		track.used_chambers = used_chambers;

		if ((max_chisq > 0) && (track.chisq > max_chisq))
		{
			break;
		}

		result.push_back(track);

		chamber_id_t	chamber_id = 0;

		BOOST_FOREACH(auto chamber_data, data)
		{
			wire_pos_ptr_t	deletion_pos = track.wire_pos_ptr[chamber_id];

			// now delete wires that compose the reconstructd track
			if (track_type == track_type_t::prop)
			{
				chamber_data->erase(chamber_data->begin() + deletion_pos);
			}
			else if (track_type == track_type_t::drift)
			{
				// for drift chamber point consists of the left and right side
				// so we need to remove both
				deletion_pos &= ~1;
				chamber_data->erase(chamber_data->begin() + deletion_pos,
				                    chamber_data->begin() + deletion_pos + 2);
			}

			chamber_id++;
		}
	}

	return result;
}

template vector<track_info_t> reconstruct_all_tracks<track_type_t::prop>( vector< vector<wire_pos_t>* > data, vector<double> normal_pos, double max_chisq );
template vector<track_info_t> reconstruct_all_tracks<track_type_t::drift>( vector< vector<wire_pos_t>* > data, vector<double> normal_pos, double max_chisq );


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/cotd.hpp>
#include <boost/simd/function/restricted.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>

STF_CASE_TPL (" cotd",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::cotd;

  using r_t = decltype(cotd(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(cotd(-bs::Zero<T>()), -bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(cotd(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cotd(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cotd(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cotd(bs::Zero<T>()), bs::Inf<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(cotd(-T(45)), bs::Mone<r_t>(), 4.0);
  STF_ULP_EQUAL(cotd(-T(90)), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(cotd(T(45)), bs::One<r_t>(), 4.0);
  STF_ULP_EQUAL(cotd(T(90)), bs::Zero<r_t>(), 0.5);
}

STF_CASE_TPL (" cotd",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::cotd;

  using r_t = decltype(cotd(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(bs::restricted_(cotd)(-bs::Zero<T>()), -bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cotd)(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cotd)(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cotd)(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cotd)(bs::Zero<T>()), bs::Inf<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(bs::restricted_(cotd)(-T(45)), bs::Mone<r_t>(), 4.0);
  STF_ULP_EQUAL(bs::restricted_(cotd)(-T(90)), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cotd)(T(45)), bs::One<r_t>(), 4.0);
  STF_ULP_EQUAL(bs::restricted_(cotd)(T(90)), bs::Nan<r_t>(), 0.5);
}


#include <iostream>
#include <fstream>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include "headers/util.h"
#include "headers/rmat.h"
#include "headers/matutil.h"
#include "headers/matrix_wrapper.h"
#include "headers/timer.h"

using namespace std;
using namespace boost::numeric::ublas;

int main(int argc, char** argv) {
  uint64_t n = atol(argv[1]);
  uint64_t m = atol(argv[2]);

  RmatConfig cfg(0.57, 0.19, 0.19);
  CustomMatrix *mat = new CustomMatrix(n);
  timer t;
  t.start();
  rmat<CustomMatrix>(mat, m, cfg);
  t.stop();
  cout << "Took: " << t.get_total() << endl;

  // timer t;
  // t.start();
  // list<Edge> l = listRmat(n, m, cfg);
  // t.stop();
  // cout << "Took: " << t.get_total() << endl;

  // fs.open("data/gen-32bit_uniform.csv", ios::trunc);
  // MatrixWrapper<CustomMatrix> *w = new MatrixWrapper<CustomMatrix>(1024, 1024, 0, true);
  // ofstream fs;
  // timer t;

  // RmatConfig cfg;
  // cout << "Starting now" << endl;

  // rmat<MatrixWrapper<CompAdjMat>>(w, 50000, cfg);
  // rmatSeq<AdjMatrix>(w, 25571, cfg);
  // rmat<CustomMatrix>(x, 5000, cfg);
  // rmatSeq<CustomMatrix>(x, 5000, cfg);
  // cout << "Finished generating graph, writing to file..." << endl;
  // will::matutil::writeAdjMatrix(*w, fs);
  // will::matutil::writeAdjMatrix(*x, fs);
  // cout << "Done!" << endl;

  // {
  //   timer t;
  //   MatrixWrapper<AdjMatrix> *w = new MatrixWrapper<AdjMatrix>(50000, 50000);
  //   RmatConfig cfg(0.57, 0.19, 0.19);
  //   t.start();
  //   rmatSeq<AdjMatrix>(w, 505571000, cfg);
  //   t.stop();
  //   cout << w->getMat() << endl;
  //   cout << "Took: " << t.get_total() << endl;
  // }

  // {
  //   timer t;
  //   RmatConfig cfg(0.57, 0.19, 0.19);
  //   t.start();
  //   std::list<Edge> res = listRmat(30000, 50557100, cfg);
  //   t.stop();
  //   cout << res.size() << endl;
  //   cout << "Took: " << t.get_total() << endl;
  // }

  // will::matutil::writeAdjMatrix(*w, std::cout);
  // std::cout << m << std::endl;
  // MatrixWrapper<CompAdjMat> w(10, 10);
  // w.insert(3, 3, 9);
  // w.set(1, 1, 9);
  // cout << static_cast<int>(w.get(3, 3)) << endl;
  // m = w.getMat();
  // will::matutil::writeAdjMatrix(*m, std::cout);
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Framework/TestingFramework.hpp"

#include <array>
#include <boost/functional/hash.hpp>
#include <cstddef>
#include <unordered_map>
#include <utility>

#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tags.hpp"
#include "DataStructures/Tensor/EagerMath/DotProduct.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "Domain/CoordinateMaps/Affine.hpp"
#include "Domain/CoordinateMaps/CoordinateMap.hpp"
#include "Domain/CoordinateMaps/CoordinateMap.tpp"
#include "Domain/CoordinateMaps/ProductMaps.hpp"
#include "Domain/CoordinateMaps/ProductMaps.tpp"
#include "Domain/ElementMap.hpp"
#include "Domain/LogicalCoordinates.hpp"
#include "Domain/SizeOfElement.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/Element.hpp"
#include "Domain/Structure/ElementId.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/Minmod.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/MinmodImpl.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/MinmodType.hpp"
#include "Evolution/Systems/NewtonianEuler/Limiters/CharacteristicHelpers.hpp"
#include "Evolution/Systems/NewtonianEuler/Limiters/Minmod.hpp"
#include "Evolution/Systems/NewtonianEuler/Tags.hpp"
#include "Framework/TestCreation.hpp"
#include "Framework/TestHelpers.hpp"
#include "Helpers/Evolution/DiscontinuousGalerkin/Limiters/TestHelpers.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "NumericalAlgorithms/Spectral/Spectral.hpp"
#include "PointwiseFunctions/Hydro/EquationsOfState/IdealFluid.hpp"
#include "Utilities/ConstantExpressions.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/MakeArray.hpp"
#include "Utilities/TMPL.hpp"
#include "Utilities/TaggedTuple.hpp"

namespace {

void test_neuler_minmod_option_parsing() noexcept {
  INFO("Testing option parsing");
  const auto lambda_pi1 =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Minmod<1>>(
          "Type: LambdaPi1\n"
          "VariablesToLimit: Characteristic\n"
          "TvbConstant: 0.0\n"
          "ApplyFlattener: True\n"
          "DisableForDebugging: False");
  const auto lambda_pi1_cons =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Minmod<1>>(
          "Type: LambdaPi1\n"
          "VariablesToLimit: Conserved\n"
          "TvbConstant: 0.0\n"
          "ApplyFlattener: True\n"
          "DisableForDebugging: False");
  const auto lambda_pi1_tvb =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Minmod<1>>(
          "Type: LambdaPi1\n"
          "VariablesToLimit: Characteristic\n"
          "TvbConstant: 1.0\n"
          "ApplyFlattener: True\n"
          "DisableForDebugging: False");
  const auto lambda_pi1_noflat =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Minmod<1>>(
          "Type: LambdaPi1\n"
          "VariablesToLimit: Characteristic\n"
          "TvbConstant: 0.0\n"
          "ApplyFlattener: False\n"
          "DisableForDebugging: False");
  const auto lambda_pi1_disabled =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Minmod<1>>(
          "Type: LambdaPi1\n"
          "VariablesToLimit: Characteristic\n"
          "TvbConstant: 0.0\n"
          "ApplyFlattener: True\n"
          "DisableForDebugging: True");
  const auto muscl =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Minmod<1>>(
          "Type: Muscl\n"
          "VariablesToLimit: Characteristic\n"
          "TvbConstant: 0.0\n"
          "ApplyFlattener: True\n"
          "DisableForDebugging: False");

  // Test operators == and !=
  CHECK(lambda_pi1 == lambda_pi1);
  CHECK(lambda_pi1 != lambda_pi1_cons);
  CHECK(lambda_pi1 != lambda_pi1_tvb);
  CHECK(lambda_pi1 != lambda_pi1_noflat);
  CHECK(lambda_pi1 != lambda_pi1_disabled);
  CHECK(lambda_pi1 != muscl);

  const auto lambda_pin_2d =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Minmod<2>>(
          "Type: LambdaPiN\n"
          "VariablesToLimit: Characteristic\n"
          "TvbConstant: 0.0\n"
          "ApplyFlattener: True\n"
          "DisableForDebugging: False");
  const auto lambda_pin_3d =
      TestHelpers::test_creation<NewtonianEuler::Limiters::Minmod<3>>(
          "Type: LambdaPiN\n"
          "VariablesToLimit: Characteristic\n"
          "TvbConstant: 10.0\n"
          "ApplyFlattener: True\n"
          "DisableForDebugging: False");

  // Test that creation from options gives correct object
  const NewtonianEuler::Limiters::Minmod<1> expected_lambda_pi1(
      Limiters::MinmodType::LambdaPi1,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 0., true);
  const NewtonianEuler::Limiters::Minmod<1> expected_lambda_pi1_cons(
      Limiters::MinmodType::LambdaPi1,
      NewtonianEuler::Limiters::VariablesToLimit::Conserved, 0., true);
  const NewtonianEuler::Limiters::Minmod<1> expected_lambda_pi1_tvb(
      Limiters::MinmodType::LambdaPi1,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 1., true);
  const NewtonianEuler::Limiters::Minmod<1> expected_lambda_pi1_noflat(
      Limiters::MinmodType::LambdaPi1,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 0., false);
  const NewtonianEuler::Limiters::Minmod<1> expected_lambda_pi1_disabled(
      Limiters::MinmodType::LambdaPi1,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 0., true,
      true);
  const NewtonianEuler::Limiters::Minmod<1> expected_muscl(
      Limiters::MinmodType::Muscl,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 0., true);
  const NewtonianEuler::Limiters::Minmod<2> expected_lambda_pin_2d(
      Limiters::MinmodType::LambdaPiN,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 0., true);
  const NewtonianEuler::Limiters::Minmod<3> expected_lambda_pin_3d(
      Limiters::MinmodType::LambdaPiN,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 10., true);

  CHECK(lambda_pi1 == expected_lambda_pi1);
  CHECK(lambda_pi1_cons == expected_lambda_pi1_cons);
  CHECK(lambda_pi1_tvb == expected_lambda_pi1_tvb);
  CHECK(lambda_pi1_noflat == expected_lambda_pi1_noflat);
  CHECK(lambda_pi1_disabled == expected_lambda_pi1_disabled);
  CHECK(muscl == expected_muscl);
  CHECK(lambda_pin_2d == expected_lambda_pin_2d);
  CHECK(lambda_pin_3d == expected_lambda_pin_3d);
}

void test_neuler_minmod_serialization() noexcept {
  INFO("Testing serialization");
  const NewtonianEuler::Limiters::Minmod<1> minmod(
      Limiters::MinmodType::LambdaPi1,
      NewtonianEuler::Limiters::VariablesToLimit::Characteristic, 1., true);
  test_serialization(minmod);
}

// Compare the specialized limiter to the generic limiter. The limiters are
// compared for limiting conserved/evolved and limiting characteristic
// variables.
//
// Note: comparing the specialized and generic limiters when limiting the
// characteristic variables is tedious to do thoroughly, because we need to
// call the generic limiter multiple times using characteristic fields w.r.t.
// multiple different unit vectors. In essence, the test would need to fully
// reimplement the specialized limiter. To avoid the test becoming so tedious,
// we test a simpler restricted problem, where in 2D and 3D we require
// input_momentum to be 0. With this simplification, the characteristic
// decomposition becomes independent of the choice of unit vector, and therefore
// the generic limiter can match the specialized limiter even when only one set
// of characteristic (e.g., w.r.t. \hat{x}) is used.
template <size_t VolumeDim>
void test_neuler_vs_generic_minmod_work(
    const Scalar<DataVector>& input_density,
    const tnsr::I<DataVector, VolumeDim>& input_momentum,
    const Scalar<DataVector>& input_energy, const Mesh<VolumeDim>& mesh,
    const tnsr::I<DataVector, VolumeDim, Frame::Logical>& logical_coords,
    const std::array<double, VolumeDim>& element_size,
    const Scalar<DataVector>& det_inv_logical_to_inertial_jacobian,
    const std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
        typename NewtonianEuler::Limiters::Minmod<VolumeDim>::PackagedData,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
        neighbor_data) noexcept {
  // Sanity check that input momentum satisfies simplifying assumptions
  if constexpr (VolumeDim > 1) {
    const auto zero_momentum = make_with_value<tnsr::I<DataVector, VolumeDim>>(
        get<0>(input_momentum), 0.);
    REQUIRE(input_momentum == zero_momentum);
  }

  const double tvb_constant = 0.;
  const auto element = TestHelpers::Limiters::make_element<VolumeDim>();
  const EquationsOfState::IdealFluid<false> equation_of_state{5. / 3.};

  {
    INFO("Compare specialized vs generic Minmod on conserved variables");
    auto density_generic = input_density;
    auto momentum_generic = input_momentum;
    auto energy_generic = input_energy;
    const Limiters::Minmod<
        VolumeDim, tmpl::list<NewtonianEuler::Tags::MassDensityCons,
                              NewtonianEuler::Tags::MomentumDensity<VolumeDim>,
                              NewtonianEuler::Tags::EnergyDensity>>
        minmod_generic(Limiters::MinmodType::LambdaPi1, tvb_constant);
    const bool activated_generic = minmod_generic(
        make_not_null(&density_generic), make_not_null(&momentum_generic),
        make_not_null(&energy_generic), mesh, element, logical_coords,
        element_size, neighbor_data);

    auto density_specialized = input_density;
    auto momentum_specialized = input_momentum;
    auto energy_specialized = input_energy;
    const auto vars_to_limit =
        NewtonianEuler::Limiters::VariablesToLimit::Conserved;
    const bool apply_flattener = false;
    const NewtonianEuler::Limiters::Minmod<VolumeDim> minmod_specialized(
        Limiters::MinmodType::LambdaPi1, vars_to_limit, tvb_constant,
        apply_flattener);
    const bool activated_specialized = minmod_specialized(
        make_not_null(&density_specialized),
        make_not_null(&momentum_specialized),
        make_not_null(&energy_specialized), mesh, element, logical_coords,
        element_size, det_inv_logical_to_inertial_jacobian, equation_of_state,
        neighbor_data);

    CHECK(activated_generic);
    CHECK(activated_generic == activated_specialized);
    CHECK_ITERABLE_APPROX(density_generic, density_specialized);
    CHECK_ITERABLE_APPROX(momentum_generic, momentum_specialized);
    CHECK_ITERABLE_APPROX(energy_generic, energy_specialized);
  }

  {
    INFO("Compare specialized vs generic Minmod on characteristic variables");
    // Cellwise means, for cons/char transforms
    const auto mean_density =
        Scalar<double>{mean_value(get(input_density), mesh)};
    const auto mean_momentum = [&input_momentum, &mesh]() noexcept {
      tnsr::I<double, VolumeDim> result{};
      for (size_t i = 0; i < VolumeDim; ++i) {
        result.get(i) = mean_value(input_momentum.get(i), mesh);
      }
      return result;
    }();
    const auto mean_energy =
        Scalar<double>{mean_value(get(input_energy), mesh)};

    // Compute characteristic transformation using x-direction...
    // Note that in 2D and 3D we know the fluid velocity is 0, so this choice
    // is arbitrary in these higher dimensions.
    const auto unit_vector = []() noexcept {
      auto components = make_array<VolumeDim>(0.);
      components[0] = 1.;
      return tnsr::i<double, VolumeDim>(components);
    }();
    const auto right_and_left =
        NewtonianEuler::Limiters::right_and_left_eigenvectors(
            mean_density, mean_momentum, mean_energy, equation_of_state,
            unit_vector);
    const auto& right = right_and_left.first;
    const auto& left = right_and_left.second;

    // Transform tensors to characteristics
    auto char_v_minus = input_density;
    auto char_v_momentum = input_momentum;
    auto char_v_plus = input_energy;
    NewtonianEuler::Limiters::characteristic_fields(
        make_not_null(&char_v_minus), make_not_null(&char_v_momentum),
        make_not_null(&char_v_plus), input_density, input_momentum,
        input_energy, left);

    using GenericMinmod =
        Limiters::Minmod<VolumeDim,
                         tmpl::list<NewtonianEuler::Tags::VMinus,
                                    NewtonianEuler::Tags::VMomentum<VolumeDim>,
                                    NewtonianEuler::Tags::VPlus>>;
    std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
        typename GenericMinmod::PackagedData,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
        neighbor_char_data{};
    for (const auto& [key, data] : neighbor_data) {
      neighbor_char_data[key].element_size = data.element_size;
      NewtonianEuler::Limiters::characteristic_fields(
          make_not_null(&(neighbor_char_data[key].means)), data.means, left);
    }

    const GenericMinmod minmod_generic(Limiters::MinmodType::LambdaPi1,
                                       tvb_constant);
    const bool activated_generic = minmod_generic(
        make_not_null(&char_v_minus), make_not_null(&char_v_momentum),
        make_not_null(&char_v_plus), mesh, element, logical_coords,
        element_size, neighbor_char_data);

    // Transform back to evolved variables
    // Note that because the fluid velocity is 0 and all sets of characteristics
    // are identical, we can skip the step of combining different limiter
    // results.
    auto density_generic = input_density;
    auto momentum_generic = input_momentum;
    auto energy_generic = input_energy;
    NewtonianEuler::Limiters::conserved_fields_from_characteristic_fields(
        make_not_null(&density_generic), make_not_null(&momentum_generic),
        make_not_null(&energy_generic), char_v_minus, char_v_momentum,
        char_v_plus, right);

    auto density_specialized = input_density;
    auto momentum_specialized = input_momentum;
    auto energy_specialized = input_energy;
    const auto vars_to_limit =
        NewtonianEuler::Limiters::VariablesToLimit::Characteristic;
    const bool apply_flattener = false;
    const NewtonianEuler::Limiters::Minmod<VolumeDim> minmod_specialized(
        Limiters::MinmodType::LambdaPi1, vars_to_limit, tvb_constant,
        apply_flattener);
    const bool activated_specialized = minmod_specialized(
        make_not_null(&density_specialized),
        make_not_null(&momentum_specialized),
        make_not_null(&energy_specialized), mesh, element, logical_coords,
        element_size, det_inv_logical_to_inertial_jacobian, equation_of_state,
        neighbor_data);

    CHECK(activated_generic);
    CHECK(activated_generic == activated_specialized);
    CHECK_ITERABLE_APPROX(density_generic, density_specialized);
    CHECK_ITERABLE_APPROX(momentum_generic, momentum_specialized);
    CHECK_ITERABLE_APPROX(energy_generic, energy_specialized);
  }
}

void test_neuler_vs_generic_minmod_1d() noexcept {
  INFO("Testing NewtonianEuler::Limiters::Minmod limiter in 1D");
  const auto mesh =
      Mesh<1>(3, Spectral::Basis::Legendre, Spectral::Quadrature::GaussLobatto);
  const auto element = TestHelpers::Limiters::make_element<1>();
  const auto logical_coords = logical_coordinates(mesh);
  using Affine = domain::CoordinateMaps::Affine;
  const Affine xi_map{-1., 1., 3.7, 4.2};
  const auto coordmap =
      domain::make_coordinate_map_base<Frame::BlockLogical, Frame::Inertial>(
          xi_map);
  const ElementMap<1, Frame::Inertial> element_map(element.id(),
                                                   coordmap->get_clone());
  const auto element_size = size_of_element(element_map);
  const auto det_inv_logical_to_inertial_jacobian =
      determinant(element_map.inv_jacobian(logical_coords));

  const auto& x = get<0>(logical_coords);
  const auto mass_density_cons = [&x]() noexcept {
    return Scalar<DataVector>{{{1. + 0.2 * x + 0.05 * square(x)}}};
  }();
  const auto momentum_density = [&x]() noexcept {
    return tnsr::I<DataVector, 1>{{{0.2 - 0.3 * x}}};
  }();
  const auto energy_density = [&mesh]() noexcept {
    return Scalar<DataVector>{DataVector(mesh.number_of_grid_points(), 1.)};
  }();

  std::unordered_map<std::pair<Direction<1>, ElementId<1>>,
                     NewtonianEuler::Limiters::Minmod<1>::PackagedData,
                     boost::hash<std::pair<Direction<1>, ElementId<1>>>>
      neighbor_data{};
  const std::array<std::pair<Direction<1>, ElementId<1>>, 2> dir_keys = {
      {{Direction<1>::lower_xi(), ElementId<1>(1)},
       {Direction<1>::upper_xi(), ElementId<1>(2)}}};
  for (const auto& dir_key : dir_keys) {
    neighbor_data[dir_key].element_size = element_size;
  }

  using mean_rho = Tags::Mean<NewtonianEuler::Tags::MassDensityCons>;
  get(get<mean_rho>(neighbor_data[dir_keys[0]].means)) = 0.4;
  get(get<mean_rho>(neighbor_data[dir_keys[1]].means)) = 1.1;

  using mean_rhou = Tags::Mean<NewtonianEuler::Tags::MomentumDensity<1>>;
  get<0>(get<mean_rhou>(neighbor_data[dir_keys[0]].means)) = 0.2;
  get<0>(get<mean_rhou>(neighbor_data[dir_keys[1]].means)) = -0.1;

  using mean_eps = Tags::Mean<NewtonianEuler::Tags::EnergyDensity>;
  get(get<mean_eps>(neighbor_data[dir_keys[0]].means)) = 1.;
  get(get<mean_eps>(neighbor_data[dir_keys[1]].means)) = 1.;

  test_neuler_vs_generic_minmod_work(
      mass_density_cons, momentum_density, energy_density, mesh, logical_coords,
      element_size, det_inv_logical_to_inertial_jacobian, neighbor_data);
}

void test_neuler_vs_generic_minmod_2d() noexcept {
  INFO("Testing NewtonianEuler::Limiters::Minmod limiter in 2D");
  const auto mesh =
      Mesh<2>(3, Spectral::Basis::Legendre, Spectral::Quadrature::GaussLobatto);
  const auto element = TestHelpers::Limiters::make_element<2>();
  const auto logical_coords = logical_coordinates(mesh);
  using Affine = domain::CoordinateMaps::Affine;
  using Affine2D = domain::CoordinateMaps::ProductOf2Maps<Affine, Affine>;
  const Affine xi_map{-1., 1., 3.7, 4.2};
  const Affine eta_map{-1., 1., 3.2, 4.2};
  const Affine2D map2d{xi_map, eta_map};
  const auto coordmap =
      domain::make_coordinate_map_base<Frame::BlockLogical, Frame::Inertial>(
          map2d);
  const ElementMap<2, Frame::Inertial> element_map(element.id(),
                                                   coordmap->get_clone());
  const auto element_size = size_of_element(element_map);
  const auto det_inv_logical_to_inertial_jacobian =
      determinant(element_map.inv_jacobian(logical_coords));

  const auto& x = get<0>(logical_coords);
  const auto& y = get<1>(logical_coords);
  const auto mass_density_cons = [&x, &y]() noexcept {
    return Scalar<DataVector>{
        {{1. + 0.2 * x - 0.1 * y + 0.05 * x * square(y)}}};
  }();
  const auto momentum_density = [&mesh]() noexcept {
    return tnsr::I<DataVector, 2>{DataVector(mesh.number_of_grid_points(), 0.)};
  }();
  const auto energy_density = [&mesh]() noexcept {
    return Scalar<DataVector>{DataVector(mesh.number_of_grid_points(), 1.)};
  }();

  std::unordered_map<std::pair<Direction<2>, ElementId<2>>,
                     NewtonianEuler::Limiters::Minmod<2>::PackagedData,
                     boost::hash<std::pair<Direction<2>, ElementId<2>>>>
      neighbor_data{};
  const std::array<std::pair<Direction<2>, ElementId<2>>, 4> dir_keys = {
      {{Direction<2>::lower_xi(), ElementId<2>(1)},
       {Direction<2>::upper_xi(), ElementId<2>(2)},
       {Direction<2>::lower_eta(), ElementId<2>(3)},
       {Direction<2>::upper_eta(), ElementId<2>(4)}}};
  for (const auto& dir_key : dir_keys) {
    neighbor_data[dir_key].element_size = element_size;
  }

  using mean_rho = Tags::Mean<NewtonianEuler::Tags::MassDensityCons>;
  get(get<mean_rho>(neighbor_data[dir_keys[0]].means)) = 0.4;
  get(get<mean_rho>(neighbor_data[dir_keys[1]].means)) = 1.1;
  get(get<mean_rho>(neighbor_data[dir_keys[2]].means)) = 2.1;
  get(get<mean_rho>(neighbor_data[dir_keys[3]].means)) = 0.9;

  for (const auto& dir_key : dir_keys) {
    using mean_rhou = Tags::Mean<NewtonianEuler::Tags::MomentumDensity<2>>;
    get<0>(get<mean_rhou>(neighbor_data[dir_key].means)) = 0.;
    get<1>(get<mean_rhou>(neighbor_data[dir_key].means)) = 0.;
    using mean_eps = Tags::Mean<NewtonianEuler::Tags::EnergyDensity>;
    get(get<mean_eps>(neighbor_data[dir_key].means)) = 1.;
  }

  test_neuler_vs_generic_minmod_work(
      mass_density_cons, momentum_density, energy_density, mesh, logical_coords,
      element_size, det_inv_logical_to_inertial_jacobian, neighbor_data);
}

void test_neuler_vs_generic_minmod_3d() noexcept {
  INFO("Testing NewtonianEuler::Limiters::Minmod limiter in 3D");
  const auto mesh =
      Mesh<3>(std::array<size_t, 3>{{3, 3, 4}}, Spectral::Basis::Legendre,
              Spectral::Quadrature::GaussLobatto);
  const auto element = TestHelpers::Limiters::make_element<3>();
  const auto logical_coords = logical_coordinates(mesh);
  using Affine = domain::CoordinateMaps::Affine;
  using Affine3D =
      domain::CoordinateMaps::ProductOf3Maps<Affine, Affine, Affine>;
  const Affine xi_map{-1., 1., 3.7, 4.2};
  const Affine eta_map{-1., 1., 3.2, 4.2};
  const Affine zeta_map{-1., 1., 1.3, 2.1};
  const Affine3D map3d{xi_map, eta_map, zeta_map};
  const auto coordmap =
      domain::make_coordinate_map_base<Frame::BlockLogical, Frame::Inertial>(
          map3d);
  const ElementMap<3, Frame::Inertial> element_map(element.id(),
                                                   coordmap->get_clone());
  const auto element_size = size_of_element(element_map);
  const auto det_inv_logical_to_inertial_jacobian =
      determinant(element_map.inv_jacobian(logical_coords));

  const auto& x = get<0>(logical_coords);
  const auto& y = get<1>(logical_coords);
  const auto& z = get<2>(logical_coords);
  const auto mass_density_cons = [&x, &y, &z]() noexcept {
    return Scalar<DataVector>{{{1. + 0.2 * x - 0.1 * y + 0.4 * z}}};
  }();
  const auto momentum_density = [&mesh]() noexcept {
    return tnsr::I<DataVector, 3>{DataVector(mesh.number_of_grid_points(), 0.)};
  }();
  const auto energy_density = [&mesh]() noexcept {
    return Scalar<DataVector>{DataVector(mesh.number_of_grid_points(), 1.)};
  }();

  std::unordered_map<std::pair<Direction<3>, ElementId<3>>,
                     NewtonianEuler::Limiters::Minmod<3>::PackagedData,
                     boost::hash<std::pair<Direction<3>, ElementId<3>>>>
      neighbor_data{};
  const std::array<std::pair<Direction<3>, ElementId<3>>, 6> dir_keys = {
      {{Direction<3>::lower_xi(), ElementId<3>(1)},
       {Direction<3>::upper_xi(), ElementId<3>(2)},
       {Direction<3>::lower_eta(), ElementId<3>(3)},
       {Direction<3>::upper_eta(), ElementId<3>(4)},
       {Direction<3>::lower_zeta(), ElementId<3>(5)},
       {Direction<3>::upper_zeta(), ElementId<3>(6)}}};
  for (const auto& dir_key : dir_keys) {
    neighbor_data[dir_key].element_size = element_size;
  }

  using mean_rho = Tags::Mean<NewtonianEuler::Tags::MassDensityCons>;
  get(get<mean_rho>(neighbor_data[dir_keys[0]].means)) = 0.;
  get(get<mean_rho>(neighbor_data[dir_keys[1]].means)) = 0.;
  get(get<mean_rho>(neighbor_data[dir_keys[2]].means)) = 0.1;
  get(get<mean_rho>(neighbor_data[dir_keys[3]].means)) = 1.2;
  get(get<mean_rho>(neighbor_data[dir_keys[4]].means)) = 1.1;
  get(get<mean_rho>(neighbor_data[dir_keys[5]].means)) = 0.9;

  for (const auto& dir_key : dir_keys) {
    using mean_rhou = Tags::Mean<NewtonianEuler::Tags::MomentumDensity<3>>;
    get<0>(get<mean_rhou>(neighbor_data[dir_key].means)) = 0.;
    get<1>(get<mean_rhou>(neighbor_data[dir_key].means)) = 0.;
    get<2>(get<mean_rhou>(neighbor_data[dir_key].means)) = 0.;
    using mean_eps = Tags::Mean<NewtonianEuler::Tags::EnergyDensity>;
    get(get<mean_eps>(neighbor_data[dir_key].means)) = 1.;
  }

  test_neuler_vs_generic_minmod_work(
      mass_density_cons, momentum_density, energy_density, mesh, logical_coords,
      element_size, det_inv_logical_to_inertial_jacobian, neighbor_data);
}

template <size_t VolumeDim>
void test_neuler_minmod_flattener() noexcept {
  INFO("Testing flattener use in NewtonianEuler::Limiters::Minmod limiter");
  CAPTURE(VolumeDim);

  const auto mesh = Mesh<VolumeDim>(2, Spectral::Basis::Legendre,
                                    Spectral::Quadrature::GaussLobatto);
  // We use an element with no neighbors so the limiter does nothing
  const auto element = Element<VolumeDim>{ElementId<VolumeDim>(0), {}};
  const auto logical_coords = logical_coordinates(mesh);
  const auto element_size = make_array<VolumeDim>(0.6);
  // inv_jac = logical volume / inertial volume
  const Scalar<DataVector> det_inv_logical_to_inertial_jacobian{DataVector(
      mesh.number_of_grid_points(), pow<VolumeDim>(2.) / pow<VolumeDim>(0.6))};
  const EquationsOfState::IdealFluid<false> equation_of_state{5. / 3.};

  const size_t num_points = mesh.number_of_grid_points();
  const auto input_density = [&num_points]() noexcept {
    // One negative value to trigger flattener
    Scalar<DataVector> density{DataVector(num_points, 0.8)};
    get(density)[0] = -0.2;
    return density;
  }();
  const auto input_momentum = [&num_points]() noexcept {
    return tnsr::I<DataVector, VolumeDim>{DataVector(num_points, 0.1)};
  }();
  const auto input_energy = [&num_points]() noexcept {
    return Scalar<DataVector>{DataVector(num_points, 1.4)};
  }();

  // Empty map because no neighbors
  const std::unordered_map<
      std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
      typename NewtonianEuler::Limiters::Minmod<VolumeDim>::PackagedData,
      boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>
      neighbor_data{};

  // First a sanity check: there should be a negative density for this test to
  // make sense... otherwise we aren't testing anything useful
  const bool has_negative_density = min(get(input_density)) < 0.;
  REQUIRE(has_negative_density);
  CHECK(true);

  // With flattener off, check the limiter does nothing (because no neighbors).
  //
  // Note that the post-limiter result is unphysical: the density is still
  // negative at one point. Because of this, we have to use the generic limiter
  // when testing the limiter does nothing: the specialized limiter has an
  // ASSERT that the post-limiting solution is valid which would fail here.
  auto density = input_density;
  auto momentum = input_momentum;
  auto energy = input_energy;
  const double tvb_constant = 0.;
  const Limiters::Minmod<
      VolumeDim, tmpl::list<NewtonianEuler::Tags::MassDensityCons,
                            NewtonianEuler::Tags::MomentumDensity<VolumeDim>,
                            NewtonianEuler::Tags::EnergyDensity>>
      minmod_noflat(Limiters::MinmodType::LambdaPi1, tvb_constant);
  const bool activated_noflat = minmod_noflat(
      make_not_null(&density), make_not_null(&momentum), make_not_null(&energy),
      mesh, element, logical_coords, element_size, neighbor_data);
  CHECK_FALSE(activated_noflat);

  // With flattener on, check limiter activates
  density = input_density;
  momentum = input_momentum;
  energy = input_energy;
  const auto vars_to_limit =
      NewtonianEuler::Limiters::VariablesToLimit::Conserved;
  const NewtonianEuler::Limiters::Minmod<VolumeDim> minmod(
      Limiters::MinmodType::LambdaPi1, vars_to_limit, tvb_constant, true);
  const bool activated = minmod(
      make_not_null(&density), make_not_null(&momentum), make_not_null(&energy),
      mesh, element, logical_coords, element_size,
      det_inv_logical_to_inertial_jacobian, equation_of_state, neighbor_data);
  CHECK(activated);

  // Finally, check the negative density is brought back to positive
  const bool density_is_positive_everywhere = min(get(density)) > 0.;
  CHECK(density_is_positive_everywhere);
}

}  // namespace

SPECTRE_TEST_CASE("Unit.Evolution.Systems.NewtonianEuler.Limiters.Minmod",
                  "[Limiters][Unit]") {
  test_neuler_minmod_option_parsing();
  test_neuler_minmod_serialization();

  // The package_data function for NewtonianEuler::Limiters::Minmod is just a
  // direct call to the generic Minmod package_data. We do not test it.

  // We test the action of NewtonianEuler::Limiters::Minmod by comparing it to
  // the action of the generic Minmod, for certain inputs where the comparison
  // is not too difficult. To fully test the specialized limiter, would need to
  // completely reimplement the logic within the test.
  test_neuler_vs_generic_minmod_1d();
  test_neuler_vs_generic_minmod_2d();
  test_neuler_vs_generic_minmod_3d();

  // Test the use of the post-processing flattener
  test_neuler_minmod_flattener<1>();
  test_neuler_minmod_flattener<2>();
  test_neuler_minmod_flattener<3>();
}


/*
  Copyright (c) Evgeny Shulgin <izaronplatz@gmail.com>, 2018

  Distributed under the license described in the LICENSE file
*/

// A hack allows to use asserts in Release mode
#undef NDEBUG

#include <iostream>
#include <limits>
#include <string>
#include <vector>

#include <boost/assert.hpp>
#include <boost/chrono.hpp>
#include <boost/container/static_vector.hpp>
#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/type_index.hpp>

#include "../include/bits.hpp"

//const int test_count = 100000;  // 10^5
//const int test_count = 1000000;  // 10^6
const int test_count = 10000000;  // 10^7

// Naive implementations of common bit algorithms
namespace naive_impl {

template <typename T>
BIT_ALGORITHM_FUNC_SPEC int bit_count(T value)
{
    int res = 0;
    while (value)
    {
        res += value & 1;
        value >>= 1;
    }
    return res;
}

template <typename T>
BIT_ALGORITHM_FUNC_SPEC bool parity(T value)
{
    int res = 0;
    while (value)
    {
        res ^= value & 1;
        value >>= 1;
    }
    return res;
}

template <typename T>
BIT_ALGORITHM_FUNC_SPEC int first_set(T value)
{
    if (value == 0)
        return 0;
    int pos = 1;
    while (!(value & 1))
    {
        pos++;
        value >>= 1;
    }
    return pos;
}

template <typename T>
BIT_ALGORITHM_FUNC_SPEC int last_set(T value)
{
    if (value == 0)
        return 0;
    int pos = 1;
    while (value)
    {
        pos++;
        value >>= 1;
    }
    return pos;
}

template <typename T>
BIT_ALGORITHM_FUNC_SPEC int trailing_zeros(T value)
{
    if (value == 0)
        return sizeof(T) * CHAR_BIT;
    return first_set(value) - 1;
}

template <typename T>
BIT_ALGORITHM_FUNC_SPEC int leading_zeros(T value)
{
    return sizeof(T) * CHAR_BIT - last_set(value);
}

} // naive_impl

// Generates test values of the type T,
// accepts only integral unsigned values
template<typename T>
typename boost::enable_if_c<
    boost::is_integral<T>::value && !std::numeric_limits<T>::is_signed,
    std::vector<T>
>::type gen_values()
{
    std::vector<T> res;
    res.reserve(test_count);

    T max_value = std::numeric_limits<T>::max();
    if (max_value <= test_count - 1) {
        for (T i = 0; res.size() < test_count; ++i) {
            res.push_back(i);
        }
    } else {
        // Using a true random number generator
        boost::random_device device;
        boost::random::uniform_int_distribution<T> random(
            0, std::numeric_limits<T>::max()
        );

        // Generate values
        for (int i = 0; i < test_count; ++i) {
            res.push_back(random(device));
        }
    }
    return res;
}

// Simply runs a function on values
template<typename T>
void raw_run(int (*func)(T),
    const std::vector<T>& vector)
{
    // Don't allow the compiler to "optimize" this part
    // by cutting this out, with "volatile" keyword
    volatile int res = 0;
    for (std::size_t i = 0; i < vector.size(); ++i)
        res += func(vector[i]);
}

// Runs functions on test values and measures running time
template<typename T>
void run_diff(int (*naive)(T), int (*fast)(T),
    const std::vector<T>& vector)
{
    using namespace boost::chrono;

    high_resolution_clock::time_point t1 = high_resolution_clock::now();
    raw_run(naive, vector);
    high_resolution_clock::time_point t2 = high_resolution_clock::now();

    duration<double> time_span = duration_cast<
        duration<double>
    >(t2 - t1);
    double t1_seconds = time_span.count();
    std::cout << "Naive solution ran \t" << t1_seconds
        << " seconds" << std::endl;

    t1 = high_resolution_clock::now();
    raw_run(fast, vector);
    t2 = high_resolution_clock::now();

    time_span = duration_cast< duration<double> >(t2 - t1);
    double t2_seconds = time_span.count();
    std::cout << "Fast solution ran  \t" << t2_seconds
        << " seconds" << std::endl;

    std::cout << "\t\t\tThe fast solution relative speed: " << (t2_seconds / t1_seconds * 100.0)
        << "\%" << std::endl;
}

// Asserts equality of both naive (which is correct by default)
// and fast functions on the same values
template<typename T>
void check_equality(int (*naive)(T), int (*fast)(T),
    const std::vector<T>& vector)
{
    for (std::size_t i = 0; i < vector.size(); ++i)
    {
        int naive_outcome = naive(vector[i]);
        int fast_outcome = fast(vector[i]);
        BOOST_ASSERT(naive_outcome == fast_outcome);
    }
}

// Checks both naive and fast functions on correctness
// and measures their running time
template<typename T>
void work_with_functions(const std::string& func_name,
    int (*naive)(T), int (*fast)(T),
    const std::vector<T>& vector)
{
    std::cout << "WORKING WITH\t\t" << func_name << std::endl;
    std::cout << "THE TYPE IS\t\t" << boost::typeindex::type_id<T>() << std::endl;

    std::cout << "Check correctness..." << std::endl;
    check_equality(naive, fast, vector);
    std::cout << "The fast realization is correct!" << std::endl;

    std::cout << "Measuring time..." << std::endl;
    run_diff(naive, fast, vector);

    std::cout << std::endl << std::endl;
}

template<typename T>
void benchmark()
{
    std::vector<T> vector = gen_values<T>();

    work_with_functions("bit_count", naive_impl::bit_count<T>,
        bit_algo::bit_count<T>, vector);
    //work_with_functions("parity", naive_impl::parity<T>,
    //    bit_algo::parity<T>, vector);
    work_with_functions("first_set", naive_impl::first_set<T>,
        bit_algo::first_set<T>, vector);
}

int main()
{
#ifdef BOOST_NO_CXX11_CONSTEXPR
    std::cout << "NO C++11!" << std::endl;
#endif

#ifdef BOOST_NO_CXX14_CONSTEXPR
    std::cout << "NO C++14!" << std::endl;
#endif

#ifdef BIT_ALGORITHM_MSVC
    std::cout << "IT'S MSVS!" << std::endl;
#endif

#ifdef BIT_ALGORITHM_MSVC_64
    std::cout << "IT'S MSVS 64-bit!" << std::endl;
#endif

    benchmark<unsigned char>();
    benchmark<unsigned short>();
    benchmark<unsigned int>();
    benchmark<unsigned long>();
    benchmark<unsigned long long>();
}


#include <fstream>
#include <iostream>
#include <string>

#include <glog/logging.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <Eigen/Geometry>
#include <boost/format.hpp>  // for formating strings
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>

using std::cerr;
using std::endl;
using std::cout;
using std::vector;
using std::ifstream;
using std::string;
using PointT = pcl::PointXYZRGB;
using PointCloud = pcl::PointCloud<PointT>;

int main(int argc, char **argv) {
  LOG_IF(FATAL, argc < 3)
      << "Usage: ./pointcloud_mapping path_2_pose_file path_2_data";

  ifstream fin(argv[1]);
  LOG_IF(FATAL, !fin) << "Cannot find pose file " << argv[1];

  vector<cv::Mat> imgs_color, imgs_depth;  // color images and depth images
  vector<Eigen::Isometry3d> poses;         // Twc

  string prefix(argv[2]);
  if (prefix.back() != '/') {
    prefix.append("/");
  }

  std::vector<double> pose;
  for (int i = 0; i < 5; ++i) {
    boost::format fmt(prefix + "%s/%d.%s");  // image file format
    imgs_color.emplace_back(
        cv::imread((fmt % "color" % (i + 1) % "png").str()));
    imgs_depth.emplace_back(cv::imread((fmt % "depth" % (i + 1) % "png").str(),
                                       cv::IMREAD_UNCHANGED));

    for (int j = 0; j < 7; ++j) {
      fin >> pose[j];
    }
    Eigen::Quaterniond q(pose[6], pose[3], pose[4], pose[5]);
    Eigen::Isometry3d T(q);
    T.pretranslate(Eigen::Vector3d(pose[0], pose[1], pose[2]));
    poses.emplace_back(T);
  }

  // Intrinsic parameters
  const double cx = 319.5;
  const double cy = 239.5;
  const double fx = 481.2;
  const double fy = -480.0;
  const double depth_scale = 5000.0;

  cout << "Converting images to point cloud..." << endl;

  // Create a new point cloud
  PointCloud::Ptr point_cloud(new PointCloud);
  for (int i = 0; i < 5; ++i) {
    PointCloud::Ptr current(new PointCloud);
    cout << "Converting image: " << i + 1 << endl;
    const cv::Mat color = imgs_color[i];
    const cv::Mat depth = imgs_depth[i];
    const Eigen::Isometry3d T = poses[i];
    for (int v = 0; v < color.rows; ++v) {
      for (int u = 0; u < color.cols; ++u) {
        const unsigned int d = depth.ptr<unsigned short>(v)[u];  // depth value
        if (d == 0) {
          // 0 means data unavailable
          continue;
        }
        Eigen::Vector3d pc;
        pc[2] = static_cast<double>(d) / depth_scale;
        pc[0] = (u - cx) * pc[2] / fx;
        pc[1] = (v - cy) * pc[2] / fy;
        const Eigen::Vector3d pw = T * pc;

        PointT p;
        p.x = pw[0];
        p.y = pw[1];
        p.z = pw[2];
        p.b = color.data[v * color.step + u * color.channels()];
        p.g = color.data[v * color.step + u * color.channels() + 1];
        p.r = color.data[v * color.step + u * color.channels() + 2];
        current->points.emplace_back(p);
      }
    }
    // depth filter and statistical removal
    PointCloud::Ptr tmp(new PointCloud);
    pcl::StatisticalOutlierRemoval<PointT> statistical_filter;
    statistical_filter.setMeanK(50);
    statistical_filter.setStddevMulThresh(1.0);
    statistical_filter.setInputCloud(current);
    statistical_filter.filter(*tmp);
    (*point_cloud) += *tmp;
  }

  point_cloud->is_dense = false;
  cout << "There are " << point_cloud->size() << " points in our point cloud."
       << endl;

  // voxel filter
  pcl::VoxelGrid<PointT> voxel_filter;
  double resolution = 0.03;
  voxel_filter.setLeafSize(resolution, resolution, resolution);  // resolution
  PointCloud::Ptr tmp(new PointCloud);
  voxel_filter.setInputCloud(point_cloud);
  voxel_filter.filter(*tmp);
  tmp->swap(*point_cloud);

  cout << "After filtering，there are total " << point_cloud->size()
       << " points in our point cloud." << endl;

  pcl::io::savePCDFileBinary("map.pcd", *point_cloud);

  return 0;
}


// Author(s): Camille Wormser, Pierre Alliez
// Example of an AABB tree used with indexed triangle set

#include <iostream>
#include <list>
#include <boost/iterator/iterator_adaptor.hpp>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>

typedef CGAL::Simple_cartesian<double> K;

// My own point type:
struct My_point {
    double x;
    double y;
    double z;

    My_point (double _x, double _y, double _z)
        : x(_x), y(_y), z(_z) {}
};

// The triangles are stored in a flat array of indices
// referring to an array of points: three consecutive
// indices represent a triangle.
typedef std::vector<size_t>::const_iterator Point_index_iterator;

// Let us now define the iterator on triangles that the tree needs:
class Triangle_iterator
    : public boost::iterator_adaptor<
    Triangle_iterator               // Derived
    , Point_index_iterator            // Base
    , boost::use_default              // Value
    , boost::forward_traversal_tag    // CategoryOrTraversal
    >
{
public:
    Triangle_iterator()
        : Triangle_iterator::iterator_adaptor_() {}

    explicit Triangle_iterator(Point_index_iterator p)
        : Triangle_iterator::iterator_adaptor_(p) {}

private:
    friend class boost::iterator_core_access;
    void increment() { this->base_reference() += 3; }
};

// The following primitive provides the conversion facilities between
// my own triangle and point types and the CGAL ones
struct My_triangle_primitive {
public:
    typedef Triangle_iterator    Id;

    // the CGAL types returned
    typedef K::Point_3    Point;
    typedef K::Triangle_3 Datum;

    // a static pointer to the vector containing the points
    // is needed to build the triangles on the fly:
    static const std::vector<My_point>* point_container;

private:
    Id m_it; // this is what the AABB tree stores internally

public:
    My_triangle_primitive() {} // default constructor needed

    // the following constructor is the one that receives the iterators from the
    // iterator range given as input to the AABB_tree
    My_triangle_primitive(Triangle_iterator a)
        : m_it(a) {}

    Id id() const { return m_it; }

    // on the fly conversion from the internal data to the CGAL types
    Datum datum() const
    {
        Point_index_iterator p_it = m_it.base();
        const My_point& mp = (*point_container)[*p_it];
        Point p(mp.x, mp.y, mp.z);
        ++p_it;
        const My_point& mq = (*point_container)[*p_it];
        Point q(mq.x, mq.y, mq.z);
        ++p_it;
        const My_point& mr = (*point_container)[*p_it];
        Point r(mr.x, mr.y, mr.z);

        return Datum(p, q, r); // assembles triangle from three points
    }

    // one point which must be on the primitive
    Point reference_point() const
    {
        const My_point& mp = (*point_container)[*m_it];
        return Point(mp.x, mp.y, mp.z);
    }
};

// types
typedef CGAL::AABB_traits<K, My_triangle_primitive> My_AABB_traits;
typedef CGAL::AABB_tree<My_AABB_traits> Tree;
const std::vector<My_point>* My_triangle_primitive::point_container = 0;

int main()
{
    // generates point set
    My_point a(1.0, 0.0, 0.0);
    My_point b(0.0, 1.0, 0.0);
    My_point c(0.0, 0.0, 1.0);
    My_point d(0.0, 0.0, 0.0);

    std::vector<My_point> points;
    My_triangle_primitive::point_container = &points;
    points.push_back(a);
    points.push_back(b);
    points.push_back(c);
    points.push_back(d);

    // generates indexed triangle set
    std::vector<size_t> triangles;
    triangles.push_back(0); triangles.push_back(1); triangles.push_back(2);
    triangles.push_back(0); triangles.push_back(1); triangles.push_back(3);
    triangles.push_back(0); triangles.push_back(3); triangles.push_back(2);

    // constructs AABB tree
    Tree tree(Triangle_iterator(triangles.begin()),
        Triangle_iterator(triangles.end()));

    // counts #intersections
    K::Ray_3 ray_query(K::Point_3(0.2, 0.2, 0.2), K::Point_3(0.0, 1.0, 0.0));
    std::cout << tree.number_of_intersected_primitives(ray_query)
        << " intersections(s) with ray query" << std::endl;

    // computes closest point
    K::Point_3 point_query(2.0, 2.0, 2.0);
    K::Point_3 closest_point = tree.closest_point(point_query);

    std::cerr << "closest point is: " << closest_point << std::endl;
    return EXIT_SUCCESS;
}


#include <iostream>
#include <cassert>
#include <vector>
#include <iomanip>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> GraphTraits;
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, GraphTraits::edge_descriptor,
                                                                              boost::property<boost::edge_weight_t, long>>>>>
    Graph;

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

struct Booking
{
  int s, t, d, a, p;
};

const int max_possible_profit = 100;

class TimeIndexer
{
public:
  void add_time(int t)
  {
    time_set.insert(t);
  }

  void rebuild()
  {
    time_vector.resize(time_set.size());
    std::copy(time_set.begin(), time_set.end(), time_vector.begin());
  }

  int time_to_index(int t) const
  {
    const auto it = std::lower_bound(time_vector.begin(), time_vector.end(), t);
    assert(it != time_vector.end() && *it == t);
    return it - time_vector.begin();
  }

  int index_to_time(int i) const
  {
    return time_vector.at(i);
  }

  int size() const
  {
    return time_set.size();
  }

private:
  std::set<int> time_set;
  std::vector<int> time_vector;
};

void testcase()
{
  int n, s;
  std::cin >> n >> s;
  assert(n >= 1 && n <= 10000 && s >= 2 && s <= 10);

  int total_cars = 0;
  std::vector<int> initial_cars_by_station(s);
  for (int &l : initial_cars_by_station)
  {
    std::cin >> l;
    assert(l >= 0 && l <= 100);
    total_cars += l;
  }

  std::vector<Booking> bookings(n);
  for (Booking &b : bookings)
  {
    std::cin >> b.s >> b.t >> b.d >> b.a >> b.p;
    assert(b.s >= 1 && b.s <= s && b.t >= 1 && b.t <= s);
    b.s--;
    b.t--;
    assert(b.d >= 0 && b.d < b.a && b.a <= 100000);
    assert(b.p >= 1 && b.p <= max_possible_profit);
  }

  TimeIndexer global_time_indexer;
  std::vector<TimeIndexer> local_time_indexers(s);
  for (Booking &b : bookings)
  {
    global_time_indexer.add_time(b.d);
    global_time_indexer.add_time(b.a);
    local_time_indexers.at(b.s).add_time(b.d);
    local_time_indexers.at(b.t).add_time(b.a);
  }

  global_time_indexer.rebuild();
  for (TimeIndexer &local_time_indexer : local_time_indexers)
  {
    // IMPORTANT The first and last global time must exist at every station, otherwise
    // the max_possible_profit * global_time_slot_delta compensation won't work correctly
    local_time_indexer.add_time(global_time_indexer.index_to_time(0));
    local_time_indexer.add_time(global_time_indexer.index_to_time(global_time_indexer.size() - 1));

    local_time_indexer.rebuild();
  }

  std::vector<int> local_time_slot_cumulative_sum{0};
  for (int i = 0; i < s; i++)
  {
    local_time_slot_cumulative_sum.push_back(local_time_slot_cumulative_sum.back() + local_time_indexers.at(i).size());
  }

  int next_free_node = 0;
  const int node_source = next_free_node++;
  const int node_target = next_free_node++;
  const auto get_station_time_node = [next_free_node, s, &local_time_indexers, &local_time_slot_cumulative_sum](const int i_time, const int i_station) {
    assert(i_station >= 0 && i_station < s && i_time >= 0 && i_time < local_time_indexers.at(i_station).size());
    return next_free_node + local_time_slot_cumulative_sum.at(i_station) + i_time;
  };
  next_free_node += local_time_slot_cumulative_sum.back();
  const auto get_trip_node = [next_free_node, n](const int i_trip) {
    assert(i_trip >= 0 && i_trip < n);
    return next_free_node + i_trip;
  };
  next_free_node += n;
  const int num_nodes = next_free_node;
  Graph G(num_nodes);

  const auto add_edge = [&G](int from, int to, long capacity, long cost) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    auto w_map = boost::get(boost::edge_weight, G);
    const Graph::edge_descriptor e = boost::add_edge(from, to, G).first;
    const Graph::edge_descriptor rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0;
    r_map[e] = rev_e;
    r_map[rev_e] = e;
    w_map[e] = cost;
    w_map[rev_e] = -cost;
  };

  for (int i_station = 0; i_station < s; i_station++)
  {
    const TimeIndexer &local_time_indexer = local_time_indexers.at(i_station);

    add_edge(node_source, get_station_time_node(0, i_station), initial_cars_by_station.at(i_station), 0);
    add_edge(get_station_time_node(local_time_indexer.size() - 1, i_station), node_target, total_cars, 0);

    for (int i_time = 1; i_time < local_time_indexer.size(); i_time++)
    {
      const int global_time_slot_delta = global_time_indexer.time_to_index(local_time_indexer.index_to_time(i_time)) - global_time_indexer.time_to_index(local_time_indexer.index_to_time(i_time - 1));
      assert(global_time_slot_delta > 0);
      add_edge(get_station_time_node(i_time - 1, i_station), get_station_time_node(i_time, i_station), total_cars, global_time_slot_delta * max_possible_profit);
    }
  }

  for (int i_trip = 0; i_trip < n; i_trip++)
  {
    Booking &b = bookings.at(i_trip);
    const int i_d = local_time_indexers.at(b.s).time_to_index(b.d), i_a = local_time_indexers.at(b.t).time_to_index(b.a);
    const int global_time_slot_delta = global_time_indexer.time_to_index(b.a) - global_time_indexer.time_to_index(b.d);
    assert(global_time_slot_delta > 0);
    add_edge(get_station_time_node(i_d, b.s), get_trip_node(i_trip), 1, max_possible_profit * global_time_slot_delta - b.p);
    add_edge(get_trip_node(i_trip), get_station_time_node(i_a, b.t), 1, 0);
  }

  boost::successive_shortest_path_nonnegative_weights(G, node_source, node_target);
  const int flow_cost = boost::find_flow_cost(G);
  const int total_profit = total_cars * (global_time_indexer.size() - 1) * max_possible_profit - flow_cost;
  std::cout << total_profit << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);
  std::cout << std::fixed << std::setprecision(0);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
    DEBUG(1, "");
  }

  return 0;
}


#include <frovedis.hpp>
#include <frovedis/ml/clustering/kmeans.hpp>

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

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    // loading the input data matrix
    auto data = make_crs_matrix_load<double> ("./data");

    // setting the algorithm input parameters
    size_t num_iteration = 100;
    size_t k = 2;
    double eps = 0.01;

    // getting the trained model
    auto model = kmeans(data, k, num_iteration, eps);

    // assigning
    auto mat = make_crs_matrix_local_load<double>("./data");
    auto res = kmeans_assign_cluster(mat, model);
    //for(auto &i: r) cout << i << " "; cout << endl;

    // confirming correctness of result
    std::vector<int> expected = {0, 1, 1, 0, 0, 1};
    BOOST_CHECK (res == expected);
}