// std includes
#include <iostream> // cout, endl
#include <vector>
#include <random> // random_device, default_random_engine, uniform_int_distribution
#include <algorithm> // generate
// thirdparties includes
#include <Eigen/Dense>
// lib includes
#include "m0sh/non_uniform.h"

using TypeScalar = double;
// Space
const unsigned int DIM = 3;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
template<typename ...Args>
using TypeRef = Eigen::Ref<Args...>;
// Mesh
template<typename ...Args>
using TypeContainer = std::vector<Args...>;
using TypeMesh = m0sh::NonUniform<TypeVector, TypeRef, TypeContainer>;
// Data
const std::size_t n = 10;
const double dx = 0.1;

void print(const TypeMesh& mesh, std::uniform_int_distribution<int>& uniform, std::default_random_engine& e) {
    TypeContainer<int> ijk;
    TypeVector x;
    std::size_t index;
    // Print info
    ijk = {uniform(e), uniform(e), uniform(e)};
    x = mesh.positionPoint(ijk);
    index = mesh.indexPoint(ijk);
    std::cout << "i: " << ijk[0] << " j: " << ijk[1] << " k: " << ijk[2] << "\nindex: " << index << "\nx: " << x.transpose() << std::endl;
    ijk = mesh.ijkPoint(x);
    std::cout << "xReverse: " << " i: " << ijk[0] << " j: " << ijk[1] << " k: " << ijk[2] << std::endl;
    ijk = mesh.ijkPoint(index);
    std::cout << "indexReverse: " << " i: " << ijk[0] << " j: " << ijk[1] << " k: " << ijk[2] << std::endl;
    std::cout << std::endl;
}

int main () {
    // Build axis
    TypeContainer<double> axis(n+1);
    std::generate(axis.begin(), axis.end(), [x = 0.0] () mutable { return x += dx; });
    // Build grid
    TypeContainer<TypeContainer<double>> grid(DIM, axis);
    // Build mesh finally
    TypeMesh mesh(grid, TypeContainer<bool>(DIM, false));
    // Random setup
    std::random_device r;
    std::default_random_engine e(r());
    std::uniform_int_distribution<int> uniform(0, n);
    // Print
    print(mesh, uniform, e);
    print(mesh, uniform, e);
    print(mesh, uniform, e);
}


#include <cstdlib>
#include <iostream>
#include <complex>
#include <type_traits>
#include <algorithm>
#define BOOST_UBLAS_NO_ELEMENT_PROXIES
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/hermitian.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/banded.hpp>
#include <boost/numeric/bindings/ublas/hermitian.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include "print.hpp"
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

int main(int argc, char *argv[]) {
  {
    typedef std::complex<double> complex;
    typedef ublas::vector<complex> vector;
    typedef ublas::banded_matrix<complex, ublas::column_major> matrix;
    typedef typename std::make_signed<vector::size_type>::type size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    matrix A(n, n, 2, 2);
    matrix A_u(n, n, 0, 2), A_l(n, n, 2, 0);
    for (size_type j=0; j<n; ++j) {
      A(j, j)=rand_normal<complex>::get().real();
      A_u(j, j)=A(j, j);
      A_l(j, j)=A(j, j);
      for (size_type i=std::max(size_type(0), j-2); i<j; ++i) {
	A(i, j)=rand_normal<complex>::get();
	A(j, i)=std::conj(A(i, j));
	A_u(i, j)=A(i, j);
	A_l(j, i)=A(j, i);
      }
    }
    ublas::hermitian_adaptor<matrix, ublas::upper> B_u(A_u);
    ublas::hermitian_adaptor<matrix, ublas::lower> B_l(A_l);
    vector x(n);
    for (size_type i=0; i<n; ++i)
      x(i)=rand_normal<complex>::get();
    vector y(n);
    for (size_type i=0; i<n; ++i)
      y(i)=rand_normal<complex>::get();
    complex alpha(rand_normal<complex>::get());
    complex beta(rand_normal<complex>::get());
    vector y1(alpha*ublas::prod(A, x)+beta*y);
    vector y2(y);
    blas::hbmv(alpha, B_l, x, beta, y2);
    vector y3(y);
    blas::hbmv(alpha, B_u, x, beta, y3);
    std::cout << "testing boost::ublas containers\n"
    	      << "using ublas       : " << print_vec(y1) << '\n'
	      << "using blas (lower): " << print_vec(y2) << '\n'
	      << "using blas (upper): " << print_vec(y3) << '\n'
    	      << '\n';
  }
  return EXIT_SUCCESS;
}


#include "stdafx.h"
#include <chrono>
#include <iostream>

#include <fmt/format.h>

#include "timer.h"
#include "judge.h"

#define USE_BOOST_GEOMETRY
#if defined(USE_BOOST_GEOMETRY)
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

namespace bg = boost::geometry;
using BoostPoint = bg::model::d2::point_xy<double>;
using BoostPolygon = bg::model::polygon<BoostPoint>;
using BoostLinestring = bg::model::linestring<BoostPoint>;

template <typename T>
BoostPoint ToBoostPoint(const T& point) {
  const auto [x, y] = point;
  return BoostPoint(x, y);
}

template <typename T>
BoostPolygon ToBoostPolygon(const std::vector<T>& points) {
  BoostPolygon polygon;
  for (std::size_t i = 0; i <= points.size(); ++i) {
    polygon.outer().push_back(ToBoostPoint(points[i % points.size()]));
  }
  if (bg::area(polygon) < 0.0) {
    bg::reverse(polygon);
  }
  return polygon;
}

template <typename T, typename U>
double SquaredDistance(const T& vertex0, const U& vertex1) {
  const auto [x0, y0] = vertex0;
  const auto [x1, y1] = vertex1;
  return (x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1);
}

template <typename T>
double SquaredEdgeLength(const T& vertices, const Edge& edge) {
  const auto [a, b] = edge;
  return SquaredDistance(vertices[a], vertices[b]);
}

void test_bg() {
    typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

    polygon blue;
    boost::geometry::read_wkt(
        "POLYGON((0 0, 10 0, 10 5, 5 5, 5 10, 0 10))", blue);

    if (bg::area(blue) < 0.0) {
      bg::reverse(blue);
    }

    BoostLinestring ls({ToBoostPoint(Point(5.0, 8.0)), ToBoostPoint(Point(8.0, 5.0))});

    LOG(INFO) << "linestring coverd by blue?:" << boost::geometry::covered_by(ls, blue);

}
#endif

SJudgeResult judge(const SProblem& problem, const SSolution& solution) {
  SJudgeResult res;
  for (auto& b : solution.bonuses) {
    if (b.type == SBonus::Type::GLOBALIST) {
      res.is_globalist_mode = true;
    }
    if (b.type == SBonus::Type::WALLHACK) {
      res.is_wallhack_mode = true;
    }
    if (b.type == SBonus::Type::SUPERFLEX) {
      res.is_superflex_mode = true;
    }
  }

  // all figure points are inside the hole.
  for (size_t ivert = 0; ivert < solution.vertices.size(); ++ivert) {
    // (c) Every point located on any line segment of the figure in the assumed pose must either lay inside the hole, or on its boundary
    if (contains(problem.hole_polygon, solution.vertices[ivert]) == EContains::EOUT) {
      res.out_of_hole_vertices.push_back(ivert);
    }
  }

  if (res.is_wallhack_mode) {
    if (res.out_of_hole_vertices.size() == 1) {
      res.wallhacking_index = res.out_of_hole_vertices[0];
    }
  }

  // figure does not intersect with edges of the hole.
#if defined(USE_BOOST_GEOMETRY)
  //test_bg();

  auto hole_polygon_ = ToBoostPolygon(problem.hole_polygon);
  for (size_t iedge = 0; iedge < problem.edges.size(); ++iedge) {
    const auto [a, b] = problem.edges[iedge];
    BoostLinestring linestring{ToBoostPoint(solution.vertices[a]), ToBoostPoint(solution.vertices[b])};
    if (!bg::covered_by(linestring, hole_polygon_)) {
      const bool wallhacking_edge = res.wallhacking_index.value_or(-1) == a || res.wallhacking_index.value_or(-1) == b;
      if (!wallhacking_edge) {
        res.out_of_hole_edges_except_wallhack.push_back(iedge);
      }
      res.out_of_hole_edges.push_back(iedge);
    }
  }
#else

  std::vector<Point2d> hole_polygon_d;
  for (auto& p : problem.hole_polygon) {
    hole_polygon_d.emplace_back(p.first, p.second);
  }

  for (size_t iedge = 0; iedge < problem.edges.size(); ++iedge) {
    const auto& edge = problem.edges[iedge];
    auto moved_i = solution.vertices[edge.first];
    auto moved_j = solution.vertices[edge.second];
    const Line figure_segment = {moved_i, moved_j};

    bool intersects = false;
    for (size_t ihole = 0; ihole < problem.hole_polygon.size(); ++ihole) {
      auto h0 = problem.hole_polygon[ihole];
      auto h1 = problem.hole_polygon[(ihole + 1) % problem.hole_polygon.size()];
      const Line hole_segment = {h0, h1};
      if (intersectSS_strict(figure_segment, hole_segment)) {
        intersects = true;
        break;
      }
    }
    if (!intersects
      && contains(problem.hole_polygon, moved_i) == EContains::EON
      && contains(problem.hole_polygon, moved_j) == EContains::EON
      ) {
      bool on_edge = false;
      for (size_t ihole = 0; ihole < problem.hole_polygon.size(); ++ihole) {
        auto h0 = problem.hole_polygon[ihole];
        auto h1 = problem.hole_polygon[(ihole + 1) % problem.hole_polygon.size()];
        if ((moved_i == h0 && moved_j == h1) ||
            (moved_i == h1 && moved_j == h0)) {
          on_edge = true;
          break;
        }
      }
      if (!on_edge) {
        // if moved_i, moved_j is exactly on the hole polygon, intersectSS / bg::difference always say that
        // the segment is INSIDE the polygon which is wrong when it is concave.
        // dirty hack:
        const double p = 1e-6;
        const Point2d interp {
          get_x(moved_i) * p + get_x(moved_j) * (1.0 - p) ,
          get_y(moved_i) * p + get_y(moved_j) * (1.0 - p) ,
        };
        if (contains(hole_polygon_d, interp) == EContains::EOUT) {
          intersects = true;
        }
      }
    }
    if (intersects) {
      res.out_of_hole_edges.push_back(iedge);
    }
  }
#endif

  // stretch
  if (res.is_globalist_mode) {
    double globalist = 0.0;
    for (size_t iedge = 0; iedge < problem.edges.size(); ++iedge) {
      const auto& edge = problem.edges[iedge];
      auto org_i = problem.vertices[edge.first];
      auto org_j = problem.vertices[edge.second];
      auto moved_i = solution.vertices[edge.first];
      auto moved_j = solution.vertices[edge.second];
      globalist += std::abs(double(distance2(moved_i, moved_j)) / double(distance2(org_i, org_j)) - 1);
    }
    if (globalist * 1000000.0 > problem.edges.size() * problem.epsilon) {
      res.violates_globalist = true;
    }
  } else {
    for (size_t iedge = 0; iedge < problem.edges.size(); ++iedge) {
      const auto& edge = problem.edges[iedge];
      auto org_i = problem.vertices[edge.first];
      auto org_j = problem.vertices[edge.second];
      auto moved_i = solution.vertices[edge.first];
      auto moved_j = solution.vertices[edge.second];
      if (!tolerate(distance2(org_i, org_j), distance2(moved_i, moved_j), problem.epsilon)) {
        res.stretch_violating_edges.push_back(iedge);
      }
    }
    if (res.is_superflex_mode && res.stretch_violating_edges.size() == 1) {
      res.superflex_index = res.stretch_violating_edges[0];
    }
  }

  // dislikes
  res.individual_dislikes.assign(problem.hole_polygon.size(), 0);
  for (size_t ihole = 0; ihole < problem.hole_polygon.size(); ++ihole) {
    const auto& h = problem.hole_polygon[ihole];
    integer minval = std::numeric_limits<integer>::max();
    for (size_t ivert = 0; ivert < solution.vertices.size(); ++ivert) {
      const auto& v = solution.vertices[ivert];
      minval = std::min(minval, distance2(h, v));
    }
    res.individual_dislikes[ihole] = minval;
    res.dislikes += minval;
  }

  // gain bonus
  res.gained_bonus_indices.clear();
  for (size_t ibonus = 0; ibonus < problem.bonuses.size(); ++ibonus) {
    for (size_t ivert = 0; ivert < solution.vertices.size(); ++ivert) {
      if (problem.bonuses[ibonus].position == solution.vertices[ivert]) {
        res.gained_bonus_indices.push_back(ibonus);
        break;
      }
    }
  }

  return res;
}

