// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas, Jason Williams
#include <ohm/DefaultLayer.h>
#include <ohm/Key.h>
#include <ohm/MapLayer.h>
#include <ohm/MapLayout.h>
#include <ohm/NdtMap.h>
#include <ohm/OccupancyMap.h>
#include <ohm/Trace.h>
#include <ohm/VoxelData.h>

#include <ohmgpu/GpuCache.h>
#include <ohmgpu/GpuNdtMap.h>

#include <gtest/gtest.h>
#include "ohmtestcommon/CovarianceTestUtil.h"
#include "ohmtestcommon/OhmTestUtil.h"

#include <chrono>
#include <random>
#include <unordered_map>

#include <glm/gtc/matrix_access.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtx/norm.hpp>
#include <glm/mat3x3.hpp>
#include <glm/vec3.hpp>

// Must come after glm includes due to usage on GPU.
#include <ohm/CovarianceVoxel.h>

#include <3esservermacros.h>

#include <Eigen/Dense>

using namespace ohm;
using namespace ohmtestutil;

namespace ndttests
{
void testNdtHits(const std::vector<glm::dvec3> &samples, double resolution)
{
  std::unordered_map<ohm::Key, CovTestVoxel> reference_voxels;

  ohm::OccupancyMap map(resolution, ohm::MapFlag::kVoxelMean);
  ohm::GpuNdtMap ndt(&map, true, ohm::NdtMode::kTraversability);

  // Simulate a sensor at the origin. Not used.
  const glm::dvec3 sensor(0.0);

  // Queue in rays.
  std::vector<glm::dvec3> rays(samples.size() * 2);
  std::vector<float> intensities(samples.size(), 0.0f);
  for (size_t i = 0; i < samples.size(); ++i)
  {
    rays[i * 2 + 0] = sensor;
    rays[i * 2 + 1] = samples[i];

    // Update reference voxel
    ohm::Key key = map.voxelKey(samples[i]);
    auto ref = reference_voxels.find(key);
    if (ref == reference_voxels.end())
    {
      // New voxel. Initialise.
      CovTestVoxel ref_voxel;
      initialiseTestVoxel(&ref_voxel, float(ndt.map().resolution()));
      ref = reference_voxels.insert(std::make_pair(key, ref_voxel)).first;
    }

    // Update the reference algorithm.
    updateHit(&ref->second, samples[i]);
  }

  ndt.integrateRays(rays.data(), rays.size(), intensities.data(), nullptr, ohm::kRfExcludeRay);
  ndt.syncVoxels();

  // Validate
  int covariance_layer_index = -1;
  if (const ohm::MapLayer *layer = map.layout().layer(ohm::default_layer::covarianceLayerName()))
  {
    covariance_layer_index = layer->layerIndex();
  }

  ASSERT_GE(covariance_layer_index, 0);

  ohm::Voxel<const ohm::VoxelMean> mean(&map, map.layout().meanLayer());
  ohm::Voxel<const ohm::CovarianceVoxel> covariance(&map, map.layout().covarianceLayer());
  for (const auto &ref : reference_voxels)
  {
    ohm::setVoxelKey(ref.first, mean, covariance);
    ASSERT_TRUE(mean.isValid());
    ASSERT_TRUE(covariance.isValid());
    // Lookup the target voxel.
    ohm::VoxelMean voxel_mean;
    ohm::CovarianceVoxel cov_voxel;
    mean.read(&voxel_mean);
    covariance.read(&cov_voxel);
    EXPECT_TRUE(validate(ohm::positionSafe(mean), voxel_mean.count, cov_voxel, ref.second));
  }
}

void testNdtMiss(const glm::dvec3 &sensor, const std::vector<glm::dvec3> samples, double voxel_resolution,
                 float sensor_noise, const glm::dvec3 &map_origin, const std::vector<glm::dvec3> test_rays)
{
  (void)sensor;
  ohm::OccupancyMap map_cpu(voxel_resolution, ohm::MapFlag::kVoxelMean);
  ohm::NdtMap ndt_cpu(&map_cpu, true);

  map_cpu.setOrigin(map_origin);
  map_cpu.setMissProbability(0.45f);
  ndt_cpu.setSensorNoise(sensor_noise);

  // First process the samples
  ohm::Key target_key;
  for (size_t i = 0; i < samples.size(); ++i)
  {
    const glm::dvec3 &sample = samples[i];

    ohm::Key key = map_cpu.voxelKey(sample);
    target_key = key;
    ohm::integrateNdtHit(ndt_cpu, key, sensor, sample);
  }

  // Clone the map for use in GPU.
  ohm::GpuNdtMap ndt_gpu(map_cpu.clone(), false);
  // Copy parameterisation.
  ndt_gpu.setSensorNoise(ndt_cpu.sensorNoise());

  // Validate that our map objects differ.
  EXPECT_NE(&map_cpu, &ndt_gpu.map());

  // Fire rays to punch through the target voxel, and compare the delta one at a time.
  // In between we reset the probability value of the target voxel.
  ohm::Voxel<float> target_voxel_cpu(&map_cpu, map_cpu.layout().occupancyLayer(), target_key);
  ASSERT_TRUE(target_voxel_cpu.isValid());
  float initial_value;
  target_voxel_cpu.read(&initial_value);

  for (size_t i = 0; i < test_rays.size(); i += 2)
  {
    // Start the update in GPU (one ray -> inefficient).
    ndt_gpu.integrateRays(test_rays.data() + i, 2);

    ohm::integrateNdtMiss(ndt_cpu, target_key, test_rays[i], test_rays[i + 1]);

    // Sync from GPU.
    ndt_gpu.syncVoxels();
    // We will be updating on CPU. Invalidate the GPU cache.
    ndt_gpu.gpuCache()->clear();

    // Read the voxel value from GPU update.
    ohm::Voxel<float> target_voxel_gpu(&ndt_gpu.map(), ndt_gpu.map().layout().occupancyLayer(), target_key);
    ASSERT_TRUE(target_voxel_gpu.isValid());
    float ndt_cpu_value;
    target_voxel_cpu.read(&ndt_cpu_value);
    float ndt_gpu_value;
    target_voxel_gpu.read(&ndt_gpu_value);

    EXPECT_NEAR(ndt_gpu_value, ndt_cpu_value, 1e-4f);

    // Restore both voxel values.
    target_voxel_cpu.write(initial_value);
    target_voxel_gpu.write(initial_value);
  }
}

TEST(Ndt, Hit)
{
  // Use a fixed seed for test repeatability.
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  std::normal_distribution<double> gaussian(0.0, 1.0);
  // Generate points within a 2m cube.
  std::uniform_real_distribution<double> uniform(0.01, 1.99);
  const size_t sample_count = 10000;
  std::vector<glm::dvec3> samples;
  samples.reserve(sample_count);

  // First generate a sample surface to target. We have to make sure this falls within a single voxel.
  glm::dvec3 mean(0);
  glm::dmat3 cov(0);

  double num_pt = 0;
  while (num_pt < 4)
  {
    // glm::dvec3 pt = 0.5 * randomVector3();
    glm::dvec3 pt;

    pt[0] = uniform(rng);
    pt[1] = uniform(rng);
    pt[2] = uniform(rng);

    const glm::dvec3 diff = pt - mean;
    const double one_on_num_pt_plus_one = 1.0 / (num_pt + 1.0);
    mean = (num_pt * mean + pt) * one_on_num_pt_plus_one;
    cov = (num_pt * cov + glm::dot((num_pt * one_on_num_pt_plus_one) * diff, diff)) * one_on_num_pt_plus_one;
    num_pt += 1.0;
  }

  {
    // TODO: this is the only part we need Eigen for. Find an alternative Eigen decomposition solution.
    Eigen::Matrix3d cov_eigen;
    cov_eigen << glm::row(cov, 0)[0], glm::row(cov, 0)[1], glm::row(cov, 0)[2],  //
      glm::row(cov, 1)[0], glm::row(cov, 1)[1], glm::row(cov, 1)[1],             //
      glm::row(cov, 2)[0], glm::row(cov, 2)[1], glm::row(cov, 2)[1];
    Eigen::LDLT<Eigen::Matrix3d, Eigen::Lower> cov_d(cov_eigen);
    const Eigen::Vector3d &vD = cov_d.vectorD();

    const Eigen::Vector3d mean_eigen(mean.x, mean.y, mean.z);
    for (size_t i = 0; i < sample_count; ++i)
    {
      Eigen::Vector3d pt;
      for (size_t i = 0; i < 3; ++i)
      {
        pt[i] = sqrt(vD[i]) * gaussian(rng);
      }

      pt = mean_eigen + cov_d.matrixL() * pt;
      samples.emplace_back(glm::dvec3(pt.x(), pt.y(), pt.z()));
    }
  }

  testNdtHits(samples, 2.0);
}

// For the miss tests, we setup a GPU and a CPU based NDT map, populate one voxel and compare the miss results on one
// focus voxel. On CPU we only interate misses for that voxel, for GPU we do the full test rays and compare the
// results on the focus voxel.

TEST(Ndt, MissPlanar)
{
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  // Generate points within a 2m cube.
  const double voxel_resolution = 2.0;
  std::uniform_real_distribution<double> uniform(0.01, 1.99);
  const size_t sample_count = 10000;
  std::vector<glm::dvec3> samples;
  samples.reserve(sample_count);

  // Create samples which populate a single voxel and define a plane.
  const glm::dvec3 sensor(1, 1, 5);
  for (size_t i = 0; i < sample_count; ++i)
  {
    glm::dvec3 sample;
    sample.x = uniform(rng);
    sample.y = uniform(rng);
    sample.z = 1.0;
    // sample.z += 0.025 * uniform(rng);
    samples.emplace_back(sample);
  }

  // Build a set of rays to punch through the target voxel.
  std::vector<glm::dvec3> rays;
  // Ray straight down through the voxel
  rays.emplace_back(sensor);
  rays.emplace_back(glm::dvec3(1, 1, -5));
  // Reverse the ray above
  rays.emplace_back(rays[1]);
  rays.emplace_back(rays[0]);
  // Ray parallel to the voxel ellipsoid - expected to miss.
  rays.emplace_back(glm::dvec3(-5, 1, 0.25));
  rays.emplace_back(glm::dvec3(5, 1, 0.25));
  // Ray parallel to the voxel ellipsoid, but near the centre.
  // Note: we build a perfect plane above in the centre of the voxel. However, the quantisation of the mean
  // can offset the plane. Running exactly on the plane on which we generated the points will not result in the same
  // effect in testing. We must offset the ray a little to ensure we are a bit off the plane either when using
  // voxel mean quantisation or not.
  rays.emplace_back(glm::dvec3(1, 5, 1.01));
  rays.emplace_back(glm::dvec3(1, -5, 1.01));
  // Ray running across the voxel, down towards the ellipsoid, but not crossing. This simulates rays running near
  // parallel a ground plane as it approaches the sample position.
  rays.emplace_back(glm::dvec3(-5, 1, 2));
  rays.emplace_back(glm::dvec3(5, 1, 1));
  // Ray running across the voxel, and through the ellipsoid.
  rays.emplace_back(glm::dvec3(-5, 1, 2));
  rays.emplace_back(glm::dvec3(5, 1, 0.5));

  testNdtMiss(sensor, samples, voxel_resolution, 0.05f, glm::dvec3(0), rays);
}

TEST(Ndt, MissCylindrical)
{
  // Generate points within a 2m cube.
  const double voxel_resolution = 2.0;
  const float sensor_noise = 0.05f;
  const glm::dvec3 map_origin(-0.5 * voxel_resolution);

  const double cylinder_radius = 0.3;
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  std::uniform_real_distribution<double> uniform(-0.99, 0.99);
  std::uniform_real_distribution<double> uniform_radius(cylinder_radius - sensor_noise, cylinder_radius + sensor_noise);
  const size_t sample_count = 10000;
  std::vector<glm::dvec3> samples;
  samples.reserve(sample_count);

  // Create samples which populate a single voxel and define a plane.
  const glm::dvec3 sensor(0, 0, 5);

  for (size_t i = 0; i < sample_count; ++i)
  {
    glm::dvec3 sample;
    // Create a random sample in the voxel
    sample.x = uniform(rng);
    sample.y = uniform(rng);
    sample.z = uniform(rng);
    // Convert into a cylinder.
    // Normalise in X/Y, create a random radius (to the limit) and scale by this radius.
    double length_xy = std::sqrt(sample.x * sample.x + sample.y * sample.y);
    if (length_xy > 1e-6)
    {
      const double radius = uniform_radius(rng);
      sample.x = radius * sample.x / length_xy;
      sample.y = radius * sample.y / length_xy;
    }
    samples.emplace_back(sample);
  }

  // Build a set of rays to punch through the target voxel.
  std::vector<glm::dvec3> rays;
  // // Ray straight down through the voxel
  rays.emplace_back(sensor);
  rays.emplace_back(glm::dvec3(0, 0, -5));
  // Reverse the ray above
  rays.emplace_back(rays[1]);
  rays.emplace_back(rays[0]);
  // Ray running parallel to the cylinder near the edge. Should be a near hit.
  rays.emplace_back(glm::dvec3(cylinder_radius, cylinder_radius, 5));
  rays.emplace_back(glm::dvec3(cylinder_radius, cylinder_radius, -5));
  // Ray running parallel to the cylinder, but should miss.
  rays.emplace_back(glm::dvec3(1.5 * cylinder_radius, 1.5 * cylinder_radius, -5));
  rays.emplace_back(glm::dvec3(2.0 * cylinder_radius, 2.0 * cylinder_radius, 5));
  // Ray across the middle of the cylinder.
  rays.emplace_back(glm::dvec3(2, -cylinder_radius, 0));
  rays.emplace_back(glm::dvec3(-2, -cylinder_radius, 0));
  // Ray across the end (top) of the cylinder.
  rays.emplace_back(glm::dvec3(2, -cylinder_radius, 0.85));
  rays.emplace_back(glm::dvec3(-2, -cylinder_radius, 0.85));
  // Ray across the voxel, missing the cylinder (top) of the cylinder.
  rays.emplace_back(glm::dvec3(2, 2.0 * cylinder_radius, 0.85));
  rays.emplace_back(glm::dvec3(-2, 2.0 * cylinder_radius, 0.85));

  testNdtMiss(sensor, samples, voxel_resolution, sensor_noise, map_origin, rays);
}

TEST(Ndt, MissSpherical)
{
  // Generate points within a 2m cube.
  const double voxel_resolution = 2.0;
  const float sensor_noise = 0.05f;
  const glm::dvec3 map_origin(-0.5 * voxel_resolution);

  const double radius = 0.3;
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  std::uniform_real_distribution<double> uniform(-0.99, 0.99);
  std::uniform_real_distribution<double> uniform_radius(radius - sensor_noise, radius + sensor_noise);
  const size_t sample_count = 10000;
  std::vector<glm::dvec3> samples;
  samples.reserve(sample_count);

  // Create samples which populate a single voxel and define a plane.
  const glm::dvec3 sensor(0, 0, 5);
  for (size_t i = 0; i < sample_count; ++i)
  {
    glm::dvec3 sample;
    double sample_len2 = 0;
    // Keep trying while we have a near zero vector.
    while (sample_len2 < 1e-6)
    {
      // Create a random sample in the voxel
      sample.x = uniform(rng);
      sample.y = uniform(rng);
      sample.z = uniform(rng);
      sample_len2 = glm::length2(sample);
    }
    sample = glm::normalize(sample);
    sample *= uniform_radius(rng);
    samples.emplace_back(sample);
  }

  // Now trace a ray through the voxel, but not ending in the voxel.
  std::vector<glm::dvec3> rays;
  // // Ray straight down through the voxel
  rays.emplace_back(sensor);
  rays.emplace_back(glm::dvec3(0, 0, -5));
  // Reverse the ray above
  rays.emplace_back(rays[1]);
  rays.emplace_back(rays[0]);
  // Edge of the sphere..
  rays.emplace_back(glm::dvec3(radius, radius, 5));
  rays.emplace_back(glm::dvec3(radius, radius, -5));
  // Near the edge of the sphere, but should miss.
  rays.emplace_back(glm::dvec3(1.5 * radius, 1.5 * radius, -5));
  rays.emplace_back(glm::dvec3(2.0 * radius, 2.0 * radius, 5));

  testNdtMiss(sensor, samples, voxel_resolution, sensor_noise, map_origin, rays);
}
}  // namespace ndttests


