/*
 * Copyright (c) 2015, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 *          Erik Nelson            ( eanelson@eecs.berkeley.edu )
 */

///////////////////////////////////////////////////////////////////////////////
//
// This header defines a set of functions for converting a fundamental matrix
// and a pair of camera intrinsics into an essential matrix, and from an
// essential matrix to and a set of camera extrinsics.
//
///////////////////////////////////////////////////////////////////////////////

#include "essential_matrix_solver.h"

#include <Eigen/Core>
#include <Eigen/LU>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <vector>

#include "point_3d.h"
#include "triangulation.h"
#include "../camera/camera.h"

DEFINE_double(min_points_visible_ratio, 0.3,
              "Fraction of keypoint matches whose triangulation must be "
              "visible from both cameras. This value should be lowered if you "
              "expect lots of noisy matches.");

namespace bsfm {

// Compute the essential matrix from a fundamental matrix and camera intrinsics.
Matrix3d EssentialMatrixSolver::ComputeEssentialMatrix(
    const Matrix3d& F, const CameraIntrinsics& intrinsics1,
    const CameraIntrinsics& intrinsics2) {
  // Extract intrinsics matrices.
  Matrix3d K1(intrinsics1.K());
  Matrix3d K2(intrinsics2.K());

  // Calculate the essential matrix.
  return K2.transpose() * F * K1;
}

// Compute the relative transformation between two cameras from an essential
// matrix and a list of keypoint matches. Note that translation can only be
// computed up to a scale factor.
// NOTE: this implementation is based on Hartley & Zisserman's MVG, pg. 258.
bool EssentialMatrixSolver::ComputeExtrinsics(
    const Matrix3d& E, const FeatureMatchList& matches,
    const CameraIntrinsics& intrinsics1, const CameraIntrinsics& intrinsics2,
    Pose& relative_pose) {
  // Initialize the W matrix.
  Matrix3d W;
  W << 0.0, -1.0, 0.0,
       1.0,  0.0, 0.0,
       0.0,  0.0, 1.0;

  // Perform an SVD on the essential matrix.
  Eigen::JacobiSVD<Matrix3d> svd;
  svd.compute(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
  if (!svd.computeU() || !svd.computeV()) {
    VLOG(1) << "Failed to compute a singular value decomposition of "
            << "the essential matrix.";
    return false;
  }

  // The essential matrix must satisfy E = U * diag(1, 1, 0) * V^T. Use U and V
  // to get a new E, run another SVD, and get the normalized U and V.
  Matrix3d sigma(Matrix3d::Identity());
  sigma(2, 2) = 0;
  Matrix3d E_augmented = svd.matrixU() * sigma * svd.matrixV().transpose();

  svd.compute(E_augmented, Eigen::ComputeFullU | Eigen::ComputeFullV);
  if (!svd.computeU() || !svd.computeV()) {
    VLOG(1) << "Failed to compute a singular value decomposition of "
            << "the essential matrix.";
    return false;
  }

  // Compute two possibilities for rotation.
  Matrix3d R1 = svd.matrixU() * W * svd.matrixV().transpose();
  Matrix3d R2 = svd.matrixU() * W.transpose() * svd.matrixV().transpose();

  // Ensure positive determinants.
  if (R1.determinant() < 0)
    R1 = -R1;
  if (R2.determinant() < 0)
    R2 = -R2;

  // Compute two possibilities for translation
  Vector3d t1, t2;
  t1 = svd.matrixU().col(2);
  t2 = -t1;

  // Build four possible Poses.
  std::vector<Pose> poses;
  poses.push_back(Pose(R1, t1));
  poses.push_back(Pose(R1, t2));
  poses.push_back(Pose(R2, t1));
  poses.push_back(Pose(R2, t2));

  // Set the first camera's position to identity in rotation and translation.
  CameraExtrinsics extrinsics1;
  extrinsics1.SetWorldToCamera(Pose());

  Camera camera1;
  camera1.SetExtrinsics(extrinsics1);
  camera1.SetIntrinsics(intrinsics1);

  // Test how many points are in front of each pose and the identity pose.
  Pose best_pose;
  int best_num_points = -1;

  double u = 0.0, v = 0.0;
  for (int ii = 0; ii < poses.size(); ii++) {
    int num_points = 0;

    CameraExtrinsics extrinsics2;
    extrinsics2.SetWorldToCamera(poses[ii]);

    Camera camera2;
    camera2.SetExtrinsics(extrinsics2);
    camera2.SetIntrinsics(intrinsics2);

    for (int jj = 0; jj < matches.size(); jj++) {
      // Triangulate points and test if the 3D estimate is in front of both
      // cameras.
      Point3D point;
      double unused = 0.0;
      if (!Triangulate(matches[jj], camera1, camera2, point, unused)) {
        continue;
      }

      num_points++;
    }

    // Update best_num_points and best_pose.
    if (num_points > best_num_points) {
      best_num_points = num_points;
      best_pose = poses[ii];
    }
  }

  // Return with false if not enough points found in front of the cameras.
  if (best_num_points < FLAGS_min_points_visible_ratio * matches.size()) {
    VLOG(1) << "Did not find enough points in front of both cameras.";
    return false;
  }

  relative_pose = best_pose;

  return true;
}

}  //\namespace bsfm


#ifndef TEST_UNIT_MATH_FWD_MAT_VECTORIZE_EXPECT_FWD_ROW_VECTOR_VALUE_HPP
#define TEST_UNIT_MATH_FWD_MAT_VECTORIZE_EXPECT_FWD_ROW_VECTOR_VALUE_HPP

#include <stan/math/fwd/mat.hpp>
#include <math/fwd/mat/vectorize/build_fwd_matrix.hpp>
#include <math/fwd/mat/vectorize/expect_val_deriv_eq.hpp>
#include <Eigen/Dense>
#include <vector>

template <typename F, typename T>
void expect_fwd_row_vector_value() {
  using stan::math::fvar;
  using std::vector;
  typedef Eigen::Matrix<T, 1, Eigen::Dynamic> row_vector_t;

  size_t num_inputs = F::valid_inputs().size();
  row_vector_t template_rv(num_inputs);

  for (size_t i = 0; i < num_inputs; ++i) {
    row_vector_t b = build_fwd_matrix<F>(template_rv, i);
    row_vector_t fb = F::template apply<row_vector_t>(b);
    EXPECT_EQ(b.size(), fb.size());
    expect_val_deriv_eq(F::apply_base(b(i)), fb(i));
  }

  size_t vector_vector_size = 2;
  for (size_t i = 0; i < vector_vector_size; ++i) {
    for (size_t j = 0; j < num_inputs; ++j) {
      vector<row_vector_t> c;
      for (size_t k = 0; k < vector_vector_size; ++k)
        if (k == i)
          c.push_back(build_fwd_matrix<F>(template_rv, j));
        else
          c.push_back(build_fwd_matrix<F>(template_rv));
      vector<row_vector_t> fc = F::template apply<vector<row_vector_t> >(c);
      EXPECT_EQ(c.size(), fc.size());
      EXPECT_EQ(c[i].size(), fc[i].size());
      expect_val_deriv_eq(F::apply_base(c[i](j)), fc[i](j));
    }
  }
}

#endif


#include "cnn/nodes.h"
#include "cnn/cnn.h"
#include "cnn/training.h"
#include "cnn/timing.h"
#include "cnn/rnn.h"
#include "cnn/gru.h"
#include "cnn/lstm.h"
#include "cnn/dglstm.h"
#include "cnn/dict.h"
#include "cnn/expr.h"
#include "cnn/cnn-helper.h"
#include "cnn/expr-xtra.h"
#include "cnn/grad-check.h"
#include "cnn/math.h"
#include <iostream>
#include <fstream>
#include <sstream>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>

using namespace std;
using namespace cnn;

unsigned int LAYERS = 2;
unsigned int INPUT_DIM = 8;  //256
unsigned int HIDDEN_DIM = 24;  // 1024
unsigned int VOCAB_SIZE = 0;

int verbose = 0;
cnn::Dict d;
int kSOS;
int kEOS;

template <class Builder>
struct RNNLanguageModel {
  LookupParameters* p_c;
  Parameters* p_R;
  Parameters* p_bias;
  Builder builder;
  explicit RNNLanguageModel(Model& model) : builder(LAYERS, vector<unsigned>{INPUT_DIM, HIDDEN_DIM}, &model) {
      if (verbose)
          cout << "building RNNLanguageModel" << endl;
    p_c = model.add_lookup_parameters(VOCAB_SIZE, {INPUT_DIM});
	p_R = model.add_parameters({ VOCAB_SIZE, HIDDEN_DIM });
	p_bias = model.add_parameters({ VOCAB_SIZE });
  }

  // return Expression of total loss
  Expression BuildLMGraph(const vector<int>& sent, ComputationGraph& cg) {
    const unsigned slen = (unsigned int) sent.size() - 1;
    if (verbose)
        cout << "start building builder graph" << endl;
    builder.new_graph(cg);  // reset RNN builder for new graph
    if (verbose)
        cout << "start new_seuence for the builder graph" << endl;
    builder.start_new_sequence();
    Expression i_R = parameter(cg, p_R); // hidden -> word rep parameter
    Expression i_bias = parameter(cg, p_bias);  // word bias
	if (verbose)
		display_value(i_R, cg, "IR at ");
	if (verbose)
		display_value(i_bias, cg, "ibias at ");
	vector<Expression> errs;
    for (unsigned t = 0; t < slen; ++t) {
      Expression i_x_t = lookup(cg, p_c, sent[t]);
      // y_t = RNN(x_t)
      Expression i_y_t = builder.add_input(i_x_t);
      Expression i_r_t =  i_bias + i_R * i_y_t;
	  if (verbose)
		  display_value(i_r_t, cg, "response at " + t);
      // we can easily look at intermidiate values
//      std::vector<cnn::real> r_t = as_vector(i_r_t.value());
  //    for (cnn::real f : r_t) cout << f << " "; cout << endl;
    //  cout << "[" << as_scalar(pick(i_r_t, sent[t+1]).value()) << "]" << endl;

      // LogSoftmax followed by PickElement can be written in one step
      // using PickNegLogSoftmax
#if 1
      Expression i_ydist = log_softmax(i_r_t);
      errs.push_back(pick(i_ydist, sent[t+1]));
#if 0
      Expression i_ydist = softmax(i_r_t);
      i_ydist = log(i_ydist)
      errs.push_back(pick(i_ydist, sent[t+1]));
#endif
#else
      Expression i_err = pickneglogsoftmax(i_r_t, sent[t+1]);
      errs.push_back(i_err);
#endif
    }
    Expression i_nerr = sum(errs);
#if 1
    return -i_nerr;
#else
    return i_nerr;
#endif
  }

