#include <boost/detail/lightweight_test.hpp>
#include <kanooth/numbers/lowlevel/generic_has_double.hpp>
#include <kanooth/numbers/lowlevel/generic_sim_double.hpp>
#include <kanooth/numbers/lowlevel/low_double_int.hpp>

template <typename T>
bool cmp(const T* u, const T* v, unsigned length)
{
    while (length--) {
        if (*u++ != *v++)
            return false;
    }
    return true;
}

template <typename T, typename V>
T* set_digits(T* A, V v1)
{
    A[0] = v1;
    return A;
}

template <typename T, typename V>
T* set_digits(T* A, V v1, V v2)
{
    A[0] = v1;  A[1] = v2;
    return A;
}

template <typename LOWLEVEL>
struct tester {

    typedef typename LOWLEVEL::digit_type digit_type;

    digit_type a[20], b[20], r[20], x[20];
    digit_type d;

    void test_add1()
    {
        d = LOWLEVEL::add(r, set_digits(a, -1, 4), 2,
                             set_digits(b,  2),    1);
        BOOST_TEST(cmp(r,    set_digits(x,  1, 5), 2));
        BOOST_TEST_EQ(d, 0);
    }

    void test_add2()
    {
        d = LOWLEVEL::add(r, set_digits(a, -3, -1), 2,
                             set_digits(b, -2, -4), 2);
        BOOST_TEST(cmp(r,    set_digits(x, -5, -4), 2));
        BOOST_TEST_EQ(d, 1);
    }

    void test_sub1()
    {
        d = LOWLEVEL::sub(r, set_digits(a, 4, 1), 2,
                             set_digits(b, 2),    1);
        BOOST_TEST(cmp(r,    set_digits(x, 2, 1), 2));
        BOOST_TEST_EQ(d, 0);
    }

    void test_sub2()
    {
        d = LOWLEVEL::sub(r, set_digits(a,  1,  2), 2,
                             set_digits(b,  3,  5), 2);
        BOOST_TEST(cmp(r,    set_digits(x, -2, -4), 2));
        BOOST_TEST_EQ(d, 1);
    }

    void test_mul_1_1()
    {
        d = LOWLEVEL::mul_1(r, set_digits(a, 7, 3), 2,
                               2);
        BOOST_TEST(cmp(r,      set_digits(x, 14, 6), 2));
        BOOST_TEST_EQ(d, 0);
    }

    void test_mul_1_2()
    {
        d = LOWLEVEL::mul_1(r, set_digits(a, -1, -1), 2,
                               -1);
        BOOST_TEST(cmp(r,      set_digits(x, 1, -1), 2));
        BOOST_TEST_EQ(d, digit_type(-2));
    }

    void run()
    {
        test_add1();
        test_add2();
        test_sub1();
        test_sub2();
        test_mul_1_1();
        test_mul_1_2();
    }
};

int main()
{
    //tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint64_t, kanooth::uint128_t> >().run();
    //tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint64_t, kanooth::numbers::lowlevel::low_double_int<kanooth::uint64_t> > >().run();
    tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint16_t, kanooth::uint32_t> >().run();
    tester<kanooth::numbers::lowlevel::generic_has_double<kanooth::uint16_t, kanooth::numbers::lowlevel::low_double_int<kanooth::uint16_t> > >().run();
    tester<kanooth::numbers::lowlevel::generic_sim_double<kanooth::uint16_t> >().run();

    return boost::report_errors();
}