#include <boost/graph/loop_erased_random_walk.hpp>


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE NumericsInplaceVsNot0Test

#include <../random_util.hpp>
#include <boost/filesystem.hpp>
#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/devicemanager.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ir.hpp>
#include <popart/names.hpp>
#include <popart/ndarraywrapper.hpp>
#include <popart/op/l1.hpp>
#include <popart/optimizer.hpp>
#include <popart/session.hpp>
#include <popart/tensordata.hpp>
#include <popart/testdevice.hpp>

#include <chrono>
#include <complex>
#include <iostream>

using namespace popart;

// Module :
//
//
//   input :             [2^N, 2]
//                          |
//                      [Sigmoid] -------------------------------
//                          |                                    |
//           slice into 2^N slices of size [1,2]                 |
//                          |                                    |
//             [Random shuffle of indices]                       |   |
//                          |                                    |   |
//                          .                                    |   |
//            --------------------------------                   |   |
//            |  |  |                   |  |  |                  |   | Submod
//        for i = 0:(2^N)/2         for i = 0:(2^N)/2            |   | 3X
//          add slices 2*i           choose a slice              |   |
//           and 2*i + 1               at random                 |   |
//            |  |  |                   |  |  |                  |   |
//           [Sigmoid]               [Random Scale]              |   |
//           |   |  |                  |   |  |                  |   |
//           ----------------------------------                  |   |
//                           .                                   |
//                           |                                   |
//                       Tree Concat                             |
//                           |                                [Scale]
//                           |                                   |
//                           |                                   |
//                   ---------------------------------------------
//                                 |
//                               [Add]
//
//
//
// The Module is repeated J times, then there is a sum reduce:
//
//                    J repeats of the Module
//                             |
//                   ------------------
//                   |          |      |
//  in [2^N,2] -- Module -- Module -- Module -- ReduceSum