  // return Expression for total loss
  void RandomSample(int max_len = 150) {
    cerr << endl;
    ComputationGraph cg;
    builder.new_graph(cg);  // reset RNN builder for new graph
    builder.start_new_sequence();

    Expression i_R = parameter(cg, p_R);
    Expression i_bias = parameter(cg, p_bias);
    vector<Expression> errs;
    int len = 0;
    int cur = kSOS;
    while(len < max_len && cur != kEOS) {
      ++len;
      Expression i_x_t = lookup(cg, p_c, cur);
      // y_t = RNN(x_t)
      Expression i_y_t = builder.add_input(i_x_t);
      Expression i_r_t = i_bias + i_R * i_y_t;

      Expression ydist = softmax(i_r_t);

      unsigned w = 0;
      while (w == 0 || (int)w == kSOS) {
        auto dist = as_vector(cg.incremental_forward());
        cnn::real p = rand01();
        for (; w < dist.size(); ++w) {
          p -= dist[w];
          if (p < 0.0) { break; }
        }
        if (w == dist.size()) w = kEOS;
      }
      cerr << (len == 1 ? "" : " ") << d.Convert(w);
      cur = w;
    }
    cerr << endl;
  }
};

template <class LM_t>
void train(Model &model, LM_t &lm,
    const vector<vector<int>>& training,
    const vector<vector<int>>& dev,
    Trainer *sgd, const string& fname,
    bool randomSample, bool checkgrad)
{
    cnn::real best = 9e+99;
    unsigned report_every_i = 50;
    unsigned dev_every_i_reports = 500;
    unsigned si = (unsigned int) training.size();
    vector<unsigned> order(training.size());
    for (unsigned i = 0; i < order.size(); ++i) order[i] = i;
    bool first = true;
    int report = 0;
    unsigned lines = 0;
    unsigned total_epoch = 40;

    if (verbose)
        cout << "saving model at " << fname << endl;
    ofstream out(fname, ofstream::out);
    boost::archive::text_oarchive oa(out);
    oa << model;
    out.close();

    size_t i_epoch = 0;
    while (sgd->epoch < total_epoch) {
        Timer iteration("completed in");
        cnn::real loss = 0;
        unsigned chars = 0;
        for (unsigned i = 0; i < report_every_i; ++i) {
            if (si == training.size()) {
                si = 0;
                if (first) { first = false; }
                else { sgd->update_epoch(); }
                cerr << "**SHUFFLE\n";
                shuffle(order.begin(), order.end(), *rndeng);
            }

            // build graph for this instance
            ComputationGraph cg;
            auto& sent = training[order[si]];
            chars += sent.size() - 1;
            ++si;
            lm.BuildLMGraph(sent, cg);
            loss += as_scalar(cg.forward());
            cg.backward();
            if (checkgrad)
                CheckGrad(model, cg);
            sgd->update();
            ++lines;
        }
        sgd->status();
        cerr << " report = " << report << " E = " << (loss / chars) << " ppl=" << exp(loss / chars) << ' ';

        if (randomSample)
            lm.RandomSample();

        // show score on dev data?
        report++;
        if (report % dev_every_i_reports == 0) {
            cnn::real dloss = 0;
            int dchars = 0;
            for (auto& sent : dev) {
                ComputationGraph cg;
                lm.BuildLMGraph(sent, cg);
                dloss += as_scalar(cg.forward());
                dchars += sent.size() - 1;
            }
            if (dloss < best) {
                best = dloss;
                ofstream out(fname, ofstream::out);
                boost::archive::text_oarchive oa(out);
                oa << model;
                out.close();
            }
            else{
                sgd->eta *= 0.5;
            }
            cerr << "\n***TEST E = " << (dloss / dchars) << " ppl=" << exp(dloss / dchars) << ' ';
        }

        i_epoch++;
    }
}

template <class LM_t>
void testcorpus(Model &model, LM_t &lm,
    const vector<vector<int>>& dev)
{
    unsigned lines = 0;
    cnn::real dloss = 0;
    int dchars = 0;
    for (auto& sent : dev) {
        ComputationGraph cg;
        lm.BuildLMGraph(sent, cg);
        dloss += as_scalar(cg.forward());
        dchars += sent.size() - 1;
    }

    cerr << "\n***DEV [epoch=" << (lines / (cnn::real)dev.size()) << "] E = " << (dloss / dchars) << " ppl=" << exp(dloss / dchars) << ' ';
}

void initialise(Model &model, const string &filename)
{
    cerr << "Initialising model parameters from file: " << filename << endl;
    ifstream in(filename, ifstream::in);
    boost::archive::text_iarchive ia(in);
    ia >> model;
}

int main(int argc, char** argv) {
  cnn::Initialize(argc, argv);

  // command line processing
  using namespace boost::program_options;
  variables_map vm;
  options_description opts("Allowed options");
  opts.add_options()
      ("help", "print help message")
      ("seed,s", value<int>()->default_value(217), "random seed number")
      ("train,t", value<string>(), "file containing training sentences")
      ("devel,d", value<string>(), "file containing development sentences.")
      ("test,T", value<string>(), "file containing testing source sentences")
      ("initialise,i", value<string>(), "load initial parameters from file")
      ("parameters,p", value<string>(), "save best parameters to this file")
      ("layers,l", value<int>()->default_value(LAYERS), "use <num> layers for RNN components")
      ("hidden,h", value<int>()->default_value(HIDDEN_DIM), "use <num> dimensions for recurrent hidden states")
      ("gru", "use Gated Recurrent Unit (GRU) for recurrent structure; default RNN")
      ("lstm", "use Long Short Term Memory (GRU) for recurrent structure; default RNN")
      ("dglstm", "use depth-gated LSTM for recurrent structure; default RNN")
      ("verbose,v", "be extremely chatty")
      ("generate,g", value<bool>()->default_value(false), "generate random samples")
      ("checkgrad", value<bool>()->default_value(false), "whether check gradient")
      ;
  store(parse_command_line(argc, argv, opts), vm);

  string flavour;
  if (vm.count("gru"))	flavour = "gru";
  else if (vm.count("lstm"))	flavour = "lstm";
  else if (vm.count("rnnem"))	flavour = "rnnem";
  else if (vm.count("dglstm")) flavour = "dglstm";
  else if (vm.count("nmn")) flavour = "nmn";
  else			flavour = "rnn";

  if (vm.count("verbose") > 0)
  {
      verbose = 1;
      cout << "extrememly chatty" << endl;
  }

  LAYERS = vm["layers"].as<int>();
  HIDDEN_DIM = vm["hidden"].as<int>();

  bool generateSample = false;
  generateSample = vm["generate"].as<bool>();

  string fname;
  if (vm.count("parameters")) {
      fname = vm["parameters"].as<string>();
  }
  else {
      ostringstream os;
      os << "lm"
          << '_' << LAYERS
          << '_' << HIDDEN_DIM
          << '_' << flavour
          << "-pid" << getpid() << ".params";
      fname = os.str();
  }

  cerr << "Parameters will be written to: " << fname << endl;

  if (vm.count("help") || vm.count("train") != 1 || (vm.count("devel") != 1 && vm.count("test") != 1)) {
      cout << opts << "\n";
      return 1;
  }

  kSOS = d.Convert("<s>");
  kEOS = d.Convert("</s>");
  vector<vector<int>> training, dev, test;
  string line;
  int tlc = 0;
  int ttoks = 0;

  string infile = vm["train"].as<string>();
  cerr << "Reading training data from " << infile << "...\n";

  {
    ifstream in(infile);
    assert(in);
    while(getline(in, line)) {
      ++tlc;
      training.push_back(ReadSentence(line, &d));
      ttoks += training.back().size();
      if (training.back().front() != kSOS && training.back().back() != kEOS) {
		  throw("Training sentence in %s : %d didnt start or end with <s>, </s>", infile.c_str(), tlc );
      }
    }
    cerr << tlc << " lines, " << ttoks << " tokens, " << d.size() << " types\n";
  }
  d.Freeze(); // no new word types allowed
  VOCAB_SIZE = d.size();

  if (vm.count("devel") > 0)
  {
      int dlc = 0;
      int dtoks = 0;
      string devfile = vm["devel"].as<string>();
      cerr << "Reading training data from " << devfile << "...\n";
      {
          ifstream in(devfile);
          assert(in);
          while (getline(in, line)) {
              ++dlc;
              dev.push_back(ReadSentence(line, &d));
              dtoks += dev.back().size();
			  if (dev.back().front() != kSOS && dev.back().back() != kEOS) {
				  throw("Dev sentence in %s : %d didn't start or end with <s>, </s> ", devfile.c_str(), tlc);
			  }
          }
          cerr << dlc << " lines, " << dtoks << " tokens\n";
      }
  }

  Model model;
  bool use_momentum = false;
  Trainer* sgd = nullptr;
  if (use_momentum)
    sgd = new MomentumSGDTrainer(&model);
  else
    sgd = new SimpleSGDTrainer(&model);

  if (vm.count("test") == 0)
  {
      if (vm.count("lstm")) {
          cerr << "%% Using LSTM recurrent units" << endl;
          RNNLanguageModel<LSTMBuilder> lm(model);
          train(model, lm, training, dev, sgd, fname, generateSample, vm["checkgrad"].as<bool>());
      }
      else if (vm.count("dglstm")) {
          cerr << "%% Using DGLSTM recurrent units" << endl;
          RNNLanguageModel<DGLSTMBuilder> lm(model);
          train(model, lm, training, dev, sgd, fname, generateSample, vm["checkgrad"].as<bool>());
      }
  }
  else
  {
      string testfile = vm["test"].as<string>();
      int dlc = 0;
      int dtoks = 0;
      cerr << "Reading training data from " << testfile << "...\n";
      {
          ifstream in(testfile);
          assert(in);
          while (getline(in, line)) {
              ++dlc;
              test.push_back(ReadSentence(line, &d));
              dtoks += test.back().size();
			  if (test.back().front() != kSOS && test.back().back() != kEOS) {
				  throw("Dev sentence in %s : %d didnt start or end with <s>, </s> ", testfile.c_str(), tlc);
			  }
          }
          cerr << dlc << " lines, " << dtoks << " tokens\n";
      }

      if (vm.count("test"))
      {
          if (vm.count("lstm")){
              cerr << "%% using LSTM recurrent units" << endl;
              RNNLanguageModel<LSTMBuilder> lm(model);
              if (vm.count("initialise"))
                  initialise(model, vm["initialise"].as<string>());
              testcorpus(model, lm, test);
          }
          if (vm.count("dglstm")){
              cerr << "%% using DGLSTM recurrent units" << endl;
              RNNLanguageModel<DGLSTMBuilder> lm(model);
              if (vm.count("initialise"))
                  initialise(model, vm["initialise"].as<string>());
              testcorpus(model, lm, test);
          }
      }
  }

  //RNNLanguageModel<SimpleRNNBuilder> lm(model);
  if (argc == 4) {
    string fname = argv[3];
    ifstream in(fname);
    boost::archive::text_iarchive ia(in);
    ia >> model;
  }

  delete sgd;
}


/*
 * Copyright 2016 Maikel Nadolski
 *
 * 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 "hidden-markov-models.t.h"

#include <tuple>
#include <vector>
#include <Eigen/Dense>
#include "maikel/hmm/hidden_markov_model.h"
#include "maikel/hmm/algorithm.h"

namespace {

CASE ( "Do we see if a bijective map is bijective onto values?" ) {
  using Index = uint8_t;
  std::map<int, Index> bijective_map_int { {0,0}, {1,1} };
  std::map<std::string, Index> bijective_map_string { {"foo",1}, {"bar",0} };
  std::map<int, Index> not_bijective_map_int1 { {0,1}, {1,2} };
  std::map<int, Index> not_bijective_map_int2 { {0,1}, {2,1} };

  EXPECT(maikel::is_bijective_index_map(bijective_map_int));
  EXPECT(maikel::is_bijective_index_map(bijective_map_string));
  EXPECT_NOT(maikel::is_bijective_index_map(not_bijective_map_int1));
  EXPECT_NOT(maikel::is_bijective_index_map(not_bijective_map_int2));
}

CASE ("convert symbols to indicies") {
  using Index = int;
  std::vector<std::string> symbols { "foo", "bar" };
  auto symbols_to_index = maikel::map_from_symbols<Index>(ranges::view::all(symbols));
  EXPECT(symbols_to_index["foo"] == 0);
  EXPECT(symbols_to_index["bar"] == 1);

  std::vector<int> symbols_2 { 1, 2 };
  auto sti = maikel::map_from_symbols<Index>(symbols_2);
  EXPECT(sti[1] == 0);
  EXPECT(sti[2] == 1);
}

CASE ( "Test forward algorithm for test case in Rabiners Paper" ) {
  Eigen::Matrix3f A;
  A << 0.4, 0.3, 0.3,
       0.2, 0.6, 0.2,
       0.1, 0.1, 0.8;
  Eigen::Matrix3f B;
  B << 1.0, 0.0, 0.0,
       0.0, 1.0, 0.0,
       0.0, 0.0, 1.0;
  Eigen::Vector3f pi;
  pi << 0.0, 0.0, 1.0;
  std::vector<int> sequence { 2, 2, 2, 0, 0, 2, 1, 2 };
//
  std::vector<float> scaling;
  std::vector<Eigen::VectorXf> alphas;
  maikel::hmm::hidden_markov_model<float> hmm(A, B, pi);
  for (auto&& alpha : maikel::hmm::forward(begin(sequence), end(sequence), hmm)) {
    scaling.push_back(alpha.first);
  }
  float probability = std::accumulate(scaling.begin(), scaling.end(), 1.0, std::multiplies<float>());
  probability = 1/probability;
  EXPECT(maikel::almost_equal(probability, 1.536f /10000));
}

//
//CASE ( "Test forward and backward algorithms for test case in Rabiners Paper" ) {
//  Eigen::Matrix3f A;
//  A << 0.4, 0.3, 0.3,
//       0.2, 0.6, 0.2,
//       0.1, 0.1, 0.8;
//  Eigen::Matrix3f B;
//  B << 1.0, 0.0, 0.0,
//       0.0, 1.0, 0.0,
//       0.0, 0.0, 1.0;
//  Eigen::Vector3f pi;
//  pi << 0.0, 0.0, 1.0;
//  std::vector<int> sequence { 2, 2, 2, 0, 0, 2, 1, 2 };
//
//  maikel::hmm::hidden_markov_model<float> hmm(A, B, pi);
//
//  using vector_type = decltype(hmm)::vector_type;
//  std::vector<float> scaling;
//  std::vector<vector_type> alphas;
//  maikel::hmm::forward(hmm, sequence.begin(), sequence.end(), std::back_inserter(alphas), std::back_inserter(scaling));
//  float probability = std::accumulate(scaling.begin(), scaling.end(), 1.0, std::multiplies<float>());
//  probability = 1/probability;
//  EXPECT(maikel::almost_equal<float>(probability,(1.536/10000),1));
//
//  std::vector<vector_type> betas_ranges;
//  maikel::hmm::backward(hmm,
//      sequence | ranges::view::reverse,
//       scaling | ranges::view::reverse,
//      std::back_inserter(betas_ranges));
//  std::vector<vector_type> betas_not_ranges;
//  maikel::hmm::backward(hmm,
//      sequence.rbegin(), sequence.rend(),
//      scaling.rbegin(), scaling.rend(),
//      std::back_inserter(betas_not_ranges));
//}
//
//CASE ( "baum-welch algorithm for test case in Rabiners Paper" ) {
//  Eigen::Matrix3f A;
//  A << 0.4, 0.3, 0.3,
//       0.2, 0.6, 0.2,
//       0.1, 0.1, 0.8;
//  Eigen::Matrix3f B;
//  B << 1.0, 0.0, 0.0,
//       0.0, 1.0, 0.0,
//       0.0, 0.0, 1.0;
//  Eigen::Vector3f pi;
//  pi << 0.0, 0.0, 1.0;
//  std::vector<int> sequence { 2, 2, 2, 0, 0, 2, 1, 2 };
//
//  maikel::hmm::hidden_markov_model<float> initial_hmm(A, B, pi);
//  EXPECT_NO_THROW (
//      auto new_hmm = maikel::hmm::naive::baum_welch(initial_hmm, sequence);
////      std::cerr << "A\n" << new_hmm.A << "\nB\n" << new_hmm.B << "\npi\n" << new_hmm.pi << std::endl;
//  );
//}

}


#include <boost/test/unit_test.hpp>
#include <complex>
#include "../../JeanBaptiste/include/AlgorithmFactory.h"
#include "../include/AlgorithmFixture.h"
#include <string>

namespace ut = boost::unit_test;
namespace jb = jeanbaptiste;
namespace jbo = jeanbaptiste::options;

class Radix2Fixture
    : public AlgorithmFixture
{
protected:
    bool initialized_;

public:
    Radix2Fixture()
        : AlgorithmFixture(),
          initialized_(false)
    {
        BOOST_TEST_MESSAGE("Setup fixture: square pulse of 64 samples.");
        BOOST_TEST((initialized_ = algorithmResult_.initialize("../../test cases/square pulse (n=64).xml", "fft.in", workingSet_, expectedOutIFFT_, "fft.out", expectedOutFFT_)), "Loading test data failed.");
    }

    ~Radix2Fixture()
    {}
};

BOOST_FIXTURE_TEST_SUITE(Radix2TestSuite, Radix2Fixture)

    BOOST_AUTO_TEST_CASE(fft_radix_2_dif)
    {
        if (!initialized_)
            return;

        BOOST_TEST_MESSAGE("Running radix 2 DIF FFT and IFFT.");

        // Create Radix-2 DIF FFT algorithms for sample counts 2 ... 256.
        jb::AlgorithmFactory<1, 8, jbo::Radix_2, jbo::Decimation_In_Frequency, jbo::Direction_Forward, jbo::Window_None,
            jbo::Normalization_Square_Root, std::complex<double>> fftFactory;

        // Create Radix-2 DIF IFFT algorithms for sample counts 2 ... 256.
        jb::AlgorithmFactory<1, 8, jbo::Radix_2, jbo::Decimation_In_Frequency, jbo::Direction_Backward, jbo::Window_None,
            jbo::Normalization_Square_Root, std::complex<double>> ifftFactory;

        runAlgorithms(fftFactory.getAlgorithm(6), ifftFactory.getAlgorithm(6));
    }

    BOOST_AUTO_TEST_CASE(fft_radix_2_dit)
    {
        if (!initialized_)
            return;

        BOOST_TEST_MESSAGE("Running radix 2 DIT FFT and IFFT.");

        // Create Radix-2 DIT FFT algorithms for sample counts 2 ... 256.
        jb::AlgorithmFactory<1, 8, jbo::Radix_2, jbo::Decimation_In_Time, jbo::Direction_Forward, jbo::Window_None,
            jbo::Normalization_Square_Root, std::complex<double>> fftFactory;

        // Create Radix-2 DIT IFFT algorithms for sample counts 2 ... 256.
        jb::AlgorithmFactory<1, 8, jbo::Radix_2, jbo::Decimation_In_Time, jbo::Direction_Backward, jbo::Window_None,
            jbo::Normalization_Square_Root, std::complex<double>> ifftFactory;

        runAlgorithms(fftFactory.getAlgorithm(6), ifftFactory.getAlgorithm(6));
    }

BOOST_AUTO_TEST_SUITE_END()


#include "attribute.hpp"

#include <boost/format.hpp>

#include <iostream>
#include <sstream>

namespace ocv{

std::ostream& operator<<(std::ostream &out, contour_attribute const &attr)
{
    std::ostringstream ostr;
    ostr<<boost::format("%=11.2f|%=11.2f|%=11.2f|%=11.2f|"
                        "%=11.2f|%=11.2f|%=11.2f")
          %attr.contour_area_%attr.bounding_area_%attr.perimeter_
          %attr.aspect_ratio_%attr.extent_%attr.solidity_
          %attr.poly_size_;
    out<<ostr.str()<<std::endl;

    return out;
}

void print_contour_attribute(std::vector<cv::Point> const &contour,
                             double epsillon,
                             std::ostream &out)
{
    contour_attribute const ca = contour_analyzer().analyze(contour, epsillon);
    out<<ca;
}

void print_contour_attribute_name(std::ostream &out)
{
    std::ostringstream ostr;
    ostr<<boost::format("%=11s|%=11s|%=11s|%=11s|"
                        "%=11s|%=11s|%=11s")
          % "CArea" % "BArea" %"Perimeter" % "Aspect"
          % "Extent" % "Solidity" % "PolySize";
    out<<ostr.str()<<std::endl;
}

contour_attribute contour_analyzer::
analyze(std::vector<cv::Point> const &contour,
        double epsillon)
{
    return analyze(contour, epsillon, buffer_);
}

contour_attribute contour_analyzer::
analyze(std::vector<cv::Point> const &contour,
        double epsillon) const
{
    std::vector<cv::Point> buffer;
    return analyze(contour, epsillon, buffer);
}

contour_attribute contour_analyzer::
analyze(std::vector<cv::Point> const &contour,
        double epsillon,
        std::vector<cv::Point> &buffer) const
{
    contour_attribute ca;

    ca.contour_area_ = cv::contourArea(contour);
    ca.bounding_rect_ = cv::boundingRect(contour);
    ca.bounding_area_ = static_cast<double>(ca.bounding_rect_.area());
    ca.aspect_ratio_ = ca.bounding_rect_.width /
            static_cast<double>(ca.bounding_rect_.height);
    ca.perimeter_ = cv::arcLength(contour, true);
    ca.extent_ = ca.contour_area_/ca.bounding_area_;

    cv::convexHull(contour, buffer);
    ca.solidity_ = ca.contour_area_/cv::contourArea(buffer);

    cv::approxPolyDP(contour, buffer, ca.perimeter_ * epsillon, true);
    ca.poly_size_ = buffer.size();

    return ca;
}

}


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


/*
 *  mesher_tester.cpp
 *  Mesh_3_applications
 *
 *  Created by Jane Tournois on 02/12/09.
 *  Copyright 2009 INRIA. All rights reserved.
 */

//#include <debug.h>
#include <CGAL/AABB_intersections.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Mesh_triangulation_3.h>
#include <CGAL/Mesh_complex_3_in_triangulation_3.h>
#include <CGAL/Mesh_criteria_3.h>
// Implicit domain
#include <CGAL/Implicit_to_labeling_function_wrapper.h>
#include <CGAL/Labeled_mesh_domain_3.h>

#include <CGAL/make_mesh_3.h>
#include "../examples/Mesh_3/implicit_functions.h"
#include <CGAL/refine_mesh_3.h>
#include <CGAL/Mesh_3/Mesh_global_optimizer.h>

/* INPUTS */
// Polyhedral domain
#include <CGAL/Mesh_3/Robust_intersection_traits_3.h>
#include <CGAL/Polyhedral_mesh_domain_3.h>
#include <CGAL/IO/Polyhedron_iostream.h>
// Implicit domain is above
// 3D Image
#include <CGAL/Labeled_image_mesh_domain_3.h>
#include <CGAL/make_mesh_3.h>
#include <CGAL/Image_3.h>

/* OUTPUT */
#include <CGAL/IO/File_medit.h>

/* tools */
#include <sstream>
#include <stdlib.h>
#include <algorithm>

/* OPTIONS and PARAMETERS */
#include <boost/program_options.hpp>
namespace po = boost::program_options;

using namespace CGAL::parameters; //to avoid verbose function and named parameters call

/* FILE SYSTEM */
#include <iostream>
#include <boost/filesystem.hpp>
#include <boost/filesystem/path.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/convenience.hpp>
namespace fs = boost::filesystem;

/* DOMAIN */
struct K: public CGAL::Exact_predicates_inexact_constructions_kernel {};
// Polyhedral domain
typedef CGAL::Mesh_3::Robust_intersection_traits_3<K> Geom_traits; // exact constructions here
typedef CGAL::Polyhedron_3<Geom_traits> Polyhedron;
typedef CGAL::Polyhedral_mesh_domain_3<Polyhedron, Geom_traits> Polyhedral_domain;
// Implicit domain
typedef FT_to_point_function_wrapper<K::FT, K::Point_3> I_Function;
typedef CGAL::Implicit_multi_domain_to_labeling_function_wrapper<I_Function> I_Function_wrapper;
typedef I_Function_wrapper::Function_vector I_Function_vector;
typedef CGAL::Labeled_mesh_domain_3<I_Function_wrapper, K> Implicit_domain;
// 3D Image
typedef CGAL::Image_3 Image;
typedef CGAL::Labeled_image_mesh_domain_3<Image,K> Image_domain;

/* TRIANGULATION */
//Polyhedral domain
typedef CGAL::Mesh_triangulation_3<Polyhedral_domain>::type Tr_polyhedron;
typedef CGAL::Mesh_complex_3_in_triangulation_3<Tr_polyhedron> C3t3_polyhedron;
//Implicit domain
typedef CGAL::Mesh_triangulation_3<Implicit_domain>::type Tr_implicit;
typedef CGAL::Mesh_complex_3_in_triangulation_3<Tr_implicit> C3t3_implicit;
//3D Image
typedef CGAL::Mesh_triangulation_3<Image_domain>::type Tr_image;
typedef CGAL::Mesh_complex_3_in_triangulation_3<Tr_image> C3t3_image;

/* MESHING CRITERIA */
//Polyhedral domain
typedef CGAL::Mesh_criteria_3<Tr_polyhedron>       Mesh_criteria_polyhedron;
//typedef Mesh_criteria_polyhedron::Facet_criteria   Facet_criteria_polyhedron;
//typedef Mesh_criteria_polyhedron::Cell_criteria    Cell_criteria_polyhedron;
//Implicit domain
typedef CGAL::Mesh_criteria_3<Tr_implicit>         Mesh_criteria_implicit;
//typedef Mesh_criteria_implicit::Facet_criteria     Facet_criteria_implicit;
//typedef Mesh_criteria_implicit::Cell_criteria      Cell_criteria_implicit;
//3D Image
typedef CGAL::Mesh_criteria_3<Tr_image>            Mesh_criteria_image;
//typedef Mesh_criteria_image::Facet_criteria        Facet_criteria_image;
//typedef Mesh_criteria_image::Cell_criteria         Cell_criteria_image;

template <typename T>
T set_arg(const std::string& param_name,
		  const std::string& param_string,
		  const po::variables_map& vm)
{
	if(vm.count(param_name))
	{
		T param_value = vm[param_name].as<T>();
		//std::cout << param_string << ": " << param_value << "\n";
		return param_value;
	}
	else
	{
		//std::cout << param_string << " ignored.\n";
		return T();
	}
}

void set_implicit_function(I_Function_vector& v,
						   I_Function& f,
						   const std::string& function_name,
						   const po::variables_map& vm)
{
	if(vm.count(function_name))
	{
		v.push_back(f);
		std::cout << function_name << " ";
	}
}

std::vector<std::string> split_line(const std::string& str)
{
	std::vector<std::string> args;

	std::string::size_type lastPos = str.find_first_not_of(" ", 0);
	std::string::size_type pos = str.find_first_of(" ", lastPos);
	while(pos != std::string::npos || lastPos != std::string::npos)
	{
		args.push_back(str.substr(lastPos, pos-lastPos));
		lastPos = str.find_first_not_of(" ", pos);
		pos = str.find_first_of(" ", lastPos);
	}
	return args;
}

template<typename C3t3>
void save_histogram(std::string& filename,
					const C3t3& c3t3)
{
	std::vector<int> histo(181,0);

	for (typename C3t3::Cell_iterator cit = c3t3.cells_begin() ;
		 cit != c3t3.cells_end() ;
		 ++cit)
	{
		if( !c3t3.is_in_complex(cit))
			continue;

		typedef typename K::Point_3 Point_3;
		const Point_3& p0 = cit->vertex(0)->point();
		const Point_3& p1 = cit->vertex(1)->point();
		const Point_3& p2 = cit->vertex(2)->point();
		const Point_3& p3 = cit->vertex(3)->point();

		double a = CGAL::to_double(CGAL::abs(CGAL::Mesh_3::dihedral_angle(p0,p1,p2,p3)));
		histo[std::floor(a)] += 1;
		a = CGAL::to_double(CGAL::abs(CGAL::Mesh_3::dihedral_angle(p0, p2, p1, p3)));
		histo[std::floor(a)] += 1;
		a = CGAL::to_double(CGAL::abs(CGAL::Mesh_3::dihedral_angle(p0, p3, p1, p2)));
		histo[std::floor(a)] += 1;
		a = CGAL::to_double(CGAL::abs(CGAL::Mesh_3::dihedral_angle(p1, p2, p0, p3)));
		histo[std::floor(a)] += 1;
		a = CGAL::to_double(CGAL::abs(CGAL::Mesh_3::dihedral_angle(p1, p3, p0, p2)));
		histo[std::floor(a)] += 1;
		a = CGAL::to_double(CGAL::abs(CGAL::Mesh_3::dihedral_angle(p2, p3, p0, p1)));
		histo[std::floor(a)] += 1;
	}
	std::ofstream file(filename.c_str());
	std::copy(histo.begin(), histo.end(), std::ostream_iterator<int>(file, "\n"));
}

template<typename MeshingCriteria>
MeshingCriteria get_parameters(const std::string& param_line,
							   po::variables_map& vm)
{
	po::options_description mesh("Mesh generation parameters");
	mesh.add_options()
  ("mesh", "Generate mesh")
	("facet_angle", po::value<double>(), "Set facet angle bound")
	("facet_size", po::value<double>(), "Set facet size bound")
	("facet_error", po::value<double>(), "Set approximation error bound")
	("tet_shape", po::value<double>(), "Set tet radius-edge bound")
	("tet_size", po::value<double>(), "Set tet size bound");

	po::options_description implicit_functions("Implicit functions");
	implicit_functions.add_options()
	("torus", "Mesh torus function")
	("sphere", "Mesh sphere function")
	("chair", "Mesh chair function")
	("tanglecube", "Mesh tanglecube function")
	("cube", "Mesh cube function")
	("ellipsoid", "Mesh ellipsoid function")
	("heart", "Mesh heart function")
	("octic", "Mesh octic function");

	po::options_description optim("Optimization parameters");
	optim.add_options()
	("exude", "Exude mesh after refinement")
	("perturb", po::value<double>(), "Perturb mesh after refinement (sliver removal)")
	("lloyd", po::value<int>(), "Lloyd smoothing after refinement. arg is max nb iterations")
	("odt", po::value<int>(), "ODT smoothing after refinement. arg is max nb iterations");

	po::options_description additional_options("Options");
	additional_options.add_options()
	("off_vertices", po::value<int>(), "Use polyhedron vertices as initialization step")
	("no_label_rebind", "Don't rebind cell labels in medit output")
	("show_patches", "Show surface patches in medit output");

	po::options_description cmdline_options("Usage", 1);
	cmdline_options.add(mesh).add(implicit_functions).add(optim).add(additional_options);

	std::vector<std::string> args = split_line(param_line);
	po::store(po::command_line_parser(args).options(cmdline_options).run(), vm);
	po::notify(vm);

	double facet_angle = set_arg<double>("facet_angle", "Facet angle", vm);
	double facet_size  = set_arg<double>("facet_size", "Facet size", vm);
	double facet_error = set_arg<double>("facet_error", "Facet approximation error", vm);
	double tet_shape = set_arg<double>("tet_shape","Tet shape (radius-edge)", vm);
	double tet_size = set_arg<double>("tet_size","Tet size", vm);

	typename MeshingCriteria::Facet_criteria fc(facet_angle, facet_size, facet_error);
	typename MeshingCriteria::Cell_criteria cc(tet_shape, tet_size);
	return MeshingCriteria(fc, cc);
}

void get_implicit_function(const po::variables_map& vm,
						   I_Function_vector& fv)
{
	// Define functions
	I_Function f1( &torus_function);
	I_Function f2( &sphere_function<3>);
	I_Function f3( &chair_function);
	I_Function f4( &tanglecube_function);
	I_Function f5( &cube_function);
	I_Function f6( &ellipsoid_function);
	I_Function f7( &heart_function);
	I_Function f8( &octic_function);

	std::cout << "Function(s): ";
	set_implicit_function(fv, f1, "torus", vm);
	set_implicit_function(fv, f2, "sphere", vm);
	set_implicit_function(fv, f3, "chair", vm);
	set_implicit_function(fv, f4, "tanglecube", vm);
	set_implicit_function(fv, f5, "cube", vm);
	set_implicit_function(fv, f6, "ellipsoid", vm);
	set_implicit_function(fv, f7, "heart", vm);
	set_implicit_function(fv, f8, "octic", vm);
	std::cout << "\n\n";

	if(fv.empty())
		std::cout << "Warning: No implicit function set.\n";
}

template <class Domain>
class Domain_builder
{
public:
  /*void build(const std::string& str);
  Domain domain();*/
};

template <>
class Domain_builder<Polyhedral_domain>
{
  typedef Polyhedral_domain Domain;
public:
  Domain_builder(const std::string& str)
  : domain_(NULL)
  {
    std::ifstream input(str.c_str());
    input >> polyhedron_;
    domain_ = new Domain(polyhedron_);
  }