/*
 * Copyright (c) 2016, Markus Achtelik, ASL, ETH Zurich, Switzerland
 * Copyright (c) 2016, Michael Burri, ASL, ETH Zurich, Switzerland
 * Copyright (c) 2016, Helen Oleynikova, ASL, ETH Zurich, Switzerland
 * Copyright (c) 2016, Rik Bähnemann, ASL, ETH Zurich, Switzerland
 * Copyright (c) 2016, Marija Popovic, ASL, ETH Zurich, Switzerland
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <iostream>
#include <limits>
#include <random>

#include <Eigen/Core>
#include "catch.hpp"

#include "mav_trajectory_generation/polynomial_optimization_linear.h"
#include "mav_trajectory_generation/timing.h"

using namespace mav_trajectory_generation;

const int N = 10;
const int max_derivative = derivative_order::SNAP;
const size_t derivative_to_optimize = derivative_order::SNAP;

Eigen::IOFormat matlab_format(Eigen::FullPrecision, 0, ", ", ";\n", "", "", "[",
                              "]");

template <class T1, class T2>
bool checkMatrices(const Eigen::MatrixBase<T1>& m1,
                   const Eigen::MatrixBase<T2>& m2, double tol) {
  return (m1 - m2).cwiseAbs().maxCoeff() < tol;
}

double getMaximumMagnitude(const std::vector<Segment>& segments,
                           size_t derivative, double dt = 0.01) {
  double maximum = -1e9;

  for (const Segment& s : segments) {
    for (double ts = 0; ts < s.getTime(); ts += dt) {
      double current_value = s.evaluate(ts, derivative).norm();
      if (current_value > maximum) maximum = current_value;
    }
  }
  return maximum;
}

double computeCostNumeric(const std::vector<Segment>& segments,
                          size_t derivative, double dt = 0.001) {
  double cost = 0;

  for (const Segment& s : segments) {
    for (double ts = 0; ts < s.getTime(); ts += dt) {
      cost += s.evaluate(ts, derivative).squaredNorm() * dt;
    }
  }
  return cost;
}

void checkPath(const Vertex::Vector& vertices,
               const std::vector<Segment>& segments) {
  const double tol = 1e-6;
  size_t n_vertices = vertices.size();
  size_t n_segments = segments.size();
  REQUIRE(n_segments == n_vertices - 1);

  for (size_t i = 0; i < n_segments; ++i) {
    const Vertex& v_begin = vertices[i];
    const Vertex& v_end = vertices[i + 1];
    const Segment& segment = segments[i];

    // check if fixed constraints are met.
    for (Vertex::Constraints::const_iterator it = v_begin.cBegin();
         it != v_begin.cEnd(); ++it) {
      const int derivative = it->first;
      const Vertex::ConstraintValue desired = it->second;
      const Vertex::ConstraintValue actual = segment.evaluate(0, derivative);
      Eigen::Matrix3d m, n;
      std::stringstream segment_derivative;
      printSegment(segment_derivative, segment, derivative);
      REQUIRE(desired.isApprox(actual, tol));
    }
    for (Vertex::Constraints::const_iterator it = v_end.cBegin();
         it != v_end.cEnd(); ++it) {
      const int derivative = it->first;
      const Vertex::ConstraintValue desired = it->second;
      const Vertex::ConstraintValue actual =
          segment.evaluate(segment.getTime(), derivative);
      std::stringstream segment_derivative;
      printSegment(segment_derivative, segment, derivative);
      REQUIRE(std::fabs((desired - actual).norm()) <= tol);
    }

    // Check if values at vertices are continuous.
    if (i > 0) {
      const Segment& last_segment = segments[i - 1];
      for (size_t derivative = 0; derivative < N / 2; ++derivative) {
        const Vertex::ConstraintValue last_segment_value =
            last_segment.evaluate(last_segment.getTime(), derivative);
        const Vertex::ConstraintValue current_segment_value =
            segment.evaluate(0, derivative);
        std::stringstream segment_derivative;
        printSegment(segment_derivative, segment, derivative);
        REQUIRE(
            last_segment_value.isApprox(current_segment_value, tol));
      }
    }
  }
}

bool checkCost(double cost_to_check, const std::vector<Segment>& segments,
               size_t derivative, double relative_tolerance) {
  REQUIRE(derivative >= size_t(0));
  bool check_tolerance = relative_tolerance >= 0.0 && relative_tolerance <= 1.0;
  REQUIRE(check_tolerance);
  const double sampling_interval = 0.001;
  double cost_numeric =
      computeCostNumeric(segments, derivative, sampling_interval);

  if (std::abs(cost_numeric - cost_to_check) >
      cost_numeric * relative_tolerance) {
    std::cout << "[FAIL]: tested cost is" << cost_to_check
              << " but real cost is " << cost_numeric
              << ". Difference vs. allowed difference: "
              << std::abs(cost_numeric - cost_to_check) << "/"
              << cost_numeric * relative_tolerance << std::endl;
    return false;
  }

  return true;
}

TEST_CASE("PathPlanning_TestVertexGeneration1D", "[MavTrajectoryGeneration]") {
  Vertex::Vector vertices;
  const double p_min = -50;
  const double p_max = 50;
  vertices = createRandomVertices1D(max_derivative, 100, p_min, p_max, 0);

  REQUIRE(vertices.front().getNumberOfConstraints() == N / 2);
  REQUIRE(vertices.back().getNumberOfConstraints() == N / 2);

  for (const Vertex& v : vertices) {
    REQUIRE(v.hasConstraint(derivative_order::POSITION) == true);
    Eigen::VectorXd c;
    v.getConstraint(derivative_order::POSITION, &c);
    REQUIRE(c[0] <= p_max);
    REQUIRE(c[0] >= p_min);
  }
}

TEST_CASE("PathPlanning_TestVertexGeneration3D", "[MavTrajectoryGeneration]") {
  Eigen::VectorXd pos_min(3), pos_max(3);
  pos_min << -10.0, -20.0, -10.0;
  pos_max << 10.0, 20.0, 10.0;
  Vertex::Vector vertices;

  vertices = createRandomVertices(max_derivative, 100, pos_min, pos_max, 12345);

  REQUIRE(vertices.front().getNumberOfConstraints() == N / 2);
  REQUIRE(vertices.back().getNumberOfConstraints() == N / 2);

  for (const Vertex& v : vertices) {
    REQUIRE(v.hasConstraint(derivative_order::POSITION) == true);
    Eigen::VectorXd c;
    v.getConstraint(derivative_order::POSITION, &c);
    for (int i = 0; i < 3; ++i) {
      REQUIRE(c[i] <= pos_max[i]);
      REQUIRE(c[i] >= pos_min[i]);
    }
  }
}

TEST_CASE("PathPlanning_A_matrix_inversion", "[MavTrajectoryGeneration]") {
  const double max_time = 60;
  for (double t = 1; t <= max_time; t += 1) {
    Eigen::Matrix<double, N, N> A, Ai, Ai_eigen;
    PolynomialOptimization<N>::setupMappingMatrix(t, &A);
    PolynomialOptimization<N>::invertMappingMatrix(A, &Ai);
    Ai_eigen = A.inverse();
    REQUIRE(Ai.isApprox(Ai_eigen, 1.0e-10));
  }
}

TEST_CASE("PathPlanningUnconstrained_1D_10_segments", "[MavTrajectoryGeneration]") {
  Vertex::Vector vertices;
  vertices = createRandomVertices1D(max_derivative, 10, -10, 10, 12);
  const double approximate_v_max = 3.0;
  const double approximate_a_max = 5.0;

  std::vector<double> segment_times =
      estimateSegmentTimes(vertices, approximate_v_max, approximate_a_max);

  timing::Timer timer_setup("setup_1D_10s");
  PolynomialOptimization<N> opt(1);
  opt.setupFromVertices(vertices, segment_times, derivative_to_optimize);
  timer_setup.Stop();
  timing::Timer timer_solve("solve_linear_1D_10s");
  opt.solveLinear();
  timer_solve.Stop();

  Segment::Vector segments;
  opt.getSegments(&segments);

  std::cout << "Base coefficients: "
            << Polynomial::base_coefficients_.block(3, 0, 1, N) << std::endl;

  checkPath(vertices, segments);
  double v_max = getMaximumMagnitude(segments, derivative_order::VELOCITY);
  double a_max = getMaximumMagnitude(segments, derivative_order::ACCELERATION);
  std::cout << "v_max: " << v_max << " a_max: " << a_max << std::endl;

  timing::Timer timer_cost("cost_1D_10s");
  double cost = opt.computeCost();
  timer_cost.Stop();
  std::cout << "cost: " << cost << std::endl;

  REQUIRE(v_max < approximate_v_max * 2.0);
  REQUIRE(a_max < approximate_a_max * 2.0);
  REQUIRE(
      checkCost(opt.computeCost(), segments, derivative_to_optimize, 0.1));
}

TEST_CASE("PathPlanningUnconstrained_1D_50_segments", "[MavTrajectoryGeneration]") {
  Vertex::Vector vertices;
  vertices = createRandomVertices1D(max_derivative, 50, -10, 10, 123);
  const double approximate_v_max = 3.0;
  const double approximate_a_max = 5.0;

  std::vector<double> segment_times =
      estimateSegmentTimes(vertices, approximate_v_max, approximate_a_max);

  timing::Timer timer_setup("setup_1D_50s");
  PolynomialOptimization<N> opt(1);
  opt.setupFromVertices(vertices, segment_times, derivative_to_optimize);
  timer_setup.Stop();
  timing::Timer timer_solve("solve_linear_1D_50s");
  opt.solveLinear();
  timer_solve.Stop();

  Segment::Vector segments;
  opt.getSegments(&segments);

  checkPath(vertices, segments);
  double v_max = getMaximumMagnitude(segments, derivative_order::VELOCITY);
  double a_max = getMaximumMagnitude(segments, derivative_order::ACCELERATION);
  std::cout << "v_max: " << v_max << " a_max: " << a_max << std::endl;

  timing::Timer timer_cost("cost_1D_50s");
  double cost = opt.computeCost();
  timer_cost.Stop();
  std::cout << "cost: " << cost << std::endl;

  REQUIRE(v_max < approximate_v_max * 2.0);
  REQUIRE(a_max < approximate_a_max);
  REQUIRE(
      checkCost(opt.computeCost(), segments, derivative_to_optimize, 0.1));
}

TEST_CASE("PathPlanningUnconstrained_1D_100_segments", "[MavTrajectoryGeneration]") {
  Vertex::Vector vertices;
  vertices = createRandomVertices1D(max_derivative, 100, -10, 10, 1234);
  const double approximate_v_max = 3.0;
  const double approximate_a_max = 5.0;

  std::vector<double> segment_times =
      estimateSegmentTimes(vertices, approximate_v_max, approximate_a_max);

  timing::Timer timer_setup("setup_1D_100s");
  PolynomialOptimization<N> opt(1);
  opt.setupFromVertices(vertices, segment_times, derivative_to_optimize);
  timer_setup.Stop();
  timing::Timer timer_solve("solve_linear_1D_100s");
  opt.solveLinear();
  timer_solve.Stop();

  Segment::Vector segments;
  opt.getSegments(&segments);

  checkPath(vertices, segments);
  double v_max = getMaximumMagnitude(segments, derivative_order::VELOCITY);
  double a_max = getMaximumMagnitude(segments, derivative_order::ACCELERATION);
  std::cout << "v_max: " << v_max << " a_max: " << a_max << std::endl;

  timing::Timer timer_cost("cost_1D_100s");
  double cost = opt.computeCost();
  timer_cost.Stop();
  std::cout << "cost: " << cost << std::endl;

  REQUIRE(v_max < approximate_v_max * 5.0);
  REQUIRE(a_max < approximate_a_max * 2.0);
  REQUIRE(
      checkCost(opt.computeCost(), segments, derivative_to_optimize, 0.1));
}

TEST_CASE("PathPlanningUnconstrained_1D_100_segments_high_segment_times",
          "[MavTrajectoryGeneration]") {
  Vertex::Vector vertices;
  vertices = createRandomVertices1D(max_derivative, 100, -50, 50, 12345);
  const double approximate_v_max = 3.0;
  const double approximate_a_max = 5.0;

  std::vector<double> segment_times =
      estimateSegmentTimes(vertices, approximate_v_max, approximate_a_max);

  timing::Timer timer_setup("setup_1D_100s_long");
  PolynomialOptimization<N> opt(1);
  opt.setupFromVertices(vertices, segment_times, derivative_to_optimize);
  timer_setup.Stop();
  timing::Timer timer_solve("solve_linear_1D_100s_long");
  opt.solveLinear();
  timer_solve.Stop();

  Segment::Vector segments;
  opt.getSegments(&segments);

  checkPath(vertices, segments);
  double v_max = getMaximumMagnitude(segments, derivative_order::VELOCITY);
  double a_max = getMaximumMagnitude(segments, derivative_order::ACCELERATION);
  std::cout << "v_max: " << v_max << " a_max: " << a_max << std::endl;

  timing::Timer timer_cost("cost_1D_100s_long");
  double cost = opt.computeCost();
  timer_cost.Stop();
  std::cout << "cost: " << cost << std::endl;

  // max. velocity estimation  beforehand is not accurate here anymore
  REQUIRE(v_max < approximate_v_max * 5.0);
  REQUIRE(a_max < approximate_a_max * 2.0);
  REQUIRE(
      checkCost(opt.computeCost(), segments, derivative_to_optimize, 0.1));
}

TEST_CASE("PathPlanningUnconstrained_3D_100_segments_high_segment_times",
          "[MavTrajectoryGeneration]") {
  Eigen::VectorXd pos_min(3), pos_max(3);
  pos_min << -10.0, -20.0, -10.0;
  pos_max << 10.0, 20.0, 10.0;
  Vertex::Vector vertices;
  vertices = createRandomVertices(max_derivative, 100, pos_min, pos_max, 12345);

  const double approximate_v_max = 3.0;
  const double approximate_a_max = 5.0;
  std::vector<double> segment_times =
      estimateSegmentTimes(vertices, approximate_v_max, approximate_a_max);

  timing::Timer timer_setup("setup_3D_100s_long");
  PolynomialOptimization<N> opt(3);
  opt.setupFromVertices(vertices, segment_times, derivative_to_optimize);
  timer_setup.Stop();
  timing::Timer timer_solve("solve_linear_3D_100s_long");
  opt.solveLinear();
  timer_solve.Stop();

  Segment::Vector segments;
  opt.getSegments(&segments);

  checkPath(vertices, segments);
  double v_max = getMaximumMagnitude(segments, derivative_order::VELOCITY);
  double a_max = getMaximumMagnitude(segments, derivative_order::ACCELERATION);
  std::cout << "v_max: " << v_max << " a_max: " << a_max << std::endl;

  timing::Timer timer_cost("cost_3D_100s_long");
  double cost = opt.computeCost();
  timer_cost.Stop();
  std::cout << "cost: " << cost << std::endl;

  /// max. velocity estimation beforehand is not accurate here anymore
  REQUIRE(v_max < approximate_v_max * 5.0);
  REQUIRE(a_max < approximate_a_max * 2.0);
  REQUIRE(
      checkCost(opt.computeCost(), segments, derivative_to_optimize, 0.1));
}

bool checkExtrema(const std::vector<double>& testee,
                  const std::vector<double>& reference, double tol = 0.01) {
  for (double t : testee) {
    bool found_match = false;
    for (double r : reference) {
      if (std::abs(t - r) < tol) {
        found_match = true;
        break;
      }
    }
    if (!found_match) {
      std::cout << "[ERROR]: did not find matching extremum for " << t
                << " in ";
      for (double r : reference) {
        std::cout << r << " ";
      }
      std::cout << std::endl;
      return false;
    }
  }
  return true;
}

TEST_CASE("PathOptimization_1D_segment_extrema_of_magnitude",
          "[MavTrajectoryGeneration]") {
  Vertex::Vector vertices;
  vertices = createRandomVertices1D(max_derivative, 100, -10, 10, 1234);
  const double approximate_v_max = 3.0;
  const double approximate_a_max = 5.0;

  std::vector<double> segment_times =
      estimateSegmentTimes(vertices, approximate_v_max, approximate_a_max);

  PolynomialOptimization<N> opt(1);
  opt.setupFromVertices(vertices, segment_times, derivative_to_optimize);
  opt.solveLinear();

  Segment::Vector segments;
  opt.getSegments(&segments);

  timing::Timer time_analytic("time_extrema_analytic_1", false);
  timing::Timer time_analytic_template_free(
      "time_extrema_analytic_1_template_free", false);
  timing::Timer time_sampling("time_extrema_sampling_1", false);
  int segment_idx = 0;
  for (const Segment& s : segments) {
    std::vector<double> res;
    time_analytic.Start();
    opt.computeSegmentMaximumMagnitudeCandidates<1>(s, 0, s.getTime(), &res);
    time_analytic.Stop();

    std::vector<double> res_template_free;
    std::vector<int> dimensions = {0};
    time_analytic_template_free.Start();
    s.computeMinMaxMagnitudeCandidateTimes(1, 0.0, s.getTime(), dimensions,
                                           &res_template_free);
    time_analytic_template_free.Stop();

    std::vector<double> res_sampling;
    time_sampling.Start();
    opt.computeSegmentMaximumMagnitudeCandidatesBySampling<1>(
        s, 0, s.getTime(), 0.001, &res_sampling);
    time_sampling.Stop();

    constexpr double check_tolerance = 0.01;
    bool success = checkExtrema(res, res_sampling, check_tolerance);
    if (!success) {
      std::cout << "############CHECK XTREMA FAILED: \n";
      std::cout << "segment idx: " << segment_idx << "/" << segments.size()
                << std::endl;

      std::cout << "real eigs for segment with time " << s.getTime() << "s : ";

      for (const double& t : res) std::cout << t << " ";
      std::cout << std::endl;
      std::cout << "sampling: ";
      for (const double& t : res_sampling) std::cout << t << " ";
      std::cout << std::endl;

      std::cout << "vx = [ "
                << s[0].getCoefficients(derivative_order::VELOCITY)
                       .reverse()
                       .format(matlab_format)
                << "];\n";
      std::cout << "t = 0:0.001:" << s.getTime() << "; \n";
    }
    REQUIRE(success);

    REQUIRE(res.size() == res_template_free.size() - 2);
    for (size_t i = 0; i < res.size(); i++) {
      REQUIRE(res[i] == res_template_free[i + 2]);
    }

    ++segment_idx;
  }

  double v_max_ref = getMaximumMagnitude(segments, derivative_order::VELOCITY);
  double a_max_ref =
      getMaximumMagnitude(segments, derivative_order::ACCELERATION);

  timing::Timer time_analytic_v("time_extrema_analytic_v");
  Extremum v_max =
      opt.computeMaximumOfMagnitude<derivative_order::VELOCITY>(nullptr);
  time_analytic_v.Stop();
  timing::Timer time_analytic_a("time_extrema_analytic_a");
  Extremum a_max =
      opt.computeMaximumOfMagnitude<derivative_order::ACCELERATION>(nullptr);
  time_analytic_a.Stop();

  std::cout << "v_max " << v_max << std::endl << "a_max " << a_max << std::endl;

  REQUIRE(std::fabs(v_max_ref - v_max.value) <= 0.01);
  REQUIRE(std::fabs(a_max_ref - a_max.value) <= 0.01);
}

TEST_CASE("PathOptimization3D_segment_extrema_of_magnitude", "[MavTrajectoryGeneration]") {
  Eigen::VectorXd pos_min(3), pos_max(3);
  pos_min << -10.0, -9.0, -8.0;
  pos_max << 8.0, 9.0, 10.0;
  Vertex::Vector vertices;
  vertices = createRandomVertices(max_derivative, 100, pos_min, pos_max, 978);

  const double approximate_v_max = 3.0;
  const double approximate_a_max = 5.0;
  std::vector<double> segment_times =
      estimateSegmentTimes(vertices, approximate_v_max, approximate_a_max);

  PolynomialOptimization<N> opt(3);
  opt.setupFromVertices(vertices, segment_times);
  opt.solveLinear();

  Segment::Vector segments;
  opt.getSegments(&segments);

  timing::Timer time_analytic("time_extrema_analytic_3", false);
  timing::Timer time_analytic_template_free(
      "time_extrema_analytic_3_template_free", false);
  timing::Timer time_sampling("time_extrema_sampling_3", false);
  int segment_idx = 0;
  for (const Segment& s : segments) {
    std::vector<double> res;
    time_analytic.Start();
    opt.computeSegmentMaximumMagnitudeCandidates<1>(s, 0, s.getTime(), &res);
    time_analytic.Stop();

    std::vector<double> res_template_free;
    std::vector<int> dimensions = {0, 1, 2};
    time_analytic_template_free.Start();
    s.computeMinMaxMagnitudeCandidateTimes(1, 0.0, s.getTime(), dimensions,
                                           &res_template_free);
    time_analytic_template_free.Stop();

    std::vector<double> res_sampling;
    time_sampling.Start();
    opt.computeSegmentMaximumMagnitudeCandidatesBySampling<1>(
        s, 0, s.getTime(), 0.001, &res_sampling);
    time_sampling.Stop();

    constexpr double check_tolerance = 0.01;
    bool success = checkExtrema(res, res_sampling, check_tolerance);
    if (!success) {
      std::cout << "############CHECK XTREMA FAILED: \n";
      std::cout << "segment idx: " << segment_idx << "/" << segments.size()
                << std::endl;

      std::cout << "real eigs for segment with time " << s.getTime() << "s : ";

      for (const double& t : res) std::cout << t << " ";
      std::cout << std::endl;
      std::cout << "sampling: ";
      for (const double& t : res_sampling) std::cout << t << " ";
      std::cout << std::endl;

      std::cout << "vx = [ "
                << s[0].getCoefficients(derivative_order::VELOCITY)
                       .reverse()
                       .format(matlab_format)
                << "];\n";
      std::cout << "vy = [ "
                << s[1].getCoefficients(derivative_order::VELOCITY)
                       .reverse()
                       .format(matlab_format)
                << "];\n";
      std::cout << "vz = [ "
                << s[2].getCoefficients(derivative_order::VELOCITY)
                       .reverse()
                       .format(matlab_format)
                << "];\n";
      std::cout << "t = 0:0.001:" << s.getTime() << "; \n";
    }
    REQUIRE(success);

    REQUIRE(res.size() == res_template_free.size() - 2);
    for (size_t i = 0; i < res.size(); i++) {
      REQUIRE(res[i] == res_template_free[i + 2]);
    }
    ++segment_idx;
  }

  double v_max_ref = getMaximumMagnitude(segments, derivative_order::VELOCITY);
  double a_max_ref =
      getMaximumMagnitude(segments, derivative_order::ACCELERATION);

  timing::Timer time_analytic_v("time_extrema_analytic_v");
  Extremum v_max =
      opt.computeMaximumOfMagnitude<derivative_order::VELOCITY>(nullptr);
  time_analytic_v.Stop();
  timing::Timer time_analytic_a("time_extrema_analytic_a");
  Extremum a_max =
      opt.computeMaximumOfMagnitude<derivative_order::ACCELERATION>(nullptr);
  time_analytic_a.Stop();

  std::cout << "v_max " << v_max << std::endl << "a_max " << a_max << std::endl;

  REQUIRE(std::fabs(v_max_ref - v_max.value) <= 0.01);
  REQUIRE(std::fabs(a_max_ref - a_max.value) <= 0.01);
}

TEST_CASE("2_vertices_setup", "[MavTrajectoryGeneration]") {
  const int kDim = 1;
  // Create a known 1D spline.
  Vertex start_vertex(kDim);
  const double kStartX = 0.0;
  start_vertex.addConstraint(derivative_order::POSITION, kStartX);
  start_vertex.addConstraint(derivative_order::VELOCITY, 0.0);
  start_vertex.addConstraint(derivative_order::ACCELERATION, 0.0);
  start_vertex.addConstraint(derivative_order::JERK, 0.0);
  start_vertex.addConstraint(derivative_order::SNAP, 0.0);

  Vertex goal_vertex = start_vertex;
  const double kGoalX = 5.0;
  goal_vertex.addConstraint(derivative_order::POSITION, kGoalX);

  const double kVMax = 2.0;
  const double kSegmentTime = std::fabs(kGoalX - kStartX) * 2.0 / kVMax;

  const int kDerivativeToOptimize = derivative_order::SNAP;
  const int kNumCoefficients = 10;

  // Setup optimization with two vertices.
  PolynomialOptimization<kNumCoefficients> opt(kDim);
  Vertex::Vector vertices{start_vertex, goal_vertex};
  std::vector<double> segment_times{kSegmentTime};
  opt.setupFromVertices(vertices, segment_times, kDerivativeToOptimize);
  opt.solveLinear();

  Segment::Vector segments;
  opt.getSegments(&segments);
  checkPath(vertices, segments);

  // Matlab solution:
  Eigen::VectorXd matlab_coeffs(kNumCoefficients);
  matlab_coeffs << -0.000000000000004, 0.000000000000004, -0.000000000000006,
      0.000000000000003, -0.000000000000001, 0.201600000000015,
      -0.134400000000012, 0.034560000000004, -0.004032000000000,
      0.000179200000000;
  // Solution with two vertices:
  Eigen::VectorXd coeffs = segments[0].getPolynomialsRef()[0].getCoefficients();

  // The matlab solution is only approximately valid, e.g., the first
  // coefficient is not equal 0.0.
  REQUIRE(matlab_coeffs.isApprox(coeffs, 0.001));
}

// Test 2 vertices setup
TEST_CASE("2_vertices_rand", "[MavTrajectoryGeneration]") {
  const int kMaxDerivative = derivative_order::ACCELERATION;
  const size_t kNumSegments = 1;
  Eigen::VectorXd min_pos, max_pos;
  min_pos = Eigen::Vector3d::Constant(-50.0);
  max_pos = -min_pos;
  const int kSeed = 12345;
  const size_t kNumSetups = 1e2;
  for (size_t i = 0; i < kNumSetups; i++) {
    Vertex::Vector vertices;
    vertices = createRandomVertices(kMaxDerivative, kNumSegments, min_pos,
                                    max_pos, kSeed + i);

    const double kApproxVMax = 3.0;
    const double kApproxAMax = 5.0;
    std::vector<double> segment_times =
        estimateSegmentTimes(vertices, kApproxVMax, kApproxAMax);

    PolynomialOptimization<N> opt(3);
    opt.setupFromVertices(vertices, segment_times);
    opt.solveLinear();

    Segment::Vector segments;
    opt.getSegments(&segments);

    checkPath(vertices, segments);
  }
}

// Test unpacking and repacking constraints between [d_f; d_p] and p.
TEST_CASE("ConstraintPacking", "[MavTrajectoryGeneration]") {
  const int kMaxDerivative = derivative_order::JERK;
  const size_t kNumSegments = 5;
  Eigen::VectorXd min_pos, max_pos;
  min_pos = Eigen::Vector3d::Constant(-50.0);
  max_pos = -min_pos;
  const int kSeed = 12345;
  const size_t kNumSetups = 100;

  for (size_t i = 0; i < kNumSetups; i++) {
    Vertex::Vector vertices;
    vertices = createRandomVertices(kMaxDerivative, kNumSegments, min_pos,
                                    max_pos, kSeed + i);

    const double kApproxVMax = 3.0;
    const double kApproxAMax = 5.0;
    std::vector<double> segment_times =
        estimateSegmentTimes(vertices, kApproxVMax, kApproxAMax);

    PolynomialOptimization<N> opt(3);
    opt.setupFromVertices(vertices, segment_times);
    opt.solveLinear();

    Segment::Vector segments;
    opt.getSegments(&segments);

    std::vector<Eigen::VectorXd> fixed_constraints;
    std::vector<Eigen::VectorXd> free_constraints;

    opt.getFixedConstraints(&fixed_constraints);
    opt.getFreeConstraints(&free_constraints);

    // Get the mapping matrices.
    Eigen::MatrixXd M, A_inv, A, M_pinv;
    opt.getM(&M);
    opt.getAInverse(&A_inv);
    opt.getA(&A);
    opt.getMpinv(&M_pinv);

    REQUIRE(fixed_constraints.size() == 3);
    REQUIRE(free_constraints.size() == 3);

    // For each dimension... We can test that [df;dp] -> p -> [df;dp].
    for (int i = 0; i < 3; ++i) {
      Eigen::VectorXd d_all_ordered(fixed_constraints[i].size() +
                                    free_constraints[i].size());
      d_all_ordered << fixed_constraints[i], free_constraints[i];
      Eigen::VectorXd p = A_inv * M * d_all_ordered;
      Eigen::VectorXd d_unordered = A * p;
      Eigen::VectorXd d_reordered = M_pinv * d_unordered;
      REQUIRE(d_all_ordered.isApprox(d_reordered, 1e-6));

      // Now also check for each segment.
      for (size_t j = 0; j < segments.size(); ++j) {
        Eigen::VectorXd p_seg = segments[j][i].getCoefficients(0);
        REQUIRE(p_seg.isApprox(p.segment<N>(j * N), 1e-6));
      }
    }
  }
}

void createTestPolynomials() {
  Vertex::Vector vertices;
  vertices = createRandomVertices1D(max_derivative, 100, -50, 50, 12345);
  const double approximate_v_max = 3.0;
  const double approximate_a_max = 5.0;

  std::vector<double> segment_times =
      estimateSegmentTimes(vertices, approximate_v_max, approximate_a_max);

  PolynomialOptimization<N> opt(1);
  opt.setupFromVertices(vertices, segment_times);
  opt.solveLinear();

  Segment::Vector segments;
  opt.getSegments(&segments);

  int s_idx = 1;
  for (const Segment& s : segments) {
    std::cout << "s(" << s_idx << ", :) = "
              << s[0].getCoefficients(derivative_order::POSITION)
                     .reverse()
                     .format(matlab_format)
              << ";\n";
    ++s_idx;
  }
}


/*
 *  Copyright (c) 2015, University of Michigan.
 *  All rights reserved.
 *
 *  This source code is licensed under the BSD-style license found in the
 *  LICENSE file in the root directory of this source tree. An additional grant
 *  of patent rights can be found in the PATENTS file in the same directory.
 *
 */