bool update_judge(const SProblem& problem, const SJudgeResult& res, nlohmann::json& solution_json) {
  if (solution_json.find("meta") == solution_json.end()) solution_json["meta"] = {};
  auto& meta_json = solution_json["meta"];
  if (meta_json.find("judge") == meta_json.end()) meta_json["judge"] = {};

  meta_json["judge"]["dislikes"] = res.dislikes;
  meta_json["judge"]["fit_in_hole"] = res.fit_in_hole();
  meta_json["judge"]["satisfy_stretch"] = res.satisfy_stretch();
  meta_json["judge"]["is_valid"] = res.is_valid();

  meta_json["judge"]["gained_bonuses"] = nlohmann::json::array();
  for (auto bid : res.gained_bonus_indices) {
    meta_json["judge"]["gained_bonuses"].push_back({
      {"position", problem.bonuses[bid].position},
      {"bonus", SBonus::bonus_name(problem.bonuses[bid].type)},
      {"problem", problem.bonuses[bid].problem_id},
      });
  }

  return true;
}


#include "scene.hpp"

#include "intersection_info.hpp"
#include "object.hpp"

#include <math/ray3d.hpp>

#include <boost/optional.hpp>
#include <boost/none.hpp>

#include <algorithm>
#include <numeric>

namespace core
{

bool intersects(const scene& scn, math::ray3d ray)
{
  return std::any_of(std::begin(scn.objects), std::end(scn.objects), [=](const object& obj){
    return intersects(obj, ray);
  });
}

boost::optional<core::intersection_info> closest_intersection(const scene& scn, math::ray3d ray)
{
  core::intersection_info info;
  float min_dist = std::numeric_limits<float>::infinity();

  for (auto &obj : scn.objects)
  {
    auto possible_inter = closest_intersection(obj, ray);

    if (possible_inter.is_initialized() == false) { continue; }

    auto curr_dist = math::distance(ray.origin, possible_inter.get().point);
    if (curr_dist < min_dist)
    {
      min_dist = curr_dist;
      info.obj = &obj;
      info.shape_info = possible_inter.get();
    }
  }

  if (min_dist == std::numeric_limits<float>::infinity()) { return boost::none; }

  return info;
}

}


//
//  meanie3D-detect
//  cf-algorithms
//
//  Created by Jürgen Lorenz Simon on 5/3/12.
//  Copyright (c) 2012 Jürgen Lorenz Simon. All rights reserved.
//

#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <boost/progress.hpp>
#include <boost/date_time.hpp>
#include <boost/date_time/local_time/local_time.hpp>
#include <sstream>

#include <meanie3D/meanie3D.h>
#include <radolan/radolan.h>

#include <map>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <fstream>
#include <exception>
#include <locale>
#include <limits>
#include <stdlib.h>
#include <netcdf>
#include <time.h>
#include <algorithm>

using namespace std;
using namespace boost;
using namespace netCDF;
using namespace m3D;

namespace fs = boost::filesystem;

typedef enum
{
    ShiftedPropertiesSatellite = 0,
    ShiftedPropertiesOthers = 1,
} ShiftedProperties;

#pragma mark -
#pragma mark Definitions

#define WRITE_PARALLAX_VECTORS 1

/** Feature-space data type
 */
typedef double T;

#pragma mark -
#pragma mark Command line parsing

void parse_commmandline(program_options::variables_map vm,
        string &filename,
        ShiftedProperties &shifted)
{
    if (vm.count("file") == 0)
    {
        cerr << "Missing 'file' argument" << endl;

        exit(1);
    }

    filename = vm["file"].as<string>();

    std::string shifted_name = vm["shifted"].as<string>();

    if (!(shifted_name == "satellite" || shifted_name == "other"))
    {
        cerr << "Illegal value for argument 'shifted'. Only 'satellite' or 'others' are accepted." << endl;
        exit(1);
    }

    if (shifted_name == "satellite")
    {
        shifted = ShiftedPropertiesSatellite;
    } else
    {
        shifted = ShiftedPropertiesOthers;
    }
};

#pragma mark -
#pragma mark Worker Methods

template <typename T>
T SQR(const T &x)
{
    return x * x;
}

/** This is a c++ translation of the method sent to me by Marianne Koenig (in F90):
 *
 * <cite>
 * Subroutine does a parallax correction for something seen at some
 * height in a position lat/lon by the satellite given by satheight,
 * satlat, satlon. The new coordinates are returned in latcorr and loncorr
 * </cite>
 *
 * @param satheight (REAL): height of the satellite in km
 * @param satlat (REAL): subsatellite latitude (deg, N is positive)
 * @param satlon (REAL): subsatellite longitude (deg, E is positive)
 * @param height (REAL): height of the cloud (km)
 * @param lat (REAL): latitude of the satellite pixel (N is positive)
 * @param lon (REAL): longitude of the satellite pixel (E is positive)
 */