  ~Domain_builder() { delete domain_; }

  Domain& domain() { return *domain_; }

private:
  Domain* domain_;
  Polyhedron polyhedron_;
};

template <>
class Domain_builder<Image_domain>
{
  typedef Image_domain Domain;
public:
  Domain_builder(const std::string& str)
  : domain_(NULL)
  {
    image_.read(str.c_str());
    domain_ = new Domain(image_, 1e-6);
  }

  ~Domain_builder() { delete domain_; }

  Domain& domain() { return *domain_; }

private:
  Domain* domain_;
  Image image_;
};

//template <>
//class Domain_builder<Implicit_domain>
//{
//  typedef Implicit_domain Domain;
//public:
//  Domain_builder()
//  :	f1_( &torus_function);
//	, f2_( &sphere_function<3>);
//	, f3_( &chair_function);
//	, f4_( &tanglecube_function);
//	, f5_( &cube_function);
//  , f6_( &ellipsoid_function);
//	, f7_( &heart_function);
//	, f8_( &octic_function);
//
//  void build(const std::string& str)
//  {
//    Image image;
//    image.read(str.c_str());
//    delete domain_;
//    domain_ = new Domain(image, 1e-6);
//  }
//
//  Domain& domain() { return *domain_; }
//
//private:
//  Domain* domain_;
//
//  // Define functions
//	I_Function f1_;
//	I_Function f2_;
//	I_Function f3_;
//	I_Function f4_;
//	I_Function f5_;
//	I_Function f6_;
//	I_Function f7_;
//	I_Function f8_;
//};

template <class C3T3, class Mesh_criteria, class Domain>
void mesh(const std::string& data, const po::variables_map& vm)
{
  if(!fs::is_directory(data))
    std::cout << "!! Problem while reading " << data << "\n";

  std::string output_dir;
  if(vm.count("outdir"))
    output_dir = vm["outdir"].as<std::string>();
  else output_dir = data + "output";
  if(!fs::is_directory(output_dir) && !fs::create_directory(output_dir))
    std::cout << "!! Problem while creating " << output_dir << "\n";

  fs::path path(data);
  for(fs::directory_iterator it(path); it != fs::directory_iterator(); ++it)
  {
    if(fs::is_directory(*it)
       || (fs::extension(*it) != ".off" && (fs::extension(*it) != ".inr" && ( fs::extension(fs::basename(*it)) != ".inr" || fs::extension(*it) != ".gz"))) )
      continue;

    std::string line_param;
    std::string filename(fs::basename(*it));
    if ( fs::extension(*it) == ".gz" )
      filename = fs::basename(fs::basename(*it));
    std::string filename_param(data + filename + ".txt");

    std::ifstream file_param(filename_param.data()); //parameters
    if(!file_param)
    {
      std::cout << "Could not read parameters in : '" << filename_param << "'. Next file." << std::endl;
      continue;
    }
    unsigned int i = 1;

    // Timer
    CGAL::Timer timer;
    timer.start();

    // we keep c3t3 between lines
    C3T3 c3t3_save;

    //Load the domain
    std::cout << "****** [" << filename << "] Create domain...";
    std::flush(std::cout);
    Domain_builder<Domain> domain_builder(it->path().string());
    std::cout <<"done (" << timer.time() << "s) ******\n\n";

    while(std::getline(file_param,line_param))
    {
      std::cout << "*** Meshing " << filename << "[" << i << "] with : " << line_param << std::endl;

      po::variables_map vm_p;
      Mesh_criteria mcp = get_parameters<Mesh_criteria>(line_param, vm_p);

      //Mesh generation (reload domain and rebuild c3t3)
      if ( vm_p.count("mesh") )
      {
        timer.stop();
        timer.reset();
        timer.start();
        std::cout << "  Generate mesh...";
        std::flush(std::cout);
        c3t3_save = CGAL::make_mesh_3<C3T3>(domain_builder.domain(), mcp, no_exude(), no_perturb());
        std::cout << "done (" << timer.time() << "s - "
                  << c3t3_save.triangulation().number_of_vertices() << " vertices, "
                  << c3t3_save.number_of_cells() << " cells)\n";
      }

      C3T3 c3t3 = c3t3_save;

      //Optimization
      timer.stop();
      timer.reset();
      timer.start();
      if(vm_p.count("lloyd"))
      {
        std::cout << "  Lloyd optimization...";
        std::flush(std::cout);
        CGAL::lloyd_optimize_mesh_3(c3t3, domain_builder.domain(), max_iteration_number=vm_p["lloyd"].as<int>());
        std::cout << "done (" << timer.time() << "s)\n";
      }
      timer.stop();
      timer.reset();
      timer.start();
      if(vm_p.count("odt"))
      {
        std::cout << "  Odt optimization...";
        std::flush(std::cout);
        CGAL::odt_optimize_mesh_3(c3t3, domain_builder.domain(), max_iteration_number=vm_p["odt"].as<int>());
        std::cout << "done (" << timer.time() << "s)\n";
      }
      timer.stop();
      timer.reset();
      timer.start();
      if(vm_p.count("perturb"))
      {
        std::cout << "  Perturbation...";
        std::flush(std::cout);
        CGAL::perturb_mesh_3(c3t3, domain_builder.domain(), time_limit=vm_p["perturb"].as<double>());
        std::cout << "done (" << timer.time() << "s)\n";
      }
      timer.stop();
      timer.reset();
      timer.start();
      if(vm_p.count("exude"))
      {
        std::cout << "  Exudation...";
        std::flush(std::cout);
        CGAL::exude_mesh_3(c3t3);
        std::cout << "done (" << timer.time() << "s)\n";
      }
      timer.stop();
      timer.reset();
      timer.start();

      //save mesh
      std::cout << "  Save mesh...";
      std::stringstream ssout;
      ssout << i;
      std::string output_filename = output_dir +"/" + filename + "-out-" + ssout.str().c_str() + ".mesh";
      std::ofstream medit_file(output_filename.c_str());
      c3t3.output_to_medit(medit_file, !vm_p.count("no_label_rebind"), vm_p.count("show_patches"));

      //save histogram
      std::cout << "done. \n  Save histogram...";
      std::string histo_filename = output_dir +"/" + filename + "-histo-" + ssout.str().c_str() + ".txt";
      save_histogram<C3T3>(histo_filename, c3t3);
      i++;
      std::cout << "done.\n\n\n";
    }
  }
}

int main(int argc, char** argv)
{
	// options
	po::options_description generic("Options");
	generic.add_options()("help", "Produce help message")
	("polyhedron", "Test the polyhedral domain mesher")
	("implicit",   "Test the implicit domain mesher")
	("image",      "Test the 3D image domain mesher")
	("outdir", po::value<std::string>(), "Output directory. arg is location");

	po::options_description cmdline_options("Usage", 1);
	cmdline_options.add(generic);//.add(others);

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
	po::notify(vm);

	if(vm.count("help") || argc < 2)
	{
		std::cout << cmdline_options << std::endl;
		std::cout << "* Polyhedra: .off files should be in applications/data/Polyhedra\n";
		std::cout << "* Images:    .inr.gz files should be in applications/data/3D_images\n";
		std::cout << "  Note: for each file toto.domain, add a toto.txt file with meshing parameters\n\n";
		return 1;
	}

	//what are we testing
	bool mesh_polyhedra = vm.count("polyhedron");
	bool mesh_implicit = vm.count("implicit");
	bool mesh_images = vm.count("image");

	// iterate on data files
	std::string data_dir("data"); // or a user defined path...
	fs::path data_path(data_dir);

	if(mesh_polyhedra)
	{
		std::string data_poly = data_dir + "/Polyhedra/";
    mesh<C3t3_polyhedron,Mesh_criteria_polyhedron,Polyhedral_domain>(data_poly,vm);
	}

	if(mesh_images)
	{
		std::string data_img = data_dir + "/3D_images/";
    mesh<C3t3_image,Mesh_criteria_image,Image_domain>(data_img,vm);
  }

	if(mesh_implicit)
	{
		std::string data_imp = data_dir + "/Implicit/";
		//mesh<C3t3_implicit,Mesh_criteria_implicit>(data_imp,vm);
	}
	return 0;
}


// -*-coding: mule-utf-8-unix; fill-column: 58; -*-

/**
 * @file
 * A retarget algo abstraction
 *
 * @author Sergei Lodyagin <serg@kogorta.dp.ua>
 */

#include <algorithm>
#include <iterator>
#include <numeric>
#include <boost/thread.hpp>
#include "types/fixed.h"
#include "types/abstract_iterator.h"
#include "algos/retarget.h"
#include "log.h"
#include "util.h"
#include "pars.h"
#include "btc_time.h"
#include "checkpoints.h"
#include "main.h"
#include "types.h"

using namespace types;

extern std::map<uint256, CBlockIndex*> mapBlockIndex;
extern CBlockIndex* pindexGenesisBlock;

namespace retarget {

template<class Algo>
class difficulty_impl;

//! Allows change difficulty only by 2 or 1/2
template<>
class difficulty_impl<pars::twice_and_half>
  : public difficulty
{
public:
  compact_bignum_t next_block_difficulty(
    const duration desired_timespan,
    const iterator& rbegin,
    const iterator& rend
  ) override
  {
    if (rbegin == rend)
      return min_difficulty_by_design;

    assert(height_diff(rbegin, rend) > 0);

    auto prev = rbegin;
    ++prev;
    const auto actual_timespan =
      rbegin->time - prev->time;
    return (
      (actual_timespan < desired_timespan)
        ? rbegin->difficulty >> 1
        : rbegin->difficulty << 1
    ).GetCompact();
  }

  compact_bignum_t dos_min_difficulty(
    compact_bignum_t last_reliable_block_difficulty,
    coin::times::block::clock::duration past
  ) const override
  {
    return min_difficulty_by_design;
  }
};

template<>
class difficulty_impl<pars::digishield>
  : public difficulty
{
public:
  //! The limit parameter for dos_min_difficulty()
  const percent_t adjustment_by_design = 110.0_pct;

  compact_bignum_t next_block_difficulty(
    const duration desired_timespan,
    const iterator& rbegin,
    const iterator& rend
  ) override
  {
    if (rbegin == rend)
      return min_difficulty_by_design;

    auto prev = rbegin;
    ++prev;
    auto nActualTimespan = rbegin->time - prev->time;

    LOG() << "  nActualTimespan = "
          << nActualTimespan
          << "  before bounds\n";

    // thanks to RealSolid & WDC for this code

    if ( pars::digishield::limit_steps() ) //switch off for development purposes
    {
      //Amplitude Filter by daft27
      nActualTimespan = desired_timespan
        + (nActualTimespan - desired_timespan)/8;

      //Guts of DigiShield Retarget
      if (nActualTimespan < desired_timespan * 3/4)
        nActualTimespan = desired_timespan * 3/4;

      if (nActualTimespan > desired_timespan * 3/2)
        nActualTimespan = desired_timespan * 3/2;
    }

    if ( coin::times::block::zero_duration == nActualTimespan )
    {
        nActualTimespan = coin::times::block::seconds(1);
    }

    // Retarget
    const auto last_block_difficulty =
      rbegin->difficulty;
    CBigNum bnNew = last_block_difficulty;
    bnNew *= to_fixed(nActualTimespan);
    bnNew /= to_fixed(desired_timespan);

    if (bnNew > min_difficulty_by_design)
      bnNew = min_difficulty_by_design;

    LOG()
      << "GetNextWorkRequired [DigiShield] RETARGET \n"
      << "retargetTimespan = " << desired_timespan
      << " nActualTimespan = " << nActualTimespan
      << "\nBefore: " << last_block_difficulty << ' '
      << CBigNum(last_block_difficulty).getuint256()
      << "\nAfter: " << bnNew.GetCompact() << ' '
      << bnNew.getuint256() << '\n';

    return bnNew.GetCompact();
  }

  compact_bignum_t dos_min_difficulty(
    compact_bignum_t last_reliable_block_difficulty,
    coin::times::block::clock::duration past
  ) const override
  {
    if (past < coin::times::block::zero_duration)
    throw types::exception<coin::except::invalid_timestamp>(
        "block with timestamp before last checkpoint"
        );

    return std::min(
      CBigNum(last_reliable_block_difficulty)
      * past / block_period_by_design
      * adjustment_by_design,
      CBigNum(min_difficulty_by_design)
      ).GetCompact();
  }
};

template<>
class difficulty_impl<pars::kgw> : public difficulty
{
public:
  const int past_blocks_min =
    pars::kgw::past_min() / block_period_by_design;
  const int past_blocks_max =
    pars::kgw::past_max() / block_period_by_design;