BOOST_AUTO_TEST_CASE(Inplace_numericsIpNip0) {

  float perturbSize = 1e-3;
  auto getValue     = [perturbSize](bool inp,
                                int seed,
                                int N,
                                int J,
                                bool outlining,
                                bool perturbInput) {
    using namespace std::chrono;
    auto t0 = steady_clock::now();

    Shape inShape{static_cast<int64_t>(std::pow(2, N)), 2};

    const bool useInitialReductionToAvoidLayoutSearch{false};
    if (useInitialReductionToAvoidLayoutSearch) {
      int64_t redFactor = 3;
      inShape.push_back(redFactor);
    }

    // The input is of shape (2^N, 2, redFactor)
    TensorInfo inInfo{"FLOAT", inShape};

    // generate random input data in the range [0, 1e-1)
    DefaultRandomEngine eng(seed);
    UniformRealDistribution<float> fdisInit(0.f, +1e-1f);
    // possibly perturb each value by a value in [0, 1e-4)
    float perturbFactor = perturbInput ? perturbSize : 0.0;
    UniformRealDistribution<float> fdisScale(0.8f, 1.3f);
    UniformRealDistribution<float> fdisPref(0.f, +10.0f);
    UniformIntDistribution<uint64_t> idis(0,
                                          std::numeric_limits<uint64_t>::max());

    std::vector<float> vInData(inInfo.nelms(), 0);
    for (uint64_t i = 0; i < inInfo.nelms(); ++i) {
      vInData[i] = fdisInit(eng);
      vInData[i] += perturbFactor * fdisInit(eng);
    }

    // Build an onnx model
    auto builder     = Builder::create();
    auto aiOnnx      = builder->aiOnnxOpset9();
    auto aiGraphcore = builder->aiGraphcoreOpset1();

    auto getSubmodule = [N,
                         &builder,
                         &eng,
                         &aiOnnx,
                         &aiGraphcore,
                         &fdisScale,
                         &fdisPref,
                         &idis](std::vector<TensorId> tensorsIn) {
      std::shuffle(tensorsIn.begin(), tensorsIn.end(), eng);
      std::vector<TensorId> tensorsOut;

      // the top half: add, sigmoid.
      for (int i = 0; i < std::pow(2, N - 1); ++i) {
        auto sum = aiOnnx.add({tensorsIn[2 * i], tensorsIn[2 * i + 1]});

        builder->setInplacePreferences(
            sum,
            {{"AddLhsInplace", 100.0f + fdisPref(eng)},
             {"AddRhsInplace", 100.0f + fdisPref(eng)}});

        auto sigmoidOut = aiOnnx.sigmoid({sum});
        builder->setInplacePreferences(
            sigmoidOut, {{"SigmoidInplace", 100.0f + fdisPref(eng)}});
        tensorsOut.push_back(sigmoidOut);
      }

      // the bottom half: scale.
      for (int i = 0; i < std::pow(2, N - 1); ++i) {
        TensorId id0  = tensorsIn[idis(eng) % tensorsIn.size()];
        auto scaleOut = aiGraphcore.scale({id0}, fdisScale(eng));
        builder->setInplacePreferences(
            scaleOut, {{"ScaleInplace", 100.0f + fdisPref(eng)}});
        tensorsOut.push_back(scaleOut);
      }
      return tensorsOut;
    };

    auto appendModule =
        [&builder, &eng, &fdisPref, &fdisScale, &aiOnnx, &getSubmodule](
            std::vector<TensorId> tensorsIn) {
          auto tensorsOut = tensorsIn;
          for (int i = 0; i < 3; ++i) {
            tensorsOut = getSubmodule(tensorsOut);
          }

          while (tensorsOut.size() != 1) {
            tensorsIn  = tensorsOut;
            tensorsOut = {};
            for (int i = 0; i < tensorsIn.size() / 2; ++i) {
              auto concat0 =
                  aiOnnx.concat({tensorsIn[2 * i], tensorsIn[2 * i + 1]}, 0);
              builder->setInplacePreferences(
                  concat0, {{"ConcatInplace", 100.0f + fdisPref(eng)}});
              tensorsOut.push_back(concat0);
            }
          }
          auto outCon = tensorsOut[0];

          return outCon;
        };

    auto getSliced = [N, &builder, &aiOnnx, &fdisPref, &eng](TensorId in) {
      std::vector<TensorId> slicedIds;
      for (int i = 0; i < std::pow(2, N); ++i) {

        auto sliceOut = aiOnnx.slice({in}, {i + 1, 2}, {i, 0}, {0, 1});
        builder->setInplacePreferences(
            sliceOut, {{"SliceInplace", 100.0f + fdisPref(eng)}});
        slicedIds.push_back(sliceOut);
      }
      return slicedIds;
    };

    auto inId = builder->addInputTensor(inInfo);

    TensorId singleTensor;
    if (useInitialReductionToAvoidLayoutSearch) {
      auto reducedOnFinal =
          aiOnnx.reducesum({inId}, std::vector<int64_t>{2}, false);
      singleTensor = aiOnnx.sigmoid({reducedOnFinal});
    } else {
      singleTensor = aiOnnx.sigmoid({inId});
    }

    builder->setInplacePreferences(
        singleTensor, {{"SigmoidInplace", 100.0f + fdisPref(eng)}});

    for (int i = 0; i < J; ++i) {
      // break the input into many [1,2] tensors:
      auto slicedTensors  = getSliced(singleTensor);
      auto residualTensor = appendModule(slicedTensors);

      // the skip connect. We alternate between highest
      // priority and lowest priority
      auto skipTensor = aiGraphcore.scale({singleTensor}, fdisScale(eng));
      builder->setInplacePreferences(
          skipTensor,
          {{"ScaleInplace", 10.0f + 200.0f * (i % 2 == 1) + fdisPref(eng)}});

      singleTensor = aiOnnx.add({skipTensor, residualTensor});

      builder->setInplacePreferences(
          singleTensor,
          {{"AddLhsInplace", 100.0f + fdisPref(eng)},
           {"AddRhsInplace", 100.0f + fdisPref(eng)}});
    }

    auto out =
        aiOnnx.reducesum({singleTensor}, std::vector<int64_t>{{0, 1}}, false);
    builder->addOutputTensor(out);

    auto proto      = builder->getModelProto();
    auto modelProto = io::getModelFromString(proto);

    // Create the IR, adding outId as an anchor
    auto art      = AnchorReturnType("All");
    auto dataFlow = DataFlow(1, {{out, art}});

    auto opts = SessionOptions();
    opts.dotChecks.insert("Bwd0");
    opts.dotChecks.insert("Fwd0");
    opts.dotChecks.insert("Final");
    opts.dotOpNames      = false;
    opts.logDir          = "./dotfiles";
    opts.enableOutlining = outlining;
    if (outlining) {
      opts.outlineThreshold = 1e-7f;
    }
    if (!boost::filesystem::exists(opts.logDir)) {
      boost::filesystem::create_directories(opts.logDir);
    }

    auto device = popart::createTestDevice(TEST_TARGET);

    auto session = popart::InferenceSession::createFromOnnxModel(
        proto,
        dataFlow,
        device,
        popart::InputShapeInfo(),
        opts,
        popart::Patterns(PatternsLevel::NoPatterns)
            .enableRuntimeAsserts(false)
            .enableInPlace(inp));

    // prepare the anchors
    float rawOutputData;
    Shape outShape{};
    popart::NDArrayWrapper<float> outData(&rawOutputData, outShape);

    std::map<popart::TensorId, popart::IArray &> anchors = {
        {out, outData},
    };

    session->prepareDevice();

    popart::NDArrayWrapper<float> inData(vInData.data(), inShape);
    std::map<popart::TensorId, popart::IArray &> inputs = {{inId, inData}};

    popart::StepIO stepio(inputs, anchors);

    session->run(stepio);

    auto t1 = steady_clock::now();
    auto dX = duration<double, std::milli>(t1 - t0).count();
    std::cout << "seed=" << seed << "  N=" << N << "  J=" << J
              << "  inplace=" << inp << "  outline=" << outlining
              << "  perturbInput=" << perturbInput << "  time=" << dX << " [ms]"
              << std::endl;

    return rawOutputData;
  };

  auto runTest = [&getValue, perturbSize](int seed, int N, int J) {
    // vanilla+outline
    auto v010 = getValue(false, seed, N, J, true, false);
    // vanilla
    auto v000 = getValue(false, seed, N, J, false, false);
    // vanilla+inplace
    auto v100 = getValue(true, seed, N, J, false, false);
    // vanilla+inplace+outline
    auto v110 = getValue(true, seed, N, J, true, false);
    // vanilla+perturb
    auto v001 = getValue(false, seed, N, J, false, true);

    std::cout << std::scientific << "Final value with Vanilla is " << v000
              << ". Some discrepencies : "
              << "\nInplace to Vanilla : " << std::fabs(v100 - v000)
              << "\nOutline to Vanilla : " << std::fabs(v010 - v000)
              << "\nPerturb to Vanilla : " << std::fabs(v001 - v000)
              << "\nInplace to Outline : " << std::fabs(v110 - v010)
              << "\nInplace and outline to Vanilla : " << std::fabs(v110 - v000)
              << std::endl;

    // hardware addition is not commutative, we cannot expect bit-wise
    // agreement.

    // inplacing, no outlining:
    BOOST_CHECK(std::fabs(v100 - v000) < 1e-4);
    // inplacing and outlining:
    BOOST_CHECK(std::fabs(v110 - v000) < 1e-4);
    // this is not a useless test, permute input has an effect.
    BOOST_CHECK(std::fabs(v001 - v000) > 1e-2 * perturbSize);
  };

  int seed = 1013;
  int J    = 3;
  int N    = 3;
  runTest(seed, N, J);
}