/**
 * @author: Johann Hauswald, Yiping Kang
 * @contact: jahausw@umich.edu, ypkang@umich.edu
 */
#include <fstream>
#include <sstream>
#include <iostream>
#include <assert.h>
#include <stdint.h>
#include <ctime>
#include <cmath>
#include <glog/logging.h>
#include <boost/chrono/thread_clock.hpp>

#include "timer.h"
#include "thread.h"

extern map<string, Net<float>*> nets;
extern bool debug;
extern bool gpu;

void SERVICE_fwd(float* in, int in_size, float* out, int out_size,
                 Net<float>* net) {
  string net_name = net->name();
  STATS_INIT("service", "DjiNN service inference");
  PRINT_STAT_STRING("network", net_name.c_str());

  if (Caffe::mode() == Caffe::CPU)
    PRINT_STAT_STRING("platform", "cpu");
  else
    PRINT_STAT_STRING("platform", "gpu");

  float loss;
  vector<Blob<float>*> in_blobs = net->input_blobs();

  tic();
  in_blobs[0]->set_cpu_data(in);
  vector<Blob<float>*> out_blobs = net->ForwardPrefilled(&loss);
  memcpy(out, out_blobs[0]->cpu_data(), sizeof(float));

  PRINT_STAT_DOUBLE("inference latency", toc());

  STATS_END();

  if (out_size != out_blobs[0]->count())
    LOG(FATAL) << "out_size =! out_blobs[0]->count())";
  else
    memcpy(out, out_blobs[0]->cpu_data(), out_size * sizeof(float));
}

pthread_t request_thread_init(int sock) {
  // Prepare to create a new pthread
  pthread_attr_t attr;
  pthread_attr_init(&attr);
  pthread_attr_setstacksize(&attr, 1024 * 1024);

  // Create a new thread starting with the function request_handler
  pthread_t tid;
  if (pthread_create(&tid, &attr, request_handler, (void*)(intptr_t)sock) != 0)
    LOG(ERROR) << "Failed to create a request handler thread.\n";

  return tid;
}

void* request_handler(void* sock) {
  int socknum = (intptr_t)sock;

  // 1. Client sends the application type
  // 2. Client sends the size of incoming data
  // 3. Client sends data

  char req_name[MAX_REQ_SIZE];
  SOCKET_receive(socknum, (char*)&req_name, MAX_REQ_SIZE, debug);
  map<string, Net<float>*>::iterator it = nets.find(req_name);
  if (it == nets.end()) {
    LOG(ERROR) << "Task " << req_name << " not found.";
    return (void*)1;
  } else
    LOG(INFO) << "Task " << req_name << " forward pass.";

  // receive the input data length (in float)
  int sock_elts = SOCKET_rxsize(socknum);
  if (sock_elts < 0) {
    LOG(ERROR) << "Error num incoming elts.";
    return (void*)1;
  }

  // reshape input dims if incoming data != current net config
  LOG(INFO) << "Elements received on socket " << sock_elts << endl;

  reshape(nets[req_name], sock_elts);

  int in_elts = nets[req_name]->input_blobs()[0]->count();
  int out_elts = nets[req_name]->output_blobs()[0]->count();
  float* in = (float*)malloc(in_elts * sizeof(float));
  float* out = (float*)malloc(out_elts * sizeof(float));

  // Main loop of the thread, following this order
  // 1. Receive input feature (has to be in the size of sock_elts)
  // 2. Do forward pass
  // 3. Send back the result
  // 4. Repeat 1-3

  // Warmup: used to move the network to the device for the first time
  // In all subsequent forward passes, the trained model resides on the
  // device (GPU)
  bool warmup = true;

  while (1) {
    LOG(INFO) << "Reading from socket.";
    int rcvd =
        SOCKET_receive(socknum, (char*)in, in_elts * sizeof(float), debug);

    if (rcvd == 0) break;  // Client closed the socket

    if (warmup && gpu) {
      float loss;
      vector<Blob<float>*> in_blobs = nets[req_name]->input_blobs();
      in_blobs[0]->set_cpu_data(in);
      vector<Blob<float>*> out_blobs;
      out_blobs = nets[req_name]->ForwardPrefilled(&loss);
      warmup = false;
    }

    LOG(INFO) << "Executing forward pass.";
    SERVICE_fwd(in, in_elts, out, out_elts, nets[req_name]);

    LOG(INFO) << "Writing to socket.";
    SOCKET_send(socknum, (char*)out, out_elts * sizeof(float), debug);
  }

  // Exit the thread
  LOG(INFO) << "Socket closed by the client.";

  free(in);
  free(out);

  return (void*)0;
}