  compact_bignum_t next_block_difficulty(
    const duration desired_timespan,
    const iterator& rbegin,
    const iterator& rend
  ) override
  {
    using namespace coin::times::block;

    // TODO don't use floating point
    using float_duration = std::chrono::duration<double>;

    // early blocks rule
    if (height_diff(rbegin, rend) < past_blocks_min)
      return min_difficulty_by_design;

    iterator start = rbegin;
    std::advance(start, past_blocks_min - 1);

    int64_t mass = past_blocks_min - 1;
    const auto breaking_el = std::find_if(
      start,
      rend,
      [
        this,
        desired_timespan,
        &rbegin,
        &mass
      ]
        (const block::info_type& bi)
      {
        // != 0: fixes the same-time blocks retargetting
        // exploit
        constexpr auto min_duration = seconds(1);

        if (++mass > past_blocks_max)
          return true;

        const clock::duration desired_passed =
          desired_timespan * mass;
        const clock::duration actual_passed =
          // the timewrap fix (part 1)
          (rbegin->time <= bi.time)
            ? min_duration
            : rbegin->time - bi.time;

        assert(actual_passed != zero_duration);
        const double PastRateAdjustmentRatio =
          (desired_passed != zero_duration)
        ?
          std::chrono::duration_cast<float_duration>(
            desired_passed
          )
          / std::chrono::duration_cast<float_duration>(
              actual_passed
          )
        :
          1.0;

        // TODO tabulate it as fixed point values
        const double EventHorizonDeviation =
          1 + (0.7084
            * pow((double(mass)/double(past_blocks_min)),
                  -1.228));

        const double EventHorizonDeviationFast =
          EventHorizonDeviation;
        const double EventHorizonDeviationSlow =
          1.0 / EventHorizonDeviation;

        return PastRateAdjustmentRatio <=
          EventHorizonDeviationSlow
          || PastRateAdjustmentRatio >=
          EventHorizonDeviationFast;
      }
    );

    const auto distance = height_diff(rbegin, breaking_el);
    assert(distance >= 0);

    const CBigNum PastDifficultyAverage =
      std::accumulate(
        rbegin,
        breaking_el,
        CBigNum(0),
        [](const CBigNum& acc, const block::info_type& bi)
        {
          return acc + bi.difficulty;
        }
      ) / (distance + 1);

    // != 0: fixes the same-time blocks retargetting
    // exploit
    constexpr auto min_duration = seconds(1);

    const auto range_desired_timespan =
      desired_timespan * (distance + 1);
    const auto range_actual_timespan =
      // the timewrap fix (part 2)
      (rbegin->time <= breaking_el->time)
      ? min_duration
      : rbegin->time - breaking_el->time;

    assert(range_actual_timespan != zero_duration);
    CBigNum bnNew(PastDifficultyAverage);
    if (range_desired_timespan != zero_duration)
    {
      // actual time passed
      bnNew *= to_fixed(range_actual_timespan);
      // desired
      bnNew /= to_fixed(range_desired_timespan);
    }
    if (bnNew > min_difficulty_by_design)
      bnNew = min_difficulty_by_design;

    LOG()
      << "GetNextWorkRequired [KGW] RETARGET ["
      << height_diff(rbegin, breaking_el)
      << " last blocks analysed]"
      << "\navg difficulty: "
      << PastDifficultyAverage.GetCompact()
      << "\ndesired timespan for the range = "
      << range_desired_timespan
      << "\nactual timespan for the range = "
      << range_actual_timespan
      << "\nBefore: "
      << rbegin->difficulty << ' '
      << CBigNum(rbegin->difficulty)
           . getuint256()
      << "\nAfter: " << bnNew.GetCompact() << ' '
      << bnNew.getuint256() << std::endl;

    return bnNew.GetCompact();
  }