/**
 *  @file
 *  @copyright defined in evt/LICENSE.txt
 */
#include <evt/chain/name128.hpp>
#include <boost/algorithm/string.hpp>
#include <evt/chain/exceptions.hpp>
#include <fc/variant.hpp>

namespace evt { namespace chain {

void
name128::set(const char* str) {
    const auto len = strnlen(str, 22);
    EVT_ASSERT(len <= 21, name128_type_exception, "Name128 is longer than 21 characters (${name}) ",
               ("name", string(str)));
    value = string_to_name128(str);
    EVT_ASSERT(to_string() == string(str), name128_type_exception,
               "Name128 not properly normalized (name: ${name}, normalized: ${normalized}) ",
               ("name", string(str))("normalized", to_string()));
}

name128::operator string() const {
    static const char* charmap = ".-0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ";

    auto str = string(21, '.');

    auto tmp = value;
    tmp >>= 2;
    for(uint32_t i = 0; i <= 20; ++i) {
        auto c = charmap[tmp & 0x3f];
        str[i] = c;
        tmp >>= 6;
    }

    str.erase(str.find_last_not_of('.') + 1);
    return str;
}

}}  // namespace evt::chain

namespace fc {

void
to_variant(const evt::chain::name128& name, fc::variant& v) {
    v = std::string(name);
}
void
from_variant(const fc::variant& v, evt::chain::name128& name) {
    name = v.get_string();
}

}  // namespace fc


#define ARMA_DONT_USE_WRAPPER
#include <carma.h>
#include <armadillo>
#include <map>

#ifndef __COSMOLIKE_INTERFACE_HPP
#define __COSMOLIKE_INTERFACE_HPP

// --- Auxiliary Code ---
namespace interface_mpp_aux
{

class RandomNumber
{
// Singleton Class that holds a random number generator

public:
  static RandomNumber& get_instance()
  {
    static RandomNumber instance;
		return instance;
  }
  ~RandomNumber() = default;

  double get()
  {
    return dist_(mt_);
  }

protected:
  std::random_device rd_;
  std::mt19937 mt_;
  std::uniform_real_distribution<double> dist_;
private:
  RandomNumber() :
    rd_(),
    mt_(rd_()),
    dist_(0.0, 1.0) {
	};
  RandomNumber(RandomNumber const&) = delete;
};

// ------------------------------------------------------------------------
// ------------------------------------------------------------------------
// ------------------------------------------------------------------------

class RealData
{
// Singleton Class that holds a data vector, covariance, maps...

public:
  static RealData& get_instance()
  {
    static RealData instance;
    return instance;
  }

  ~RealData() = default;

  void set_data(std::string DATA);

  void set_mask(std::string MASK);

  void set_inv_cov(std::string COV);

  int get_ndim() const;

  int get_nreduced_dim() const;

  int get_index_reduced_dim(const int ci) const;

  arma::Col<int> get_mask() const;

  int get_mask(const int ci) const;

  arma::Col<double> get_data_masked() const;

  double get_data_masked(const int ci) const;

  arma::Col<double> get_data_masked_reduced_dim() const;

  double get_data_masked_reduced_dim(const int ci) const;

  arma::Mat<double> get_covariance_masked() const;

  arma::Mat<double> get_covariance_masked_reduced_dim() const;

  arma::Mat<double> get_inverse_covariance_masked() const;

  double get_inverse_covariance_masked(const int ci, const int cj) const;

  arma::Mat<double> get_inverse_covariance_masked_reduced_dim() const;

  double get_inverse_covariance_masked_reduced_dim(const int ci,
    const int cj) const;

  double get_chi2(std::vector<double> datavector) const;

  bool is_mask_set() const;

  bool is_data_set() const;

  bool is_inv_cov_set() const;

  arma::Col<double> get_expand_dim_from_masked_reduced_dim(
    arma::Col<double> reduced_dim_vector) const;

private:
  bool is_mask_set_ = false;
  bool is_data_set_ = false;
  bool is_inv_cov_set_ = false;

  int ndata_;
  int ndata_masked_; // for baryon project, reduced dim

  std::string mask_filename_;
  std::string cov_filename_;
  std::string data_filename_;

  arma::Col<int> index_reduced_dim_;
  arma::Col<int> mask_;

  arma::Col<double> data_masked_;
  arma::Col<double> data_masked_reduced_dim_; // for baryon project, reduced dim

  arma::Mat<double> cov_masked_;
  arma::Mat<double> cov_masked_reduced_dim_; // for baryon project, reduced dim
  arma::Mat<double> inv_cov_masked_;
  arma::Mat<double> inv_cov_masked_reduced_dim_;

  RealData() = default;
  RealData(RealData const&) = delete;
};

arma::Mat<double> read_table(const std::string file_name);

std::vector<double> convert_arma_col_to_stl_vector(arma::Col<double> in);

// https://en.cppreference.com/w/cpp/types/numeric_limits/epsilon
template<class T>
typename std::enable_if<!std::numeric_limits<T>::is_integer, bool>::type
almost_equal(T x, T y, int ulp = 100)
{
    // the machine epsilon has to be scaled to the magnitude of the values used
    // and multiplied by the desired precision in ULPs (units in the last place)
    return std::fabs(x-y) <= std::numeric_limits<T>::epsilon() * std::fabs(x+y) * ulp
        // unless the result is subnormal
        || std::fabs(x-y) < std::numeric_limits<T>::min();
}

// ------------------------------------------------------------------------
// ------------------------------------------------------------------------
// ------------------------------------------------------------------------

class PointMass
{
// Singleton Class that Evaluate Point Mass Marginalization

public:
  static PointMass& get_instance()
  {
    static PointMass instance;
    return instance;
  }
  ~PointMass() = default;

  void set_pm_vector(std::vector<double> pm);

  std::vector<double> get_pm_vector() const;

  double get_pm(const int zl, const int zs, const double theta) const;

private:
  std::vector<double> pm_;

  PointMass() = default;
  PointMass(PointMass const&) = delete;
};

// ------------------------------------------------------------------------
// ------------------------------------------------------------------------
// ------------------------------------------------------------------------

class BaryonScenario
{
// Singleton Class that map Baryon Scenario (integer to name)

public:
  static BaryonScenario& get_instance()
  {
    static BaryonScenario instance;
    return instance;
  }
  ~BaryonScenario() = default;

  int nscenarios() const;

  void set_scenarios(std::string scenarios);

  std::string get_scenario(const int i) const;

private:
  int nscenarios_;
  std::map<int, std::string> scenarios_;