// Copyright (c) 2018 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ReduceSpecial23
#include <poplar/Engine.hpp>
#include <poplibs_support/TestDevice.hpp>
// codelets
#include "../../lib/popops/reduction/ReductionVertex.hpp"
#include "poplar/Target.hpp"
#include "poplibs_test/Check.hpp"
#include "poplibs_test/Util.hpp"
#include "popops/codelets.hpp"
#include "poputil/VertexTemplates.hpp"
#include <boost/program_options.hpp>
#include <poplibs_test/Reduce.hpp>
#include <popops/Reduce.hpp>
#include <stdexcept>
#include <string.h>

using namespace poplar;
using namespace poplar::program;
using namespace popops;
using namespace poputil;
using namespace poplibs_test::util;
using namespace poplibs_test::reduce;
using namespace poplibs_support;

const OptionFlags options;

static bool doTest(const DeviceType &deviceType, const Type &partialsType,
                   const Type &outType, const unsigned outerDim,
                   const unsigned innerDim,
                   const boost::optional<unsigned> numPartials,
                   const boost::optional<unsigned> outerStride,
                   const boost::optional<unsigned> numOuterStrides,
                   const unsigned outputDim, const popops::Operation op,
                   const float scale, bool isUpdate,
                   ReductionSpecialisation specialisation) {
  auto device = createTestDevice(deviceType);
  auto &target = device.getTarget();
  Graph graph(target);
  popops::addCodelets(graph);

  // Using negative initial value is important for log-add operations and using
  // integers (in float type variables) is helpful for exact comparison
  // for other operations
  const float initialValue = -1.0;
  const auto partialsGrainSize = partialsType == poplar::HALF ? 4 : 2;

  // Check constraints:
  if (specialisation == ReductionSpecialisation::SCALAR_OUTPUT_SINGLE_INPUT) {
    if (outputDim != 1) {
      std::cerr
          << "Output dimension must be 1 for SCALAR_OUTPUT_SINGLE_INPUT\n";
      return false;
    }

  } else if (specialisation == ReductionSpecialisation::STRIDED_REDUCE) {
    if (innerDim % partialsGrainSize) {
      std::cerr << "Inner dimension must be a multiple of 4 (half) or 2 "
                   "(float) for STRIDED_REDUCE\n";
      return false;
    }
    if (outputDim % partialsGrainSize) {
      std::cerr << "Output dimension must be a multiple of 4 (partials type = "
                   "half) or 2 "
                   "(partials type = float) for STRIDED_REDUCE\n";
      return false;
    }
  } else {
    std::cerr << "Unsupported specialisation\n";
    return false;
  }

  // Claim enough space for floats
  std::vector<char> data(innerDim * outerDim * 4);
  std::vector<float> nums(innerDim * outerDim);
  std::vector<int> intData(innerDim * outerDim);

  // Using negative input data is important for log-add operations and using
  // integers (in float type variables) is helpful for exact comparison
  // for other operations
  const float inputScale = op == popops::Operation::LOG_ADD ? -0.1f : 1.0f;
  for (unsigned i = 0; i < outerDim; ++i) {
    for (unsigned j = 0; j < innerDim; ++j) {
      nums[(i * innerDim) + j] = inputScale * (i + j);
      intData[(i * innerDim + j)] = i + j;
    }
  }

  copy(target, nums.data(), innerDim * outerDim, partialsType, data.data());
  std::vector<float> answers(outputDim, initialValue);
  std::vector<char> ans_data((outputDim)*4);
  copy(target, answers.data(), outputDim, outType, ans_data.data());

  Sequence prog;

  auto cs = graph.addComputeSet("cs");

  Tensor partials;
  partials = graph.addVariable(partialsType, {outerDim, innerDim});
  Tensor out;
  out = graph.addVariable(outType, {outputDim});

  bool useScale = (op == popops::Operation::LOG_ADD && scale != 0.0f) ||
                  (op != popops::Operation::LOG_ADD && scale != 1.0f);

  const auto vertexClass = [&]() {
    auto vertexTypeSpecialisation = specialisation;
    if (vertexTypeSpecialisation == ReductionSpecialisation::STRIDED_REDUCE &&
        (numOuterStrides.get() - 1) != 0) {
      vertexTypeSpecialisation = ReductionSpecialisation::STRIDED_REDUCE_OUTER;
    }

    return templateVertex(useScale ? "popops::ScaledReduce" : "popops::Reduce",
                          "popops::" + getReductionVertexOpName(op),
                          partialsType, outType, isUpdate,
                          vertexTypeSpecialisation);
  }();

  auto v1 = graph.addVertex(cs, vertexClass);

  graph.connect(v1["partials"], partials.flatten());
  graph.connect(v1["out"], out);
  if (specialisation == ReductionSpecialisation::SCALAR_OUTPUT_SINGLE_INPUT) {
    graph.setInitialValue(v1["numPartials"], innerDim * outerDim);
  } else {
    graph.setInitialValue(v1["numOutputsM1"],
                          outputDim / partialsGrainSize - 1);
    graph.setInitialValue(v1["numPartialsM1"], numPartials.get() - 1);
    graph.setInitialValue(v1["partialsWidth"], innerDim / partialsGrainSize);
    graph.setInitialValue(v1["numOuterStridesM1"], numOuterStrides.get() - 1);
    if (numOuterStrides.get() - 1 != 0) {
      graph.setInitialValue(v1["outerStride"], (outerStride.get() + 1) *
                                                   innerDim /
                                                   partialsGrainSize);
    }
  }
  auto scaleTensor = graph.addVariable(FLOAT, {});
  graph.setTileMapping(scaleTensor, 0);
  graph.setInitialValue(scaleTensor, scale);
  if (useScale) {
    graph.connect(v1["k"], scaleTensor.reshape({1}));
  }
  graph.setTileMapping(v1, 0);
  graph.setTileMapping(partials, 0);
  graph.setTileMapping(out, 0);

  graph.createHostWrite("partials", partials);
  graph.createHostWrite("outw", out);
  graph.createHostRead("out", out);

  prog.add(Execute(cs));

  Engine e(graph, prog, options);
  auto outSize = out.numElements() * target.getTypeSize(outType);

  device.bind([&](const Device &d) {
    e.load(d);
    if (outType == FLOAT || outType == HALF) {
      e.writeTensor("partials", data.data(),
                    data.data() + partials.numElements() *
                                      target.getTypeSize(partialsType));
      e.writeTensor("outw", ans_data.data(), ans_data.data() + outSize);
    } else if (outType == INT) {
      e.writeTensor("partials", intData.data(),
                    intData.data() + partials.numElements() *
                                         target.getTypeSize(partialsType));
      e.writeTensor("outw", ans_data.data(), ans_data.data() + outSize);
    }
    e.readTensor("out", ans_data.data(), ans_data.data() + outSize);

    e.run();

    e.readTensor("out", ans_data.data(), ans_data.data() + outSize);
  });

  copy(target, partialsType, data.data(), nums.data(), innerDim * outerDim);

  copy(target, outType, ans_data.data(), answers.data(), outputDim);
  copy(target, outType, ans_data.data(), intData.data(), outputDim);

  // Create / reduce the whole result, later we may only check part of it based
  // on outputDim.
  const auto arrayDim =
      (specialisation == ReductionSpecialisation::STRIDED_REDUCE) ? innerDim
                                                                  : outputDim;
  const auto numPartialsToReduce =
      specialisation == ReductionSpecialisation::SCALAR_OUTPUT_SINGLE_INPUT
          ? outerDim
          : numPartials.get() * numOuterStrides.get();

  // Loop extracting only the partials of interest in the case of strided
  // reduce, or all partials in the case of scalar output
  MultiArray<float> input{(numPartialsToReduce * innerDim) / arrayDim,
                          arrayDim};
  unsigned inRow = 0;
  unsigned numPartialsCount = 0;
  for (unsigned outRow = 0; outRow < numPartialsToReduce; outRow++) {
    if (specialisation == ReductionSpecialisation::SCALAR_OUTPUT_SINGLE_INPUT) {
      inRow = outRow;
    }
    for (unsigned column = 0; column < innerDim; column++) {
      input.data()[outRow * innerDim + column] =
          nums[inRow * innerDim + column];
    }
    if (specialisation == ReductionSpecialisation::STRIDED_REDUCE) {
      inRow++;
      numPartialsCount++;
      if (numPartialsCount == numPartials.get()) {
        inRow += outerStride.get();
        numPartialsCount = 0;
      }
      if (inRow > nums.size() / innerDim) {
        break;
      }
    }
  }

  auto result = reduce(input, {0}, op);

  std::vector<float> correct_answer(outputDim, initialValue);
  if (op == popops::Operation::LOG_ADD) {
    for (unsigned i = 0; i < outputDim; i++) {

      const auto scaledResult = log::mul<float>(result[i], scale);
      if (isUpdate) {
        correct_answer[i] = log::add(scaledResult, correct_answer[i]);
      } else {
        correct_answer[i] = scaledResult;
      }
    }
  } else {
    for (unsigned i = 0; i < outputDim; i++) {
      if (isUpdate) {
        correct_answer[i] += result[i] * scale;
      } else {
        correct_answer[i] = result[i] * scale;
      }
    }
  }

  bool success = true;
  if (outType == FLOAT || outType == HALF) {
    // When using half data the log-add and square-add results can be slightly
    // inaccurate.  log-add always, and square-add depending on the input values
    // Other operations should be exact with integer value float/half inputs
    const bool logAdd = op == popops::Operation::LOG_ADD;
    const bool squareAddHalf =
        op == popops::Operation::SQUARE_ADD && outType == HALF;
    const double tolerance = logAdd || squareAddHalf ? 0.001 : 0.0;
    for (unsigned i = 0; i < outputDim; ++i) {
      bool isClose = checkIsClose(correct_answer[i], answers[i], tolerance);
      if (!isClose) {
        std::cerr << "Result:" << answers[i]
                  << " Expected:" << correct_answer[i] << "\n";
        success = false;
      }
    }
    if (!success) {
      std::cerr << "Errors in result\n";
    }
  } else if (outType == INT) {
    for (unsigned i = 0; i < outputDim; ++i) {
      CHECK_IF(success, correct_answer[i] == intData[i]);
    }
  } else {
    success = false;
  }
  return success;
}

int main(int argc, char **argv) {
  namespace po = boost::program_options;

  popops::Operation op = popops::Operation::ADD;
  DeviceType deviceType;
  Type partialsType;
  Type outType;
  float scale = 2.0f;
  bool isUpdate = true;
  unsigned specialisation;
  unsigned outerDim, innerDim, outputDim;
  boost::optional<unsigned> outerStride = boost::none;
  boost::optional<unsigned> numOuterStrides = boost::none;
  boost::optional<unsigned> numPartials = boost::none;
  po::options_description desc("Options");
  // clang-format off
  desc.add_options()
    ("help", "Print help")
    ("device-type",
     po::value<DeviceType>(&deviceType)->required(),
     "Device Type")
    ("partials-type",
     po::value<Type>(&partialsType)->required(),
     "Partials Type")
    ("specialisation",
     po::value<unsigned>(&specialisation)->required(),
     "Specialisation: 2 = SCALAR_OUTPUT_SINGLE_INPUT or"
     " 3 = STRIDED_REDUCE")
    ("out-type",
     po::value<Type>(&outType)->required(),
     "Output type")
    ("operation",
     po::value(&op),
     "operation:ADD SQUARE_ADD LOG_ADD MAX MIN MUL LOGICAL_OR or LOGICAL_AND")
    ("update",
     po::value<bool>(&isUpdate)->default_value(isUpdate),
     "reduce with update")
    ("scale",
     po::value<float>(&scale)->default_value(scale),
     "scale")
    ("outer-dim",
     po::value<unsigned>(&outerDim)->required(),
     "Outer dimension of the whole input, only part of which may be reduced")
    ("inner-dim",
     po::value<unsigned>(&innerDim)->required(),
     "Inner dimension of the whole input, only part of which may be reduced")
    ("outer-stride",
     po::value(&outerStride),
     "Outer stride to take when testing STRIDED_REDUCE vertices")
    ("num-outer-strides",
     po::value(&numOuterStrides),
     "Number of outer-strides to take when testing STRIDED_REDUCE vertices")
    ("num-partials",
     po::value(&numPartials),
     "Number of partials to reduce per outerStride when testing STRIDED_REDUCE"
     " vertices")
    ("output-dim",
     po::value<unsigned>(&outputDim)->required(),
     "Output dimension");
  // clang-format on
  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n\n";
      return 1;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }
  if (specialisation == 2) {
    if (outerStride || numOuterStrides || numPartials) {
      std::cerr << "outer-stride, num-outer-stride and num-partials parameters"
                   " are specific to specialisation 3\n";
      return 1;
    }
  }
  if (specialisation == 3) {
    if (!outerStride || !numOuterStrides || !numPartials) {
      std::cerr << "outer-stride, num-outer-stride and num-partials parameters"
                   " are required for specialisation 3\n";
      return 1;
    }
    const auto dataUsed =
        (outerStride.get() + numPartials.get()) * (numOuterStrides.get() - 1) +
        numPartials.get();
    if (outerDim < dataUsed) {
      std::cerr << "outer-dim needs to be at least " << dataUsed
                << " for this test\n";
      return 1;
    }
  }
  const auto specialisationType =
      specialisation == 2 ? ReductionSpecialisation::SCALAR_OUTPUT_SINGLE_INPUT
                          : ReductionSpecialisation::STRIDED_REDUCE;
  if (!doTest(deviceType, partialsType, outType, outerDim, innerDim,
              numPartials, outerStride, numOuterStrides, outputDim, op, scale,
              isUpdate, specialisationType))
    return 1;
  return 0;
}