  compact_bignum_t dos_min_difficulty(
    compact_bignum_t last_reliable_block_difficulty,
    coin::times::block::clock::duration past
  ) const override
  {
    return min_difficulty_by_design;
  }
};

difficulty& difficulty::instance()
{
  static boost::once_flag of = BOOST_ONCE_INIT;
  static difficulty* instance = nullptr;
  boost::call_once([]()
  {
    instance = new difficulty_impl
      <pars::retarget_algorithm>();
  }, of);

  assert(instance);
  return *instance;
}

compact_bignum_t difficulty
::next_block_difficulty(const CBlockIndex* pindexLast)
{
  using namespace types;

  // Genesis block
  if (!pindexLast)
    return min_difficulty_by_design;

  // Limit adjustment step
  const auto pindexFirst = pindexLast->pprev;
  if (!pindexFirst)
    return pindexLast->nBits; // slod

  return next_block_difficulty(
    block_period_by_design,
    block::const_iterator<CBlockIndex>(pindexLast),
    block::const_iterator<CBlockIndex>(pindexGenesisBlock)
  );
}

difficulty::difficulty()
  : min_difficulty_by_design(
      pars::testnet_switch(pars::min_difficulty_by_design)
    ),
    block_period_by_design(
      pars::testnet_switch(pars::block_period_by_design)
    )
{
}

const CBlockIndex* difficulty
::dos_last_reliable_block()
{
  static const CBlockIndex* last_checkpointed_block =
    Checkpoints::GetLastCheckpoint(mapBlockIndex);

  if (last_checkpointed_block)
    return last_checkpointed_block;
  else
  {
    assert(pindexGenesisBlock);
    return pindexGenesisBlock;
  }
}

} // retarget


//
//  dt_util.h
//  Classifer_RF
//
//  Created by jimmy on 2017-02-16.
//  Copyright (c) 2017 Nowhere Planet. All rights reserved.
//

#ifndef __Classifer_RF__dt_util__
#define __Classifer_RF__dt_util__

// decision tree util
#include <stdio.h>
#include <vector>
#include <Eigen/Dense>
#include <unordered_map>
#include <string>

using std::vector;
using std::string;
using Eigen::VectorXf;
using Eigen::VectorXd;
using Eigen::VectorXi;

using std::vector;

namespace dt {
    // randomly generate a subset of dimensions
    template<class intType>
    vector<intType> randomDimension(const intType dim, const intType num);