  BaryonScenario() = default;
  BaryonScenario(BaryonScenario const&) = delete;
};

// ------------------------------------------------------------------------
// ------------------------------------------------------------------------
// ------------------------------------------------------------------------

}  // namespace interface_mpp_aux
#endif // HEADER GUARD


// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>

// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

#ifndef KDL_JACOBIAN_HPP
#define KDL_JACOBIAN_HPP

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

namespace KDL
{
    // Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4)
    class Jacobian;
    bool Equal(const Jacobian& a,const Jacobian& b,double eps=epsilon);
    void SetToZero(Jacobian& jac);

    class Jacobian
    {
    public:

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        Eigen::Matrix<double,6,Eigen::Dynamic> data;
        Jacobian();
        explicit Jacobian(unsigned int nr_of_columns);
        Jacobian(const Jacobian& arg);

        ///Allocates memory for new size (can break realtime behavior)
        void resize(unsigned int newNrOfColumns);

        ///Allocates memory if size of this and argument is different
        Jacobian& operator=(const Jacobian& arg);

        bool operator ==(const Jacobian& arg)const;
        bool operator !=(const Jacobian& arg)const;

        friend bool Equal(const Jacobian& a,const Jacobian& b,double eps);

        ~Jacobian();

        double operator()(unsigned int i,unsigned int j)const;
        double& operator()(unsigned int i,unsigned int j);
        unsigned int rows()const;
        unsigned int columns()const;

        friend void SetToZero(Jacobian& jac);

        friend bool changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest);
        friend bool changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest);
        friend bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest);

        Twist getColumn(unsigned int i) const;
        void setColumn(unsigned int i,const Twist& t);

        void changeRefPoint(const Vector& base_AB);
        void changeBase(const Rotation& rot);
        void changeRefFrame(const Frame& frame);

    };

    bool changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest);
    bool changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest);
    bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest);

}

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_FREXP_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_FREXP_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/limitexponent.hpp>
#include <boost/simd/constant/mask1frexp.hpp>
#include <boost/simd/constant/mask2frexp.hpp>
#include <boost/simd/constant/maxexponentm1.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/function/simd/bitwise_and.hpp>
#include <boost/simd/function/simd/bitwise_andnot.hpp>
#include <boost/simd/function/simd/bitwise_cast.hpp>
#include <boost/simd/function/simd/bitwise_or.hpp>
#include <boost/simd/function/simd/if_else_zero.hpp>
#include <boost/simd/function/simd/is_greater.hpp>
#include <boost/simd/function/simd/is_nez.hpp>
#include <boost/simd/function/simd/logical_notand.hpp>
#include <boost/simd/function/simd/minus.hpp>
#include <boost/simd/function/simd/multiplies.hpp>
#include <boost/simd/function/simd/seladd.hpp>
#include <boost/simd/function/simd/shr.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/function/simd/is_less.hpp>
#include <boost/simd/function/simd/is_nez.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/abs.hpp>
#include <boost/simd/function/simd/logical_and.hpp>
#include <boost/simd/constant/twotonmb.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(frexp_
                             , (typename A0, typename A1, typename A2, typename X, typename Y)
                             , bd::cpu_
                             , bs::pack_<bd::floating_<A0>, X>
                             , bs::pack_<bd::floating_<A1>, X>
                             , bs::pack_<bd::integer_<A2>,Y>
                             )
   {
     BOOST_FORCEINLINE void operator()(A0 const& a0, A1 & r0, A2 & r1) const
      {
        using int_type = bd::as_integer_t<A0, signed>;
        using s_type = bd::scalar_of_t<A0>;
  #ifndef BOOST_SIMD_NO_DENORMALS
        auto test = logical_and(is_less(bs::abs(a0), Smallestposval<A0>()), is_nez(a0));
        A0 aa0 = if_else(test, Twotonmb<A0>()*a0, a0);
        A2 t = if_else_zero(test,Nbmantissabits<A0>());
  #else
        A0 aa0 = a0;
  #endif
        r1 = simd::bitwise_cast<int_type>(bitwise_and(aa0, Mask1frexp<A0>())); //extract exp.
        A0  x   = bitwise_andnot(aa0, Mask1frexp<A0>());
        r1  = shr(r1,Nbmantissabits<s_type>()) - Maxexponentm1<A0>();
        r0  = bitwise_or(x,Mask2frexp<A0>());
        auto test0 = is_nez(aa0);
        auto test1 = is_greater(r1,Limitexponent<A0>());
        r1 = if_else_zero(logical_notand(test1, test0), r1);
  #ifndef BOOST_SIMD_NO_DENORMALS
        r1 -= t ;
  #endif
        r0 = if_else_zero(test0, seladd(test1,r0,aa0));
      }
   };

} } }

#endif


#include "map/pathfinder.h"

#include "database/defines.h"
#include "map/province.h"
#include "util/container_util.h"

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

namespace metternich {

class pathfinder::impl
{
	using cost = int;
	using graph = boost::adjacency_list<boost::listS, boost::vecS, boost::undirectedS, boost::no_property, boost::property<boost::edge_weight_t, cost>>;
	using vertex = graph::vertex_descriptor;
	using edge = graph::edge_descriptor;

public:
	impl(const std::set<province *> &provinces);

	find_trade_path_result find_trade_path(const province *start_province, const province *goal_province) const;

private:
	size_t get_province_index(const province *province) const
	{
		return this->province_to_index.find(province)->second;
	}

	cost get_trade_cost(const edge e) const;

private:
	std::vector<province *> provinces;
	std::map<const province *, size_t> province_to_index;
	graph province_graph;
};

template <class graph, class cost>
class trade_path_heuristic final : public boost::astar_heuristic<graph, cost>
{
public:
	using vertex = typename boost::graph_traits<graph>::vertex_descriptor;

	trade_path_heuristic(vertex goal, const std::vector<province *> &provinces)
		: goal(goal), provinces(provinces)
	{}

	cost operator()(vertex v)
	{
		return this->provinces[v]->get_kilometers_distance_to(this->provinces[this->goal]) * 100 / province::base_distance * defines::get()->get_trade_cost_modifier_per_distance() / 100;
	}

private:
	vertex goal;
	const std::vector<province *> &provinces;
};

template <class vertex>
class astar_visitor final : public boost::default_astar_visitor
{
public:
	astar_visitor(vertex goal) : goal(goal)
	{}

	template <class graph>
	void examine_vertex(vertex v, graph &g) {
		Q_UNUSED(g)

		if (v == this->goal) {
			throw found_goal();
		}
	}

private:
	vertex goal;
};

struct found_goal
{
};

pathfinder::pathfinder(const std::set<province *> &provinces)
{
	this->implementation = std::make_unique<impl>(provinces);
}

pathfinder::~pathfinder()
{
}

find_trade_path_result pathfinder::find_trade_path(const province *start_province, const province *goal_province) const
{
	return this->implementation->find_trade_path(start_province, goal_province);
}

pathfinder::impl::impl(const std::set<province *> &provinces)
	: provinces(container::to_vector(provinces)), province_graph(provinces.size())
{
	for (size_t i = 0; i < this->provinces.size(); ++i) {
		const province *province = this->provinces[i];
		this->province_to_index[province] = i;
	}

	for (province *province : this->provinces) {
		for (metternich::province *border_province : province->get_border_provinces()) {
			edge e;
			bool inserted;
			boost::tie(e, inserted) = boost::add_edge(this->province_to_index[province], this->province_to_index[border_province], this->province_graph);
		}
	}
}

find_trade_path_result pathfinder::impl::find_trade_path(const province *start_province, const province *goal_province) const
{
	vertex start = this->get_province_index(start_province);
	vertex goal = this->get_province_index(goal_province);

	std::vector<vertex> vertex_predecessors(boost::num_vertices(this->province_graph));
	std::vector<cost> vertex_costs(boost::num_vertices(this->province_graph));

	auto weight_function = boost::make_function_property_map<edge, cost>([this](edge e) {
		return this->get_trade_cost(e);
	});

	try {
		astar_search_tree(this->province_graph, start, trade_path_heuristic<graph, cost>(goal, this->provinces),
			weight_map(weight_function).
			predecessor_map(make_iterator_property_map(vertex_predecessors.begin(), get(boost::vertex_index, this->province_graph))).
			distance_map(make_iterator_property_map(vertex_costs.begin(), get(boost::vertex_index, this->province_graph))).
			visitor(astar_visitor<vertex>(goal)));
	} catch (found_goal) {
		find_trade_path_result result(true);
		for (vertex v = goal;; v = vertex_predecessors[v]) {
			result.path.push_back(this->provinces[v]);
			if (vertex_predecessors[v] == v) {
				break;
			}
		}
		std::reverse(result.path.begin(), result.path.end());
		result.trade_cost = vertex_costs[goal];
		return result;
	}

	return find_trade_path_result(false);
}

pathfinder::impl::cost pathfinder::impl::get_trade_cost(const edge e) const
{
	const province *source_province = this->provinces[e.m_source];
	const province *target_province = this->provinces[e.m_target];

	int trade_cost = source_province->get_kilometers_distance_to(target_province) * 100 / province::base_distance * defines::get()->get_trade_cost_modifier_per_distance() / 100;

	if (source_province->is_water() != target_province->is_water()) {
		trade_cost += defines::get()->get_base_port_trade_cost_modifier();
	}

	return trade_cost;
}

}