/**
 *    \file include/geombd/io/parser.hpp
 *    \author Keny Ordaz
 *    \version 1.0
 *    \date 2021
 *
 *    Class to parse a multibody system from URDF
 *    Copyright (c) 2021 Cinvestav
 *    This library is distributed under the MIT License.
 */

#ifndef HEADER_IO_PARSER_HPP
#define HEADER_IO_PARSER_HPP
#include <iostream>
#include <iomanip>
#include <vector>
#include <fstream>
#include <sstream>
#include <optional>
#include <memory>
#include <unordered_map>
//#include <chrono>
#include <Eigen/Core>
//#include "pugixml.hpp"

namespace pugi {
    class xml_node;
}
using namespace Eigen;

// support {{{1
RowVectorXd string_to_vector(const std::string& vec, int size=3);

class Pose
{
public:
    Pose(): xyz(0, 0, 0), rpy(0, 0, 0) {}

    void init_rpy(double roll, double pitch, double yaw)
    {
        auto phi = roll / 2.0;
        auto the = pitch / 2.0;
        auto psi = yaw / 2.0;
        auto sphi = sin(phi);  auto cphi = cos(phi);
        auto sthe = sin(the);  auto cthe = cos(the);
        auto spsi = sin(psi);  auto cpsi = cos(psi);

        this->x = sphi * cthe * cpsi - cphi * sthe * spsi;
        this->y = cphi * sthe * cpsi + sphi * cthe * spsi;
        this->z = cphi * cthe * spsi - sphi * sthe * cpsi;
        this->w = cphi * cthe * cpsi + sphi * sthe * spsi;

        this->normalize();
    };
    void normalize()
    {
        double s = sqrt(this->x * this->x +
                this->y * this->y +
                this->z * this->z +
                this->w * this->w);
        if (s == 0.0) {
            this->x = 0.0;
            this->y = 0.0;
            this->z = 0.0;
            this->w = 1.0;
        } else {
            this->x /= s;
            this->y /= s;
            this->z /= s;
            this->w /= s;
        }
    };
    double x, y, z, w;
    RowVector3d xyz;
    RowVector3d rpy;
};

class Axis
{
public:
    Axis(): xyz(1.0, 0., 0.) {}

    RowVector3d xyz;
};

class Inertia { // optional zero inertia
public:
    Inertia(double ixx=0.0, double ixy=0.0, double ixz=0.0, double iyy=0.0, double iyz=0.0, double izz=0.0)
        : ixx(ixx), ixy(ixy), ixz(ixz), iyy(iyy), iyz(iyz), izz(izz)
        { setMatrix(ixx, ixy, ixz, iyy, iyz, izz); }
    void setMatrix(double ixx=0.0, double ixy=0.0, double ixz=0.0, double iyy=0.0, double iyz=0.0, double izz=0.0)
        { value << ixx, ixy, ixz, ixy, iyy, iyz, ixz, iyz, izz; }

    double ixx;
    double ixy;
    double ixz;
    double iyy;
    double iyz;
    double izz;
    Matrix3d value;
};

class Inertial {
public:
    Inertial(): origin(), mass(0.0), inertia(), spatial() {}
    Inertial(const pugi::xml_node & node);
    std::string to_string() const {
        static IOFormat CommaInitFmt(StreamPrecision, DontAlignCols, ", ", ", ", "", "", " << ", ";");
        static IOFormat OctaveFmt(StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");
        std::ostringstream out;
        out << "mass (" << mass << "),\n\t origin[xyz] (" << origin.xyz.format(CommaInitFmt);
        out << "), origin[rpy] (" << origin.rpy.format(CommaInitFmt) << "); inertia tensor:\n";
        out << inertia.value.format(OctaveFmt);
        return out.str();

    }
    Pose origin;
    double mass;
    Inertia inertia;
    Matrix<double, 6, 6> spatial;
};
// support }}}1

class Element // {{{2
{
public:
    Element(const std::string& name, int id): name(name), id(id), tree_id(-1) {}
    std::string name;
    int id;
    int tree_id;
    //~Element() { std::cout << __PRETTY_FUNCTION__ << "\n"; }
};  // }}}2

class Body : public Element  // {{{1
{
public:
    Body(const std::string& name="unassigned", int id=-1);
    std::string to_string() const;
    void addChild(int child_id);
    void addParent(int parent_id);
    void addJoint(int joint_id);
    void setInertialProperties(const Inertial& value);
    bool has_inertial;
    Inertial inertial_properties;
    std::vector<int> child_links;
    std::vector<int> parent_links;
    std::vector<int> parent_joints;
    int parent;
    std::vector<int> Pre;
    std::vector<int> Suc;
};  // }}}1

class Joint : public Element // {{{1
{
    // MAke enum for joint types
public:
    enum joint_type {
        revolute = 'r',  /*!< The type of the joint is revolute. */
        prismatic = 'p', /*!< The type of the joint is prismatic. */
        fixed = 'f',      /*!< The type of the joint is glue. */
        notype = 'n'      /*!< There is no information about the type. */
    };

    Joint(const std::string& name="unassigned", int id=-1): nq(0), Element(name, id) {}
    Joint(const std::string& name, unsigned int id,
            const std::string& parent, const std::string& child, const std::string& type)
        : /*name(name), id(id),*/ predecessor(parent), successor(child)
        , axis(1.0, 0, 0), Element(name, id) {
            if (type == "revolute") {
                this->type = Joint::joint_type::revolute;
            } else if (type == "prismatic") {
                this->type = Joint::joint_type::prismatic;
            } else if (type == "fixed") {
                this->type = Joint::joint_type::fixed;
            } else {
                this->type = Joint::joint_type::notype;

            }

            switch(this->type) {
            case joint_type::revolute:
            case joint_type::prismatic:
                // TODO: check limits
                setLimit();
                nq = 1;
                break;
            default:
                nq = 0;// Do nothing?;
            }
        }
    //~Joint() { std::cout << __PRETTY_FUNCTION__ << "\n"; }

    std::string to_string() {
        std::ostringstream out;
        out << "joint " << name << ", id " << id << ", type " << std::string(1, type);
        out << ", axis " << axis;
        if (type == joint_type::revolute || type == joint_type::prismatic) {
            out << ", limits(" << upper << ", " << lower << ", " << effort << ", " << velocity << ") ";
        }
        out << joint_axis_indicator();
        out << "\t/" << successor << "/" << predecessor << "/ ";
        return out.str();
    }

    std::string joint_axis_indicator() {
        std::ostringstream out;
        out << "[";
        switch(type) {
        case Joint::joint_type::revolute:
            out << "⚙";
            break;
        case Joint::joint_type::prismatic:
            out << "↕";
            break;
        case Joint::joint_type::fixed:
            out << "⚓";
            break;
        case Joint::joint_type::notype:
            out << "⦻";
        }
        if (type == joint_type::revolute || type == joint_type::prismatic) {
            if (axis == RowVector3d(1.0, 0.0, 0.0))
                out << "+X";
            else if (axis == RowVector3d(0.0, 1.0, 0.0))
                out << "+Y";
            else if (axis == RowVector3d(-1.0, 0.0, 0.0))
                out << "-X";
            else if (axis == RowVector3d(0.0, -1.0, 0.0))
                out << "-Y";
            else if (axis == RowVector3d(0.0, 0.0, 1.0))
                out << "+Z";
            else if (axis == RowVector3d(0.0, 0.0, -1.0))
                out << "-Z";
            else
                out << "⦻";
        }

        out << "]";
        return out.str();
    }

    void setLimit(double upper=0.0, double lower=0.0, double effort=0.0, double velocity=0.0) {
        this->upper = upper; this->lower = lower; this->effort = effort; this->velocity = velocity;
    }

    void setOrigin(const RowVector3d& xyz, const RowVector3d& rpy) {
        origin.xyz = xyz;
        origin.rpy = rpy;
    }