    template <class intType>
    vector<intType> range(int start, int end, int step)
    {
        assert((end - start) * step >= 0);
        vector<intType> ret;
        for (int i = start; i < end; i += step) {
            ret.push_back((intType)i);
        }
        return ret;
    }

    // mean and standard deviation
    template <class vectorType>
    void meanStd(const vector<vectorType> & labels, vectorType & mean, vectorType & sigma);

    template<class vectorType, class intType>
    void meanStd(const vector<vectorType> & labels, const vector<intType> & indices,
                 vectorType & mean, vectorType & sigma);

    // balance examples in each category
    // return: example indices with balanced training examples
    template<class intType>
    vector<intType> balanceSamples(const vector<intType> & example_indices,
                                   const vector<intType> & labels, const int category_num);

    // regression loss
    template<class VectorType, class IntType>
    double sumOfVariance(const vector<VectorType> & labels, const vector<IntType> & indices);

    // find most common number in the vector
    template<class intType>
    intType mostCommon(const vector<intType> & data);

    template <class VectorType>
    void meanMedianError(const vector<VectorType> & errors, VectorType & mean, VectorType & median);

}  // namespace

class DTUtil
{
public:
    // randomly generate a subset of dimensions
    static vector<unsigned int> randomDimensions(const int dimension, const int ccandidate_dimension);

    template <class T>
    static double spatialVariance(const vector<T> & labels, const vector<unsigned int> & indices);

    // full variance of Gaussian model
    template <class T>
    static double fullVariance(const vector<T>& labels, const vector<unsigned int> & indices);

    template <class MatrixType>
    static double sumOfVariance(const vector<MatrixType> & labels, const int row_index,
                                const vector<unsigned int> & indices);

    template <class Type1, class Type2>
    static double spatialVariance(const vector<Type1> & labels,
                                  const vector<unsigned int> & indices, const vector<Type2> & wt);

    template <class T>
    static void meanStddev(const vector<T> & labels, const vector<unsigned int> & indices, T & mean, T & sigma);

    template <class vectorT, class indexT>
    static vectorT mean(const vector<vectorT> & data, const vector<indexT> & indices);

    template <class T>
    static T mean(const vector<T> & data);

    // mean and standard of particular row
    template <class matrixType, class vectorType>
    static void rowMeanStddev(const vector<matrixType> & labels, const vector<unsigned int> & indices,
                              const int row_index, vectorType & mean,   vectorType & sigma);

    // https://en.wikipedia.org/wiki/Quartile
    // q1, q2, q3: first, second and third quartile. The second quartile is median
    template <class vectorT>
    static void quartileError(const vector<vectorT> & errors, vectorT & q1, vectorT& q2, vectorT& q3);

    // mean error of each row of a list of matrixes
    template <class MatrixType>
    static void matrixMeanError(const vector<MatrixType> & errors, MatrixType & mean);

    static double crossEntropy(const Eigen::VectorXd& prob);

    static double crossEntropy(const Eigen::VectorXf& prob);

    static double balanceLoss(const int leftNodeSize, const int rightNodeSize);

    static bool isSameLabel(const vector<unsigned int> & labels, const vector<unsigned int> & indices);
    static bool isSameLabel(const vector<int>& labels, const vector<int>& indices);

    // minimum number of examples in all category
    static int minLabelNumber(const vector<unsigned int> & labels,
                              const vector<unsigned int> & indices,
                              const int num_category);

    // label is a sequential data
    static int minLabelNumber(const vector<VectorXi> & labels,
                              const vector<unsigned int> & indices,
                              const int time_step,
                              const int num_category);

    template <class integerType>
    static Eigen::MatrixXd confusionMatrix(const vector<integerType> & predictions,
                                           const vector<integerType> & labels,
                                           const int category_num,
                                           bool normalize);

    // accuracy (should be precision) of each category and average
    static Eigen::VectorXd accuracyFromConfusionMatrix(const Eigen::MatrixXd & conf);
    static Eigen::VectorXd precisionFromConfusionMatrix(const Eigen::MatrixXd & conf);

    template <class T>
    static vector<T> range(int start, int end, int step)
    {
        assert((end - start) * step >= 0);
        vector<T> ret;
        for (int i = start; i < end; i += step) {
            ret.push_back((T)i);
        }
        return ret;
    }

};

#endif /* defined(__Classifer_RF__dt_util__) */


// Copyright 2019 Bold Hearts
//
// 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 "imu_fusion_madgwick/imu_fusion_madgwick.hpp"

#include <rclcpp/rclcpp.hpp>
#include <Eigen/Geometry>

#include <gtest/gtest.h>
#include <memory>

template<typename T, int N>
inline ::testing::AssertionResult VectorsEqual(
  Eigen::Matrix<T, N, 1> const & expected,
  Eigen::Matrix<T, N, 1> const & actual,
  const double delta = 0.00001)
{
  double d = (expected - actual).norm();
  if (d < delta) {
    return ::testing::AssertionSuccess();
  } else {
    return ::testing::AssertionFailure() << "Actual: " << actual.transpose() << ", expected: " <<
           expected.transpose() << " d = " << d;
  }
}

inline ::testing::AssertionResult MatricesEqual(
  Eigen::MatrixXd const & expected,
  Eigen::MatrixXd const & actual,
  double delta = 0.000001)
{
  double d = (expected - actual).array().abs().sum();
  if (d < delta) {
    return ::testing::AssertionSuccess();
  } else {
    return ::testing::AssertionFailure() << "Actual: " << std::endl << actual << std::endl <<
           "Expected: " << std::endl << expected << std::endl << " d = " << d;
  }
}

TEST(OrientationTests, construction)
{
  imu_fusion_madgwick::IMUFusionMadgwick orientation;
  auto quaternion = orientation.getEigenQuaternion();

  EXPECT_EQ(1.0, quaternion.w() );
  EXPECT_EQ(Eigen::Vector3d::Zero(), quaternion.vec() );
}

TEST(OrientationTests, gyro_halfPiXHighFreq)
{
  imu_fusion_madgwick::IMUFusionMadgwick orientation;

  for (unsigned i = 0; i < 512; ++i) {
    orientation.integrate(Eigen::Vector3d{0.5 * M_PI, 0, 0}, 1.0 / 512);
  }

  auto quaternion = orientation.getEigenQuaternion();

  EXPECT_TRUE(
    MatricesEqual(
      Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitX()).matrix(),
      quaternion.matrix()));
}