template <typename T>
void parallax(T satheight, T satlat, T satlon, T height, T lat, T lon, T& latcorr, T& loncorr)
{
    T dpi;
    T radius_eq;
    T radius_pole;
    T radius_ratio;
    T mean_radius;
    T dheight;
    T alat, alon;
    T asatlat, asatlon;
    T satlat_geod, satlon_geod;
    T xsat, ysat, zsat;
    T xsurf, ysurf, zsurf;
    T alat_geod;
    T radius_surf;
    T radius_ratio_local;
    T xdiff, ydiff, zdiff;
    T xfact, zen;
    T e1, e2, e3;
    T corr;
    T xcorr, ycorr, zcorr;

    dpi = 3.14159265;

    // varius earth radius information

    radius_eq = 6378.077;
    dheight = satheight;
    radius_pole = 6356.577;
    radius_ratio = radius_eq / radius_pole;
    mean_radius = 0.5 * (radius_eq + radius_pole);
    zdiff = 0.0;

    //     angle conversion to radians

    asatlat = satlat * dpi / 180.0;
    asatlon = satlon * dpi / 180.0;
    alat = lat * dpi / 180.0;
    alon = lon * dpi / 180.0;

    //     cartesian coordinates for the satellite
    //     satlat_geod is the geodetic satellite latitude

    satlat_geod = atan(tan(asatlat) * SQR(radius_ratio));
    xsat = dheight * cos(satlat_geod) * sin(asatlon);
    ysat = dheight * sin(satlat_geod);
    zsat = dheight * cos(satlat_geod) * cos(asatlon);

    //     cartesian coordinates of the surface point

    alat_geod = atan(tan(alat) * SQR(radius_ratio));
    radius_surf = radius_eq / sqrt(SQR(cos(alat_geod)) + SQR(radius_ratio) * SQR(sin(alat_geod)));
    xsurf = radius_surf * cos(alat_geod) * sin(alon);
    ysurf = radius_surf * sin(alat_geod);
    zsurf = radius_surf * cos(alat_geod) * cos(alon);

    //     compute new radius ratio depending on height

    radius_ratio_local = SQR((radius_eq + height) / (radius_pole + height));

    //     Satellite minus surface location

    xdiff = xsat - xsurf;
    ydiff = ysat - ysurf;
    zdiff = zsat - zdiff;

    //     compute local zenith angle

    xfact = sqrt(SQR(xdiff) + SQR(ydiff) + SQR(zdiff));
    zen = (xdiff * xsurf + ydiff * ysurf + zdiff * zsurf) / (mean_radius * xfact);
    zen = acos(zen);
    zen = zen * 180.0 / dpi;

    //     equation to solve for the line of sight at height Z

    e1 = SQR(xdiff) + radius_ratio_local * SQR(ydiff) + SQR(zdiff);
    e2 = 2.0 * (xsurf * xdiff + radius_ratio_local * ysurf * ydiff + zsurf * zdiff);
    e3 = SQR(xsurf) + SQR(zsurf) + radius_ratio_local * SQR(ysurf) - SQR(radius_eq + height);

    corr = (sqrt(e2 * e2 - 4.0 * e1 * e3) - e2) / 2.0 / e1;

    //     corrected surface coordinates

    xcorr = xsurf + corr*xdiff;
    ycorr = ysurf + corr*ydiff;
    zcorr = zsurf + corr*zdiff;

    //     convert back to latitude and longitude

    latcorr = atan(ycorr / sqrt(SQR(xcorr) + SQR(zcorr)));
    latcorr = atan(tan(latcorr) / SQR(radius_ratio)) * 180.0 / dpi;

    loncorr = atan2(xcorr, zcorr) * 180.0 / dpi;
}

template <typename T>
T** allocate_array(size_t dim_y, size_t dim_x)
{
    T **array = new T*[dim_y];
    for (int i = 0; i < dim_y; ++i)
        array[i] = new T[dim_x];
    return array;
}

#define deallocate_array(array,dim) for (int i=0; i<dim; i++) delete[] array[i]; delete[] array;

/** Corrects the parallax on all seviri satellite variables
 * in the national 2D OASE composite
 * @param in_path path to the netcdf file to be corrected. The data
 * in the file is overwritten.
 */
void correct_parallax(boost::filesystem::path in_path, const ShiftedProperties shifted)
{
    // Some constants
    const double SAT_LON = 9.5; // longitude of METEOSAT-9
    const double SAT_LAT = 0.0; // latitute of METEOSAT-9
    const double SAT_HEIGHT = 35785.83; // height of METEOSAT-9 [km]

#define dim_x 900
#define dim_y 900

    try
    {
        NcFile file(in_path.generic_string(), NcFile::write);

        try
        {
            std::string type;
            file.getAtt("parallax_corrected").getValues(type);
            cout << "Parallax is already corrected (parallax_corrected=" << type << ")" << endl;
            cout << "Skipping file" << endl;
            return;
        } catch (const netCDF::exceptions::NcBadId &e)
        {
        }

        typedef std::multimap<std::string, NcVar> vmap_t;

        vmap_t variables = file.getVars();

        // Cloud-Top-Height is needed as input

        static float cloud_top_height[dim_y][dim_x];

        float cth_min = std::numeric_limits<float>::max();
        float cth_max = std::numeric_limits<float>::min();

        vmap_t::iterator fi = variables.find("msevi_l2_nwcsaf_cth");
        if (fi == variables.end())
        {
            cerr << "ERROR: could not find cloud top height (msevi_l2_nwcsaf_cth) variable" << endl;
            return;
        }

        fi->second.getVar(&cloud_top_height[0][0]);

        float cth_scale_factor = 1.0;
        float cth_offset = 0.0;
        float cth_valid_min, cth_valid_max;
        float cth_fill_value = std::numeric_limits<float>::min();

        fi->second.getAtt("scale_factor").getValues(&cth_scale_factor);
        fi->second.getAtt("add_offset").getValues(&cth_offset);
        fi->second.getAtt("_FillValue").getValues(&cth_fill_value);
        fi->second.getAtt("valid_min").getValues(&cth_valid_min);
        fi->second.getAtt("valid_max").getValues(&cth_valid_max);

        // create a variable for input and one for output

        static int corrected_iy[dim_y][dim_x];
        static int corrected_ix[dim_y][dim_x];

        // initialize output data with flag to find pixels
        // later that have not been set

        for (size_t iy = 0; iy < dim_y; iy++)
        {
            for (size_t ix = 0; ix < dim_x; ix++)
            {
                corrected_ix[iy][ix] = 0;
                corrected_iy[iy][ix] = 0;

                if (cloud_top_height[iy][ix] < cth_valid_min
                        || cloud_top_height[iy][ix] > cth_valid_max
                        || cloud_top_height[iy][ix] == cth_fill_value)
                {
                    cloud_top_height[iy][ix] = 0.0;
                } else
                {
                    cloud_top_height[iy][ix] = cth_scale_factor * cloud_top_height[iy][ix] + cth_offset;
                }

                if (cloud_top_height[iy][ix] > cth_max)
                    cth_max = cloud_top_height[iy][ix];

                if (cloud_top_height[iy][ix] < cth_min)
                    cth_min = cloud_top_height[iy][ix];
            }
        }

        // cout << endl << "cth_min=" << cth_min << " cth_max=" << cth_max << endl;

        // Coordinate system for lat/lon transformation

        RDCoordinateSystem rcs(RD_RX);

        typedef std::vector< std::vector<T> > vec_list_t;

#if WRITE_PARALLAX_VECTORS
        vec_list_t origins;
        vec_list_t correction_vectors;
#endif
        // correct the parallax

        for (size_t iy = 0; iy < dim_y; iy++)
        {
            for (size_t ix = 0; ix < dim_x; ix++)
            {
                RDGridPoint gp = rdGridPoint(ix, iy);

                // get lat/lon for this pixel
                RDGeographicalPoint coord = rcs.geographicalCoordinate(gp);

                // Get Marianne Koenig's correction values

                T cth = boost::numeric_cast<float> (cloud_top_height[iy][ix]) / 1000.0f;

                T lat_corrected = 0;
                T lon_corrected = 0;

                parallax<double> (SAT_HEIGHT, SAT_LAT, SAT_LON, cth, coord.latitude, coord.longitude, lat_corrected, lon_corrected);

                //                if (cth > 0)
                //                {
                //                    T lat_corr_0, lon_corr_0;
                //                    parallax<double> ( SAT_HEIGHT, SAT_LAT, SAT_LON, 0, coord.latitude, coord.longitude, lat_corr_0, lon_corr_0 );
                //                    cout << "(lat="<<coord.latitude << "N,lon="<<coord.longitude<<"E,cth="<<cth<<"km)"
                //                        << " (lat_corr="<<lat_corrected<<",lon_corr="<<lon_corrected<<")"
                //                        << " @cth=0.0:"
                //                        << " (lat_corr="<<lat_corr_0<<",lon_corr="<<lon_corr_0<<")"
                //                        << endl;
                //                }

                RDGeographicalPoint coord_corrected;

                // Figure out the grid point again and set
                // data at corrected position

                if (shifted == ShiftedPropertiesSatellite)
                {
                    // The correction shifts the satellite data to the
                    // corrected position. The parallax of the satellite
                    // is now corrected, but the other data stays in place

                    coord_corrected.latitude = lat_corrected;
                    coord_corrected.longitude = lon_corrected;
                } else
                {
                    // The correction shifts the other data to the
                    // corrected position. The parallax of the satellite
                    // is not corrected, but the other data is shifted
                    // to be congruent

                    // Experimental

                    T dLat = (lat_corrected - coord.latitude);
                    T dLon = (lon_corrected - coord.longitude);

                    coord_corrected.latitude = coord.latitude - dLat;
                    coord_corrected.longitude = coord.longitude - dLon;
                }

                bool is_inside = false;
                RDGridPoint gp_corrected = rcs.gridPoint(coord_corrected, is_inside);

                // Figure out the parallax vector
                RDCartesianPoint cartesian = rcs.cartesianCoordinate(gp);
                RDCartesianPoint cartesian_corr = rcs.cartesianCoordinate(gp_corrected);

                // TODO: what if two pixels are moved to the same place?
                // The way things are now is 'last write wins'

                if (is_inside)
                {
#if WRITE_PARALLAX_VECTORS
                    vector<T> origin(2);
                    origin[0] = cartesian.x;
                    origin[1] = cartesian.y;
                    origins.push_back(origin);

                    vector<T> correction(2);
                    correction[0] = cartesian_corr.x - cartesian.x;
                    correction[1] = cartesian_corr.y - cartesian.y;
                    correction_vectors.push_back(correction);
#endif
                    corrected_ix[iy][ix] = gp_corrected.ix;
                    corrected_iy[iy][ix] = gp_corrected.iy;
                }
            }
        }
#if WITH_VTK
#if WRITE_PARALLAX_VECTORS

        string vector_path = in_path.filename().stem().string() + "-parallax.vtk";
        VisitUtils<T>::write_vectors_vtk(vector_path, origins, correction_vectors, "parallax");
#endif
#endif

        static int input_data[dim_y][dim_x];
        static int output_data[dim_y][dim_x];

        for (vmap_t::iterator vi = variables.begin(); vi != variables.end(); vi++)
        {
            NcVar variable = vi->second;

            int fill_value = 0;
            try
            {
                // Get the official _FillValue value if the
                // variable has one
                NcVarAtt fillValue = variable.getAtt("_FillValue");
                fillValue.getValues(&fill_value);
            } catch (netCDF::exceptions::NcException e)
            {
                // if not, put the value just outside the valid range
                int valid_min = std::numeric_limits<int>::min();
                fill_value = valid_min - 1;
            }

            if ((shifted == ShiftedPropertiesSatellite && boost::starts_with(variable.getName(), "msevi_"))
                    || (shifted == ShiftedPropertiesOthers && !boost::starts_with(variable.getName(), "msevi_")))
            {
                // Initialize arrays

                cout << "Correcting " << variable.getName() << " ... ";

                for (size_t iy = 0; iy < dim_y; iy++)
                {
                    for (size_t ix = 0; ix < dim_x; ix++)
                    {
                        input_data[iy][ix] = 0;
                        output_data[iy][ix] = fill_value;
                    }
                }

                // Read the satellite variable

                variable.getVar(&input_data[0][0]);

                // apply the correction derived from cloud top height

                for (size_t iy = 0; iy < dim_y; iy++)
                {
                    for (size_t ix = 0; ix < dim_x; ix++)
                    {
                        // get corrected indicees

                        size_t iy_corr = corrected_iy[iy][ix];
                        size_t ix_corr = corrected_ix[iy][ix];

                        // copy data over

                        output_data[iy_corr][ix_corr] = input_data[iy][ix];
                    }
                }

                // TODO: post-processing of points that got no values

                for (size_t iy = 0; iy < dim_y; iy++)
                {
                    for (size_t ix = 0; ix < dim_x; ix++)
                    {
                        if (output_data[iy][ix] == fill_value)
                        {
                            if (variable.getName() == "msevi_l2_nwcsaf_ct" || variable.getName() == "msevi_l2_nwcsaf_cma")
                            {
                                // use the most prevalent value in 25 neighborhood
                                map<int, int> value_count;

                                int num_values = 0;
                                int interpolation_width = 2;
                                int min_neighbours = 8;

                                for (int iiy = iy - interpolation_width; iiy < iy + interpolation_width; iiy++)
                                {
                                    for (int iix = ix - interpolation_width; iix < ix + interpolation_width; iix++)
                                    {
                                        if (iix == ix && iiy == iy) continue;

                                        if (iiy >= 0 && iiy < dim_y && iix >= 0 && iix < dim_x)
                                        {
                                            if (output_data[iiy][iix] != fill_value)
                                            {
                                                int val = boost::numeric_cast<int>(output_data[iiy][iix]);

                                                map<int, int>::iterator mi = value_count.find(val);

                                                if (mi == value_count.end())
                                                {
                                                    value_count[val] = 1;
                                                } else
                                                {
                                                    mi->second = mi->second + 1;
                                                }

                                                num_values++;
                                            }
                                        }
                                    }
                                }

                                // only replace if you have at least 4 valid neighbours

                                if (num_values >= min_neighbours)
                                {
                                    int most_used = value_count.begin()->first;

                                    for (map<int, int>::iterator mi = value_count.begin(); mi != value_count.end(); ++mi)
                                    {
                                        if (mi->second > value_count[most_used])
                                        {
                                            most_used = mi->first;
                                        }
                                    }

                                    output_data[iy][ix] = most_used;
                                }
                            } else
                            {
                                // replace with geometric average of 25 neighborhood

                                T sum = 0.0;
                                int num_values = 0;

                                int interpolation_width = 2;
                                int min_neighbours = 8;

                                for (int iiy = iy - interpolation_width; iiy < iy + interpolation_width; iiy++)
                                {
                                    for (int iix = ix - interpolation_width; iix < ix + interpolation_width; iix++)
                                    {
                                        if (iix == ix && iiy == iy) continue;

                                        if (iiy >= 0 && iiy < dim_y && iix >= 0 && iix < dim_x)
                                        {
                                            if (output_data[iiy][iix] != fill_value)
                                            {
                                                sum += output_data[iiy][iix];
                                                num_values++;
                                            }
                                        }
                                    }
                                }

                                // only replace if you have at least 4 valid neighbours

                                if (num_values >= min_neighbours)
                                {
                                    output_data[iy][ix] = (sum / num_values);
                                }
                            }
                        }
                    }
                }

                // Write data back

                variable.putVar(&output_data[0][0]);

                cout << "done." << endl;
            }
        }

        nc_redef(file.getId());
        file.putAtt("parallax_corrected", (shifted == ShiftedPropertiesSatellite ? "satellite" : "others"));
        nc_enddef(file.getId());

    } catch (netCDF::exceptions::NcException &e)
    {
        cerr << "ERROR:exception " << e.what() << endl;
        return;
    }

}