    void setDampingAndFriction(double damping=0.0, double friction=0.0) {} //FIXME: If required
    std::string predecessor;
    std::string successor;
    joint_type type;
    RowVector3d axis;
    Pose origin;
    double upper, lower, effort, velocity;
    double damping, friction;
    int nq;
};  // }}}1

class Robot { // {{{1
public:
    Robot(const std::string& name="unknown");
    void addJoint(const std::string& name, unsigned int id,
            const std::string& parent, const std::string& child, const std::string& type);
    void addBody(const std::string& name, unsigned int id);
    void dump(std::ostream & os);
    void print_featherstone(std::ostream & os);
    void print_tree(std::ostream & os, const std::string & prefix, int id, int parent_id=-1);
    void print_tree(std::ostream & os);
    int getBodyID(const std::string& frame_name) const;
    int getJointID(const std::string& name) const;
    std::shared_ptr<Joint> & operator()(int i) { // robot(j)
        return joints[i];
    }
    std::shared_ptr<Body> & operator[](int i) { // robot[i]
        return bodies[i];
    }
    int getRootBodyID() const;
    const std::string& getName() const { return name; }
    static std::optional<std::shared_ptr<Robot>> build_model(const std::string& urdf_file);
    void add_pre(std::shared_ptr<Body> body, std::shared_ptr<Body> body_s);
    void add_suc(std::shared_ptr<Body> body, std::shared_ptr<Body> body_p);
    void build_PSvector( );
    void build_spatial_inertias( );
protected:
    bool load_from_urdf(const pugi::xml_node & node);
    void visit_spanning_tree(int root_id);
    void build_graph();
protected:
    std::string name;
    std::vector<std::shared_ptr<Joint>> joints;
    std::vector<std::shared_ptr<Body>> bodies;
    std::unordered_map<std::string,int> joint_dico;
    std::unordered_map<std::string,int> joint_pc_dico;
    std::unordered_map<std::string,int> link_dico;
public:
    std::vector<int> body_l;
    std::vector<int> body_m;
    std::vector<int> joint_p;
    std::vector<int> joint_s;
    Eigen::RowVectorXi tree_ids;
    Eigen::RowVectorXi j_tree_ids;
public:
    int nq;
private:
    int _count;
};  // }}}1

#endif


/*
 Copyright (C) 2019 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <ored/configuration/conventions.hpp>
#include <ored/configuration/curveconfigurations.hpp>
#include <ored/marketdata/csvloader.hpp>
#include <ored/marketdata/todaysmarket.hpp>
#include <ored/marketdata/todaysmarketparameters.hpp>
#include <ored/portfolio/enginefactory.hpp>
#include <ored/portfolio/portfolio.hpp>
#include <oret/datapaths.hpp>
#include <oret/toplevelfixture.hpp>

#include <ql/cashflows/cashflows.hpp>
#include <ql/cashflows/couponpricer.hpp>
#include <ql/instruments/capfloor.hpp>
#include <ql/pricingengines/capfloor/blackcapfloorengine.hpp>
#include <ql/termstructures/volatility/optionlet/constantoptionletvol.hpp>
#include <ql/time/calendar.hpp>
#include <ql/time/daycounter.hpp>

using namespace ore::data;
using namespace QuantLib;
using namespace boost::unit_test_framework;

using ore::test::TopLevelFixture;
using std::ostream;

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, TopLevelFixture)

BOOST_AUTO_TEST_SUITE(MXNIrCurvesTest)

// Test in-currency yield curve bootstrap
BOOST_AUTO_TEST_CASE(testSingleCurrencyYieldCurveBootstrap) {

    // Evaluation date
    Date asof(17, Apr, 2019);
    Settings::instance().evaluationDate() = asof;

    // Market
    auto conventions = boost::make_shared<Conventions>();
    conventions->fromFile(TEST_INPUT_FILE("conventions_01.xml"));
    auto todaysMarketParams = boost::make_shared<TodaysMarketParameters>();
    todaysMarketParams->fromFile(TEST_INPUT_FILE("todaysmarket_01.xml"));
    auto curveConfigs = boost::make_shared<CurveConfigurations>();
    curveConfigs->fromFile(TEST_INPUT_FILE("curveconfig_01.xml"));
    auto loader =
        boost::make_shared<CSVLoader>(TEST_INPUT_FILE("market_01.txt"), TEST_INPUT_FILE("fixings.txt"), false);
    boost::shared_ptr<TodaysMarket> market =
        boost::make_shared<TodaysMarket>(asof, todaysMarketParams, loader, curveConfigs, conventions, false);

    // Portfolio to test market
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->fromFile(TEST_INPUT_FILE("pricingengine_01.xml"));
    boost::shared_ptr<EngineFactory> factory = boost::make_shared<EngineFactory>(engineData, market);
    boost::shared_ptr<Portfolio> portfolio = boost::make_shared<Portfolio>();
    portfolio->load(TEST_INPUT_FILE("mxn_ir_swap.xml"));
    portfolio->build(factory);

    // The single trade in the portfolio is a MXN 10Y swap, i.e. 10 x 13 28D coupons, with nominal 100 million. The
    // rate on the swap is equal to the 10Y rate in the market file 'market_01.txt' so we should get an NPV of 0.
    BOOST_CHECK_EQUAL(portfolio->size(), 1);
    BOOST_CHECK_SMALL(portfolio->trades()[0]->instrument()->NPV(), 0.01);
}

// Test cross-currency yield curve bootstrap
BOOST_AUTO_TEST_CASE(testCrossCurrencyYieldCurveBootstrap) {

    // Evaluation date
    Date asof(17, Apr, 2019);
    Settings::instance().evaluationDate() = asof;

    // Market
    auto conventions = boost::make_shared<Conventions>();
    conventions->fromFile(TEST_INPUT_FILE("conventions_02.xml"));
    auto todaysMarketParams = boost::make_shared<TodaysMarketParameters>();
    todaysMarketParams->fromFile(TEST_INPUT_FILE("todaysmarket_02.xml"));
    auto curveConfigs = boost::make_shared<CurveConfigurations>();
    curveConfigs->fromFile(TEST_INPUT_FILE("curveconfig_02.xml"));
    auto loader =
        boost::make_shared<CSVLoader>(TEST_INPUT_FILE("market_02.txt"), TEST_INPUT_FILE("fixings.txt"), false);
    boost::shared_ptr<TodaysMarket> market =
        boost::make_shared<TodaysMarket>(asof, todaysMarketParams, loader, curveConfigs, conventions, false);

    // Portfolio to test market
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->fromFile(TEST_INPUT_FILE("pricingengine_02.xml"));
    boost::shared_ptr<EngineFactory> factory = boost::make_shared<EngineFactory>(engineData, market);
    boost::shared_ptr<Portfolio> portfolio = boost::make_shared<Portfolio>();
    portfolio->load(TEST_INPUT_FILE("mxn_usd_xccy_swap.xml"));
    portfolio->build(factory);

    // The single trade in the portfolio is a USD/MXN 10Y cross currency basis swap, i.e. 10 x 13 28D coupons, with
    // nominal USD 100 million. The spread on the swap is equal to the 10Y basis spread in the market file
    // 'market_02.txt' so we should get an NPV of 0.
    BOOST_CHECK_EQUAL(portfolio->size(), 1);
    BOOST_CHECK_SMALL(portfolio->trades()[0]->instrument()->NPV(), 0.01);
}

// Test cap floor strip
BOOST_AUTO_TEST_CASE(testCapFloorStrip) {

    // Evaluation date
    Date asof(17, Apr, 2019);
    Settings::instance().evaluationDate() = asof;

    // Market
    auto conventions = boost::make_shared<Conventions>();
    conventions->fromFile(TEST_INPUT_FILE("conventions_03.xml"));
    auto todaysMarketParams = boost::make_shared<TodaysMarketParameters>();
    todaysMarketParams->fromFile(TEST_INPUT_FILE("todaysmarket_03.xml"));
    auto curveConfigs = boost::make_shared<CurveConfigurations>();
    curveConfigs->fromFile(TEST_INPUT_FILE("curveconfig_03.xml"));
    auto loader =
        boost::make_shared<CSVLoader>(TEST_INPUT_FILE("market_03.txt"), TEST_INPUT_FILE("fixings.txt"), false);
    boost::shared_ptr<TodaysMarket> market =
        boost::make_shared<TodaysMarket>(asof, todaysMarketParams, loader, curveConfigs, conventions, false);

    // Portfolio to test market
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->fromFile(TEST_INPUT_FILE("pricingengine_03.xml"));
    boost::shared_ptr<EngineFactory> factory = boost::make_shared<EngineFactory>(engineData, market);
    boost::shared_ptr<Portfolio> portfolio = boost::make_shared<Portfolio>();
    portfolio->load(TEST_INPUT_FILE("mxn_ir_cap.xml"));
    portfolio->build(factory);

    // The single trade in the portfolio is a MXN 10Y cap, i.e. 10 x 13 28D coupons (without first caplet), with
    // nominal USD 100 million.
    BOOST_CHECK_EQUAL(portfolio->size(), 1);

    // Get the npv of the trade using the market i.e. the stripped optionlet surface from TodaysMarket
    Real npvTodaysMarket = portfolio->trades()[0]->instrument()->NPV();
    BOOST_TEST_MESSAGE("NPV using TodaysMarket is: " << npvTodaysMarket);

    // Price the same cap using the constant volatility from the market
    BOOST_REQUIRE(portfolio->trades()[0]);
    BOOST_REQUIRE(portfolio->trades()[0]->legs().size() == 1);
    auto pricer = boost::make_shared<BlackIborCouponPricer>(Handle<OptionletVolatilityStructure>(
        boost::make_shared<ConstantOptionletVolatility>(0, NullCalendar(), Unadjusted, 0.20320, Actual365Fixed())));
    Leg leg = portfolio->trades()[0]->legs().front();
    setCouponPricer(leg, pricer);
    Real npvMarketVol = CashFlows::npv(leg, **market->discountCurve("MXN"), false);
    BOOST_TEST_MESSAGE("NPV using the constant market volatility is: " << npvMarketVol);

    // Check the difference in the NPVs. Should be small if the stripping is working correctly.
    BOOST_CHECK_SMALL(fabs(npvTodaysMarket - npvMarketVol), 0.01);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <Geometry/ConvexPolygon.hpp>

#include <Eigen/Dense>
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/types_c.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/opencv.hpp>

#include <filesystem>

namespace pcv
{

template<class BitDepth, std::size_t ChannelNumber>
class Image
{
public:
    /**
     * Delete default constructor because that makes this type difficult to work
     * with.
     */
    Image() = delete;

    /**
     * Construct with a path to an image. Will read in data, using appropriate
     * codec, into a matrix that can later be accessed directly, if necessary.
     */
    explicit Image(const std::filesystem::path &imagePath);

    /**
     * Prevent a deep copy default constructor to prevent excessive copying.
     * There should exist helper methods to do deep copies in order to keep the
     * library computationally efficent.
     */
    Image(const Image& other) = delete;
    Image &operator=(const Image& other) = delete;

    /**
     * Default move constructor is exactly how one would implment a custom move
     * constructor. Don't be fooled by the lack of a default constructor!
     */
    Image(Image &&other) = default;
    Image &operator=(Image&& other) = default;

    /**
     * Writes the image to a file using a specified codec. Right now the output
     * file type is specified by the file name extension.
     */
    void write(const std::filesystem::path &savePath);

    /**
     * Crop the image to any polygon specified while retaining the original
     * image size.
     */
    template<class NumericType>
    void crop(const ConvexPolygon<NumericType> &polygon);

    /**
     * Draw a polygon onto the image. Will overwrite the image held in memory.
     * This is a very slow function and meant for debugging purposes only.
     */
    template<class NumericType>
    void drawPolygon(const ConvexPolygon<NumericType> &polygon);

    /**
     * Will keep the transformed image "within frame" -- meaning it will
     * increase (but never decrease) the rasterization frame size to include
     * the full transformed image. This may require using a transformation
     * different from the one intended by the caller, often by a translation to
     * keep the transformed image's bounding box only in positive pixel
     * coordinates. This function will return the actual transformation applied,
     * leaving the caller with complete information to deduce what happened.
     */
    Eigen::Matrix3d transform(const Eigen::Matrix3d &transform);

private:
    std::array<
        Eigen::Matrix<BitDepth, Eigen::Dynamic, Eigen::Dynamic>,
        ChannelNumber> matrices;
};

template<class BitDepth, std::size_t ChannelNumber>
inline Image<BitDepth, ChannelNumber>::Image(
    const std::filesystem::path &imagePath)
{
    cv::Mat cvBuffer = cv::imread(imagePath.native(), 1);

    if (! cvBuffer.data)
    {
        throw std::runtime_error("OpenCV failed to load data from file.");
    }

    std::array<cv::Mat, ChannelNumber> channels;
    cv::split(cvBuffer, channels.data());
    std::transform(
        std::cbegin(channels), std::cend(channels), std::begin(matrices),
        [](const cv::Mat &channel)
        {
            Eigen::Matrix<BitDepth, Eigen::Dynamic, Eigen::Dynamic> eigenBuffer;
            cv::cv2eigen<BitDepth>(channel, eigenBuffer);
            return eigenBuffer;
        });
}

template<class BitDepth, std::size_t ChannelNumber>
inline void Image<BitDepth, ChannelNumber>::write(
    const std::filesystem::path &savePath)
{
    std::array<cv::Mat, ChannelNumber> channels;
    std::transform(
        std::cbegin(matrices), std::cend(matrices), std::begin(channels),
        [](const Eigen::Matrix<BitDepth, Eigen::Dynamic, Eigen::Dynamic> &channel)
        {
            cv::Mat cvBuffer;
            cv::eigen2cv<BitDepth>(channel, cvBuffer);
            return cvBuffer;
        });

    cv::Mat output;
    cv::merge(channels.data(), 3, output);
    cv::imwrite(savePath.native(), output);
}

template<class BitDepth, std::size_t ChannelNumber>
template<class NumericType>
inline void Image<BitDepth, ChannelNumber>::crop(
    const ConvexPolygon<NumericType> &polygon)
{
    decltype(matrices) matrixBuffers{};

    std::transform(
        std::cbegin(matrices), std::cend(matrices), std::begin(matrixBuffers),
        [&polygon](const Eigen::Matrix<BitDepth, Eigen::Dynamic, Eigen::Dynamic> &tmp)
        {
            auto channel = tmp;
            for (auto r = 0; r < channel.rows(); ++r)
            {
                for (auto c = 0; c < channel.cols(); ++c)
                {
                    if (! polygon.isPointContained(Eigen::Vector2d({r, c})))
                    {
                        channel(r, c) = 0;
                    }
                }
            }
            return channel;
        });

    matrices = std::move(matrixBuffers);
}

template<class BitDepth, std::size_t ChannelNumber>
template<class NumericType>
inline void Image<BitDepth, ChannelNumber>::drawPolygon(
    const ConvexPolygon<NumericType> &polygon)
{
    decltype(matrices) matrixBuffers{};

    std::transform(
        std::cbegin(matrices), std::cend(matrices), std::begin(matrixBuffers),
        [&polygon](const Eigen::Matrix<BitDepth, Eigen::Dynamic, Eigen::Dynamic> &tmp)
        {
            auto channel = tmp;
            for (auto r = 0; r < channel.rows(); ++r)
            {
                for (auto c = 0; c < channel.cols(); ++c)
                {
                    if (polygon.isPointContained(Eigen::Vector2d({r, c})))
                    {
                        channel(r, c) = 0;
                    }
                }
            }
            return channel;
        });

    matrices = std::move(matrixBuffers);
}

template<class BitDepth, std::size_t ChannelNumber>
inline Eigen::Matrix3d transform(const Eigen::Matrix3d &transform)
{
    // Transform the bounding box (represented as a convex poly.) to get new
    // transformed poly. Get new transformed poly's bounding box size and
    // top-left corner to potentially add a translation on top of the transform
    // or allocate more memory to this->matrices due to increased bounding box
    // size. Don't need to worry about decreasing memory usage if the image
    // shrinks -- for now.
}

}