TEST(OrientationTests, gyro_halfPiX)
{
  imu_fusion_madgwick::IMUFusionMadgwick orientation;

  orientation.integrate(Eigen::Vector3d{0.5 * M_PI, 0, 0}, 1.0);

  EXPECT_TRUE(
    MatricesEqual(
      Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitX()).matrix(),
      orientation.getEigenQuaternion().matrix()));
}

TEST(OrientationTests, gyro_halfPiY)
{
  imu_fusion_madgwick::IMUFusionMadgwick orientation;

  orientation.integrate(Eigen::Vector3d{0, 0.5 * M_PI, 0}, 1.0);

  EXPECT_TRUE(
    MatricesEqual(
      Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY()).matrix(),
      orientation.getEigenQuaternion().matrix()));
}

TEST(OrientationTests, gyro_halfPiZ)
{
  imu_fusion_madgwick::IMUFusionMadgwick orientation;
  orientation.integrate(Eigen::Vector3d{0, 0, 0.5 * M_PI}, 1.0);

  EXPECT_TRUE(
    MatricesEqual(
      Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitZ()).matrix(),
      orientation.getEigenQuaternion().matrix()));
}

TEST(OrientationTests, gyro_piX)
{
  imu_fusion_madgwick::IMUFusionMadgwick orientation;
  orientation.integrate(Eigen::Vector3d{0.5 * M_PI, 0, 0}, 2.0);

  EXPECT_TRUE(
    MatricesEqual(
      Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()).matrix(),
      orientation.getEigenQuaternion().matrix()));
}

TEST(OrientationTests, gyro_piY)
{
  imu_fusion_madgwick::IMUFusionMadgwick orientation;
  orientation.integrate(Eigen::Vector3d{0, 0.5 * M_PI, 0}, 2.0);

  EXPECT_TRUE(
    MatricesEqual(
      Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY()).matrix(),
      orientation.getEigenQuaternion().matrix()));
}

TEST(OrientationTests, gyro_piZ)
{
  imu_fusion_madgwick::IMUFusionMadgwick orientation;
  orientation.integrate(Eigen::Vector3d{0, 0, 0.5 * M_PI}, 2.0);

  EXPECT_TRUE(
    MatricesEqual(
      Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()).matrix(),
      orientation.getEigenQuaternion().matrix()));
}

TEST(OrientationTests, gyro_quartPiX)
{
  imu_fusion_madgwick::IMUFusionMadgwick orientation;
  orientation.integrate(Eigen::Vector3d{0.5 * M_PI, 0, 0}, .5);

  EXPECT_TRUE(
    MatricesEqual(
      Eigen::AngleAxisd(0.25 * M_PI, Eigen::Vector3d::UnitX()).matrix(),
      orientation.getEigenQuaternion().matrix()));
}

TEST(OrientationTests, gyro_quartPiY)
{
  imu_fusion_madgwick::IMUFusionMadgwick orientation;
  orientation.integrate(Eigen::Vector3d{0, 0.5 * M_PI, 0}, .5);

  EXPECT_TRUE(
    MatricesEqual(
      Eigen::AngleAxisd(0.25 * M_PI, Eigen::Vector3d::UnitY()).matrix(),
      orientation.getEigenQuaternion().matrix()));
}

TEST(OrientationTests, gyro_quartPiZ)
{
  imu_fusion_madgwick::IMUFusionMadgwick orientation;
  orientation.integrate(Eigen::Vector3d{0, 0, 0.5 * M_PI}, .5);

  EXPECT_TRUE(
    MatricesEqual(
      Eigen::AngleAxisd(0.25 * M_PI, Eigen::Vector3d::UnitZ()).matrix(),
      orientation.getEigenQuaternion().matrix()));
}

TEST(OrientationTests, gyro_multiAxis)
{
  imu_fusion_madgwick::IMUFusionMadgwick orientation;
  orientation.integrate(Eigen::Vector3d{1.0, 1.0, 1.0}.normalized() * 2.0 / 3.0 * M_PI, 1.0);

  EXPECT_TRUE(
    MatricesEqual(
      Eigen::AngleAxisd(
        2.0 / 3.0 * M_PI,
        Eigen::Vector3d(1.0, 1.0, 1.0).normalized()).matrix(),
      orientation.getEigenQuaternion().matrix()));
}

TEST(OrientationTests, gyro_zero)
{
  imu_fusion_madgwick::IMUFusionMadgwick orientation;
  Eigen::Vector4d oldCoeffs = orientation.getEigenQuaternion().coeffs();
  orientation.integrate(Eigen::Vector3d::Zero(), 0.008);

  EXPECT_TRUE(MatricesEqual(oldCoeffs, orientation.getEigenQuaternion().coeffs()));
}

TEST(OrientationTests, gyro_chained)
{
  imu_fusion_madgwick::IMUFusionMadgwick orientation;
  orientation.integrate(Eigen::Vector3d{0.5 * M_PI, 0, 0}, 1.0);
  orientation.integrate(Eigen::Vector3d{0, 0, 0.5 * M_PI}, 1.0);

  EXPECT_TRUE(
    MatricesEqual(
      (Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitX()) *
      Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitZ())).matrix(),
      orientation.getEigenQuaternion().matrix()));
}

TEST(OrientationTests, merged_no_error)
{
  imu_fusion_madgwick::IMUFusionMadgwick orientation;
  auto q0 = orientation.getEigenQuaternion();
  orientation.integrate(Eigen::Vector3d::Zero(), 1.0, Eigen::Vector3d{0.0, 0.0, 1.0}, 0.1);

  EXPECT_TRUE(VectorsEqual(q0.coeffs(), orientation.getEigenQuaternion().coeffs()));
}

TEST(OrientationTests, merged_turned_no_error)
{
  imu_fusion_madgwick::IMUFusionMadgwick orientation;
  // Turn 90 deg around x axis
  orientation.integrate(Eigen::Vector3d{0.5 * M_PI, 0, 0}, 1.0);
  auto q1 = orientation.getEigenQuaternion();
  // Gravity is now along y axis
  orientation.integrate(Eigen::Vector3d::Zero(), 1.0, Eigen::Vector3d{0.0, 1.0, 0.0}, 0.1);

  EXPECT_TRUE(VectorsEqual(q1.coeffs(), orientation.getEigenQuaternion().coeffs()));
}

TEST(OrientationTests, merged_90deg_error)
{
  imu_fusion_madgwick::IMUFusionMadgwick orientation;

  // Gravity is now along y axis
  orientation.integrate(Eigen::Vector3d::Zero(), 1.0, Eigen::Vector3d{0.0, 1.0, 0.0}, 0.1);

  EXPECT_FLOAT_EQ(1.0, orientation.getEigenQuaternion().norm());
  EXPECT_LT(0.0, orientation.getEigenQuaternion().x());

  // Test convergence after 20 seconds
  for (unsigned i = 0; i < 10000; ++i) {
    orientation.integrate(Eigen::Vector3d::Zero(), 1.0 / 500, Eigen::Vector3d{0.0, 1.0, 0.0}, 0.1);
  }

  // Quaternion element is sin(theta / 2)
  EXPECT_NEAR(sin(.25 * M_PI), orientation.getEigenQuaternion().coeffs().x(), 0.00002);
}

int main(int argc, char ** argv)
{
  testing::InitGoogleTest(&argc, argv);

  rclcpp::init(argc, argv);

  return RUN_ALL_TESTS();
}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/set.hpp>
#include <boost/hana/string.hpp>
#include <boost/hana/type.hpp>
using namespace boost::hana;

int main() {

{

//! [comparable]
BOOST_HANA_CONSTANT_CHECK(
    set(int_<0>, type<char>, int_<1>) == set(int_<1>, int_<0>, type<char>)
);

BOOST_HANA_CONSTEXPR_CHECK(set(1, '2', 3.3) == set('2', 1, 3.3));
BOOST_HANA_CONSTANT_CHECK(set(1, '2', 3.3) != set('2', 1));
//! [comparable]

}{

//! [searchable]
constexpr auto xs = set(int_<0>, int_<1>, int_<2>);
BOOST_HANA_CONSTANT_CHECK(find(xs, int_<0>) == just(int_<0>));
BOOST_HANA_CONSTANT_CHECK(find(xs, int_<3>) == nothing);
//! [searchable]

}{

//! [foldable]
constexpr auto xs = set(int_<0>, int_<1>, int_<2>);
static_assert(minimum(xs) == int_<0>, "");
static_assert(maximum(xs) == int_<2>, "");
static_assert(sum(xs) == int_<3>, "");

// folding is not really meaningful since the order of the
// elements is unspecified.
//! [foldable]

}{

//! [insert]
constexpr auto xs = set(int_<0>, type<int>);
BOOST_HANA_CONSTANT_CHECK(
    insert(xs, BOOST_HANA_STRING("abc")) ==
    set(int_<0>, type<int>, BOOST_HANA_STRING("abc"))
);

BOOST_HANA_CONSTANT_CHECK(
    insert(xs, int_<0>) == set(int_<0>, type<int>)
);
//! [insert]

}

}


// BLAS level 3 -- complex numbers

//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <iostream>
#include <complex>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/conj.hpp>
#ifdef F_USE_STD_VECTOR
#include <vector>
#include <boost/numeric/bindings/std/vector.hpp>
#endif
#include "utils.h"

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

using std::cout;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

#ifndef F_USE_STD_VECTOR
typedef ublas::matrix<cmplx_t, ublas::row_major> m_t;
#else
typedef ublas::matrix<cmplx_t, ublas::column_major, std::vector<cmplx_t> > m_t;
#endif

int main() {

  cout << endl;

  m_t a (2, 2);
  a (0, 0) = cmplx_t (1., 0.);
  a (0, 1) = cmplx_t (2., 0.);
  a (1, 0) = cmplx_t (3., 0.);
  a (1, 1) = cmplx_t (4., 0.);
  print_m (a, "A");
  cout << endl;

  m_t b (2, 3);
  b (0, 0) = cmplx_t (1., 0.);
  b (0, 1) = cmplx_t (2., 0.);
  b (0, 2) = cmplx_t (3., 0.);
  b (1, 0) = cmplx_t (1., 0.);
  b (1, 1) = cmplx_t (2., 0.);
  b (1, 2) = cmplx_t (3., 0.);
  print_m (b, "B");
  cout << endl;

  m_t c (2, 3);

  // c = a b
  blas::gemm ( 1.0, a, b, 0.0, c);
  print_m (c, "A B");
  cout << endl;

  a (0, 0) = cmplx_t (0., 1.);
  a (0, 1) = cmplx_t (0., 2.);
  a (1, 0) = cmplx_t (0., 3.);
  a (1, 1) = cmplx_t (0., 4.);
  print_m (a, "A");
  cout << endl;

  // c = a b
  blas::gemm (1.0, a, b, 0.0, c);
  print_m (c, "A B");
  cout << endl;

  // c = a^T b
  blas::gemm ( 1.0, bindings::trans(a), b, 0.0, c);
  print_m (c, "A^T B");
  cout << endl;

  // c = a^H b
  blas::gemm (1.0, bindings::conj(a), b, 0.0, c);
  print_m (c, "A^H B");

  cout << endl;

}