#pragma mark -
#pragma mark MAIN

/* MAIN
 */
int main(int argc, char** argv)
{
    using namespace m3D;

    // Declare the supported options.

    program_options::options_description desc("Applies parallax correction to mseviri satellite data in OASE composite files.");
    desc.add_options()
            ("help", "Produces this help.")
            ("version", "print version information and exit")
            ("file,f", program_options::value<string>(), "A single file or a directory to be processed. Only files ending in .nc will be processed.")
            ("shifted,s", program_options::value<string>()->default_value("satellite"), "Which values are to be shifted? [satellite|other] (default:satellite)");

    program_options::variables_map vm;

    try
    {
        program_options::store(program_options::parse_command_line(argc, argv, desc), vm);
        program_options::notify(vm);
    } catch (std::exception &e)
    {
        cerr << "ERROR:parsing command line caused exception: " << e.what()
                << ":check meanie3D-trackplot --help for command line options" << endl;
        exit(EXIT_FAILURE);
    }

    // Version

    if (vm.count("version") != 0)
    {
        cout << m3D::VERSION << endl;
        exit(EXIT_SUCCESS);
    }

    if (vm.count("help") == 1 || argc < 2)
    {
        cout << desc << "\n";
        exit(EXIT_SUCCESS);
    }

    // Evaluate user input

    string source_path;
    ShiftedProperties shifted = ShiftedPropertiesSatellite;

    try
    {
        parse_commmandline(vm, source_path, shifted);
    } catch (const std::exception &e)
    {
        cerr << "FATAL:" << e.what() << endl;
        exit(EXIT_FAILURE);
    }

    typedef set<fs::path> fset_t;

    fset_t files;

    if (fs::is_directory(source_path))
    {
        fs::directory_iterator dir_iter(source_path);
        fs::directory_iterator end;

        while (dir_iter != end)
        {
            fs::path f = dir_iter->path();

            if (fs::is_regular_file(f) && fs::extension(f) == ".nc")
            {
                //cout << "Adding " << f.generic_string() << endl;
                files.insert(f);
            } else
            {
                cout << "Skipping " << f.generic_string() << endl;
            }

            dir_iter++;
        }
    } else
    {
        fs::path f = fs::path(source_path);

        std::string extension = fs::extension(f);

        if (fs::is_regular_file(f) && extension == ".nc")
        {
            files.insert(f);
        }
    }

    fset_t::iterator it;

    //	boost::progress_display *progress = NULL;

    //	if ( files.size() > 1 ) {
    //		progress = new progress_display ( files.size() );
    //	}

    for (it = files.begin(); it != files.end(); ++it)
    {
        //		if ( progress != NULL ) {
        //			progress->operator++();
        //		}

        boost::filesystem::path path = *it;

        // Correct

        try
        {
            cout << "Correcting " << path << "...";
            correct_parallax(path, shifted);
            cout << "done." << endl;
        } catch (std::exception &e)
        {
            cerr << "ERORO:Exception processing " << path.filename().generic_string()
                    << ":" << e.what() << endl;
        }
    }

    //	if ( progress != NULL ) {
    //		delete progress;
    //	}

    return 0;
};


/*******************************************************************************
 *         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
 *         Copyright 2009 & onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 0x01.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#define NT2_UNIT_MODULE "nt2::memory::is_power_of_2"

#include <nt2/sdk/memory/meta/is_power_of_2.hpp>

#include <boost/mpl/int.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>

////////////////////////////////////////////////////////////////////////////////
// Test the meta::is_power_of_2 version on int_
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE(is_power_of_2)
{
  using nt2::meta::is_power_of_2;
  using boost::mpl::int_;

  NT2_TEST(!(is_power_of_2< int_<0x00> >::value));
  NT2_TEST( (is_power_of_2< int_<0x01> >::value));
  NT2_TEST( (is_power_of_2< int_<0x02> >::value));
  NT2_TEST(!(is_power_of_2< int_<0x03> >::value));
  NT2_TEST( (is_power_of_2< int_<0x04> >::value));
  NT2_TEST(!(is_power_of_2< int_<0x05> >::value));
  NT2_TEST(!(is_power_of_2< int_<0x06> >::value));
  NT2_TEST(!(is_power_of_2< int_<0x07> >::value));
  NT2_TEST( (is_power_of_2< int_<0x08> >::value));
  NT2_TEST( (is_power_of_2< int_<0x10> >::value));
  NT2_TEST( (is_power_of_2< int_<0x20> >::value));
  NT2_TEST( (is_power_of_2< int_<0x40> >::value));
  NT2_TEST( (is_power_of_2< int_<0x80> >::value));
  NT2_TEST(!(is_power_of_2< int_<1337> >::value));
}

////////////////////////////////////////////////////////////////////////////////
// Test the meta::is_power_of_2 version on integer
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE(is_power_of_2_c)
{
  using nt2::meta::is_power_of_2_c;

  NT2_TEST(!(is_power_of_2_c< 0x00 >::value));
  NT2_TEST( (is_power_of_2_c< 0x01 >::value));
  NT2_TEST( (is_power_of_2_c< 0x02 >::value));
  NT2_TEST(!(is_power_of_2_c< 0x03 >::value));
  NT2_TEST( (is_power_of_2_c< 0x04 >::value));
  NT2_TEST(!(is_power_of_2_c< 0x05 >::value));
  NT2_TEST(!(is_power_of_2_c< 0x06 >::value));
  NT2_TEST(!(is_power_of_2_c< 0x07 >::value));
  NT2_TEST( (is_power_of_2_c< 0x08 >::value));
  NT2_TEST( (is_power_of_2_c< 0x10 >::value));
  NT2_TEST( (is_power_of_2_c< 0x20 >::value));
  NT2_TEST( (is_power_of_2_c< 0x40 >::value));
  NT2_TEST( (is_power_of_2_c< 0x80 >::value));
  NT2_TEST(!(is_power_of_2_c< 1337 >::value));
}


/**
 * SAMPLE APPLICATION FOR DEMONSTRATING gdb-plot functionality
 *
 * BUILD: g++ -g -I/usr/include/eigen3 eigen_example.cpp -o eigen_example
 * RUN: gdb eigen_example
 *      (gdb) b 51
 *      (gdb) plot sin_array cos_array
 */