//  rational_constant.hpp  ---------------------------------------------------------------//
//  Copyright 2011 Vicente J. Botet Escriba
//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

#ifndef BOOST_RATIO_MPL_RATIONAL_CONSTANT_HPP
#define BOOST_RATIO_MPL_RATIONAL_CONSTANT_HPP

#include <boost/ratio/mpl/rational_c_tag.hpp>
#include <boost/ratio/mpl/numeric_cast.hpp>
#include <boost/ratio/mpl/arithmetic.hpp>
#include <boost/ratio/mpl/comparison.hpp>

#endif  // BOOST_RATIO_HPP


#include "SM_common.h"

#include <CGAL/Surface_mesh.h>
#include <CGAL/Simple_cartesian.h>

#include <boost/bind.hpp>
#include <boost/range/algorithm.hpp>

#include <CGAL/use.h>

void constructors_test()
{
  Sm mesh;
  Sm mesh2(mesh);
  mesh = mesh2;
  // Surface_mesh::assign
}

template<typename Iterator>
void test_iterator(Iterator begin, Iterator end,
                   typename std::iterator_traits<Iterator>::difference_type distance)
{
  assert(begin != end);
  assert(begin == begin);
  assert(std::distance(begin, end) == distance);
}

void standard_iterators()
{
  Surface_fixture f;

  Sm::Vertex_iterator vb, ve;
  boost::tie(vb, ve) = f.m.vertices();
  test_iterator(vb, ve, 5);

  Sm::Halfedge_iterator hb, he;
  boost::tie(hb, he) = f.m.halfedges();
  test_iterator(hb, he, 14);

  Sm::Edge_iterator eb, ee;
  boost::tie(eb, ee) = f.m.edges();
  test_iterator(eb, ee, 7);

  Sm::Face_iterator fb, fe;
  boost::tie(fb, fe) = f.m.faces();
  test_iterator(fb, fe, 3);
}

void test_descriptors()
{
  Sm::Vertex_index v;
  Sm::Halfedge_index h;
  CGAL_USE(v < v);
  CGAL_USE(h == h);
}

void test_remove_edge ()
{
  Surface_fixture f;
  Sm::Halfedge_index wv, uw, wx, xv, vu;
  wv = f.m.halfedge(f.w, f.v);
  assert(f.m.target(wv) == f.v);
  assert(wv.is_valid());
  uw = f.m.halfedge(f.u, f.w);
  assert(uw.is_valid());
  wx = f.m.halfedge(f.w, f.x);
  assert(wx.is_valid());
  xv = f.m.halfedge(f.x, f.v);
  assert(xv.is_valid());
  vu = f.m.halfedge(f.v, f.u);
  assert(vu.is_valid());
  assert(f.m.degree(f.v)== 4);
  assert(f.m.degree(f.w)== 3);

  f.m.set_next(uw, wx);
  f.m.set_next(xv, vu);
  f.m.remove_edge(Sm::Edge_index(wv));
  Sm::Halfedge_around_target_circulator a(f.m.halfedge(f.w),f.m), b(a);
  assert(CGAL::circulator_distance(a, b)== 2);
  assert(f.m.degree(f.w)== 2);
  a = b = Sm::Halfedge_around_target_circulator(f.m.halfedge(f.v),f.m);
  assert(CGAL::circulator_distance(a, b)== 3);
  assert(f.m.degree(f.v)== 3);

  // now remove a border edge to check if this works
  // this should not lower the number of faces
  Sm::size_type old_removed_faces_size = f.m.number_of_removed_faces();
  f.m.remove_edge(Sm::Edge_index(wx));
  assert(f.m.number_of_removed_faces()== old_removed_faces_size);
}

void memory_reuse_test()
{
  Cube_fixture f;
  // buffer all faces
  typedef std::vector<Sm::Vertex_index> VecFace;
  typedef std::vector<VecFace> Faces;

  Faces faces;
  Sm::Face_iterator fb, fe;
  for(boost::tie(fb, fe) = f.m.faces(); fb != fe; ++fb) {
    faces.push_back(VecFace());
    Sm::Vertex_around_face_circulator vafb(f.m.halfedge(*fb), f.m), vafe(vafb);
    if(vafb)
      do {
        faces.back().push_back(*vafb);
      } while(++vafb != vafe);
    assert(f.m.degree(*fb)== faces.back().size());
  }

  Sm::Vertex_iterator vb, ve;
  for(boost::tie(vb, ve) = f.m.vertices(); vb != ve; ++vb) {
    f.m.set_halfedge(*vb, Sm::Halfedge_index());
  }

  // remove all faces
  std::size_t old_face_size = f.m.number_of_faces();
  std::size_t old_removed_face_size = f.m.number_of_removed_faces();
  boost::range::for_each(f.m.faces(), boost::bind(&Sm::remove_face, boost::ref(f.m), _1));
  assert(f.m.number_of_faces()== 0);
  assert(f.m.number_of_removed_faces()== old_face_size + old_removed_face_size);
  // remove all edges
  std::size_t old_edge_size = f.m.number_of_edges();
  std::size_t old_removed_edge_size = f.m.number_of_removed_edges();
  boost::range::for_each(f.m.edges(),
                         boost::bind(static_cast<void (Sm::*)(Sm::Edge_index)>(&Sm::remove_edge),
                                     boost::ref(f.m), _1));
  assert(f.m.number_of_faces() == 0);
  assert(f.m.number_of_removed_edges()== old_edge_size + old_removed_edge_size);

  // add all again
  for(Faces::iterator it = faces.begin(); it != faces.end(); ++it) {
    Sm::Face_index fd = f.m.add_face(*it);
    assert(fd.is_valid());
    f.m.set_vertex_halfedge_to_border_halfedge(f.m.halfedge(fd));
    for(VecFace::iterator it2 = it->begin(); it2 != it->end(); ++it2) {

      Sm::Halfedge_index h = f.m.halfedge(*it2);
      Sm::Face_index fa = f.m.face(h);
      CGAL_USE(fa);
    }
  }

  assert(f.m.number_of_edges() == old_edge_size);
  assert(f.m.number_of_faces() == old_face_size);

  // remove all vertices
  std::size_t old_size = f.m.number_of_vertices();
  std::size_t old_removed_size = f.m.number_of_removed_vertices();

  boost::range::for_each(f.m.vertices(), boost::bind(&Sm::remove_vertex, boost::ref(f.m), _1));
  assert(f.m.number_of_vertices() == 0);
  assert(f.m.number_of_removed_vertices()== old_size + old_removed_size);

  for(unsigned int i = 0; i < old_size; ++i)
  {
    f.m.add_vertex(K::Point_3());
  }

  // the size must remain the same
  assert(f.m.number_of_vertices()== old_size);
}

void test_validate()
{
  Cube_fixture cf;
  Surface_fixture f1;
  Surface_fixture_2 f2;
  Surface_fixture_3 f3;
  assert(cf.m.is_valid());
  assert(f1.m.is_valid());
  assert(f2.m.is_valid());
  assert(f3.m.is_valid());
}

void isolated_vertex_check()
{
  Surface_fixture f;
  Sm::Vertex_index isolated = f.m.add_vertex(Point_3(10, 10, 10));
  assert(f.m.is_isolated(isolated));
  assert(!f.m.halfedge(isolated).is_valid());
  assert(f.m.is_border(isolated));
  assert(f.m.is_border(isolated, false));
  assert(f.m.degree(isolated) == 0);
}

void embedded_vertex_check()
{
  Surface_fixture_2 f;
  assert(!f.m.is_isolated(f.y));
  assert(!f.m.is_border(f.y));
  assert(f.m.halfedge(f.y).is_valid());
  assert(f.m.degree(f.y) == 4);
}

void border_vertex_check()
{
  Surface_fixture f;
  assert(!f.m.is_isolated(f.y));
  assert(f.m.is_border(f.y));
  assert(f.m.degree(f.y) == 2);
  assert(f.m.halfedge(f.y).is_valid());
  set_halfedge(f.y, opposite(next(f.m.halfedge(f.y),f.m), f.m), f.m);
  assert(f.m.is_border(f.y));
  assert(! f.m.is_border(f.y, false));
}

void point_position_accessor ()
{
  Surface_fixture f;
  // by property
  f.m.points()[f.x];
  // non-const
  f.m.point(f.x);
  // const as an lvalue
  f.m.point(f.x) = CGAL::ORIGIN;
  assert(f.m.point(f.x) == CGAL::ORIGIN);
}

void properties () {
  Surface_fixture f;

  Sm::Property_map<Sm::Vertex_index, int> prop;
  bool created = false;

  boost::tie(prop,created) = f.m.add_property_map<Sm::Vertex_index, int>("illuminatiproperty", 23);
  assert(created == true);

  boost::tie(prop, created)= f.m.add_property_map<Sm::Vertex_index, int>("illuminatiproperty");
  assert(created == false);
}

int main()
{
  constructors_test();
  standard_iterators();
  test_descriptors();
  test_remove_edge();
  memory_reuse_test();
  test_validate();
  isolated_vertex_check();
  embedded_vertex_check();
  border_vertex_check();
  point_position_accessor();
  properties();
  std::cout << "done" << std::endl;
  return 0;
}


// -*-C++-*-
/*
  (c) Copyright 2007, Hewlett-Packard Development Company, LP

  See the file named COPYING for license details
*/

// Note this is intended to be a simple example of how to merge
// multiple extents, it does not handle multiple data types.
// Nor does it let you map multiple things onto a single output
// name, which would make sense in the fake example setup below
// where both the sizes should map onto the same output column.

#include <inttypes.h>

#include <boost/format.hpp>

#include <Lintel/HashMap.hpp>
#include <Lintel/MersenneTwisterRandom.hpp>
#include <Lintel/StatsQuantile.hpp>
#include <Lintel/StringUtil.hpp>

#include <DataSeries/DataSeriesModule.hpp>
#include <DataSeries/DStoTextModule.hpp>
#include <DataSeries/RowAnalysisModule.hpp>
#include <DataSeries/SequenceModule.hpp>

using namespace std;
using namespace boost;

/// Sample source class, generates data ab-initio; one column "time"
/// is generated from start time with each row separated by a uniform
/// random draw.  one column "rownum" is just incremented from
/// start_row to max_rowid-1 n columns are generated as random values
/// (col-#)

class RandomSource : public DataSeriesModule {
  public:
    RandomSource(const string &type_name, uint32_t _max_rowid, uint32_t ncolumns,
                 uint32_t start_row, double start_time)
            : max_rowid(_max_rowid), cur_row(start_row), cur_time(start_time),
              // Prepare our statically known fields (columns) in the constructor.
              time(series, "time"), rownum(series, "rownum")
    {
        vector<string> spec;

        // Since we don't know the extent name, or number of columns
        // we have to dynamically generate the output specification.

        spec.push_back((format("<ExtentType name=\"%s\">") % type_name).str());
        // Fake up all the columns as doubles even though rownum should
        // be int32/64 to make writing the merger easier.
        spec.push_back("  <field type=\"double\" name=\"time\" />");
        spec.push_back("  <field type=\"double\" name=\"rownum\" />");
        for (uint32_t i=0; i<ncolumns; ++i) {
            spec.push_back((format("  <field type=\"double\" name=\"col-%d\" />") % i).str());
            // Prepare the dynamically known fields at runtime.
            columns.push_back(new DoubleField(series, (format("col-%d") % i).str()));
        }
        spec.push_back("</ExtentType>");

        // Now we can make the type and initialize the series.
        string type = join("",spec);
        output_type = &ExtentTypeLibrary::sharedExtentType(type);
        series.setType(*output_type);
    }

    virtual Extent::Ptr getSharedExtent() {
        if (cur_row == max_rowid) {
            return Extent::Ptr(); // No more extents
        }

        INVARIANT(cur_row < max_rowid, "bad");

        // Pick a random number of rows to put into an extent; unless
        // the constants in main() are changed, this doesn't actually
        // matter.
        uint32_t extent_rows = 1000 + MTRandom.randInt(1000);
        if (cur_row + extent_rows > max_rowid) {
            extent_rows = max_rowid - cur_row;
        }

        // Set the series' type.
        series.setType(output_type);
        // Make a new extent
        series.newExtent();

        // Generate all of the rows.
        for (uint32_t i = 0; i < extent_rows; ++i) {
            series.newRecord();  // new row.

            time.set(cur_time); // fixed column 1
            rownum.set(cur_row); // fixed column 2
            for (uint32_t j = 0; j < columns.size(); ++j) { // variable columns
                // multiply by 100 to get a bigger range
                columns[j]->set(100.0*MTRandom.randDoubleOpen());
            }
            // increment dependent things
            cur_time += MTRandom.randDoubleOpen();
            ++cur_row;
        }
        return series.getSharedExtent();
    };
  private:
    ExtentSeries series;