#include "Trajectory.h"
#include "promp.h"
#include <iostream>

#include <Eigen/Core>
#include <Eigen/SVD>
#include <cmath>

#include <random>

using namespace promp;
int example_counter = 0;

typedef Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> Matrix;
typedef Eigen::Map<Eigen::Matrix<double, 1, Eigen::Dynamic, Eigen::RowMajor>> Vector;
typedef Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> constMatrix;
typedef Eigen::Map<const Eigen::Matrix<double, 1, Eigen::Dynamic, Eigen::RowMajor>> constVector;

std::vector<ConditionPoint> ConditionPoint::fromMatrix(MatrixXd pointMatrix)
{
  std::vector<ConditionPoint> ret;

  for (int i = 0; i < pointMatrix.rows(); i++)
  {
    ret.push_back(ConditionPoint());
    ret.back().timestamp = pointMatrix(i, 0);
    ret.back().dimension = pointMatrix(i, 1);
    ret.back().derivative = pointMatrix(i, 2);
    ret.back().mean = pointMatrix(i, 3);
    ret.back().variance = pointMatrix(i, 4);
  }

  return ret;
}

Trajectory::Trajectory(const TrajectoryData &data) : numWeights_(data.numBF_), numDim_(data.numDim_), overlap_(data.overlap_)
{
  type_ = data.isStroke_ ? Stroke : Periodic;
  weightMean_ = constVector(data.mean_.data(), data.numDim_ * data.numBF_);
  weightCovars_ = constMatrix(data.covariance_.data(), data.numDim_ * data.numBF_, data.numDim_ * data.numBF_);
  conditionPoints_ = ConditionPoint::fromMatrix(constMatrix(data.conditions_.data(), data.conditions_.size() / ConditionPoint::NUM_FIELDS, ConditionPoint::NUM_FIELDS));
  setBF();
  condition(weightMean_, weightCovars_);
}

Trajectory::Trajectory(const int numWeights, const VectorXd &weights, const double overlap, const MatrixXd &covars,
                       const TrajectoryType type)
    : numWeights_(numWeights), numDim_(weights.size() / numWeights), overlap_(overlap), weightMean_(weights),
      weightCovars_(covars),
      type_(type)
{
  setBF();
}

Trajectory::Trajectory(const std::vector<VectorXd> &timestamps, const std::vector<MatrixXd> &values,
                       const double overlap, int numWeights,
                       const int iterationLimit, const TrajectoryType type)
    : numWeights_(numWeights), numDim_(values.front().cols()), overlap_(overlap), type_(type)
{
  setBF();
  imitate(timestamps, values, iterationLimit);
}

void Trajectory::imitate(const std::vector<VectorXd> &timestamps, const std::vector<MatrixXd> &values, const int iterationLimit)
{
  weightMean_ = VectorXd::Zero(numDim_ * numWeights_);
  weightCovars_ = MatrixXd::Identity(numDim_ * numWeights_, numDim_ * numWeights_);
  standardDev_ = 1.;

  MatrixXd means(values.size(), numDim_ * numWeights_);
  std::vector<MatrixXd> covs(values.size(), MatrixXd::Identity(numFunc_ * numWeights_, numFunc_ * numWeights_));

  std::vector<MatrixXd> H(values.size());
  for (size_t i = 0; i < values.size(); i++)
  {
    MatrixXd H_partial = MatrixXd::Identity(values[i].rows(), values[i].rows());

    for (int y = 0; y < values[i].rows(); y++)
    {
      for (int preview = 1; preview < 2 && (y + preview) < values[i].rows(); preview++)
      {
        H_partial(y, y + preview) = -std::pow(0.7, preview);
      }
    }

    H[i] = (MatrixXd::Zero(values[i].size(), values[i].size()));
    for (int j = 0; j < numDim_; j++)
      H[i].block(H_partial.rows() * j, H_partial.rows() * j, H_partial.rows(), H_partial.rows()) = H_partial;
  }

  // value restructuring
  std::vector<MatrixXd> val(values.size());

  for (size_t i = 0; i < values.size(); i++)
  {
    val[i] = MatrixXd(values[i].size(), 1);
    for (int j = 0; j < numDim_; j++)
      val[i].block(timestamps[i].size() * j, 0, timestamps[i].size(), 1) = values[i].block(0, j, timestamps[i].size(), 1);
  }

  std::vector<MatrixXd> BF(values.size());

  for (size_t i = 0; i < values.size(); i++)
  {
    BF[i] = basisFunctions_->getValue(timestamps[i], numDim_).transpose();
  }

  std::vector<MatrixXd> R(values.size());

  for (size_t i = 0; i < values.size(); i++)
  {
    R[i] = H[i] * val[i];
  }

  std::vector<MatrixXd> RR(values.size());

  for (size_t i = 0; i < values.size(); i++)
  {
    RR[i] = R[i].transpose() * R[i];
  }

  std::vector<MatrixXd> BH(values.size());

  for (size_t i = 0; i < values.size(); i++)
  {
    BH[i] = BF[i] * H[i].transpose();
  }

  std::vector<MatrixXd> mean_eStep(values.size());

  for (size_t i = 0; i < values.size(); i++)
  {
    mean_eStep[i] = (BH[i] * R[i]);
  }

  std::vector<MatrixXd> cov_eStep(values.size());

  for (size_t i = 0; i < values.size(); i++)
  {
    cov_eStep[i] = (BH[i] * BH[i].transpose());
  }

  int sampleCount = 0;
  for (size_t i = 0; i < values.size(); i++)
  {
    sampleCount += values[i].rows();
  }

  int counter = 0;
  VectorXd weightMean_old;
  MatrixXd weightCovars_old;
  do
  {
    counter++;
    weightMean_old = weightMean_;
    weightCovars_old = weightCovars_;

    for (size_t i = 0; i < values.size(); i++)
    {
      E_Step(1. / standardDev_ * mean_eStep[i], 1. / standardDev_ * cov_eStep[i], means.row(i), covs[i]);
    }

    M_Step(means, covs, RR, mean_eStep, cov_eStep, sampleCount);
  } while (counter < iterationLimit && !weightMean_old.isApprox(weightMean_) && !weightCovars_old.isApprox(weightCovars_));
}

MatrixXd Trajectory::getWeightMean() const
{
  return weightMean_;
};

MatrixXd Trajectory::getWeightCovars() const
{
  return weightCovars_;
};

MatrixXd Trajectory::getValueMean(const VectorXd &time) const
{
  MatrixXd out(numDim_ * numFunc_, time.size());

  MatrixXd bf = basisFunctions_->getValue(time);
  MatrixXd bfd = basisFunctions_->getValueDeriv(time);
  for (int dimension = 0; dimension < numDim_; dimension++)
  {
    out.row(numFunc_ * dimension) = bf * weightMean_.segment(numWeights_ * dimension, numWeights_);
    out.row((numFunc_ * dimension) + 1) = bfd * weightMean_.segment(numWeights_ * dimension, numWeights_);
  }

  return out;
}

VectorXd Trajectory::getValueMean(const double time) const
{
  VectorXd timeVec(1);
  timeVec << time;
  return getValueMean(timeVec).col(0);
}

MatrixXd Trajectory::getValueCovars(const VectorXd &time) const
{
  MatrixXd out(time.size(), numDim_ * numFunc_ * numDim_ * numFunc_);
  MatrixXd value = basisFunctions_->getValue(time);
  MatrixXd valueDeriv = basisFunctions_->getValueDeriv(time);

  for (int y = 0; y < numDim_; y++)
  {
    for (int x = 0; x < numDim_; x++)
    {

      out.transpose().row((numFunc_ * y * numDim_ * numFunc_) + (numFunc_ * x)) = (value *
                                                                                weightCovars_.block(y * numWeights_, x * numWeights_, numWeights_,
                                                                                numWeights_) *
                                                                                value.transpose())
                                                                                .diagonal();

      out.transpose().row((numFunc_ * y * numDim_ * numFunc_) + (numFunc_ * x) + 1) = (value *
                                                                                weightCovars_.block(y * numWeights_, x * numWeights_, numWeights_,
                                                                                numWeights_) *
                                                                                valueDeriv.transpose())
                                                                                .diagonal();

      out.transpose().row((numFunc_ * y * numDim_ * numFunc_) + (numFunc_ * x) + (numDim_ * numFunc_)) = (valueDeriv *
                                                                                weightCovars_.block(y * numWeights_, x * numWeights_, numWeights_,
                                                                                numWeights_) *
                                                                                value.transpose())
                                                                                .diagonal();

      out.transpose().row((numFunc_ * y * numDim_ * numFunc_) + (numFunc_ * x) + (numDim_ * numFunc_) + 1) = (valueDeriv *
                                                                                weightCovars_.block(y * numWeights_, x * numWeights_, numWeights_,
                                                                                numWeights_) *
                                                                                valueDeriv.transpose())
                                                                                .diagonal();
    }
  }
  return out;
}