#include <iostream>
#include <complex>
#include <list>
#include <set>
#include <map>
#include <vector>

// Eigen Types
#include <Eigen/Dense>

int main(void)
{

    Eigen::MatrixXd m(1,10);
    Eigen::ArrayXcd eigen_complex_array(10);
    Eigen::ArrayXd eigen_double_array(10);
    Eigen::ArrayXcd eca2(2048);
    Eigen::ArrayXd  eda2(2048);
    Eigen::ArrayXd  sin_array(2048);
    Eigen::ArrayXd  cos_array(2048);
    Eigen::ArrayXd  tan_array(2048);

    // Push some data around
    for ( std::size_t ii=0; ii < 10; ii++ )
    {
        m(0,ii) = ii * M_PI;
        eigen_double_array( ii ) = M_PI * ii;
        eigen_complex_array( ii ) = M_PI * ii + 0.1j;
    }

    for ( std::size_t ii=0; ii < eca2.size(); ii++ )
    {
        eda2(ii) = ii * 0.01;
        eca2(ii) = ii + 0.1j;
    }

    sin_array = eda2.sin();
    cos_array = eda2.cos();
    tan_array = eda2.tan();

    std::cout << "\n\n----------------------------------------\n\n"
              << "Break on line " <<  __LINE__ << " to plot: \n"
              << "  plot eda2 for an Eigen double array\n"
              << "  plot eca2 for an Eigen complex array\n"
              << "  plot sin_array cos_array  tan_array for trig plots\n"
              << std::endl;

    return 0;
}


/*
Тест алгоритма нахождения минимального остовного дерева
из библиотеки boost. Минимальное остовное дерево соединяет
вершины графа так, что суммарный вес дерева минимальный.

В программе при компиляции граф можно представить как в
виде списка смежности (ADJ_LIST), так и в виде матрицы
смежности (ADJ_MATRIX)
*/
#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/graph_traits.hpp>

#include <iostream>
#include <utility>
#include <algorithm>

//использовать только совместно
#include <boost/type_traits/ice.hpp>
#include <boost/graph/adjacency_matrix.hpp>

//#define ADJ_MATRIX
#define ADJ_LIST

using namespace boost;

//свойство ребер - вес (для нас - расстояние)
typedef property<edge_weight_t, double> EdgeWeightProperty;
#ifdef ADJ_LIST
//граф представлен в виде списка смежности
typedef boost::adjacency_list<vecS,vecS,undirectedS,no_property,EdgeWeightProperty> mygraph;
#endif
#ifdef ADJ_MATRIX
//граф представлен в виде матрицы смежности
typedef boost::adjacency_matrix<undirectedS, no_property, EdgeWeightProperty> mygraph;
#endif

typedef mygraph::edge_descriptor Edge;

struct Point2D
{
	float x;
	float y;
};

//случайное число в интервале
float RandomNum(float min, float max)
{
	return
		min + static_cast<float>(rand())
		/ (static_cast<float>(RAND_MAX) / (max - min));
}

//случайная точка
Point2D RandomPoint(float xmin,float ymin,float xmax,float ymax)
{
	Point2D p;
	p.x = RandomNum(xmin, xmax);
	p.y = RandomNum(ymin, ymax);
	return p;
}

//расстояние между точками
float Distance(Point2D a, Point2D b)
{
	float dx = a.x - b.x;
	float dy = a.y - b.y;
	return sqrt(dx*dx + dy*dy);
}

int main()
{
	using std::cout;

	//число точек
	const int pnum = 600;
	//граничное расстояние между точками
	//если расстояние между точками меньше max_dst,
	//то между точками создается ребро графа,
	//иначе нет. Нужно для оптимизации расхода памяти программы
	const float max_dst = 10.0;
	//создаем массив из хаотично расположенных точек
	Point2D points[pnum];
	for (int i = 0;i < pnum;i++)
		points[i] = RandomPoint(-100.0, -100.0, 100.0, 100.0);

	mygraph g(pnum);

	//в граф записываем все хаотично расположенные точки
	for (int i = 0;i < pnum;i++)
		for (int j = 0;j < pnum;j++)
		{
			float dst = Distance(points[i], points[j]);
			if (dst < max_dst)
				add_edge(i, j, Distance(points[i], points[j]), g);
		}

	cout << "number of edges: " << num_edges(g) << std::endl;
	cout << "number of vertices: " << num_vertices(g) << std::endl;

	//обход графа

	//находим минимальное остовное дерево
	//оно в виде списка пар вершин
	std::list<Edge> spanning_tree;
	kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));

	cout << "spanning tree length: " << spanning_tree.size() << std::endl;

	for (std::list<Edge>::iterator i = spanning_tree.begin();
	i != spanning_tree.end();i++)
	{
		//достаем индексы вершин из spanning_tree
		Edge e = *i;
		//вершина, из которой идет ребро
		size_t source_index = e.m_source;
		//вершина, в которую идет ребро
		size_t dest_index = e.m_target;
		//просто выводим индексы, хотя можем делать что-то полезное
		//например, строить мостики между полигонами с заданными индексами
		printf("src = %zd target = %zd\n", source_index, dest_index);
	}
	cout << std::endl;

	getc(stdin);
	return 0;
}


#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <chrono>
#include <iostream>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace std;
using namespace cv;

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

// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d& p, const Mat& K);

void bundleAdjustment(const vector<Point3f> points_3d,
                      const vector<Point2f> points_2d, const Mat& K, Mat& R,
                      Mat& t);

int main(int argc, char** argv) {
  if (argc != 5) {
    cout << "usage: pose_estimation_3d2d img1 img2 depth1 depth2" << endl;
    return 1;
  }
  //-- 读取图像
  Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);

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

  // 建立3D点
  Mat d1 = imread(argv[3],
                  CV_LOAD_IMAGE_UNCHANGED);  // 深度图为16位无符号数，单通道图像
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  vector<Point3f> pts_3d;
  vector<Point2f> pts_2d;
  for (DMatch m : matches) {
    ushort d = d1.ptr<unsigned short>(
        int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
    if (d == 0)  // bad depth
      continue;
    float dd = d / 1000.0;
    Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
    pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));
    pts_2d.push_back(keypoints_2[m.trainIdx].pt);
  }

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

  Mat r, t;
  solvePnP(pts_3d, pts_2d, K, Mat(), r, t,
           false);  // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
  Mat R;
  cv::Rodrigues(r, R);  // r为旋转向量形式，用Rodrigues公式转换为矩阵

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

  cout << "calling bundle adjustment" << endl;

  bundleAdjustment(pts_3d, pts_2d, K, R, t);
}

void find_feature_matches(const Mat& img_1, const Mat& img_2,
                          std::vector<KeyPoint>& keypoints_1,
                          std::vector<KeyPoint>& keypoints_2,
                          std::vector<DMatch>& matches) {
  //-- 初始化
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB"
  // );
  Ptr<DescriptorMatcher> matcher =
      DescriptorMatcher::create("BruteForce-Hamming");
  //-- 第一步:检测 Oriented FAST 角点位置
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);

  //-- 第二步:根据角点位置计算 BRIEF 描述子
  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);

  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
  vector<DMatch> match;
  // BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);

  //-- 第四步:匹配点对筛选
  double min_dist = 10000, max_dist = 0;

  //找出所有匹配之间的最小距离和最大距离,
  //即是最相似的和最不相似的两组点之间的距离
  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }

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

  //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}

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