/*
 * Tools.hpp
 *
 *  Created on: Feb 19, 2012
 *      Author: david
 */

#ifndef POINTSANDNORMALS_HPP_
#define POINTSANDNORMALS_HPP_

#include <Slimage/Slimage.hpp>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Eigenvalues>
#include <boost/math/constants/constants.hpp>
#include <vector>
#include <cmath>
#include <ctype.h>
#include <ostream>

namespace dasp {

template<typename K>
K Square(K x) {
	return x*x;
}

template<typename K>
std::ostream& operator<<(std::ostream& os, const std::vector<K>& v)
{
	os << "{";
	for(unsigned int i=0; i<v.size(); i++) {
		os << v[i];
		if(i + 1 != v.size()) {
			os << ", ";
		}
	}
	os << "}";
	return os;
}

template<typename K,typename U=unsigned int>
struct Histogram
{
	Histogram()
	: min_(0), max_(1) {}
	Histogram(unsigned int bin_count, K min, K max)
	: min_(min), max_(max), bins_(bin_count) {}
	void add(K x, U v=U(1)) {
		float p = float(x - min_) * float(bins_.size()) / float(max_ - min_);
		int i = std::round(p);
		if(i < 0) i = 0;
		if(i >= int(bins_.size())) i = bins_.size() - 1;
		bins_[i] += v;
	}
	const std::vector<U>& bins() const {
		return bins_;
	}
	const U operator[](unsigned int i) const {
		return bins_[i];
	}
	friend std::ostream& operator<<(std::ostream& os, const Histogram<K,U>& h) {
		os << h.bins();
		return os;
	}
private:
	K min_, max_;
	std::vector<U> bins_;
};

/***
 *
 */
struct Camera
{
	float cx, cy;
        float crx;
        float cry;
	float focal;
	float z_slope;

	/** Projects a 3D point into the image plane */
	Eigen::Vector2f project(const Eigen::Vector3f& p) const {
		return Eigen::Vector2f(p[0] / p[2] * focal + cx - crx, p[1] / p[2] * focal + cy - cry);
	}

	/** Computes a 3D point from pixel position and z/focal */
	Eigen::Vector3f unprojectImpl(float px, float py, float z_over_f) const {
		return z_over_f * Eigen::Vector3f(px - cx + crx, py - cy + cry, focal);
	}

	/** Computes a 3D point from pixel position and depth */
	Eigen::Vector3f unproject(int x, int y, uint16_t depth) const {
		return unprojectImpl(
			static_cast<float>(x), static_cast<float>(y),
			convertKinectToMeter(depth) / focal
		);
	}

	/** Gets kinect depth for a 3D point */
	uint16_t depth(const Eigen::Vector3f& p) const {
		return convertMeterToKinect(p[2]);
	}

	/** Convert kinect depth to meter */
	float convertKinectToMeter(uint16_t d) const {
		return static_cast<float>(d) * z_slope;
	}

	float convertKinectToMeter(int d) const {
		return static_cast<float>(d) * z_slope;
	}

	float convertKinectToMeter(float d) const {
		return d * z_slope;
	}

	/** Convert meter to kinect depth */
	uint16_t convertMeterToKinect(float z) const {
		return static_cast<uint16_t>(z / z_slope);
	}

	float computeOpenGLFOV(unsigned int height) const {
		return 2.0f * std::atan(static_cast<float>(height)/focal*0.5f) / boost::math::constants::pi<float>() * 180.0f;
	}

};

template<typename K>
inline float LocalFiniteDifferencesKinect(K v0, K v1, K v2, K v3, K v4)
{
	if(v0 == 0 && v4 == 0 && v1 != 0 && v3 != 0) {
		return float(v3 - v1);
	}

	bool left_invalid = (v0 == 0 || v1 == 0);
	bool right_invalid = (v3 == 0 || v4 == 0);
	if(left_invalid && right_invalid) {
		return 0.0f;
	}
	else if(left_invalid) {
		return float(v4 - v2);
	}
	else if(right_invalid) {
		return float(v2 - v0);
	}
	else {
		float a = static_cast<float>(std::abs(v2 + v0 - static_cast<K>(2)*v1));
		float b = static_cast<float>(std::abs(v4 + v2 - static_cast<K>(2)*v3));
		float p, q;
		if(a + b == 0.0f) {
			p = q = 0.5f;
		}
		else {
			p = a / (a + b);
			q = b / (a + b);
		}
		return q * static_cast<float>(v2 - v0) + p * static_cast<float>(v4 - v2);
	}
}

template<typename K>
inline K LocalFiniteDifferencesKinectSimple(K v0, K v1, K v2, K v3, K v4)
{
	if(v0 == 0 && v4 == 0 && v1 != 0 && v3 != 0) {
		return v3 - v1;
	}

	bool left_invalid = (v0 == 0 || v1 == 0);
	bool right_invalid = (v3 == 0 || v4 == 0);
	if(left_invalid && right_invalid) {
		return 0;
	}
	else if(left_invalid) {
		return v4 - v2;
	}
	else if(right_invalid) {
		return v2 - v0;
	}
	else {
		K a = std::abs(v2 + v0 - static_cast<K>(2)*v1);
		K b = std::abs(v4 + v2 - static_cast<K>(2)*v3);
		if(a < b) {
			return v2 - v0;
		}
		else {
			return v4 - v2;
		}
	}
}

inline Eigen::Vector2f LocalDepthGradient(const slimage::Image1ui16& depth, unsigned int j, unsigned int i, float z_over_f, float window, const Camera& camera)
{
	// compute w = base_scale*f/d
	unsigned int w = std::max(static_cast<unsigned int>(window + 0.5f), 4u);
	if(w % 2 == 1) w++;

	// can not compute the gradient at the border, so return 0
	if(i < w || depth.height() - w <= i || j < w || depth.width() - w <= j) {
		return Eigen::Vector2f::Zero();
	}

	float dx = LocalFiniteDifferencesKinect<int>(
		depth(j-w,i),
		depth(j-w/2,i),
		depth(j,i),
		depth(j+w/2,i),
		depth(j+w,i)
	);

	float dy = LocalFiniteDifferencesKinect<int>(
		depth(j,i-w),
		depth(j,i-w/2),
		depth(j,i),
		depth(j,i+w/2),
		depth(j,i+w)
	);

	// Theoretically scale == base_scale, but w must be an integer, so we
	// compute scale from the actually used w.

	// compute 1 / scale = 1 / (w*d/f)
	float scl = 1.0f / (float(w) * z_over_f);

	return scl * Eigen::Vector2f(camera.convertKinectToMeter(dx), camera.convertKinectToMeter(dy));
}

inline Eigen::Vector2f LocalDepthGradient(const slimage::Image1ui16& depth, unsigned int j, unsigned int i, float base_radius_m, const Camera& camera)
{
	uint16_t d00 = depth(j,i);
	if(d00 == 0) {
		return Eigen::Vector2f::Zero();
	}
	float z_over_f = camera.convertKinectToMeter(d00) / camera.focal;
	float window = base_radius_m/z_over_f;
	return LocalDepthGradient(depth, j, i, z_over_f, window, camera);
}

template<typename T, typename F>
Eigen::Matrix3f PointCovariance(const std::vector<T>& points, F f)
{
//	Eigen::Matrix3f A = Eigen::Matrix3f::Zero();
//	for(const Eigen::Vector3f& p : points) {
//		A += p * p.transpose();
//	}
	float xx=0.0f, xy=0.0f, xz=0.0f, yy=0.0f, yz=0.0f, zz=0.0f;
	for(auto it=points.begin(); it!=points.end(); ++it) {
		const Eigen::Vector3f& p = f(*it);
		float x = p[0];
		float y = p[1];
		float z = p[2];
		xx += x*x;
		xy += x*y;
		xz += x*z;
		yy += y*y;
		yz += y*z;
		zz += z*z;
	}
	Eigen::Matrix3f A; A << xx, xy, xz, xy, yy, yz, xz, yz, zz;
	A /= static_cast<float>(points.size());
	return A;
}

template<typename T, typename F>
Eigen::Matrix<float,6,1> Shape(const std::vector<T>& points, F f)
{
	typedef double K;
	const K SCL = 100.0f;
	typedef Eigen::Matrix<K,6,6> Mat6;
	typedef Eigen::Matrix<K,6,1> Vec6;
	K Sx=0, Sy=0;
	K Sxx=0, Sxy=0, Syy=0;
	K Sxxx=0, Sxxy=0, Sxyy=0, Syyy=0;
	K Sxxxx=0, Sxxxy=0, Sxxyy=0, Sxyyy=0, Syyyy=0;
	K Sz=0, Szx=0, Szy=0, Szxx=0, Szxy=0, Szyy=0;
	for(auto it=points.begin(); it!=points.end(); ++it) {
		const Eigen::Vector3f& p = f(*it);
		K x = SCL*p[0];
		K y = SCL*p[1];
		K z = SCL*p[2];
		K xx = x*x;
		K xy = x*y;
		K yy = y*y;
		Sx += x;
		Sy += y;
		Sxx += xx;
		Sxy += xy;
		Syy += yy;
		Sxxx += x*xx;
		Sxxy += x*xy;
		Sxyy += x*yy;
		Syyy += y*yy;
		Sxxxx += xx*xx;
		Sxxxy += xx*xy;
		Sxxyy += xx*yy;
		Sxyyy += xy*yy;
		Syyyy += yy*yy;
		Sz += z;
		Szx += z*x;
		Szy += z*y;
		Szxx += z*xx;
		Szxy += z*xy;
		Szyy += z*yy;
	}
	K S0 = points.size();
	Mat6 A;
	A << S0, Sx, Sy, Sxy, Sxx, Syy,
		 Sx, Sxx, Sxy, Sxxy, Sxxx, Sxyy,
		 Sy, Sxy, Syy, Sxyy, Sxxy, Syyy,
		 Sxy, Sxxy, Sxyy, Sxxyy, Sxxxy, Sxyyy,
		 Sxx, Sxxx, Sxxy, Sxxxy, Sxxxx, Sxxyy,
		 Syy, Sxyy, Syyy, Sxyyy, Sxxyy, Syyyy;
	Vec6 b;
	b << Sz, Szx, Szy, Szxy, Szxx, Szyy;
	Vec6 r = A.colPivHouseholderQr().solve(b);
	r[0] /= SCL;
	r[3] *= SCL;
	r[4] *= SCL;
	r[5] *= SCL;
	return r.cast<float>();
}

/** Fits a plane into points and returns the plane normal */
template<typename T, typename F>
Eigen::Vector3f FitNormal(const std::vector<T>& points, F f)
{
//		return Eigen::Vector3f(0.0f, 0.0f, 1.0f);
	// compute covariance matrix
	Eigen::Matrix3f A = PointCovariance(points, f);
	// compute eigenvalues/-vectors
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver;
	solver.compute(A);
	// take eigenvector (first eigenvalue is smallest!)
	return solver.eigenvectors().col(0).normalized();
}

}

#endif