    uint32_t max_rowid, cur_row;
    double cur_time;
    DoubleField time;
    DoubleField rownum;
    vector<DoubleField *> columns;
    const ExtentType *output_type;
};

/// This class does the merge of multiple input extents mapping the
/// columns in the input extent into various columns in the output
/// extents and potentially pruning out the columns that are not
/// needed.  It only handles double fields right now.  This could be
/// fixed by the use of GeneralField/GeneralValue, or by handling the
/// different fields separately, e.g. having a vector for each of the
/// field types.  The latter is probably faster but more complicated.
/// Nor does it support merging output fields.  This could be
/// implemented just by excluding rows from the type specification if
/// they duplicate an earlier name after checking the types are the
/// same.

class Merger : public DataSeriesModule {
  public:
    // No known upstream modules at creation time.
    Merger(const string &_type_name, const string &time_name)
            : type_name(_type_name), output_type(NULL),
              // Two fixed fields, one with a variable name.
              orig_src(output_series, "orig_src"), time(output_series, time_name)
    { }

    // How do we map between source fields (columns) and destination ones.
    struct Mapping {
        Mapping(const string &src) : source_name(src), dest_name(src) {}
        Mapping(const string &src, const string &dest)
                : source_name(src), dest_name(dest) {}

        string source_name;
        string dest_name;
    };

    // The complete description of an individual source
    struct Source {
        typedef vector<Source *>::iterator iter;
        DataSeriesModule &src;
        ExtentSeries series; // the series for this source.
        bool done; // Avoid repeatedly calling getExtent() on sources that are done.
        DoubleField time_field; // Fixed fields.
        vector<DoubleField *> srcs; // use internal series
        vector<DoubleField *> dests; // use output series
        Source(DataSeriesModule &_src, const string &time_field_name)
                : src(_src), done(false), time_field(series, time_field_name) { }
    };

    // This function specifies an input module for the merger, the
    // time field it should use for ordering, and the mapping of the
    // names.
    void addSource(DataSeriesModule &srcmod, const string &time_field, vector<Mapping> &mapping) {
        INVARIANT(output_type == NULL, "no addSource after first getExtent");

        Source *src = new Source(srcmod, time_field);
        for (vector<Mapping>::iterator i = mapping.begin();
            i != mapping.end(); ++i) {
            // All these fields are variable, so have to be constructed dynamically.
            src->srcs.push_back(new DoubleField(src->series, i->source_name, Field::flag_nullable));
            src->dests.push_back(new DoubleField(output_series, i->dest_name, Field::flag_nullable));
        }
        sources.push_back(src);
    }

    virtual Extent *getExtent() {
        if (output_type == NULL) {
            vector<string> spec;
            // As with the RandomSource, we don't know our spec so we have to construct it.
            spec.push_back((format("<ExtentType name=\"%s\">\n") % type_name).str());
            // Two fixed fields.
            spec.push_back("  <field type=\"int32\" name=\"orig_src\" />\n");
            spec.push_back((format("  <field type=\"double\" name=\"%s\" />\n") % time.getName()).str());
            // All the variable fields from the various sources.
            for (Source::iter i = sources.begin();
                i != sources.end(); ++i) {
                for (vector<DoubleField *>::iterator j = (**i).dests.begin();
                    j != (**i).dests.end(); ++j) {
                    spec.push_back((format("  <field type=\"double\" name=\"%s\" opt_nullable=\"yes\" />\n")
                                    % (**j).getName()).str());
                }
            }
            spec.push_back("</ExtentType>\n");
            output_type = &ExtentTypeLibrary::sharedExtentType(join("",spec));
            // For debugging, dump out the final spec.
            cout << join("", spec);
        }

        // Make output extent and prepare to use it.
        Extent *out_extent = new Extent(*output_type);
        output_series.setExtent(out_extent);

        // In practice a way to small number (10), but it forces
        // multiple extents with the constants used in main.
        const uint32_t max_rows_per_extent = 10;
        for (uint32_t rownum = 0; rownum < max_rows_per_extent; ++rownum) {
            Source::iter least = sources.end();

            // Search for the source with the least input, more efficient if
            // it used a priority queue.
            for (Source::iter i = sources.begin(); i != sources.end(); ++i) {
                if ((**i).series.morerecords() == false) {
                    if ((**i).done) {
                        continue; // definitely nothing in this source
                    }
                    // Might have more
                    Extent *e = (**i).src.getExtent();
                    if (e == NULL) {
                        (**i).done = true; // nope, don't bother to ask again.
                        continue;
                    }
                    (**i).series.setExtent(e);
                }
                if (least == sources.end()) {
                    least = i; // first source with a value, take it.
                } else if ((**i).time_field.val() < (**least).time_field.val()) {
                    least = i; // better than the previous least source, take it.
                }
            }
            if (least == sources.end()) { // didn't find any source
                if (rownum == 0) { // didn't make this row.
                    delete out_extent;
                    return NULL; // We're done.
                } else {
                    return out_extent; // We got a partial extent.
                }
            }

            output_series.newRecord(); // Now we know we have something so we can make a row.
            // probably could move this up above and do it in the same
            // loop as the search for least.

            // Default all outputs to null
            for (Source::iter i = sources.begin(); i != sources.end(); ++i) {
                for (vector<DoubleField *>::iterator j = (**i).dests.begin();
                    j != (**i).dests.end(); ++j) {
                    (**j).setNull();
                }
            }

            // Set the fixed fields.
            orig_src.set(least - sources.begin());
            time.set((**least).time_field.val());
            // Set the variable fields.
            for (uint32_t j = 0; j < (**least).dests.size(); ++j) {
                if ((**least).srcs[j]->isNull()) {
                    (**least).dests[j]->setNull();
                } else {
                    (**least).dests[j]->set((**least).srcs[j]->val());
                }
            }
            // Advance to the next row in the least.
            ++((**least).series);
        }
        // Give full extent to caller.
        return out_extent;
    }
  private:
    ExtentSeries output_series;
    const string type_name;
    const ExtentType *output_type;
    vector<Source *> sources;
    Int32Field orig_src;
    DoubleField time;
};

// Not in Lintel, not clear if building a hashmap over doubles is even
// a good idea.
template <>
struct HashMap_hash<const double> {
    unsigned operator()(const double _a) const {
        BOOST_STATIC_ASSERT(sizeof(double) == sizeof(uint64_t));
        union {
            uint64_t i;
            double d;
        } v;
        v.d = _a;
        return lintel::BobJenkinsHashMixULL(v.i);
    }
};

/// This is a simple analysis, we are going to do self join in the
/// series, find matching open and close records and calculate the
/// latency between opens and closes, as well as the change in the
/// data and resource fork sizes between opens and closes.  The module
/// assumes that the input and output sources are numbered 0 and 1.
/// This could be changed to be an argument at the cost of changing a
/// switch to an if statement.  If we made that change, then we could
/// stack two analysis one that treated the inputs as (open, close) and
/// the other that treated the inputs as (close, open)
class OpenCloseAnalysis : public RowAnalysisModule {
  public:
    OpenCloseAnalysis(DataSeriesModule &src)
    : RowAnalysisModule(src),
      // All our fields are static.
      data_src(series, "orig_src"),
      time(series, "time"),
      open_id(series, "open_file_id", Field::flag_nullable),
      close_id(series, "close_file_id", Field::flag_nullable),
      open_datasize(series, "open_data_fork_KiB", Field::flag_nullable),
      open_resourcesize(series, "open_resource_fork_KiB", Field::flag_nullable),
      close_datasize(series, "close_data_fork_KiB", Field::flag_nullable),
      close_resourcesize(series, "close_resource_fork_KiB", Field::flag_nullable),
      opens_without_closes(0),
      closes_without_opens(0),
      rowcount(0)
    { }

    // We just want to handle all the rows separately, so we can use
    // the row analysis module.
    virtual void processRow() {
        ++rowcount;
        switch(data_src.val()) {
            case 0: // open
                {
                    INVARIANT(!fileid_to_info.exists(open_id.val()), "already opened file??");

                    // Save the information about the open.
                    fileid_to_info[open_id.val()] = finfo(time.val(), open_datasize.val(), close_datasize.val());
                }
                break;
            case 1: // close
                {
                    if (fileid_to_info.exists(close_id.val())) {
                        // Found the previous open, update all the statistics.
                        finfo &f = fileid_to_info[close_id.val()];
                        open_close_latency.add(time.val() - f.opentime);
                        datasize_change.add(close_datasize.val() - f.datasize);
                        resourcesize_change.add(close_resourcesize.val() - f.resourcesize);
                        fileid_to_info.remove(close_id.val());
                    } else {
                        // Hmm, no open?
                        ++closes_without_opens;
                    }
                }
                break;
            default:
                FATAL_ERROR("??");
        }
    }

    virtual void completeProcessing() {
        // Everything here we never saw a close for.
        opens_without_closes = fileid_to_info.size();
    }

    virtual void printResult() {
        cout << "BEGIN Results for " << __PRETTY_FUNCTION__ << "\n";
        // My integer counting bits...
        cout << boost::format("processed %d rows\n") % rowcount;
        cout << boost::format("opens without closes: %d\n") % opens_without_closes;
        cout << boost::format("closes without opens: %d\n") % closes_without_opens;
        // My complicated statistics.
        cout << "open to close latency:\n";
        open_close_latency.printRome(2, cout);
        cout << "datasize change:\n";
        datasize_change.printRome(2, cout);
        cout << "resourcesize change:\n";
        resourcesize_change.printRome(2, cout);
        cout << "END Results for " << __PRETTY_FUNCTION__ << "\n";
    }

  private:
    Int32Field data_src;
    DoubleField time, open_id, close_id;
    DoubleField open_datasize, open_resourcesize, close_datasize, close_resourcesize;

    struct finfo {
        double opentime, datasize, resourcesize;
        finfo(double a, double b, double c)
                : opentime(a), datasize(b), resourcesize(c) { }
        finfo() {} // needed by HashMap for some reason.
    };

    HashMap<double, finfo> fileid_to_info;

    // Example of generating different types of statistics.
    StatsQuantile open_close_latency;
    Stats datasize_change;
    Stats resourcesize_change;

    uint32_t opens_without_closes, closes_without_opens, rowcount;
};

int
main()
{
    MTRandom.init(1972); // Fixed so everyone gets the same output.

    // We are going to fake this up as if we have a source or open
    // events and a source of close events.  We are pretending to have
    // traced on a macintosh and we happened to get the data and
    // resource fork sizes at both open and close.

    // Offset the ids and the times to give us a bit of control over
    // what we will see in the analysis.
    RandomSource a("src-a", 20, 3, 5, 0.0); // open's ids 5..20
    RandomSource b("src-b", 15, 2, 0, 10.0); // closes ids 0..15

    // Construct the merger
    Merger *m = new Merger("merged", "time");

    vector<Merger::Mapping> mapping;

    mapping.push_back(Merger::Mapping("rownum","open_file_id"));
    mapping.push_back(Merger::Mapping("col-0","open_data_fork_KiB"));
    // Note that we dropped a column from RandomSource a; we don't need it.
    mapping.push_back(Merger::Mapping("col-2","open_resource_fork_KiB"));

    m->addSource(a, "time", mapping);

    // Now build the mapping for the second source...
    mapping.clear();

    mapping.push_back(Merger::Mapping("rownum","close_file_id"));
    mapping.push_back(Merger::Mapping("col-0","close_data_fork_KiB"));
    mapping.push_back(Merger::Mapping("col-1","close_resource_fork_KiB"));

    m->addSource(b, "time", mapping);

    SequenceModule seq(m);
    // It's nice to see this series since it's random, but if you
    // comment out the next line it will be suppressed.  Normally
    // the list of modules in the sequence would be controlled by
    // command line arguments.
    seq.addModule(new DStoTextModule(seq.tail()));
    seq.addModule(new OpenCloseAnalysis(seq.tail()));

    seq.getAndDelete();

    RowAnalysisModule::printAllResults(seq);

    return 0;
}


#pragma once
#ifndef SIGNATURES_HH
#define SIGNATURES_HH

#include <memory>

#include <boost/dynamic_bitset.hpp>

namespace planner::util {
// Because the Universe is determined by a compound space of discrete spaces for each of the
// eqclass-identifying predicates, and we can compactly represent this with a bitvector. Also
// helps with hashing.
using DiscreteSig    = boost::dynamic_bitset<>;
using UniverseSig    = DiscreteSig;
using ConfigSig      = DiscreteSig;
using UniverseSigPtr = std::shared_ptr<UniverseSig>;
}  // namespace planner::util
#endif /* end of include guard */