void bundleAdjustment(const vector<Point3f> points_3d,
                      const vector<Point2f> points_2d, const Mat& K, Mat& R,
                      Mat& t) {
  // 初始化g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>>
      Block;  // pose 维度为 6, landmark 维度为 3
  Block::LinearSolverType* linearSolver =
      new g2o::LinearSolverCSparse<Block::PoseMatrixType>();  // 线性方程求解器
  Block* solver_ptr = new Block(linearSolver);  // 矩阵块求解器
  g2o::OptimizationAlgorithmLevenberg* solver =
      new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
  g2o::SparseOptimizer optimizer;
  optimizer.setAlgorithm(solver);

  // vertex
  g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();  // camera pose
  Eigen::Matrix3d R_mat;
  R_mat << R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
      R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
      R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2);
  pose->setId(0);
  pose->setEstimate(g2o::SE3Quat(
      R_mat, Eigen::Vector3d(t.at<double>(0, 0), t.at<double>(1, 0),
                             t.at<double>(2, 0))));
  optimizer.addVertex(pose);

  int index = 1;
  for (const Point3f p : points_3d)  // landmarks
  {
    g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
    point->setId(index++);
    point->setEstimate(Eigen::Vector3d(p.x, p.y, p.z));
    point->setMarginalized(true);  // g2o 中必须设置 marg 参见第十讲内容
    optimizer.addVertex(point);
  }

  // parameter: camera intrinsics
  g2o::CameraParameters* camera = new g2o::CameraParameters(
      K.at<double>(0, 0),
      Eigen::Vector2d(K.at<double>(0, 2), K.at<double>(1, 2)), 0);
  camera->setId(0);
  optimizer.addParameter(camera);

  // edges
  index = 1;
  for (const Point2f p : points_2d) {
    g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
    edge->setId(index);
    edge->setVertex(
        0, dynamic_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(index)));
    edge->setVertex(1, pose);
    edge->setMeasurement(Eigen::Vector2d(p.x, p.y));
    edge->setParameterId(0, 0);
    edge->setInformation(Eigen::Matrix2d::Identity());
    optimizer.addEdge(edge);
    index++;
  }

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.setVerbose(true);
  optimizer.initializeOptimization();
  optimizer.optimize(100);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used =
      chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "optimization costs time: " << time_used.count() << " seconds."
       << endl;

  cout << endl << "after optimization:" << endl;
  cout << "T=" << endl << Eigen::Isometry3d(pose->estimate()).matrix() << endl;
}


#include "test.hpp"
#include "geometry/utils.hpp"
#include "geometry/plane.hpp"
#include "optim/deformingMesh.hpp"
#include <functional>

#include "geometrycentral/surface/manifold_surface_mesh.h"
#include "geometrycentral/surface/meshio.h"
#include "geometrycentral/surface/vertex_position_geometry.h"

#include "polyscope/polyscope.h"
#include "polyscope/surface_mesh.h"
#include "polyscope/point_cloud.h"

#include <Eigen/Sparse>

// Static variables declaration
std::unique_ptr<VertexPositionGeometry> Test::geometryFlat;
std::unique_ptr<VertexPositionGeometry> Test::newGeometry;
std::unique_ptr<ManifoldSurfaceMesh> Test::meshFlat;
std::unique_ptr<FaceData<Vector3>> Test::normals;
FaceData<Vector3> Test::refNormals;
std::unique_ptr<VertexData<Vector3>> Test::bLoopData;
polyscope::SurfaceMesh* Test::psReconsMesh;
VertexData<Vector3> Test::debugGradient;
float Test::lr = 0.04f;
float Test::nw = 1.0f;
int i = 0;
int cpt = 0;

Test::Test() {

}

bool adjacent(int i, int j, int W, int H){
    return (i-j == -1 || i-j == 1 || i-j == W || i-j == -W);
}

void Test::callback1(){
    // ImGui
    ImGui::PushItemWidth(100);

    ImGui::InputInt("Iterations : ", &i);
    ImGui::SliderFloat("LR : ", &lr, 0.001f, 0.05f);
    ImGui::SliderFloat("Normal weight : ", &nw, 0.1f, 100.0f);

    ImGui::PopItemWidth();

    i++;
    newGeometry = DeformingMesh::iterativeSolve(*meshFlat, *newGeometry, *geometryFlat, *bLoopData, *normals, debugGradient, lr, nw);
    Utils::centerPoints(*newGeometry);
    psReconsMesh->updateVertexPositions(newGeometry->vertexPositions);
    psReconsMesh->addVertexVectorQuantity("Debug Gradient", debugGradient);

}

void Test::callback2(){
    // ImGui
    ImGui::PushItemWidth(100);

    ImGui::InputInt("Iterations : ", &i);
    ImGui::SliderFloat("LR : ", &lr, 0.001f, 0.05f);
    ImGui::SliderFloat("Normal weight : ", &nw, 0.1f, 100.0f);

    ImGui::PopItemWidth();

    i++;
    newGeometry = DeformingMesh::iterativeSolve(*meshFlat, *newGeometry, *geometryFlat, refNormals, debugGradient, lr, nw);
    //Utils::centerPoints(*newGeometry);
    psReconsMesh->updateVertexPositions(newGeometry->vertexPositions);
    psReconsMesh->addVertexVectorQuantity("Debug Gradient", debugGradient);
    newGeometry->requireFaceNormals();
    psReconsMesh->addFaceVectorQuantity("Real Normals", newGeometry->faceNormals);
}

void Test::callback3(){
    // ImGui
    ImGui::PushItemWidth(100);

    ImGui::InputInt("Iterations : ", &i);
    ImGui::SliderFloat("LR : ", &lr, 0.001f, 0.05f);
    ImGui::SliderFloat("Normal weight : ", &nw, 0.1f, 100.0f);

    ImGui::PopItemWidth();

    /*
    int W = 10, H = 10;
    // Laplacian Solve
    double t = 1000.0;
    Eigen::SparseMatrix<double> lap(W*H, W*H);
    Eigen::SparseMatrix<double> Id(W*H, W*H);
    Id.setIdentity();

    Eigen::VectorXd u_0 = Eigen::VectorXd::Zero(W*H);
    Eigen::VectorXd phi_0 = Eigen::VectorXd::Zero(W*H);
    Eigen::VectorXd v_x0 = Eigen::VectorXd::Zero(W*H);
    Eigen::VectorXd v_y0 = Eigen::VectorXd::Zero(W*H);

    v_x0[27] = 1.0;
    v_y0[27] = 1.0;
    u_0[27] = std::sqrt(2.0);
    phi_0[27] = 1.0;

    v_x0[72] = -1.0;
    v_y0[72] = -1.0;
    u_0[72] = std::sqrt(3.0);
    phi_0[72] = 1.0;

    double theta = 2*3.141592*double(cpt++)/20;
    double x = cos(theta);
    double y = sin(theta);
    v_x0[11] = x;
    v_y0[11] = y;
    u_0[11] = std::sqrt(1.0);
    phi_0[11] = 1.0;

    for(auto i=0; i < W*H; ++i){
        for(auto j=0; j < W*H; ++j){
            if(i == j)
                lap.coeffRef(i, j) = -4.0;
            if(adjacent(i,j,W,H))
                lap.coeffRef(i,j) = 1.0;
        }
    }

    Eigen::SparseMatrix<double> A = Id - t*lap;
    Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int> >   solver;
    solver.analyzePattern(A);
    solver.factorize(A);

    Eigen::VectorXd u_t = solver.solve(u_0);
    Eigen::VectorXd phi_t = solver.solve(phi_0);
    Eigen::VectorXd v_xt = solver.solve(v_x0);
    Eigen::VectorXd v_yt = solver.solve(v_y0);

    Eigen::VectorXd final_x(W*H);
    Eigen::VectorXd final_y(W*H);

    for(auto i=0; i < W*H; ++i){
        final_x[i] = v_xt[i] / std::sqrt((v_xt[i]*v_xt[i] + v_yt[i]*v_yt[i])) * u_t[i] / phi_t[i];
        final_y[i] = v_yt[i] / std::sqrt((v_xt[i]*v_xt[i] + v_yt[i]*v_yt[i])) * u_t[i] / phi_t[i];
    }

    // Fill
    std::vector<Vector3> final(W*H);
    int i = 0;
    for(Vector3& val : final){
        val = Vector3{final_x[i], 0.0f, final_y[i]};

        i++;
    }

    polyscope::getPointCloud("VF")->addVectorQuantity("Value", final);*/

}

void Test::test1() {

    // Creates a planar mesh with height values
    std::unique_ptr<ManifoldSurfaceMesh> mesh;
    std::unique_ptr<VertexPositionGeometry> geometry;
    std::tie(mesh, geometry) = Utils::createMeshPlane(30, 30, 5, 5, [](float x, float y)->float{return sin(x)*sin(y);});
    Utils::centerPoints(*geometry);
    geometry->requireVertexNormals();

    // Creates a planar mesh
    //std::unique_ptr<ManifoldSurfaceMesh> meshFlat;
    std::tie(meshFlat, geometryFlat) = Utils::createMeshPlane(30, 30, 5, 5, [](float x, float y)->float{return 0.0f;});

    // Gives projected normals
    std::unique_ptr<FaceData<Vector3>> projectedPoints;
    std::unique_ptr<FaceData<Vector3>> projectedNormals;
    std::tie(projectedPoints, projectedNormals) = Utils::getProjectedNormals(*mesh, *geometry);

    // Gives normals from projected normals
    normals = Utils::getNormals(*projectedNormals);

    // Find boundary loop
    bLoopData = Utils::setBoundaryPositions(*mesh, *geometry);

    // Deforms a planar mesh so that the geometry matches given normals
    debugGradient = VertexData<Vector3>(*mesh);
    newGeometry = DeformingMesh::iterativeSolve(*meshFlat, *geometryFlat, *geometryFlat, *bLoopData, *normals, debugGradient, lr, nw);
    Utils::centerPoints(*newGeometry);

    // Visualization with polyscope
    polyscope::init();

    polyscope::SurfaceMesh* psMesh = polyscope::registerSurfaceMesh("Surface Mesh", geometry->vertexPositions, mesh->getFaceVertexList());
    psMesh->addVertexVectorQuantity("Vertex Normals", geometry->vertexNormals);

    psReconsMesh = polyscope::registerSurfaceMesh("Reconstructed Surface Mesh", newGeometry->vertexPositions, mesh->getFaceVertexList());
    psReconsMesh->addFaceVectorQuantity("Normals", *normals);
    psReconsMesh->addVertexVectorQuantity("Debug Gradient", debugGradient);

    polyscope::PointCloud* psPC = polyscope::registerPointCloud("Projected Points", *projectedPoints);
    psPC->addVectorQuantity("Projected Normals", *projectedNormals);
    psPC->addVectorQuantity("Normals", *normals);

    //polyscope::state::userCallback = Test::callback1;

    polyscope::show();

}