VectorXd Trajectory::getValueCovars(const double time) const
{
  VectorXd timeVec(1);
  timeVec << time;
  return getValueCovars(timeVec).row(0);
}

void Trajectory::condition(VectorXd &weightMean, MatrixXd &weightCovars) const
{
  if (conditionPoints_.empty())
    return;
  MatrixXd basisFunc_tmp = MatrixXd::Zero(conditionPoints_.size(), numDim_ * numWeights_);
  VectorXd means(conditionPoints_.size());
  VectorXd variances(conditionPoints_.size());

  for (unsigned i = 0; i < conditionPoints_.size(); i++)
  {
    const ConditionPoint &point = conditionPoints_[i];
    if (point.derivative == 0)
    {
      basisFunc_tmp.block(i, numWeights_ * point.dimension, 1, numWeights_).row(0) = basisFunctions_->getValue(
                                                                                point.timestamp)
                                                                                .row(0);
    }
    else
    {
      basisFunc_tmp.block(i, numWeights_ * point.dimension, 1, numWeights_).row(0) = basisFunctions_->getValueDeriv(point.timestamp).row(0);
    }

    means(i) = point.mean;
    variances(i) = point.variance;
  }

  MatrixXd basisFunc = basisFunc_tmp.transpose();

  MatrixXd cov = variances.asDiagonal();
  cov = (cov + (basisFunc.transpose() * weightCovars * basisFunc)).inverse();

  VectorXd weightMeanNew =
      weightMean + weightCovars * basisFunc * cov * (means - (basisFunc.transpose() * weightMean));
  MatrixXd weightCovarsNew =
      weightCovars - (weightCovars * basisFunc * cov * basisFunc.transpose() * weightCovars);
  weightMean = weightMeanNew;
  weightCovars = weightCovarsNew;
}

void Trajectory::getData(TrajectoryData &data) const
{
  std::memcpy(data.mean_.data(), weightMean_.data(), weightMean_.size() * sizeof(double));
  std::memcpy(data.covariance_.data(), weightCovars_.data(), weightCovars_.size() * sizeof(double));
}

Trajectory Trajectory::sampleTrajectoty(unsigned &seed) const
{
  std::default_random_engine generator(seed);
  MatrixXd A = weightCovars_.selfadjointView<Lower>().llt().matrixL();
  VectorXd z(weightMean_.size());

  std::normal_distribution<double> normalDist(0, 1);

  for (int i = 0; i < weightMean_.size(); i++)
  {
    z(i) = normalDist(generator);
  }

  VectorXd newMean = weightMean_ + (A * z);
  MatrixXd newCovars = MatrixXd::Zero(weightMean_.size(), weightMean_.size());
  seed = generator();
  return Trajectory(numWeights_, newMean, overlap_, newCovars, type_);
  ;
}
Trajectory Trajectory::sampleTrajectoty() const
{
  unsigned seed = time(0);
  return sampleTrajectoty(seed);
}

void Trajectory::E_Step(const MatrixXd &mean_eStep, const MatrixXd &cov_eStep, Ref<VectorXd, 0, InnerStride<>> mean,
                        MatrixXd &cov)
{
  cov = (cov_eStep + weightCovars_.inverse()).inverse();
  mean = (cov * (mean_eStep + (weightCovars_.inverse() * weightMean_))).col(0);
}

void Trajectory::M_Step(const MatrixXd &mean, const std::vector<MatrixXd> &cov, const std::vector<MatrixXd> &RR,
                        const std::vector<MatrixXd> &RH, const std::vector<MatrixXd> &HH, const int sampleCount)
{

  weightMean_ = mean.colwise().mean().row(0);
  MatrixXd centered = mean.rowwise() - mean.colwise().mean();
  weightCovars_ = centered.transpose() * centered;

  for (int i = 0; i < mean.rows(); i++)
  {
    weightCovars_ += cov[i];
  }

  weightCovars_ /= mean.rows();

  standardDev_ = 0;
  for (int i = 0; i < mean.rows(); i++)
  {
    standardDev_ += (HH[i] * cov[i]).trace();
    standardDev_ += RR[i](0, 0);
    standardDev_ -= 2 * (RH[i].transpose() * mean.row(i).transpose())(0, 0);
    standardDev_ += (mean.row(i) * HH[i] * mean.row(i).transpose())(0, 0);
  }

  standardDev_ /= mean.norm() * mean.rows() * numDim_ * sampleCount + 2; // magic number from the paper
}

void Trajectory::setBF()
{
  if (type_ == Stroke)
  {
    basisFunctions_ = std::shared_ptr<BasisFunctions>(new StrokeBasisFunctions(numWeights_, overlap_));
  }
  else
  {
    basisFunctions_ = std::shared_ptr<BasisFunctions>(new PeriodicBasisFunctions(numWeights_, overlap_));
  }
}


//  Copyright (c) 2012 Robert Ramey
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <boost/logic/tribool_io.hpp>

#include <boost/core/demangle.hpp>
#include <boost/safe_numerics/checked_result_operations.hpp>
#include <boost/safe_numerics/checked_integer.hpp>

// works for both GCC and clang
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-value"

// note: T should be of type checked_result<R> for some integer type R
template<class T>
bool test_checked_less_than(
    T v1,
    T v2,
    char expected_result
){
    using namespace boost::safe_numerics;
    const boost::logic::tribool result = v1 < v2;
    std::cout
        << std::boolalpha << v1 << " < " << v2 << " -> " << result
        << std::endl;

    switch(expected_result){
    case '<':
        if(result)
            return true;
        break;
    case '>':
    case '=':
        if(!result)
            return true;
        break;
    case '!':
        if(indeterminate(result))
            return true;
        break;
    }
    std::cout
        << "failed to detect error in comparison "
        << std::hex << result << "(" << std::dec << result << ")"
        << " != "<< v1 << " < " << v2
        << std::endl;
    v1 < v2;
    return false;
}

#pragma GCC diagnostic pop

#include "test_checked_comparison.hpp"

template<typename T, typename First, typename Second>
struct test_signed_pair {
    bool operator()() const {
        std::size_t i = First();
        std::size_t j = Second();
        std::cout << std::dec << i << ',' << j << ','
        << "testing " << boost::core::demangle(typeid(T).name()) << ' ';
        return test_checked_less_than(
            signed_values<T>[i],
            signed_values<T>[j],
            signed_comparison_results[i][j]
        );
    };
};

template<typename T, typename First, typename Second>
struct test_unsigned_pair {
    bool operator()() const {
        std::size_t i = First();
        std::size_t j = Second();
        std::cout << std::dec << i << ',' << j << ','
        << "testing " << boost::core::demangle(typeid(T).name()) << ' ';
        return test_checked_less_than(
            unsigned_values<T>[i],
            unsigned_values<T>[j],
            unsigned_comparison_results[i][j]
        );
    };
};

#include <boost/mp11/algorithm.hpp>

int main(){
    using namespace boost::mp11;

    bool x;
    x = test_checked_less_than(77, -24, '>');
    x = test_checked_less_than(77, 82, '<');

    bool rval = true;

    mp_for_each<
        mp_product<
            test_signed_pair,
            signed_test_types,
            signed_value_indices,
            signed_value_indices
        >
    >([&](auto I){
        rval &= I();
    });

    mp_for_each<
        mp_product<
            test_unsigned_pair,
            unsigned_test_types,
            unsigned_value_indices, unsigned_value_indices
        >
    >([&](auto I){
        rval &= I();
    });

    std::cout << (rval ? "success!" : "failure") << std::endl;
    return rval ? 0 : 1;
}


//
// Created by huangkun on 2020/9/18.
//

#ifndef OPENGV2_EVENTFRAME_HPP
#define OPENGV2_EVENTFRAME_HPP

#include <opencv2/opencv.hpp>
#include <Eigen/Eigen>
#include <Eigen/StdVector>
#include <unordered_set>