void Test::test2() {

    // Creates an icosphere and twitched its normals
    std::unique_ptr<ManifoldSurfaceMesh> meshIco;
    std::unique_ptr<VertexPositionGeometry> geometryIco;
    std::tie(meshIco, geometryIco) = Utils::createIcoSphere(2);
    geometryIco->requireFaceNormals();
    refNormals = geometryIco->faceNormals;
    Utils::twitchNormals(refNormals);

    std::tie(meshFlat, geometryFlat) = Utils::createIcoSphere(2);

    // Deforms an icosphere so that the geometry matches given normals
    debugGradient = VertexData<Vector3>(*meshIco);
    newGeometry = DeformingMesh::iterativeSolve(*meshFlat, *geometryFlat, *geometryFlat, refNormals, debugGradient, lr, nw);

    // Visualization with polyscope
    polyscope::init();

    psReconsMesh = polyscope::registerSurfaceMesh("Icosphere", newGeometry->vertexPositions, meshIco->getFaceVertexList());
    psReconsMesh->addFaceVectorQuantity("Real Normals", geometryIco->faceNormals);
    psReconsMesh->addFaceVectorQuantity("Twitched Normals", refNormals);
    std::cout << refNormals.size() << std::endl;

    polyscope::state::userCallback = Test::callback2;

    polyscope::show();

}

void Test::test3() {

    // Creates a 2D vector Field
    int W = 40, H = 40;

    std::vector<Vector3> pts = Utils::createPointsPlane(W, H);
    std::vector<Vector3> values(W*H);
    for(Vector3& val : values){
        float theta = 2*3.141592*polyscope::randomUnit();
        float x = cos(theta);
        float y = sin(theta);
        val = Vector3{x, 0.0f, y};
    }

    // Creates a planar mesh with height values
    std::unique_ptr<ManifoldSurfaceMesh> mesh;
    std::unique_ptr<VertexPositionGeometry> geometry;
    std::tie(mesh, geometry) = Utils::createMeshPlane(W, H, 5, 5, [](float x, float y)->float{return sin(x)*sin(y);});
    Utils::centerPoints(*geometry);
    geometry->requireVertexNormals();
    geometry->requireFaceNormals();

    // Creates a planar mesh
    //std::unique_ptr<ManifoldSurfaceMesh> meshFlat;
    std::tie(meshFlat, geometryFlat) = Utils::createMeshPlane(W, H, 5, 5, [](float x, float y)->float{return 0.0f;});

    // Gives projected normals
    std::unique_ptr<VertexData<Vector3>> projectedPoints;
    std::unique_ptr<VertexData<Vector3>> projectedNormals;
    std::tie(projectedPoints, projectedNormals) = Utils::getProjectedVertexNormals(*mesh, *geometry);

    // Laplacian Solve
    double t = 0.01;
    Eigen::SparseMatrix<double> lap(W*H, W*H);
    Eigen::SparseMatrix<double> Id(W*H, W*H);
    Id.setIdentity();

    Eigen::VectorXd u_0 = Eigen::VectorXd::Zero(W*H);
    Eigen::VectorXd phi_0 = Eigen::VectorXd::Zero(W*H);
    Eigen::VectorXd v_x0 = Eigen::VectorXd::Zero(W*H);
    Eigen::VectorXd v_y0 = Eigen::VectorXd::Zero(W*H);

    std::vector<int> preservedNormalsId = {0,1,2,3,4,5,6,7,8,9,90,91,92,93,94,95,96,97,98,99,10,20,30,40,50,60,70,80,19,29,39,49,59,69,79,89};

    //for(int id : preservedNormalsId){
    for(int id = 0; id < 1600; ++id){
        v_x0[id] = (*projectedNormals)[id][0];
        v_y0[id] = (*projectedNormals)[id][2];
        u_0[id] = norm((*projectedNormals)[id]);
        phi_0[id] = 1.0;
    }
    /*
    v_x0[27] = 1.0;
    v_y0[27] = 1.0;
    u_0[27] = std::sqrt(2.0);
    phi_0[27] = 1.0;

    v_x0[72] = -1.0;
    v_y0[72] = -1.0;
    u_0[72] = std::sqrt(6.0);
    phi_0[72] = 1.0;

    double theta = 2*3.141592*double(cpt++)/360;
    double x = cos(theta);
    double y = sin(theta);
    v_x0[11] = x;
    v_y0[11] = y;
    u_0[11] = std::sqrt(1.0);
    phi_0[11] = 1.0; */

    for(auto i=0; i < W*H; ++i){
        for(auto j=0; j < W*H; ++j){
            if(i == j){
                lap.coeffRef(i, j) = -4.0;
                if(i == 0 || i == W*H-1 || j == 0 || j == W*H-1)
                    lap.coeffRef(i, j) = -3.0;
            }

            if(adjacent(i,j,W,H))
                lap.coeffRef(i,j) = 1.0;
        }
    }

    Eigen::SparseMatrix<double> A = Id - t*lap;
    Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int> >   solver;
    solver.analyzePattern(A);
    solver.factorize(A);

    Eigen::VectorXd u_t = solver.solve(u_0);
    Eigen::VectorXd phi_t = solver.solve(phi_0);
    Eigen::VectorXd v_xt = solver.solve(v_x0);
    Eigen::VectorXd v_yt = solver.solve(v_y0);

    Eigen::VectorXd final_x(W*H);
    Eigen::VectorXd final_y(W*H);

    for(auto i=0; i < W*H; ++i){
        final_x[i] = v_xt[i] / std::sqrt((v_xt[i]*v_xt[i] + v_yt[i]*v_yt[i])) * u_t[i] / phi_t[i];
        final_y[i] = v_yt[i] / std::sqrt((v_xt[i]*v_xt[i] + v_yt[i]*v_yt[i])) * u_t[i] / phi_t[i];
    }

    // Fill
    std::vector<Vector3> final(W*H);
    VertexData<Vector3> estimatedProjectedVertexNormals(*meshFlat);
    int i = 0;
    for(Vector3& val : final){
        val = Vector3{final_x[i], 0.0f, final_y[i]};
        estimatedProjectedVertexNormals[i] = Vector3{final_x[i], 0.0f, final_y[i]};
        i++;
    }

    std::unique_ptr<FaceData<Vector3>> estimatedProjectedFaceNormals = Utils::getFaceNormalsFromVertexNormals(estimatedProjectedVertexNormals, *meshFlat);

    // Gives normals from projected normals
    normals = Utils::getNormals(*estimatedProjectedFaceNormals);

    // Find boundary loop
    bLoopData = Utils::setBoundaryPositions(*mesh, *geometry);

    // Deforms a planar mesh so that the geometry matches given normals
    debugGradient = VertexData<Vector3>(*mesh);
    newGeometry = DeformingMesh::iterativeSolve(*meshFlat, *geometryFlat, *geometryFlat, *bLoopData, *normals, debugGradient, lr, nw);
    Utils::centerPoints(*newGeometry);
    newGeometry->requireFaceNormals();

    // Visualization with polyscope
    polyscope::init();

    polyscope::PointCloud* pointCloudVF = polyscope::registerPointCloud("VF", *projectedPoints);
    pointCloudVF->addVectorQuantity("Estimated Normals", final);
    pointCloudVF->addVectorQuantity("Real Normals", *projectedNormals);

    psReconsMesh = polyscope::registerSurfaceMesh("Reconstructed Mesh", newGeometry->vertexPositions, mesh->getFaceVertexList());
    psReconsMesh->addFaceVectorQuantity("Estimated normals", *normals);
    psReconsMesh->addFaceVectorQuantity("Groundtruth", geometry->faceNormals);
    psReconsMesh->addFaceVectorQuantity("Real normals", newGeometry->faceNormals);

    polyscope::SurfaceMesh* psMesh = polyscope::registerSurfaceMesh("Surface Mesh", geometry->vertexPositions, mesh->getFaceVertexList());

    //polyscope::state::userCallback = Test::callback3;

    polyscope::show();

}


/*
 * Copyright (c) 2011 Seiya Tokui <beam.web@gmail.com>
 * Copyright (c) 2014 Burkhard Ritter <burkhard@ualberta.ca>
 * This code is distributed under the MIT license.
 *
 * Performance and accuracy of the ARPACK eigensolver compared to the Eigen
 * eigensolver.
 *
 * Computes execution time of both the ARPACK and the Eigen eigensolver for
 * random symmetric matrices for a range of matrix dimensions. Computes the mean
 * error of the ARPACK eigenvalues and eigenvectors as well. The number of
 * eigenvalues to be computed (for ARPACK) and the number of zeros in each matrix
 * can be specified as a percentage of the matrix dimension. Produces output
 * suitable for plotting.
 *
 * Note: This program uses the Arpaca solver in a very simple, black-box like
 * fashion. It is very likely possible to achieve better performance by tuning
 * various parameters.
 *
 * To compile this program:
 * g++ \
 *    -std=c++11 \
 *    -I [/path/to/eigen] \
 *    -O3 \
 *    -DNDEBUG \
 *    performance_plot.cpp \
 *    -L [/path/to/libarpack.a] \
 *    -larpack \
 *    -o performance_plot
 *
 * This assumes that arpaca.hpp is in the same directory.
 */

#include <iostream>
#include <iomanip>
#include <random>
#include <chrono>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include "arpaca.hpp"
using namespace Eigen;
using namespace arpaca;
using namespace std::chrono;

std::mt19937 generator(42);

VectorXd diagonalize_random_matrix(int n_dim, double r_ev, double r_zeros)
{
    const int n_ev = r_ev * n_dim;
    int n_zeros = r_zeros * n_dim*n_dim;
    if (n_zeros%2 != 0) n_zeros--;

    /*
     * Create matrix
     *
     * Is there an easier way to get a random symmetric matrix with a specified
     * number of zero elements?
     */
    MatrixXd dm = MatrixXd::Random(n_dim,n_dim).selfadjointView<Upper>();
    std::uniform_int_distribution<int> distribution(0,n_dim-1);
    int n_diag = n_zeros / n_dim;
    if (n_diag%2 != 0) n_diag--;
    int n_rest = n_zeros - n_diag;
    while (n_diag > 0)
    {
        int i = distribution(generator);
        if (dm(i,i) != 0)
        {
            dm(i,i) = 0;
            n_diag--;
        }
    }
    while (n_rest > 0)
    {
        int i = distribution(generator);
        int j = distribution(generator);
        if (i!=j && dm(i,j)!=0)
        {
            dm(i,j) = 0;
            dm(j,i) = 0;
            n_rest -= 2;
        }
    }
    SparseMatrix<double> sm = dm.sparseView();
    assert(sm.nonZeros() == n_dim*n_dim - n_zeros);

    /*
     * Diagonalize Arpaca
     *
     * Arpaca (and ARPACK?) do not seem to support the calculation of all
     * eigenvalues directly. The most primitive way to work around this is to
     * calculate n_ev-1 eigenvalues from the bottom and 1 eigenvalue from the
     * top, and that's what I do here.
     */
    steady_clock::time_point begin_a;
    steady_clock::time_point end_a;
    Eigen::VectorXd eigenvalues_a(n_ev);
    Eigen::MatrixXd eigenvectors_a(n_ev,n_ev);
    SymmetricEigenSolver<double> s_a;
    SymmetricEigenSolver<double> s_a_;
    if (n_ev < n_dim)
    {
        begin_a = steady_clock::now();
        s_a = Solve(sm, n_ev, ALGEBRAIC_SMALLEST);
        end_a = steady_clock::now();
        // We could avoid copying the vector and matrix by using references.
        eigenvalues_a = s_a.eigenvalues();
        eigenvectors_a = s_a.eigenvectors();
    }
    else // n_ev == n_dim
    {
        begin_a = steady_clock::now();
        s_a = Solve(sm, n_ev-1, ALGEBRAIC_SMALLEST);
        s_a_ = Solve(sm, 1, ALGEBRAIC_LARGEST);
        end_a = steady_clock::now();

        eigenvalues_a.head(n_ev-1) = s_a.eigenvalues();
        eigenvectors_a.leftCols(n_ev-1) = s_a.eigenvectors();

        eigenvalues_a.tail(1) = s_a_.eigenvalues();
        eigenvectors_a.rightCols(1) = s_a_.eigenvectors();
    }
    duration<double> d_a = duration_cast<duration<double>>(end_a - begin_a);
    double time_a = d_a.count();

    /*
     * Diagonalize Eigen
     */
    Eigen::VectorXd eigenvalues_e(n_ev);
    Eigen::MatrixXd eigenvectors_e(n_ev,n_ev);

    steady_clock::time_point begin_e = steady_clock::now();
    SelfAdjointEigenSolver<MatrixXd> s_e(dm);
    steady_clock::time_point end_e = steady_clock::now();

    // We could avoid copying the vector and matrix by using references.
    eigenvalues_e = s_e.eigenvalues().topRows(n_ev);
    eigenvectors_e = s_e.eigenvectors().leftCols(n_ev);

    duration<double> d_e = duration_cast<duration<double>>(end_e - begin_e);
    double time_e = d_e.count();

    /*
     * Calculate error
     */
    double error_values = (eigenvalues_a - eigenvalues_e).cwiseAbs().mean();
    double error_vectors = 0;
    for (int i = 0; i < n_ev; i++)
    {
        double d1 =
            (eigenvectors_a.col(i) - eigenvectors_e.col(i)).cwiseAbs().mean();
        double d2 =
            (eigenvectors_a.col(i) + eigenvectors_e.col(i)).cwiseAbs().mean();
        error_vectors += std::min(d1, d2);
    }
    error_vectors /= n_ev;

    /*
     * Output
     */
    /*
    std::cout << "Matrix: " << std::endl;
    std::cout << dm << std::endl << std::endl;
    std::cout << "Arpack eigenvalues: " << std::endl;
    std::cout << eigenvalues_a << std::endl << std::endl;
    std::cout << "Eigen eigenvalues: " << std::endl;
    std::cout << eigenvalues_e << std::endl << std::endl;
    std::cout << "Time in seconds of Arpaca: " << time_a << std::endl;
    std::cout << "Time in seconds of Eigen: " << time_e << std::endl;
    std::cout << "Mean error of eigenvalues: " << error_values << std::endl;
    std::cout << "Mean error of eigenvectors: " << error_vectors << std::endl;
    std::cout << "info: " << s_a.GetInfo() << std::endl;
    std::cout << "# actual iterations: "
              << s_a.num_actual_iterations() << std::endl;
    std::cout << "# converged eigenvalues: "
              << s_a.num_converged_eigenvalues() << std::endl;
    */

    /*
     * Result
     */
    VectorXd r(6);
    r << time_a, time_e, error_values, error_vectors,
         s_a.num_actual_iterations(),
         s_a.num_converged_eigenvalues();
    return r;
}

void print_usage(char* program)
{
    std::cerr
        << "Performance and accuracy of the ARPACK eigensolver compared "
        << std::endl
        << "to the Eigen eigensolver." << std::endl << std::endl
        << "Computes execution time and error over matrix dimensions. "
        << std::endl
        << "Produces output suitable for plotting." << std::endl
        << std::endl
        << "Usage: " << std::endl
        << "   " << program
        << " [n_dim_min] [n_dim_max] [n_dp] [n_rep] [r_ev] [r_zeros]"
        << std::endl << std::endl
        << "with: " << std::endl
        << "   n_dim_min: start value for matrix dimension" << std::endl
        << "   n_dim_end: end value for matrix dimension" << std::endl
        << "   n_dp: number of data points to compute" << std::endl
        << "   n_rep: number of repetitions for each matrix dimension" << std::endl
        << "   r_ev: number of eigenvalues to compute as a percentage" << std::endl
        << "         of matrix dimension, ranges from 0 to 1" << std::endl
        << "   r_zeros: number of zeros in matrix as a percentage of" << std::endl
        << "            matrix size, ranges from 0 to 1" << std::endl
        << std::endl
        << "Example: " << std::endl
        << "   " << program
        << " 100 1000 10 10 0.3 0.1" << std::endl;
}

void print_header(int n_rep, double r_ev, double r_zeros, int w)
{
    std::cout << std::setprecision(6);
    std::cout << std::left;
    std::cout
        << "# Performance and accuracy of the ARPACK eigensolver compared "
        << "to the Eigen eigensolver." << std::endl
        << "# " << std::endl
        << "# n_rep="  << n_rep << std::endl
        << "# r_ev=" << r_ev << std::endl
        << "# r_zeros=" << r_zeros << std::endl
        << "# " << std::endl
        << "# "
        << std::setw(w-2) << "n_dim"
        << std::setw(w) << "time_a"
        << std::setw(w) << "time_e"
        << std::setw(w) << "error_values"
        << std::setw(w) << "error_vectors"
        << std::setw(w) << "n_it"
        << std::setw(w) << "n_conv"
        << std::endl;
}

int main (int argc, char** argv)
{
    if (argc != 7)
    {
        print_usage(argv[0]);
        std::exit(EXIT_SUCCESS);
    }

    int n_dim_min = std::atoi(argv[1]);
    int n_dim_max = std::atoi(argv[2]);
    int n_dp = std::atoi(argv[3]);
    int n_rep = std::atoi(argv[4]);
    double r_ev = std::atof(argv[5]);
    double r_zeros = std::atof(argv[6]);

    if (n_dim_min < 1) n_dim_min = 1;
    if (n_dim_max < n_dim_min) n_dim_max = n_dim_min;
    if (n_dp < 1) n_dp = 1;
    if (n_rep < 1) n_rep = 1;
    if (r_ev < 0) r_ev = 0;
    if (r_ev > 1) r_ev = 1;
    if (r_zeros < 0) r_zeros = 0;
    if (r_zeros > 1) r_zeros = 1;

    int delta_dim = n_dim_max - n_dim_min + 1;
    if (n_dp > 1)
        delta_dim = (n_dim_max - n_dim_min) / (n_dp-1);
    if (delta_dim == 0) delta_dim = 1;

    const int w = 15;
    print_header(n_rep, r_ev, r_zeros, w);
    for (int n_dim = n_dim_min; n_dim <= n_dim_max; n_dim += delta_dim)
    {
        Eigen::VectorXd v(6);
        v.setZero();
        for (int i=0; i<n_rep; i++)
        {
            VectorXd r = diagonalize_random_matrix(n_dim,r_ev,r_zeros);
            v += r;
        }
        v /= n_rep;
        std::cout << std::setw(w) << n_dim;
        for (int i=0; i<v.size(); i++)
            std::cout << std::setw(w) << v(i);
        std::cout << std::endl;
    }

    return 0;
}