#include <opengv2/frame/CameraFrame.hpp>
#include <opengv2/sensor/CameraBase.hpp>
#include <opengv2/feature/FeatureBase.hpp>
#include <opengv2/event/Event.hpp>
#include <opengv2/event/EventContainer.hpp>
#include <opengv2/utility/utility.hpp>
#include <opengv2/frame/Bodyframe.hpp>

namespace opengv2 {
    class EventFrame : public CameraFrame {
    public:
        /**
         * @brief The duration should be in 3e-3 second order for hand-hold, smaller for fast movement.
         * Range: [duration.first, duration.second)
         */
        EventFrame(EventContainer::Ptr container, const std::pair<double, double> &duration);

        /**
         * @brief release event set to save space after feature extracted
         */
        inline void releaseEventSet() {
            positiveEvents_.clear();
            negativeEvents_.clear();
        }

        /*
        * rectify features after opencv calibration, rediscovery raw data
        */
        virtual bool rectifyFeatures(const std::unordered_set<int> &outlierIdxs,
                                     const Eigen::Ref<const Eigen::Matrix3d> &Rcw,
                                     const Eigen::Ref<const Eigen::Vector3d> &tcw) = 0;

        inline int eventsNum() const noexcept {
            return positiveEvents_.size() + negativeEvents_.size();
        }

        cv::Mat undistortedImage(CameraBase::Ptr camera) const;

    protected:
        EventContainer::Ptr container_;
        std::pair<double, double> duration_;

        // could be released after rectifying features.
        vectorofEigenMatrix<Eigen::Vector2d> positiveEvents_, negativeEvents_;
    };
}

#endif //OPENGV2_EVENTFRAME_HPP


//  Copyright 2015 Stephan Menzel. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

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

#include "../Random.hpp"

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/moment.hpp>

using namespace boost::accumulators;

BOOST_AUTO_TEST_CASE(RunOnce) {

	BOOST_CHECK_NO_THROW(moose::tools::urand(1));
}

BOOST_AUTO_TEST_CASE(Statistics) {

	// well, how can I test randomness.
	// been doing it wrong some times before.
	// let's try and use boost accumulators
	accumulator_set<float, stats<tag::mean> > acc;

	// push a random number between 0 and 100 a geat many
	// times and assume an average of 50.
	// This has proven surprisingly accurate
	for (unsigned int i = 0; i < 100000; ++i) {

		BOOST_REQUIRE_NO_THROW(acc(moose::tools::urand(100)));
	}

	BOOST_CHECK(mean(acc) < 52);
	BOOST_CHECK(mean(acc) > 48);
}

BOOST_AUTO_TEST_CASE(SmallStatistics) {

	// make sure this goes well for small numbers
	accumulator_set<double, stats<tag::mean> > acc;
	for (unsigned int i = 0; i < 100000; ++i) {

		double val = static_cast<double>(moose::tools::urand(2));
		BOOST_REQUIRE_NO_THROW(acc(val));
	}

	BOOST_CHECK_CLOSE(mean(acc), 1.0, 1.0);
}


#include <boost/make_shared.hpp>
#include "caffe/array/array.hpp"
#include "caffe/array/math.hpp"

using boost::make_shared;
namespace caffe {

template<typename T, typename F>
void Unary<T, F>::eval_cpu(const Array<T> & a, Array<T> * c) {
  CHECK(!!c) << "Output array does not exist!";
  CHECK_EQ(a.shape(), c->shape()) << "Shape does not match!";
  const int N = count(a.shape());
  const T * pa = a.cpu_data();
  T * pc = c->mutable_cpu_data();
#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (int i = 0; i < N; i++)
    pc[i] = F::eval(pa[i]);
}
template<typename T, typename F>
void Unary<T, F>::eval(const Array<T> & a, Array<T> * c, ArrayMode m) {
  if (m == AR_DEFAULT) m = a.effectiveMode();
#ifndef CPU_ONLY
  if (m == AR_GPU)
    return eval_gpu(a, c);
#endif
  return eval_cpu(a, c);
}

template<typename T, typename F>
void Binary<T, F>::eval_cpu(const Array<T> &a, const Array<T> &b, Array<T> *c) {
  CHECK(!!c) << "Output array does not exist!";
  CHECK_EQ(a.shape(), c->shape()) << "Shape does not match!";
  CHECK_EQ(b.shape(), c->shape()) << "Shape does not match!";
  const int N = count(a.shape());
  const T * pa = a.cpu_data(), * pb = b.cpu_data();
  T * pc = c->mutable_cpu_data();
#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (int i = 0; i < N; i++)
    pc[i] = F::eval(pa[i], pb[i]);
}
template<typename T, typename F>
void Binary<T, F>::eval_cpu(T a, const Array<T> & b, Array<T> * c) {
  CHECK(!!c) << "Output array does not exist!";
  CHECK_EQ(b.shape(), c->shape()) << "Shape does not match!";
  const int N = count(b.shape());
  const T * pb = b.cpu_data();
  T * pc = c->mutable_cpu_data();
#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (int i = 0; i < N; i++)
    pc[i] = F::eval(a, pb[i]);
}
template<typename T, typename F>
void Binary<T, F>::eval_cpu(const Array<T> & a, T b, Array<T> * c) {
  CHECK(!!c) << "Output array does not exist!";
  CHECK_EQ(a.shape(), c->shape()) << "Shape does not match!";
  const int N = count(a.shape());
  const T * pa = a.cpu_data();
  T * pc = c->mutable_cpu_data();
#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (int i = 0; i < N; i++)
    pc[i] = F::eval(pa[i], b);
}
template<typename T, typename F>
void Binary<T, F>::eval(const Array<T> & a, const Array<T> & b, Array<T> * c,
                        ArrayMode m) {
  if (m == AR_DEFAULT) m = a.effectiveMode();
  if (m != b.effectiveMode()) LOG(WARNING) << "Mixing CPU and GPU mode";
#ifndef CPU_ONLY
  if (m == AR_GPU)
    return eval_gpu(a, b, c);
#endif
  return eval_cpu(a, b, c);
}
template<typename T, typename F>
void Binary<T, F>::eval(T a, const Array<T> & b, Array<T> * c, ArrayMode m) {
  if (m == AR_DEFAULT) m = b.effectiveMode();
#ifndef CPU_ONLY
  if (m == AR_GPU)
    return eval_gpu(a, b, c);
#endif
  return eval_cpu(a, b, c);
}
template<typename T, typename F>
void Binary<T, F>::eval(const Array<T> & a, T b, Array<T> * c, ArrayMode m) {
  if (m == AR_DEFAULT) m = a.effectiveMode();
#ifndef CPU_ONLY
  if (m == AR_GPU)
    return eval_gpu(a, b, c);
#endif
  return eval_cpu(a, b, c);
}

template<typename T, typename F>
T Reduction<T, F>::eval_cpu(const Array<T> & a) {
  const int N = count(a.shape());
  CHECK_GT(N, 0) << "At least one element required for reduction";
  const T * pa = a.cpu_data();
  T r = pa[0];
#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (int i = 1; i < N; i++)
    r = F::eval(pa[i], r);
  return r;
}
template<typename T, typename F>
T Reduction<T, F>::eval(const Array<T> & a, ArrayMode m) {
  if (m == AR_DEFAULT) m = a.effectiveMode();
#ifndef CPU_ONLY
  if (m == AR_GPU)
    return eval_gpu(a);
#endif
  return eval_cpu(a);
}

INSTANTIATE_ALL;

}  // namespace caffe


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2014 Barend Gehrels, Amsterdam, the Netherlands.

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

//[wkt
//` Shows the usage of wkt

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

int main()
{
    namespace geom = boost::geometry;
    typedef geom::model::d2::point_xy<double> point_type;

    point_type point = geom::make<point_type>(3, 6);
    geom::model::polygon<point_type> polygon;
    geom::append(geom::exterior_ring(polygon), geom::make<point_type>(0, 0));
    geom::append(geom::exterior_ring(polygon), geom::make<point_type>(0, 4));
    geom::append(geom::exterior_ring(polygon), geom::make<point_type>(4, 4));
    geom::append(geom::exterior_ring(polygon), geom::make<point_type>(4, 0));
    geom::append(geom::exterior_ring(polygon), geom::make<point_type>(0, 0));

    std::cout << boost::geometry::wkt(point) << std::endl;
    std::cout << boost::geometry::wkt(polygon) << std::endl;

    return 0;
}

//]

//[wkt_output
/*`
Output:
[pre
POINT(3 2)
POLYGON((0 0,0 4,4 4,4 0,0 0))
POINT(3.14 27.2)
POLYGON((0 0,0 4,4 4,4 0,0 0))
]

*/
//]