//    Copyright 2017 Rainer Gemulla
//
//    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.
/** \file
 *
 * Illustrates matrix factorization with parallel SGD. We first create factors and then a data matrix
 * from these factors. This process ensures that we know the best factorization of the input.
 * These matrices are distributed across a cluster. We then try to reconstruct the factors
 * using PSGD.
 */
#include <iostream>
#include <sstream>

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/random/uniform_real.hpp>
#include <mf/matrix/io/generateDistributedMatrix.h>

#include <util/evaluation.h>

#include <mpi2/mpi2.h>
#include <mf/mf.h>

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

using namespace std;
using namespace mf;
using namespace mpi2;
using namespace rg;
using namespace boost::numeric::ublas;

// type of SGD
typedef UpdateTruncate<UpdateNzslL2> Update;
typedef RegularizeNone Regularize;
typedef SumLoss<NzslLoss, L2Loss> Loss;
typedef NzslLoss TestLoss;

int main(int argc, char* argv[]) {
	using namespace boost::program_options;
	// initialize mf library and mpi2
	boost::mpi::communicator& world = mfInit(argc, argv);

	mfStart();

	if (world.rank() == 0)
	{
#ifndef NDEBUG
		LOG4CXX_WARN(logger, "Warning: Debug mode activated (runtimes may be slow).");
#endif
		//data
		mf_size_type epochs;
		string inputSampleMatrixFile;
		string inputMatrixFile;
		string inputRowFacFile;
		string inputColFacFile;
		string outputRowFacFile;
		string outputColFacFile;
		string inputTestMatrixFile;
		string traceFile,traceVar;
		string shuffleStr;

		double lambda = 50;
		double eps0 = 0.01;
		// parameters for distribution
		int tasks;

		options_description desc("Options");
		desc.add_options()
				("help", "produce help message")
				("epochs", value<mf_size_type>(&epochs)->default_value(10), "number of epochs to run [10]")
				("lambda", value<double>(&lambda)->default_value(50), "lambda")
				("eps0", value<double>(&eps0)->default_value(0.01), "initial step size for BoldDriver")
				("tasks-per-rank", value<int>(&tasks)->default_value(1), "number of concurrent tasks [1]")
				("trace", value<string>(&traceFile)->default_value("trace.R"), "filename of trace [trace.R]")
				("traceVar", value<string>(&traceVar)->default_value("trace"), "variable name for trace [traceVar]")
				("input-file", value<string>(&inputMatrixFile), "input matrix")
				("input-test-file", value<string>(&inputTestMatrixFile), "input test matrix")
			    ("input-row-file", value<string>(&inputRowFacFile), "input initial row factor")
			    ("input-col-file", value<string>(&inputColFacFile), "input initial column factor")
			    ("output-row-file", value<string>(&outputRowFacFile), "output initial row factor")
			    ("output-col-file", value<string>(&outputColFacFile), "output initial column factor")
			    ("shuffle",value<string>(&shuffleStr)->default_value("seq"),"shuffle method eg seq, par, parAdd")
				;

		positional_options_description pdesc;
		pdesc.add("input-file", 1);
		pdesc.add("input-test-file", 2);
		pdesc.add("input-sample-matrix-file", 3);
		pdesc.add("input-row-file", 4);
		pdesc.add("input-col-file", 5);

		variables_map vm;
		store(command_line_parser(argc, argv).options(desc).positional(pdesc).run(), vm);
		notify(vm);

		if (vm.count("help") || vm.count("input-file")==0) {
			cout << "psgd with L2 NoLock (Hogwild-style) [options] <input-file> " << endl << endl;
			cout << desc << endl;
			return 1;
		}

		LOG4CXX_INFO(logger, "Using " << tasks << " parallel tasks");

		PsgdShuffle shuffle;
		if (shuffleStr.compare("seq") == 0)
			shuffle= PSGD_SHUFFLE_SEQ;
		else if (shuffleStr.compare("par") == 0)
			shuffle= PSGD_SHUFFLE_PARALLEL;
		else
			shuffle= PSGD_SHUFFLE_PARALLEL_ADDITIONAL_TASK;

		LOG4CXX_INFO(logger, "Using " << shuffle);

		// Read matrices
		Random32 random;
// 		SparseMatrix v,vTest;
// 		DenseMatrix w;
// 		DenseMatrixCM h;
// 		readMatrix(inputMatrixFile,v);
// 		readMatrix(inputTestMatrixFile,vTest);
// 		readMatrix(inputRowFacFile,w);
// 		readMatrix(inputColFacFile,h);

		Timer t;
		t.start();
		std::vector<DistributedSparseMatrix> dataVector=getDataMatrices<SparseMatrix>(inputMatrixFile,"V",true,
				tasks, 1, 1, 1, true, false, &inputTestMatrixFile);

		SparseMatrix& v = *mpi2::env().get<SparseMatrix>(dataVector[0].blocks()(0,0).var());
		SparseMatrix& vTest = *mpi2::env().get<SparseMatrix>(dataVector[1].blocks()(0,0).var());

		std::pair<DistributedDenseMatrix, DistributedDenseMatrixCM> factorsPair= getFactors(inputRowFacFile,
			inputColFacFile,  tasks, 1, 1, 1, true);

		t.stop();
		LOG4CXX_INFO(logger, "Total time for loading matrices: " << t);

		DenseMatrix w = *mpi2::env().get<DenseMatrix>(factorsPair.first.blocks()(0,0).var());
		DenseMatrixCM h = *mpi2::env().get<DenseMatrixCM>(factorsPair.second.blocks()(0,0).var());

		// parameters for SGD
		SgdOrder order = SGD_ORDER_WOR;
		Update update = Update(UpdateNzslL2(lambda), -100, 100); // truncate for numerical stability
		Regularize regularize;
		Loss loss((NzslLoss()), L2Loss(lambda));
		TestLoss testLoss;
		BalanceType balanceType = BALANCE_NONE;
		BalanceMethod balanceMethod = BALANCE_OPTIMAL;

		// initialize the DSGD

		PsgdRunner psgdRunner(random);
		PsgdJob<Update,Regularize> psgdJob(v, w, h, update, regularize, order, tasks, shuffle);
		BoldDriver decay(eps0);
		Trace trace;

		trace.addField("Loss", "L2");
		trace.addField("Shuffle_method", shuffle);
		trace.addField("input_file", inputMatrixFile);
		trace.addField("sample_matrix", inputSampleMatrixFile);
		trace.addField("tasks", tasks);

		// print the test loss
		FactorizationData<> testData(vTest, w, h);
		LOG4CXX_INFO(logger, "Initial test loss: " << testLoss(testData));

		// run HogwildSGD to try to reconstruct the original factors
		t.start();
		psgdRunner.run(psgdJob, loss, epochs, decay, trace, balanceType, balanceMethod, &testData, &testLoss);

		t.stop();
		LOG4CXX_INFO(logger, "Total time: " << t);

		// print the test loss
		LOG4CXX_INFO(logger, "Final test loss: " << testLoss(testData));

		// write trace to an R file
		LOG4CXX_INFO(logger, "Writing trace to " << traceFile);
		trace.toRfile(traceFile, traceVar);

					// write computed factors to file
				if (outputRowFacFile.length() > 0) {
					LOG4CXX_INFO(logger, "Writing row factors to " << outputRowFacFile);
					//DenseMatrix w0;
					//unblock(dw, w0);
					writeMatrix(outputRowFacFile, w);
				}
				if (outputColFacFile.length() > 0) {
					LOG4CXX_INFO(logger, "Writing column factors to " << outputColFacFile);
					//DenseMatrixCM h0;
					//unblock(dh, h0);
					writeMatrix(outputColFacFile, h);
				}
	}

	mfStop();
	mfFinalize();

	return 0;
}


#include "wave-io.h"
#include <boost/algorithm/string.hpp>
#include <boost/iostreams/copy.hpp>
#include <iostream>
#include <sstream>
#include <cmath>
#include <cstdint>
#include <cassert>

static int sat16(int x) {
  return (x < INT16_MIN) ? INT16_MIN : (x > INT16_MAX) ? INT16_MAX : x;
}
static int sat8(int x) {
  return (x < INT8_MIN) ? INT8_MIN : (x > INT8_MAX) ? INT8_MAX : x;
}

static float satfloat(float x) {
  if (!std::isfinite(x))
    return 0.0f;
  return (x < -1.0f) ? -1.0f : (x > 1.0f) ? 1.0f : x;
}

static void convert_to_float(float *samples,
                             unsigned sample_count,
                             WaveDataType type) {
  for (unsigned i = 0; i < sample_count; ++i) {
    float s = samples[i];
    switch (type) {
     case WaveFloat: break;
     case WaveInt16: s /= INT16_MAX; break;
     case WaveInt8: s /= INT8_MAX; break;
     default: assert(false);
    }
    samples[i] = satfloat(s);
  }
}

static void convert_from_float(float *samples,
                               unsigned sample_count,
                               WaveDataType type) {
  for (unsigned i = 0; i < sample_count; ++i) {
    float s = samples[i];
    switch (type) {
    case WaveFloat: s = satfloat(s); break;
    case WaveInt16: s = sat16(std::lround(samples[i] * INT16_MAX)); break;
    case WaveInt8: s = sat8(std::lround(samples[i] * INT8_MAX)); break;
    default: assert(false);
    }
    samples[i] = s;
  }
}

void write_wave(const float *in_samples,
                unsigned in_sample_count,
                unsigned out_sample_count,
                WaveFormat fmt,
                WaveDataType type,
                std::ostream &out) {
  std::vector<float> out_samples(out_sample_count);
  resample(in_samples, in_sample_count,
           out_samples.data(), out_sample_count);

  convert_from_float(out_samples.data(), out_sample_count, type);

  switch (fmt) {
   case WaveDat:
    for (int i = 0; i < out_sample_count; ++i)
      out << out_samples[i] << '\n';
    break;

   case WaveCpp:
   case WaveC: {
     const char *ctypename =
       (type == WaveFloat) ? "float" :
       (type == WaveInt16) ? "int16_t" :
       (type == WaveInt8) ? "int8_t" : nullptr;
     assert(ctypename);
     if (fmt == WaveCpp)
         out << "#include <array>\n" "#include <cstdint>\n\n"
             "[[gnu::unused]] static constexpr std::array<"
             << ctypename << ", " << out_sample_count << "> table {\n";
     else
         out << "#include <stdint.h>\n\n"
             "static const "
             << ctypename << " table [" << out_sample_count << "] = {\n";
     for (int i = 0; i < out_sample_count; ++i)
       out << ' ' << out_samples[i] << ',';
     out << " };\n";
     break;
   }

   default:
     throw std::runtime_error("unsupported wave output format");
  }
}

bool read_wave_from_stream(float *out_samples,
                           unsigned out_sample_count,
                           std::istream &in,
                           WaveFormat fmt,
                           unsigned channel) {
  std::ostringstream tmp(std::ios::binary);
  boost::iostreams::copy(in, tmp);

  if (in.bad())
    return false;

  return read_wave_from_string(
    out_samples, out_sample_count, tmp.str(), fmt, channel);
}

static WaveDataType detect_data_type(const float *samples,
                                     unsigned sample_count) {
  if (sample_count == 0)
    return WaveFloat;

  bool allinteger = true;
  float min = samples[0];
  float max = samples[0];

  for (unsigned i = 0; i < sample_count; ++i) {
    float s = samples[i];
    if (s < min) min = s;
    if (s > max) max = s;
    if (s != int(s)) allinteger = false;
  }

  WaveDataType type = WaveFloat;
  if (allinteger) {
    if (min >= INT8_MIN && max <= INT8_MAX)
      type = WaveInt8;
    else
      type = WaveInt16;
  }

  static const char *datatypenames[] = WAVE_DATA_TYPE_NAMES;
  std::cerr << "detected data type " << datatypenames[type] << '\n';

  return type;
}

static bool read_wave_from_dat(float *out_samples,
                               unsigned out_sample_count,
                               const std::string &in_,
                               unsigned channel) {
  std::vector<float> in_samples;
  in_samples.reserve(8192);

  std::istringstream in(in_, std::ios::binary);

  std::string line;
  line.reserve(256);
  std::vector<float> row;
  row.reserve(16);

  do {
    line.clear();
    std::getline(in, line);

    size_t commentpos = line.rfind('#');
    if (commentpos != line.npos)
      line.resize(commentpos);
    boost::trim_if(line, boost::is_any_of("\t "));

    if (!line.empty()) {
      float sample;
      row.clear();
      for (std::istringstream linestream(line, std::ios::binary);
           linestream >> sample;)
        row.push_back(sample);
    }

    in_samples.push_back(
      (channel < row.size()) ? row[channel] : 0.0f);
  } while (in);

  WaveDataType type = detect_data_type(in_samples.data(), in_samples.size());
  convert_to_float(in_samples.data(), in_samples.size(), type);

  resample(in_samples.data(), in_samples.size(),
           out_samples, out_sample_count);
  return true;
}

static bool read_wave_from_cpp(float *out_samples,
                               unsigned out_sample_count,
                               const std::string &in,
                               unsigned channel) {
  std::vector<float> in_samples;
  in_samples.reserve(8192);

  ///

  size_t index = 0;

  auto is_whitespace = [&](char c) {
    return c == ' ' || c == '\t' || c == '\r' || c == '\n'; };
  auto nth_char = [&](size_t n) {
    return (index + n >= in.size()) ? '\0' : in[index + n]; };
  auto skip = [&](size_t n) {
    index = std::min(in.size(), index + n); };
  auto consume_whitespace = [&]() {
    while (is_whitespace(nth_char(0))) skip(1); };
  auto end_of_input = [&]() {
    return index >= in.size(); };

  ///
  auto read_sample_array = [&]() -> bool {
    if (nth_char(0) != '{')
      return false;
    skip(1);
    in_samples.clear();
    while (nth_char(0) != '}') {
      consume_whitespace();
      float num {};
      unsigned nscanf {};
      /* TODO more C++ literal forms than scanf can handle */
      if (sscanf(in.c_str() + index, "%f%n", &num, &nscanf) != 1)
        return false;
      index += nscanf;
      in_samples.push_back(num);
      consume_whitespace();
      if (nth_char(0) == '}') {
      } else if (nth_char(0) == ',') {
        skip(1);
        consume_whitespace();
      } else
        return false;
    }
    return true;
  };

  ///
  bool have_samples = false;
  for (unsigned c = 0; !have_samples && c <= channel;) {
    while (!end_of_input() && nth_char(0) != '{')
      skip(1);
    if (end_of_input())
      break;
    if (!read_sample_array())
      continue;
    if (c == channel)
      have_samples = true;
    ++c;
  }
  if (!have_samples)
    return false;

  ///
  WaveDataType type = detect_data_type(in_samples.data(), in_samples.size());
  convert_to_float(in_samples.data(), in_samples.size(), type);

  resample(in_samples.data(), in_samples.size(),
           out_samples, out_sample_count);
  return true;
};

bool read_wave_from_string(float *out_samples,
                           unsigned out_sample_count,
                           const std::string &in,
                           WaveFormat fmt,
                           unsigned channel) {
  switch (fmt) {
    case WaveDat:
      return read_wave_from_dat(
        out_samples, out_sample_count, in, channel);

    case WaveCpp:
    case WaveC:
      return read_wave_from_cpp(
        out_samples, out_sample_count, in, channel);

   default:
     throw std::runtime_error("unsupported wave input format");
  }

  return false;
}


#include "catch.hpp"

#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/reverse_graph.hpp>

#include "test_types.hpp"
#include "utils.hpp"
#include <arlib/details/arlib_utils.hpp>
#include <arlib/graph_types.hpp>
#include <arlib/graph_utils.hpp>
#include <arlib/routing_kernels/bidirectional_dijkstra.hpp>

#include <experimental/filesystem>
#include <memory>
#include <string>

#include <string_view>

using namespace arlib::test;

TEST_CASE("Bidirectional Dijkstra finds the same shortest path of "
          "unidirectional dijkstra",
          "[bi_dijkstra") {
  using namespace boost;
  using arlib::VPair;

  auto G = arlib::read_graph_from_string<Graph>(std::string(graph_gr_esx));

  Vertex s = 0, t = 6;

  // Bidirectional Dijkstra
  auto weight_bi = get(edge_weight, G);
  auto distance_bi_vec = std::vector<Length>(num_vertices(G), 6);
  auto predecessor_bi_vec =
      std::vector<Vertex>(vertices(G).first, vertices(G).second);
  auto vertex_id = get(vertex_index, G);
  auto distance_bi =
      make_iterator_property_map(std::begin(distance_bi_vec), vertex_id);
  auto predecessor_bi =
      make_iterator_property_map(std::begin(predecessor_bi_vec), vertex_id);

  auto rev = make_reverse_graph(G);
  auto weight_b = get(edge_weight, rev);
  auto vertex_id_b = get(vertex_index, G);

  // Run Bidirectional dijkstra
  arlib::bidirectional_dijkstra(G, s, t, predecessor_bi, distance_bi, weight_bi,
                                rev, weight_b, vertex_id_b);

  auto sp_bi =
      arlib::details::build_edge_list_from_dijkstra(G, s, t, predecessor_bi);

  // Unidirectional Dijkstra
  auto predecessor_uni =
      std::vector<Vertex>(vertices(G).first, vertices(G).second);
  dijkstra_shortest_paths(G, s,
                          predecessor_map(make_iterator_property_map(
                              std::begin(predecessor_uni), vertex_id, s)));
  auto sp_uni =
      arlib::details::build_edge_list_from_dijkstra(G, s, t, predecessor_uni);

  std::cout << "***** BiDirectional vs Unidirectional solutions*****\n";
  for (auto e : sp_bi) {
    auto u = source(e, G);
    auto v = target(e, G);
    std::cout << "(" << u << ", " << v << ") ";
  }
  std::cout << "\n";

  for (auto e : sp_uni) {
    auto u = source(e, G);
    auto v = target(e, G);
    std::cout << "(" << u << ", " << v << ") ";
  }
  std::cout << "\n";

  REQUIRE(sp_bi == sp_uni);
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_GAMMA_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_GAMMA_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a gamma random variate for the given shape and inverse
 * scale parameters using the specified random number generator.
 *
 * alpha and beta can each be a scalar or a one-dimensional container. Any
 * non-scalar inputs must be the same size.
 *
 * @tparam T_shape Type of shape parameter
 * @tparam T_inv Type of inverse scale parameter
 * @tparam RNG type of random number generator
 * @param alpha (Sequence of) positive shape parameter(s)
 * @param beta (Sequence of) positive inverse scale parameter(s)
 * @param rng random number generator
 * @return (Sequence of) gamma random variate(s)
 * @throw std::domain_error if alpha or beta are nonpositive
 * @throw std::invalid_argument if non-scalar arguments are of different
 * sizes
 */
template <typename T_shape, typename T_inv, class RNG>
inline typename VectorBuilder<true, double, T_shape, T_inv>::type gamma_rng(
    const T_shape& alpha, const T_inv& beta, RNG& rng) {
  using boost::gamma_distribution;
  using boost::variate_generator;

  static const char* function = "gamma_rng";

  check_positive_finite(function, "Shape parameter", alpha);
  check_positive_finite(function, "Inverse scale parameter", beta);
  check_consistent_sizes(function, "Shape parameter", alpha,
                         "Inverse scale Parameter", beta);

  scalar_seq_view<T_shape> alpha_vec(alpha);
  scalar_seq_view<T_inv> beta_vec(beta);
  size_t N = max_size(alpha, beta);
  VectorBuilder<true, double, T_shape, T_inv> output(N);

  for (size_t n = 0; n < N; ++n) {
    // Convert rate (inverse scale) argument to scale for boost
    variate_generator<RNG&, gamma_distribution<> > gamma_rng(
        rng, gamma_distribution<>(alpha_vec[n],
                                  1 / static_cast<double>(beta_vec[n])));
    output[n] = gamma_rng();
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


/*
 * Copyright (c) 2020. Mohit Deshpande.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include "ekf/ekf_node.h"

#include <chrono>

#include <ros/ros.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/utils.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <laser_geometry/laser_geometry.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>

#include <std_msgs/Float32.h>

#include "ekf/ekf.h"
#include "ekf/utils.h"
#include "ekf/scan_matcher.h"

#include <Eigen/Eigen>

#include <robot_config/robot_config.h>

static constexpr auto kOdometryTopic = "/wheels/encoders";
static constexpr auto kImuTopic = "/camera/imu";
static constexpr auto kScanTopic = "/scan";

static constexpr auto UPDATE_RATE = 40;
static constexpr auto STATE_SIZE = 5;
// motors can't go this fast and no one is going to move it this fast
// still WIP on why the MCU occasionally produces really high counts
static constexpr auto V_MAX_SANITY = 2.0;
static constexpr auto W_MAX_SANITY = M_2_PI;

namespace ekf {

EkfNode::EkfNode()
        : prev_cloud_(new pcl::PointCloud<pcl::PointXYZ>),
        tf_listener_(tf_buffer_) {
    ros::NodeHandle nh;

    odometry_sub_ = nh.subscribe(kOdometryTopic, 1, &EkfNode::ReceiveOdometry, this);
    imu_sub_ = nh.subscribe(kImuTopic, 1, &EkfNode::ReceiveImu, this);
    scan_sub_ = nh.subscribe(kScanTopic, 1, &EkfNode::ReceiveScan, this);

    imu_H_ = Eigen::MatrixXd::Zero(1, 5);
    imu_H_(0, 4) = 1;
    imu_R_ = Eigen::MatrixXd::Identity(1, 1) * 0.1;

    odom_H_ = Eigen::MatrixXd::Zero(1, 5);
    odom_H_(0, 3) = 1;
    odom_R_ = Eigen::MatrixXd::Identity(1, 1) * 0.1;

    scan_H_ = Eigen::MatrixXd::Zero(2, 5);
    scan_H_(0, 0) = 1;
    scan_H_(1, 1) = 1;
    scan_R_ = Eigen::MatrixXd::Identity(2, 2) * 0.1;

    // [x, y, theta, v, w]
    Eigen::VectorXd initial_state = Eigen::VectorXd::Zero(STATE_SIZE);
    Eigen::MatrixXd initial_covariance = Eigen::MatrixXd::Identity(initial_state.size(), initial_state.size());
    ekf_.state = initial_state;
    ekf_.covariance = initial_covariance;

    // publishers
    pose_pub_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/pose/odom", 1);
    last_imu_stamp_ = ros::Time::now();
    last_odom_stamp_ = ros::Time::now();
}

void EkfNode::ReceiveImu(const sensor_msgs::ImuConstPtr& imu) {
    if (first_imu_msg) {
        first_imu_msg = false;
        last_imu_stamp_ = imu->header.stamp;
        return;
    }

    geometry_msgs::TransformStamped camera_link;
    try {
        camera_link = tf_buffer_.lookupTransform("camera_link", imu->header.frame_id, imu->header.stamp);
    } catch (const std::exception& e) {
        ROS_ERROR_STREAM(e.what());
        return;
    }

    geometry_msgs::Vector3 transformed_angular_velocity;
    tf2::doTransform(imu->angular_velocity, transformed_angular_velocity, camera_link);

    double dt = (imu->header.stamp - last_imu_stamp_).toSec();
    auto w = transformed_angular_velocity.z;

    // sanity checking on velocity
    if (w > W_MAX_SANITY) {
        ROS_WARN_STREAM("Impossible angular velocity!: " << w << "rad/s");
        return;
    }

    Eigen::VectorXd z(1);
    z(0) = w;

    if (!Update(ekf_, z, imu_H_, imu_R_)) {
        ROS_ERROR("Not updating state! S is singular!");
        return;
    }
    Predict(ekf_, dt);

    last_imu_stamp_ = imu->header.stamp;
}

void EkfNode::ReceiveOdometry(const motion_controller_msgs::WheelEncodersConstPtr& odometry) {
    if (first_odom_msg) {
        first_odom_msg = false;
        last_odom_stamp_ = odometry->header.stamp;
        return;
    }

    double dt = (odometry->header.stamp - last_odom_stamp_).toSec();
    double counts = 0.5 * (odometry->left + odometry->right);
    double v = counts * robot_config::DISTANCE_PER_COUNT / dt;

    // sanity checking on velociy
    if (v > V_MAX_SANITY) {
        ROS_WARN_STREAM("Impossible velocity!: " << v << "m/s (l="
            << odometry->left << ", r=" << odometry->right << ")");
        return;
    }

    Eigen::VectorXd z(1);
    z(0) = v;

    if (!Update(ekf_, z, odom_H_, odom_R_)) {
        ROS_ERROR("Not updating state! S is singular!");
        return;
    }

    Predict(ekf_, dt);

    last_odom_stamp_ = odometry->header.stamp;
}

void EkfNode::ReceiveScan(const sensor_msgs::LaserScanConstPtr& scan) {
    if (first_scan_msg) {
        sensor_msgs::PointCloud2 cloud;
        laser_projector_.transformLaserScanToPointCloud("base_link", *scan, cloud, tf_buffer_);
        pcl::moveFromROSMsg(cloud, *prev_cloud_);
        prev_scan_ekf_ = ekf_;
        first_scan_msg = false;
        return;
    }

    sensor_msgs::PointCloud2 cloud;
    laser_projector_.transformLaserScanToPointCloud("base_link", *scan, cloud, tf_buffer_);
    pcl::PointCloud<pcl::PointXYZ>::Ptr current_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::moveFromROSMsg(cloud, *current_cloud);

    Eigen::Affine3d transform(ScanMatch({}, prev_cloud_, current_cloud));

    Eigen::Vector2d z = Eigen::Vector2d::Zero();
    z(0) = transform.translation()(0) + prev_scan_ekf_.state(0);
    z(1) = transform.translation()(1) + prev_scan_ekf_.state(1);

    if (!Update(ekf_, z, scan_H_, scan_R_)) {
        ROS_ERROR("Not updating state! S is singular!");
    }

    prev_cloud_ = current_cloud;
    prev_scan_ekf_ = ekf_;
}

void EkfNode::Run() {
    ros::Rate rate(UPDATE_RATE);
    while (ros::ok()) {
        geometry_msgs::PoseWithCovarianceStamped pose;
        pose.header.frame_id = "odom";
        pose.header.stamp = ros::Time::now();
        pose.pose.pose.position.x = ekf_.state(0);
        pose.pose.pose.position.y = ekf_.state(1);
        pose.pose.pose.position.z = 0;
        tf2::Quaternion orientation;
        orientation.setRPY(0, 0, ekf_.state(2));
        pose.pose.pose.orientation = tf2::toMsg(orientation);
        pose.pose.covariance = toRos(ekf_.covariance);
        ROS_INFO_STREAM("x=" << pose.pose.pose.position.x
            << ", y=" << pose.pose.pose.position.y
            << ", theta=" << tf2::getYaw(pose.pose.pose.orientation));
        pose_pub_.publish(pose);

        geometry_msgs::TransformStamped transform;
        transform.transform.translation.x = pose.pose.pose.position.x;
        transform.transform.translation.y = pose.pose.pose.position.y;
        transform.transform.translation.z = pose.pose.pose.position.z;
        transform.transform.rotation = tf2::toMsg(orientation);
        transform.header.stamp = ros::Time::now();
        transform.header.frame_id = "odom";
        transform.child_frame_id = "base_link";
        tf_broadcaster_.sendTransform(transform);

        rate.sleep();
        ros::spinOnce();
    }
}

}

int main(int argc, char** argv) {
    ros::init(argc, argv, "ekf");
    ros::NodeHandle nh;

    ekf::EkfNode slam;
    slam.Run();

    return 0;
}


/**
 * Copyright Soramitsu Co., Ltd. 2017 All Rights Reserved.
 * http://soramitsu.co.jp
 *
 * 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.
 */

#ifndef IROHA_SHARED_MODEL_AMOUNT_HPP
#define IROHA_SHARED_MODEL_AMOUNT_HPP

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <vector>
#include "amount/amount.hpp"
#include "interfaces/base/primitive.hpp"
#include "interfaces/common_objects/types.hpp"
#include "utils/string_builder.hpp"

namespace shared_model {
  namespace interface {

    /**
     * Representation of fixed point number
     */
    class Amount : public PRIMITIVE_WITH_OLD(Amount, iroha::Amount) {
     public:
      /**
       * Gets integer representation value, which ignores precision
       * @return amount represented as integer value, which ignores precision
       */
      virtual const boost::multiprecision::uint256_t &intValue() const = 0;

      /**
       * Gets the position of precision
       * @return the position of precision
       */
      virtual types::PrecisionType precision() const = 0;

      /**
       * Checks equality of objects inside
       * @param rhs - other wrapped value
       * @return true, if wrapped objects are same
       */
      bool operator==(const ModelType &rhs) const override {
        return intValue() == rhs.intValue() and precision() == rhs.precision();
      }

      /**
       * String representation.
       * @return string representation of the asset.
       */
      std::string toStringRepr() const {
        if (precision() > 0) {
          boost::multiprecision::cpp_dec_float_50 float50(intValue());
          float50 /= pow(10, precision());
          return float50.str(precision(), std::ios_base::fixed);
        }
        return intValue().str(0, std::ios_base::fixed);
      }

      /**
       * Stringify the data.
       * @return the content of asset.
       */
      std::string toString() const override {
        return detail::PrettyStringBuilder()
            .init("Amount")
            .append("intValue", intValue().str())
            .append("precision", std::to_string(precision()))
            .finalize();
      }

#ifndef DISABLE_BACKWARD
      /**
       * Makes old model.
       * @return An allocated old model of account asset response.
       */
      OldModelType *makeOldModel() const override {
        return new OldModelType(intValue(), precision());
      }

#endif
    };
  }  // namespace interface
}  // namespace shared_model
#endif  // IROHA_SHARED_MODEL_AMOUNT_HPP


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
//! OPENOR_INTERFACE_FILE(Image_ImageData)
//****************************************************************************
/**
* @file
* @author Christian Winne
* \ingroup Image_ImageData
*/

#ifndef openOR_Image_Image2DRawData_hpp
#define openOR_Image_Image2DRawData_hpp

#include <boost/tr1/memory.hpp>

#include <openOR/Plugin/CreateInterface.hpp>
#include <openOR/Math/vector.hpp>
#include <openOR/Utility/Types.hpp>

namespace openOR {
	namespace Image {

		/**
		* \brief Image2DRawData
		* \ingroup Image_ImageData
		*/
		template<class Type>
		class Image2DRawData
		{
		public:

			typedef Type ValueType;
			typedef ValueType* Pointer;
			typedef const ValueType* ConstPointer;

			/**
			* Getter for the volume-size as a Size3UI object in indices
			* @return The volume size in indices
			*/
			virtual Math::Vector2ui size() const = 0;

			/**
			* Setter for the volume-size as a Size3UI object in indices
			* @param size The volume size in indices
			*/
			virtual void setSize(const Math::Vector2ui& size) = 0;

			virtual Pointer mutableData() = 0;

			virtual ConstPointer data() const = 0;

		};

		typedef Image2DRawData<uint8> Image2DRawDataUI8;
		typedef Image2DRawData<uint16> Image2DRawDataUI16;
		typedef Image2DRawData<float> Image2DRawDataFloat;
		typedef Image2DRawData<double> Image2DRawDataDouble;
		typedef Image2DRawData<UI8Quad> Image2DRawDataUI8Quad;
		typedef Image2DRawData<UI16Quad> Image2DRawDataUI16Quad;
		typedef Image2DRawData<FloatQuad> Image2DRawDataFloatQuad;
	}

	/**
	* Getter for the width of the whole volume object in indices
	* @return The width of the volume object
	*/
	template<class Type>
	const uint width(std::tr1::shared_ptr<const Image::Image2DRawData<Type> > pImageData) { return pImageData->size()(0); }
	template<class Type>
	const uint width(std::tr1::shared_ptr<Image::Image2DRawData<Type> > pImageData) { return pImageData->size()(0); }

	/**
	* Getter for the height of the whole volume object in indices
	* @return The height of the volume object
	*/
	template<class Type>
	const uint height(std::tr1::shared_ptr<const Image::Image2DRawData<Type> > pImageData) { return pImageData->size()(1); }
	template<class Type>
	const uint height(std::tr1::shared_ptr<Image::Image2DRawData<Type> > pImageData) { return pImageData->size()(1); }

	template<class Type>
	Type& voxelIndexRef(std::tr1::shared_ptr<Image::Image2DRawData<Type> > pImageData, const Math::Vector2ui& vecPosition)
	{
		return *(pImageData->mutableData() + vecPosition(0) + width(pImageData) * (vecPosition(1)));
	}

	template<class Type>
	const Type& voxelIndexRef(std::tr1::shared_ptr<const Image::Image2DRawData<Type> > pImageData, const Math::Vector2ui& vecPosition)
	{
		return *(pImageData->data() + vecPosition(0) + width(pImageData) * (vecPosition(1)));
	}

	template<class Type>
	Type& voxelIndexRef(std::tr1::shared_ptr<Image::Image2DRawData<Type> > pImageData, const uint index)
	{
		return *(pImageData->mutableData() + index);
	}

	template<class Type>
	const Type& voxelIndexRef(std::tr1::shared_ptr<const Image::Image2DRawData<Type> > pImageData, const uint index)
	{
		return *(pImageData->data() + index);
	}

	/************************************************************************/
	/*                                                                      */
	/************************************************************************/

	//template<class Image>
	//struct ImageCreator
	//{
	//  typedef Image ImageType;

	//  virtual std::tr1::shared_ptr<ImageType> operator()() const = 0;
	//  virtual void setImage(std::tr1::shared_ptr<ImageType> pImage) = 0;
	//};

	//typedef ImageCreator<ImageRawDataUI8> ImageRawDataUI8Creator;
	//typedef ImageCreator<ImageRawDataUI16> ImageRawDataUI16Creator;

	/************************************************************************/
	/*                                                                      */
	/************************************************************************/

}

OPENOR_CREATE_TEMPLATED_INTERFACE_1(Type, openOR::Image::Image2DRawData<Type>)

	Math::Vector2ui size() const { return adaptee()->size(); }
void setSize(const Math::Vector2ui& size) { adaptee()->setSize(size); }

Type* mutableData() { return adaptee()->mutableData(); }
const Type* data() const { return adaptee()->data(); }

OPENOR_CREATE_INTERFACE_END

	//OPENOR_CREATE_TEMPLATED_INTERFACE_1(Image, openOR::ImageCreator<Image>)
	//  std::tr1::shared_ptr<Image> operator()() const { return adaptee()->operator()(); }
	//  void setImage(std::tr1::shared_ptr<Image> pImage) { adaptee()->setImage(pImage); }
	//OPENOR_CREATE_INTERFACE_END
	//

#endif


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_DETAIL_SHUFFLE_PATTERN_HPP_INCLUDED
#define BOOST_SIMD_DETAIL_SHUFFLE_PATTERN_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch/hierarchy/unspecified.hpp>
#include <boost/simd/detail/dispatch/hierarchy_of.hpp>

namespace boost { namespace simd
{
  // -----------------------------------------------------------------------------------------------
  // normalized const-expr pattern holder - prevent MSVC 2016 shenanigans with 0
  template<int (*M)(int,int)> struct pattern {};

  namespace detail
  {
    // -----------------------------------------------------------------------------------------------
    // normalized permutation pattern holder - also acts as its own hierarchy
    template<int... Ps> struct pattern_ : boost::dispatch::unspecified_<pattern_<Ps...>>
    {
      static const std::size_t static_size = sizeof...(Ps);
      using size_type = std::integral_constant<std::size_t,static_size>;
      using parent = boost::dispatch::unspecified_<pattern_<Ps...>>;
    };

    // -----------------------------------------------------------------------------------------------
    // take a meta-function class and build a pattern
    template<typename M, typename C, typename L> struct make_meta_pattern;

    template<typename M, typename C, typename P>
    struct idx : brigand::apply<M, P, C>::type {};

    template<typename M, typename C, typename... Ps>
    struct make_meta_pattern<M, C, brigand::list<Ps...> >
    {
      using type = pattern_< idx<M,C,Ps>::value... >;
    };

    template<typename M, typename T>
    struct meta_pattern
    {
      using card = cardinal_of<T>;
      using type = typename make_meta_pattern<M, card, brigand::range<int,0,card::value>>::type;
    };

    // -----------------------------------------------------------------------------------------------
    // take a constexpr function and build a pattern
    template<int (*M)(int,int), typename C, typename L> struct make_const_pattern;

    template<int (*M)(int,int), typename C, typename... Ps>
    struct make_const_pattern<M, C, brigand::list<Ps...> >
    {
      using type = pattern_< M(Ps::value,C::value)... >;
    };

    template<int (*M)(int,int), typename T>
    struct meta_pattern<pattern<M>,T>
    {
      using card = cardinal_of<T>;
      using type = typename make_const_pattern<M, card, brigand::range<int,0,card::value>>::type;
    };
  } }
}

namespace boost { namespace dispatch
{
  namespace ext
  {
    template<typename Ps,typename Origin, typename Enable = void> struct pattern_hierarchy
    {
      using type = Ps;
    };
  }

  // ---------------------------------------------------------------------------------------------
  // Give a proper hierarchy to identify pattern
  template<int... Ps,typename Origin>
  struct  hierarchy_of < boost::simd::detail::pattern_<Ps...>,Origin>
        : ext::pattern_hierarchy<boost::simd::detail::pattern_<Ps...>,Origin>
  {};
} }

#endif


/*
 * Basic type
 */
#ifndef AERO_BASIC_TYPE_H_INCLUDED
#define AERO_BASIC_TYPE_H_INCLUDED

// intptr_t
#if defined(_MSC_VER) && _MSC_VER < 1600 /* VS 2010 has cstdint */
#include <crtdefs.h>
#else
#include <cstdint>
#endif

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

/* Provide definitions for some commonly used macros.
 *  Some of them are only provided if they haven't already
 *  been defined. It is assumed that if they are already
 *  defined then the current definition is correct.
 */
#ifndef NULL
#  ifdef __cplusplus
#    define NULL        (0)
#  else /* !__cplusplus */
#    define NULL        ((void *)0)
#  endif /* !__cplusplus */
#endif

#ifndef	FALSE
#define	FALSE	(0)
#endif

#ifndef	TRUE
#define	TRUE	(!(0))
#endif

namespace aero
{
	typedef intptr_t nint_t;
	typedef uintptr_t nuint_t;
	typedef intptr_t ssize_t;
	typedef uintptr_t size_t;

	typedef nint_t nbool_t;

	typedef char   char_t;
	typedef short  short_t;
	typedef long   long_t;
	typedef int    int_t;

	typedef unsigned char   uchar_t;
	typedef unsigned short  ushort_t;
	typedef unsigned long   ulong_t;
	typedef unsigned int    uint_t;

	typedef float   float_t;
	typedef double  double_t;

	typedef boost::int8_t s8;
	typedef boost::uint8_t u8;
	typedef boost::int16_t s16;
	typedef boost::uint16_t u16;
	typedef boost::int32_t s32;
	typedef boost::uint32_t u32;
	typedef boost::int64_t s64;
	typedef boost::uint64_t u64;
}

#if defined(__GNUC__)  && __GNUC__ >= 4
#  define AERO_STRUCT_OFFSET(struct_type, member) \
      ((intptr_t) offsetof (struct_type, member))
#else
#  define AERO_STRUCT_OFFSET(struct_type, member)	\
      ((intptr_t) ((boost::uint8_t*) &((struct_type*) 0)->member))
#endif

#define AERO_STRUCT_MEMBER_P(struct_p, struct_offset)   \
    ((boost::uint8_t*) (struct_p) + (aero::long_t) (struct_offset))
#define AERO_STRUCT_MEMBER(member_type, struct_p, struct_offset)   \
    (*(member_type*) AERO_STRUCT_MEMBER_P ((struct_p), (struct_offset)))

#define AERO_R64(x) (*(volatile boost::uint64_t *)(x))
#define AERO_R32(x) (*(volatile boost::uint32_t *)(x))
#define AERO_R16(x) (*(volatile boost::uint16_t *)(x))
#define AERO_R8(x) (*(volatile boost::uint8_t *)(x))

#endif // AERO_BASIC_TYPE_H_INCLUDED


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * 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.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE floatingpointcomparison_test

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/tools/floatingpointcomparison.h"

using namespace std;
using namespace votca::tools;

BOOST_AUTO_TEST_SUITE(floatingpointcomparison_test)

BOOST_AUTO_TEST_CASE(comparison_test) {
  double var1 = 1.000;
  double var2 = 1.010;
  BOOST_CHECK(isApproximatelyEqual(var1, var2, 0.1));
  BOOST_CHECK(isApproximatelyEqual(var1, var2, 0.01));
  BOOST_CHECK(!isApproximatelyEqual(var1, var2, 0.001));
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/transform_width.hpp>
#include <boost/asio/basic_streambuf.hpp>
#include <boost/asio/buffers_iterator.hpp>
#include <boost/crc.hpp>
#include <sstream>
#include <string>

namespace util {
static uint16_t hex_to_dec(char *hex_c) {
  char hex_value[5];
  hex_value[4] = '\0';
  memcpy(hex_value, hex_c, 4);

  uint16_t decimal_value;
  std::stringstream ss;
  ss << std::string(hex_value);
  ss >> std::hex >> decimal_value;
  return decimal_value;
};

static void dec_to_hex(uint16_t decimal, char *hex_c) {
  std::stringstream ss;
  std::string hex;
  ss << std::hex << decimal;
  ss >> hex;
  hex.copy(hex_c + 4 - hex.size(), hex.size());
  std::string("0000").copy(hex_c, 4 - hex.size());
}

static uint16_t calc_crc(char *data, size_t data_len) {
  boost::crc_optimal<16, 0x1021, 0xFFFF, 0, false, false> crc_ccitt;
  crc_ccitt = std::for_each(data, data + data_len, crc_ccitt);
  return crc_ccitt();
};

static int base64_encode(const std::string &input, std::string &output) {
  using namespace boost::archive::iterators;
  std::stringstream result;
  try {
    copy(base64_from_binary<transform_width<std::string::const_iterator, 6, 8>>(
             input.begin()),
         base64_from_binary<transform_width<std::string::const_iterator, 6, 8>>(
             input.end()),
         std::ostream_iterator<char>(result));
  } catch (...) {
    return -1;
  }

  size_t num = (3 - input.length() % 3) % 3;
  for (size_t i = 0; i < num; i++) {
    result.put('=');
  }
  output = result.str();
  return 0;
};

static int base64_decode(const std::string &input, std::string &output) {
  using namespace boost::archive::iterators;
  std::stringstream result;
  try {
    copy(transform_width<binary_from_base64<std::string::const_iterator>, 8, 6>(
             input.begin()),
         transform_width<binary_from_base64<std::string::const_iterator>, 8, 6>(
             input.end()),
         std::ostream_iterator<char>(result));
  } catch (...) {
    return -1;
  }
  output = result.str();
  return 0;
};
};  // namespace util


#pragma once
#include <Eigen/Dense>
#include <memory>
#include <tuple>
#include <unordered_map>
#include <utility>
#include <vector>

#include "../datastructures/multi_tree_vector.hpp"
#include "basis.hpp"
#include "sparse_vector.hpp"

namespace Time {

template <template <typename, typename> class Operator, typename I_in,
          typename I_out>
class BilinearForm {
 public:
  using WaveletBasisIn =
      std::remove_pointer_t<std::tuple_element_t<0, typename I_in::TupleNodes>>;
  using WaveletBasisOut = std::remove_pointer_t<
      std::tuple_element_t<0, typename I_out::TupleNodes>>;
  using ScalingBasisIn = typename FunctionTrait<WaveletBasisIn>::Scaling;
  using ScalingBasisOut = typename FunctionTrait<WaveletBasisOut>::Scaling;

  // Create a stateful BilinearForm.
  BilinearForm(I_in *root_vec_in, I_out *root_vec_out);

  void Apply() {
    InitializeInput();
    auto [_, f] = ApplyRecur(0, {}, {});
    FinalizeOutput(f);
  }

  void ApplyUpp() {
    InitializeInput();
    auto [_, f] = ApplyUppRecur(0, {}, {});
    FinalizeOutput(f);
  }

  void ApplyLow() {
    InitializeInput();
    auto f = ApplyLowRecur(0, {});
    FinalizeOutput(f);
  }

  auto Transpose() const {
    auto transpose = BilinearForm<Operator, I_out, I_in>();
    transpose.vec_in_ = vec_out_;
    transpose.vec_out_ = vec_in_;
    transpose.nodes_vec_in_ = nodes_vec_out_;
    transpose.nodes_vec_out_ = nodes_vec_in_;
    transpose.InitializeOutput();
    return transpose;
  }

  // Debug function, O(n^2).
  Eigen::MatrixXd ToMatrix();

 protected:
  // Protected constructor, and give transpose operator access.
  BilinearForm() : vec_in_(nullptr), vec_out_(nullptr) {}
  friend BilinearForm<Operator, I_out, I_in>;

  // Roots of the treeviews we are considering.
  I_in *vec_in_;
  I_out *vec_out_;

  // A flattened (levelwise) view of input/output vectors.
  std::shared_ptr<std::vector<std::vector<I_in *>>> nodes_vec_in_;
  std::shared_ptr<std::vector<std::vector<I_out *>>> nodes_vec_out_;

  // Another flattened (levelwise) view of the vectors, in another data format.
  std::vector<SparseVector<WaveletBasisIn>> lvl_vec_in_;
  std::vector<SparseIndices<WaveletBasisOut>> lvl_ind_out_;

  // Helper variables.
  SparseVector<WaveletBasisIn> empty_vec_in_;
  SparseIndices<WaveletBasisOut> empty_ind_out_;

  // Helper function to set the levelwise input/output vector.
  void InitializeOutput();
  void InitializeInput();
  void FinalizeOutput(const SparseVector<WaveletBasisOut> &f);

  // Recursive apply.
  std::pair<SparseVector<ScalingBasisOut>, SparseVector<WaveletBasisOut>>
  ApplyRecur(size_t l, SparseIndices<ScalingBasisOut> &&Pi_out,
             const SparseVector<ScalingBasisIn> &d);

  // Recursive apply upper part.
  std::pair<SparseVector<ScalingBasisOut>, SparseVector<WaveletBasisOut>>
  ApplyUppRecur(size_t l, SparseIndices<ScalingBasisOut> &&Pi_out,
                const SparseVector<ScalingBasisIn> &d);

  // Recursive apply lower part.
  SparseVector<WaveletBasisOut> ApplyLowRecur(
      size_t l, const SparseVector<ScalingBasisIn> &d);

  // Index sets.
  std::pair<SparseIndices<ScalingBasisOut>, SparseIndices<ScalingBasisOut>>
  ConstructPiOut(SparseIndices<ScalingBasisOut> &&Pi_out,
                 bool construct_Pi_A_out = true);

  SparseIndices<ScalingBasisIn> ConstructPiBIn(
      SparseIndices<ScalingBasisIn> &&Pi_in,
      const SparseIndices<ScalingBasisOut> &Pi_B_out);
};

// Helper functions .
template <template <typename, typename> class Operator, typename I_in,
          typename I_out>
BilinearForm<Operator, I_in, I_out> CreateBilinearForm(I_in *root_vec_in,
                                                       I_out *root_vec_out) {
  return BilinearForm<Operator, I_in, I_out>(root_vec_in, root_vec_out);
}

// Helper functions .
template <template <typename, typename> class Operator, typename WaveletBasisIn,
          typename WaveletBasisOut>
BilinearForm<Operator, datastructures::NodeVector<WaveletBasisIn>,
             datastructures::NodeVector<WaveletBasisOut>>
CreateBilinearForm(const datastructures::TreeVector<WaveletBasisIn> &vec_in,
                   const datastructures::TreeVector<WaveletBasisOut> &vec_out) {
  return {vec_in.root(), vec_out.root()};
}

}  // namespace Time

#include "bilinear_form.ipp"


/*-----------------------------------------------------------------------------+
Copyright (c) 2010-2010: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_CONCEPT_INTERVAL_SET_HPP_JOFA_100920
#define BOOST_ICL_CONCEPT_INTERVAL_SET_HPP_JOFA_100920

#include <boost/icl/type_traits/is_combinable.hpp>
#include <boost/icl/type_traits/interval_type_of.hpp>
#include <boost/icl/detail/set_algo.hpp>
#include <boost/icl/detail/interval_set_algo.hpp>
#include <boost/icl/concept/interval.hpp>

namespace boost{ namespace icl
{

//==============================================================================
//= Containedness<IntervalSet>
//==============================================================================
//------------------------------------------------------------------------------
//- bool contains(c T&, c P&) T:{S} P:{e i S} fragment_types
//------------------------------------------------------------------------------
template<class Type>
typename enable_if<is_interval_set<Type>, bool>::type
contains(const Type& super, const typename Type::element_type& element)
{
    return !(icl::find(super, element) == super.end());
}

template<class Type>
typename enable_if<is_interval_set<Type>, bool>::type
contains(const Type& super, const typename Type::segment_type& inter_val)
{
    typedef typename Type::const_iterator const_iterator;
    if(icl::is_empty(inter_val))
        return true;

    std::pair<const_iterator, const_iterator> exterior
        = super.equal_range(inter_val);
    if(exterior.first == exterior.second)
        return false;

    const_iterator last_overlap = cyclic_prior(super, exterior.second);

    return
        icl::contains(hull(*(exterior.first), *last_overlap), inter_val)
    &&  Interval_Set::is_joinable(super, exterior.first, last_overlap);
}

template<class Type, class OperandT>
typename enable_if<has_same_concept<is_interval_set, Type, OperandT>,
                   bool>::type
contains(const Type& super, const OperandT& sub)
{
    return Interval_Set::contains(super, sub);
}

//==============================================================================
//= Addition<IntervalSet>
//==============================================================================
//------------------------------------------------------------------------------
//- T& add(T&, c P&) T:{S} P:{e i} fragment_types
//------------------------------------------------------------------------------
template<class Type>
typename enable_if<is_interval_set<Type>, Type>::type&
add(Type& object, const typename Type::segment_type& operand)
{
    return object.add(operand);
}

template<class Type>
inline typename enable_if<is_interval_set<Type>, Type>::type&
add(Type& object, const typename Type::element_type& operand)
{
    typedef typename Type::segment_type segment_type;
    return icl::add(object, icl::singleton<segment_type>(operand));
}

//------------------------------------------------------------------------------
//- T& add(T&, J, c P&) T:{S} P:{i} interval_type
//------------------------------------------------------------------------------
template<class Type>
inline typename enable_if<is_interval_set<Type>, typename Type::iterator>::type
add(Type& object, typename Type::iterator      prior,
            const typename Type::segment_type& operand)
{
    return object.add(prior, operand);
}

//==============================================================================
//= Insertion<IntervalSet>
//==============================================================================
//------------------------------------------------------------------------------
//- T& insert(T&, c P&) T:{S} P:{e i} fragment_types
//------------------------------------------------------------------------------
template<class Type>
inline typename enable_if<is_interval_set<Type>, Type>::type&
insert(Type& object, const typename Type::segment_type& operand)
{
    return icl::add(object, operand);
}

template<class Type>
inline typename enable_if<is_interval_set<Type>, Type>::type&
insert(Type& object, const typename Type::element_type& operand)
{
    return icl::add(object, operand);
}

//------------------------------------------------------------------------------
//- T& insert(T&, J, c P&) T:{S} P:{i} with hint
//------------------------------------------------------------------------------
template<class Type>
inline typename enable_if<is_interval_set<Type>, typename Type::iterator>::type
insert(Type& object, typename Type::iterator      prior,
               const typename Type::segment_type& operand)
{
    return icl::add(object, prior, operand);
}

//==============================================================================
//= Subtraction<IntervalSet>
//==============================================================================
//------------------------------------------------------------------------------
//- T& subtract(T&, c P&) T:{S} P:{e i} fragment_type
//------------------------------------------------------------------------------
template<class Type>
typename enable_if<is_interval_set<Type>, Type>::type&
subtract(Type& object, const typename Type::segment_type& operand)
{
    return object.subtract(operand);
}

template<class Type>
inline typename enable_if<is_interval_set<Type>, Type>::type&
subtract(Type& object, const typename Type::element_type& operand)
{
    typedef typename Type::segment_type segment_type;
    return icl::subtract(object, icl::singleton<segment_type>(operand));
}

//==============================================================================
//= Erasure<IntervalSet>
//==============================================================================
//------------------------------------------------------------------------------
//- T& erase(T&, c P&) T:{S} P:{e i} fragment_types
//------------------------------------------------------------------------------
template<class Type>
typename enable_if<is_interval_set<Type>, Type>::type&
erase(Type& object, const typename Type::segment_type& minuend)
{
    return icl::subtract(object, minuend);
}

template<class Type>
typename enable_if<is_interval_set<Type>, Type>::type&
erase(Type& object, const typename Type::element_type& minuend)
{
    return icl::subtract(object, minuend);
}

//==============================================================================
//= Intersection
//==============================================================================
//------------------------------------------------------------------------------
//- void add_intersection(T&, c T&, c P&) T:{S} P:{e i} fragment_types
//------------------------------------------------------------------------------
template<class Type>
typename enable_if<is_interval_set<Type>, void>::type
add_intersection(Type& section, const Type& object,
                 const typename Type::element_type& operand)
{
    typedef typename Type::const_iterator const_iterator;
    const_iterator found = icl::find(object, operand);
    if(found != object.end())
        icl::add(section, operand);
}

template<class Type>
typename enable_if<is_interval_set<Type>, void>::type
add_intersection(Type& section, const Type& object,
                 const typename Type::segment_type& segment)
{
    typedef typename Type::const_iterator const_iterator;
    typedef typename Type::iterator       iterator;
    typedef typename Type::interval_type  interval_type;

    if(icl::is_empty(segment))
        return;

    std::pair<const_iterator, const_iterator> exterior
        = object.equal_range(segment);
    if(exterior.first == exterior.second)
        return;

    iterator prior_ = section.end();
    for(const_iterator it_=exterior.first; it_ != exterior.second; it_++)
    {
        interval_type common_interval = key_value<Type>(it_) & segment;
        if(!icl::is_empty(common_interval))
            prior_ = section.insert(prior_, common_interval);
    }
}

//==============================================================================
//= Symmetric difference<IntervalSet>
//==============================================================================
//------------------------------------------------------------------------------
//- T& flip(T&, c P&) T:{S} P:{e i S'} fragment_types
//------------------------------------------------------------------------------
template<class Type>
typename enable_if<is_interval_set<Type>, Type>::type&
flip(Type& object, const typename Type::element_type& operand)
{
    if(icl::contains(object, operand))
        return object -= operand;
    else
        return object += operand;
}

template<class Type>
typename enable_if<is_interval_set<Type>, Type>::type&
flip(Type& object, const typename Type::segment_type& segment)
{
    typedef typename Type::const_iterator const_iterator;
    typedef typename Type::interval_type  interval_type;
    // That which is common shall be subtracted
    // That which is not shall be added
    // So x has to be 'complementary added' or flipped
    interval_type span = segment;
    std::pair<const_iterator, const_iterator> exterior
        = object.equal_range(span);

    const_iterator fst_ = exterior.first;
    const_iterator end_ = exterior.second;

    interval_type covered, left_over;
    const_iterator it_ = fst_;
    while(it_ != end_)
    {
        covered = *it_++;
        //[a      ...  : span
        //     [b ...  : covered
        //[a  b)       : left_over
        left_over = right_subtract(span, covered);
        icl::subtract(object, span & covered); //That which is common shall be subtracted
        icl::add(object, left_over);           //That which is not shall be added

        //...      d) : span
        //... c)      : covered
        //     [c  d) : span'
        span = left_subtract(span, covered);
    }

    //If span is not empty here, it_ is not in the set so it_ shall be added
    icl::add(object, span);
    return object;
}

template<class Type, class OperandT>
typename enable_if<is_concept_compatible<is_interval_set, Type, OperandT>, Type>::type&
flip(Type& object, const OperandT& operand)
{
    typedef typename OperandT::const_iterator const_iterator;

    if(operand.empty())
        return object;

    const_iterator common_lwb, common_upb;

    if(!Set::common_range(common_lwb, common_upb, operand, object))
        return object += operand;

    const_iterator it_ = operand.begin();

    // All elements of operand left of the common range are added
    while(it_ != common_lwb)
        icl::add(object, *it_++);
    // All elements of operand in the common range are symmertrically subtracted
    while(it_ != common_upb)
        icl::flip(object, *it_++);
    // All elements of operand right of the common range are added
    while(it_ != operand.end())
        icl::add(object, *it_++);

    return object;
}

//==============================================================================
//= Set selection
//==============================================================================
template<class Type>
typename enable_if<is_interval_set<Type>, Type>::type&
domain(Type& dom, const Type& object)
{
    typedef typename Type::const_iterator const_iterator;
    typedef typename Type::iterator       iterator;
    dom.clear();
    const_iterator it_    = object.begin();
    iterator       prior_ = dom.end();

    while(it_ != object.end())
        prior_ = icl::insert(dom, prior_, *it_++);

    return dom;
}

template<class Type>
typename enable_if<is_interval_set<Type>, Type>::type&
between(Type& in_between, const Type& object)
{
    typedef typename Type::const_iterator const_iterator;
    typedef typename Type::iterator       iterator;
    in_between.clear();
    const_iterator it_ = object.begin(), pred_;
    iterator prior_ = in_between.end();

    if(it_ != object.end())
        pred_ = it_++;

    while(it_ != object.end())
        prior_ = icl::insert(in_between, prior_,
                             icl::between(*pred_++, *it_++));

    return in_between;
}

//==============================================================================
//= Streaming
//==============================================================================
template<class CharType, class CharTraits, class Type>
typename enable_if<is_interval_set<Type>,
                   std::basic_ostream<CharType, CharTraits> >::type&
operator << (std::basic_ostream<CharType, CharTraits>& stream, const Type& object)
{
    stream << "{";
    ICL_const_FORALL(typename Type, it_, object)
        stream << (*it_);

    return stream << "}";
}

}} // namespace boost icl

#endif


#pragma once

#include <array>
#include <boost/mpl/if.hpp>
#include <vector>
#include <Eigen/Dense>
#include <type_traits>

namespace boltzmann {
template <int DIM>
class Quadrature
{
 public:
  const static unsigned int dim = DIM;
  typedef typename boost::mpl::if_c<DIM == 1, double, std::array<double, DIM> >::type coord_type;

 protected:
  typedef std::vector<coord_type> v_pts_t;
  typedef std::vector<double> v_wts_t;
  typedef Eigen::Map<const Eigen::VectorXd> mvec_t;

 public:
  Quadrature(int size)
      : pts_(size)
      , wts_(size)
  { /* empty */
  }

  Quadrature(const v_pts_t& pts, const v_wts_t& wts)
      : pts_(pts)
      , wts_(wts)
  { /* empty */
  }

  Quadrature()
      : pts_(0)
      , wts_(0)
  { /* empty */
  }

  const coord_type* points_data() const { return pts_.data(); }
  const double* weights_data() const { return wts_.data(); }

  const double& wts(unsigned int i) const { return wts_[i]; }
  const std::vector<double>& wts() const { return wts_; }
  const std::vector<coord_type>& pts() const { return pts_; }
  const coord_type& pts(unsigned int i) const { return pts_[i]; }
  template<int D>
  typename std::enable_if<D == 1, const mvec_t>::type vpts() const
  {
    return Eigen::Map<const Eigen::VectorXd>(pts_.data(), pts_.size());
  }

  template<int D>
  typename std::enable_if<D == 1, mvec_t>::type vwts() const
  {
    return Eigen::Map<const Eigen::VectorXd>(wts_.data(), wts_.size());
  }

  unsigned int size() const { return pts_.size(); }

 protected:
  v_pts_t pts_;
  v_wts_t wts_;
};

} // namespace boltzmann


#include <boost/graph/matrix_as_graph.hpp>


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 3, 2> test_double_3_2(const Eigen::Matrix<double, 3, 2> & M)
{
	return M;
}
void export_double_3_2()
{
	boost::python::def("test_double_3_2",test_double_3_2);
}


/* ----------------------------------------------------------------------------
 * Copyright 2018, Ross Hartley <m.ross.hartley@gmail.com>
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 *  @file   large_state_speed.cpp
 *  @author Ross Hartley
 *  @brief  Tests the speed of the InEKF propagation and correction algorithms when the state is large
 *  @date   September 25, 2018
 **/

#include <iostream>
#include <chrono>
#include <Eigen/Dense>
#include "inekf/inekf.hpp"

#define DT_MIN 1e-6
#define DT_MAX 1

using namespace std;
using namespace inekf;

int main() {
    // Initialize filter
    InEKF filter;
    cout << "Robot's state is initialized to: \n";
    cout << filter.getState() << endl;

    const int NUM_FRAMES = 100;
     chrono::high_resolution_clock::time_point start_time = chrono::high_resolution_clock::now();
    for (int i=0; i<NUM_FRAMES; ++i) {
        cout << "Frame: " << i << endl;

        // Add lots of landmarks
        const int NUM_LANDMARKS = 30;
        vectorLandmarks measured_landmarks;
        Eigen::Matrix3d cov = 0.01*Eigen::Matrix3d::Identity();
        for (int i=0; i<NUM_LANDMARKS; ++i) {
            int id = i;
            Eigen::Vector3d p;
            p << i,i,i;
            Landmark landmark(id,p,cov);
            measured_landmarks.push_back(landmark);
        }
        filter.CorrectLandmarks(measured_landmarks);
        // cout << filter.getState() << endl;

        // Integrate IMU data
        const int NUM_IMU = 30;
        double dt = 0.01;
        Eigen::Matrix<double,6,1> imu;
        for (int i=0; i<NUM_IMU; ++i) {
            imu << 1,2,3,4,5,6;
            filter.Propagate(imu, dt);
        }
        // cout << filter.getState() << endl;
    }
    chrono::high_resolution_clock::time_point end_time = chrono::high_resolution_clock::now();
    auto duration = chrono::duration_cast<chrono::seconds>( end_time - start_time ).count();
    cout << "average frequency: " << 1.0/(double(duration)/double(NUM_FRAMES)) << " Hz" << endl;

    return 0;
}


#ifndef MBA_MBA_HPP
#define MBA_MBA_HPP

/*
The MIT License

Copyright (c) 2015 Denis Demidov <dennis.demidov@gmail.com>

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

/**
 * \file   mba/mba.hpp
 * \author Denis Demidov <dennis.demidov@gmail.com>
 * \brief  Multilevel B-spline interpolation.
 */

#include <iostream>
#include <iomanip>
#include <map>
#include <list>
#include <utility>
#include <algorithm>
#include <array>
#include <memory>
#include <functional>

#include <boost/container/flat_map.hpp>
#include <boost/multi_array.hpp>
#include <type_traits>
#include <boost/io/ios_state.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>

namespace mba {
namespace detail {

template <size_t N, size_t M>
struct power : std::integral_constant<size_t, N * power<N, M-1>::value> {};

template <size_t N>
struct power<N, 0> : std::integral_constant<size_t, 1> {};

/// N-dimensional grid iterator (nested loop with variable depth).
template <unsigned NDim>
class grid_iterator {
    public:
        typedef std::array<size_t, NDim> index;

        explicit grid_iterator(const std::array<size_t, NDim> &dims)
            : N(dims), idx(0)
        {
            std::fill(i.begin(), i.end(), 0);
            done = (i == N);
        }

        explicit grid_iterator(size_t dim) : idx(0) {
            std::fill(N.begin(), N.end(), dim);
            std::fill(i.begin(), i.end(), 0);
            done = (0 == dim);
        }

        size_t operator[](size_t d) const {
            return i[d];
        }

        const index& operator*() const {
            return i;
        }

        size_t position() const {
            return idx;
        }

        grid_iterator& operator++() {
            done = true;
            for(size_t d = NDim; d--; ) {
                if (++i[d] < N[d]) {
                    done = false;
                    break;
                }
                i[d] = 0;
            }

            ++idx;

            return *this;
        }

        operator bool() const { return !done; }

    private:
        index N, i;
        bool  done;
        size_t idx;
};

template <typename T, size_t N>
std::array<T, N> operator+(std::array<T, N> a, const std::array<T, N> &b) {
    std::transform(a.begin(), a.end(), b.begin(), a.begin(), std::plus<T>());
    return a;
}

template <typename T, size_t N>
std::array<T, N> operator-(std::array<T, N> a, T b) {
    std::transform(a.begin(), a.end(), a.begin(), std::bind2nd(std::minus<T>(), b));
    return a;
}

template <typename T, size_t N>
std::array<T, N> operator*(std::array<T, N> a, T b) {
    std::transform(a.begin(), a.end(), a.begin(), std::bind2nd(std::multiplies<T>(), b));
    return a;
}

// Value of k-th B-Spline basic function at t.
inline double Bspline(size_t k, double t) {
    assert(0 <= t && t < 1);
    assert(k < 4);

    switch (k) {
        case 0:
            return (t * (t * (-t + 3) - 3) + 1) / 6;
        case 1:
            return (t * t * (3 * t - 6) + 4) / 6;
        case 2:
            return (t * (t * (-3 * t + 3) + 3) + 1) / 6;
        case 3:
            return t * t * t / 6;
        default:
            return 0;
    }
}

// Checks if p is between lo and hi
template <typename T, size_t N>
bool boxed(const std::array<T,N> &lo, const std::array<T,N> &p, const std::array<T,N> &hi) {
    for(unsigned i = 0; i < N; ++i) {
        if (p[i] < lo[i] || p[i] > hi[i]) return false;
    }
    return true;
}

inline double safe_divide(double a, double b) {
    return b == 0.0 ? 0.0 : a / b;
}

template <unsigned NDim>
class control_lattice {
    public:
        typedef std::array<size_t, NDim> index;
        typedef std::array<double, NDim> point;

        virtual ~control_lattice() {}

        virtual double operator()(const point &p) const = 0;

        virtual void report(std::ostream&) const = 0;

        template <class CooIter, class ValIter>
        double residual(CooIter coo_begin, CooIter coo_end, ValIter val_begin) const {
            double res = 0.0;

            CooIter p = coo_begin;
            ValIter v = val_begin;

            for(; p != coo_end; ++p, ++v) {
                (*v) -= (*this)(*p);
                res = std::max(res, std::abs(*v));
            }

            return res;
        }
};

template <unsigned NDim>
class initial_approximation : public control_lattice<NDim> {
    public:
        typedef typename control_lattice<NDim>::point point;

        initial_approximation(std::function<double(const point&)> f)
            : f(f) {}

        double operator()(const point &p) const {
            return f(p);
        }

        void report(std::ostream &os) const {
            os << "initial approximation";
        }
    private:
        std::function<double(const point&)> f;
};

template <unsigned NDim>
class control_lattice_dense : public control_lattice<NDim> {
    public:
        typedef typename control_lattice<NDim>::index index;
        typedef typename control_lattice<NDim>::point point;

        template <class CooIter, class ValIter>
        control_lattice_dense(
                const point &coo_min, const point &coo_max, index grid_size,
                CooIter coo_begin, CooIter coo_end, ValIter val_begin
                ) : cmin(coo_min), cmax(coo_max), grid(grid_size)
        {
            for(unsigned i = 0; i < NDim; ++i) {
                hinv[i] = (grid[i] - 1) / (cmax[i] - cmin[i]);
                cmin[i] -= 1 / hinv[i];
                grid[i] += 2;
            }

            boost::multi_array<double, NDim> delta(grid);
            boost::multi_array<double, NDim> omega(grid);

            std::fill(delta.data(), delta.data() + delta.num_elements(), 0.0);
            std::fill(omega.data(), omega.data() + omega.num_elements(), 0.0);

            CooIter p = coo_begin;
            ValIter v = val_begin;

            for(; p != coo_end; ++p, ++v) {
                if (!boxed(coo_min, *p, coo_max)) continue;

                index i;
                point s;

                for(unsigned d = 0; d < NDim; ++d) {
                    double u = ((*p)[d] - cmin[d]) * hinv[d];
                    i[d] = floor(u) - 1;
                    s[d] = u - floor(u);
                }

                std::array< double, power<4, NDim>::value > w;
                double sum_w2 = 0.0;

                for(grid_iterator<NDim> d(4); d; ++d) {
                    double prod = 1.0;
                    for(unsigned k = 0; k < NDim; ++k) prod *= Bspline(d[k], s[k]);

                    w[d.position()] = prod;
                    sum_w2 += prod * prod;
                }

                for(grid_iterator<NDim> d(4); d; ++d) {
                    double w1  = w[d.position()];
                    double w2  = w1 * w1;
                    double phi = (*v) * w1 / sum_w2;

                    index j = i + (*d);

                    delta(j) += w2 * phi;
                    omega(j) += w2;
                }
            }

            phi.resize(grid);

            std::transform(
                    delta.data(), delta.data() + delta.num_elements(),
                    omega.data(), phi.data(), safe_divide
                    );
        }

        double operator()(const point &p) const {
            index i;
            point s;

            for(unsigned d = 0; d < NDim; ++d) {
                double u = (p[d] - cmin[d]) * hinv[d];
                i[d] = floor(u) - 1;
                s[d] = u - floor(u);
            }

            double f = 0;

            for(grid_iterator<NDim> d(4); d; ++d) {
                double w = 1.0;
                for(unsigned k = 0; k < NDim; ++k) w *= Bspline(d[k], s[k]);

                f += w * phi(i + (*d));
            }

            return f;
        }

        void report(std::ostream &os) const {
            boost::io::ios_all_saver stream_state(os);

            os << "dense  [" << grid[0];
            for(unsigned i = 1; i < NDim; ++i)
                os << ", " << grid[i];
            os << "] (" << phi.num_elements() * sizeof(double) << " bytes)";
        }

        void append_refined(const control_lattice_dense &r) {
            static const std::array<double, 5> s = {
                0.125, 0.500, 0.750, 0.500, 0.125
            };

            for(grid_iterator<NDim> i(r.grid); i; ++i) {
                double f = r.phi(*i);

                if (f == 0.0) continue;

                for(grid_iterator<NDim> d(5); d; ++d) {
                    index j;
                    bool skip = false;
                    for(unsigned k = 0; k < NDim; ++k) {
                        j[k] = 2 * i[k] + d[k] - 3;
                        if (j[k] >= grid[k]) {
                            skip = true;
                            break;
                        }
                    }

                    if (skip) continue;

                    double c = 1.0;
                    for(unsigned k = 0; k < NDim; ++k) c *= s[d[k]];

                    phi(j) += f * c;
                }
            }
        }

        double fill_ratio() const {
            size_t total    = phi.num_elements();
            size_t nonzeros = total - std::count(phi.data(), phi.data() + total, 0.0);

            return static_cast<double>(nonzeros) / total;
        }

    private:
        point cmin, cmax, hinv;
        index grid;

        boost::multi_array<double, NDim> phi;

};

template <unsigned NDim>
class control_lattice_sparse : public control_lattice<NDim> {
    public:
        typedef typename control_lattice<NDim>::index index;
        typedef typename control_lattice<NDim>::point point;

        template <class CooIter, class ValIter>
        control_lattice_sparse(
                const point &coo_min, const point &coo_max, index grid_size,
                CooIter coo_begin, CooIter coo_end, ValIter val_begin
                ) : cmin(coo_min), cmax(coo_max), grid(grid_size)
        {
            for(unsigned i = 0; i < NDim; ++i) {
                hinv[i] = (grid[i] - 1) / (cmax[i] - cmin[i]);
                cmin[i] -= 1 / hinv[i];
                grid[i] += 2;
            }

            std::map<index, two_doubles> dw;

            CooIter p = coo_begin;
            ValIter v = val_begin;

            for(; p != coo_end; ++p, ++v) {
                if (!boxed(coo_min, *p, coo_max)) continue;

                index i;
                point s;

                for(unsigned d = 0; d < NDim; ++d) {
                    double u = ((*p)[d] - cmin[d]) * hinv[d];
                    i[d] = floor(u) - 1;
                    s[d] = u - floor(u);
                }

                std::array< double, power<4, NDim>::value > w;
                double sum_w2 = 0.0;

                for(grid_iterator<NDim> d(4); d; ++d) {
                    double prod = 1.0;
                    for(unsigned k = 0; k < NDim; ++k) prod *= Bspline(d[k], s[k]);

                    w[d.position()] = prod;
                    sum_w2 += prod * prod;
                }

                for(grid_iterator<NDim> d(4); d; ++d) {
                    double w1  = w[d.position()];
                    double w2  = w1 * w1;
                    double phi = (*v) * w1 / sum_w2;

                    two_doubles delta_omega = {w2 * phi, w2};

                    append(dw[i + (*d)], delta_omega);
                }
            }

            phi.insert(//boost::container::ordered_unique_range,
                    boost::make_transform_iterator(dw.begin(), delta_over_omega),
                    boost::make_transform_iterator(dw.end(),   delta_over_omega)
                    );
        }

        double operator()(const point &p) const {
            index i;
            point s;

            for(unsigned d = 0; d < NDim; ++d) {
                double u = (p[d] - cmin[d]) * hinv[d];
                i[d] = floor(u) - 1;
                s[d] = u - floor(u);
            }

            double f = 0;

            for(grid_iterator<NDim> d(4); d; ++d) {
                double w = 1.0;
                for(unsigned k = 0; k < NDim; ++k) w *= Bspline(d[k], s[k]);

                f += w * get_phi(i + (*d));
            }

            return f;
        }

        void report(std::ostream &os) const {
            boost::io::ios_all_saver stream_state(os);

            size_t grid_size = grid[0];

            os << "sparse [" << grid[0];
            for(unsigned i = 1; i < NDim; ++i) {
                os << ", " << grid[i];
                grid_size *= grid[i];
            }

            size_t bytes = phi.size() * sizeof(std::pair<index, double>);
            size_t dense_bytes = grid_size * sizeof(double);

            double compression = static_cast<double>(bytes) / dense_bytes;
            os << "] (" << bytes << " bytes, compression: "
                << std::fixed << std::setprecision(2) << compression << ")";
        }
    private:
        point cmin, cmax, hinv;
        index grid;

        typedef boost::container::flat_map<index, double> sparse_grid;
        sparse_grid phi;

        typedef std::array<double, 2> two_doubles;

        static std::pair<index, double> delta_over_omega(const std::pair<index, two_doubles> &dw) {
            return std::make_pair(dw.first, safe_divide(dw.second[0], dw.second[1]));
        }

        static void append(two_doubles &a, const two_doubles &b) {
            std::transform(a.begin(), a.end(), b.begin(), a.begin(), std::plus<double>());
        }

        double get_phi(const index &i) const {
            typename sparse_grid::const_iterator c = phi.find(i);
            return c == phi.end() ? 0.0 : c->second;
        }
};

} // namespace detail

template <unsigned NDim>
class linear_approximation {
    public:
        typedef typename detail::control_lattice<NDim>::point point;

        template <class CooIter, class ValIter>
        linear_approximation(CooIter coo_begin, CooIter coo_end, ValIter val_begin)
        {
            namespace ublas = boost::numeric::ublas;

            size_t n = std::distance(coo_begin, coo_end);

            if (n <= NDim) {
                // Not enough points to get a unique plane
                std::fill(C.begin(), C.end(), 0.0);
                C[NDim] = std::accumulate(val_begin, val_begin + n, 0.0) / n;
                return;
            }

            ublas::matrix<double> A(NDim+1, NDim+1); A.clear();
            ublas::vector<double> f(NDim+1);         f.clear();

            CooIter p = coo_begin;
            ValIter v = val_begin;

            double sum_val = 0.0;

            // Solve least-squares problem to get approximation with a plane.
            for(; p != coo_end; ++p, ++v, ++n) {
                std::array<double, NDim+1> x;
                std::copy(p->begin(), p->end(), boost::begin(x));
                x[NDim] = 1.0;

                for(unsigned i = 0; i <= NDim; ++i) {
                    for(unsigned j = 0; j <= NDim; ++j) {
                        A(i,j) += x[i] * x[j];
                    }
                    f(i) += x[i] * (*v);
                }

                sum_val += (*v);
            }

            ublas::permutation_matrix<size_t> pm(NDim+1);
            ublas::lu_factorize(A, pm);

            bool singular = false;
            for(unsigned i = 0; i <= NDim; ++i) {
                if (A(i,i) == 0.0) {
                    singular = true;
                    break;
                }
            }

            if (singular) {
                std::fill(C.begin(), C.end(), 0.0);
                C[NDim] = sum_val / n;
            } else {
                ublas::lu_substitute(A, pm, f);
                for(unsigned i = 0; i <= NDim; ++i) C[i] = f(i);
            }
        }

        double operator()(const point &p) const {
            double f = C[NDim];

            for(unsigned i = 0; i < NDim; ++i)
                f += C[i] * p[i];

            return f;
        }
    private:
        std::array<double, NDim+1> C;
};

template <unsigned NDim>
class MBA {
    public:
        typedef std::array<size_t, NDim> index;
        typedef std::array<double, NDim> point;

        template <class CooIter, class ValIter>
        MBA(
                const point &coo_min, const point &coo_max, index grid,
                CooIter coo_begin, CooIter coo_end, ValIter val_begin,
                unsigned max_levels = 8, double tol = 1e-8, double min_fill = 0.5,
                std::function<double(point)> initial = std::function<double(point)>()
           )
        {
            init(
                    coo_min, coo_max, grid,
                    coo_begin, coo_end, val_begin,
                    max_levels, tol, min_fill, initial
                );
        }

        template <class CooRange, class ValRange>
        MBA(
                const point &coo_min, const point &coo_max, index grid,
                CooRange coo, ValRange val,
                unsigned max_levels = 8, double tol = 1e-8, double min_fill = 0.5,
                std::function<double(point)> initial = std::function<double(point)>()
           )
        {
            init(
                    coo_min, coo_max, grid,
                    boost::begin(coo), boost::end(coo), boost::begin(val),
                    max_levels, tol, min_fill, initial
                );
        }

        double operator()(const point &p) const {
            double f = 0.0;

            for(const auto &psi : cl) {
                f += (*psi)(p);
            }

            return f;
        }

        friend std::ostream& operator<<(std::ostream &os, const MBA &h) {
            size_t level = 0;
            for(const auto &psi : h.cl) {
                os << "level " << ++level << ": ";
                psi->report(os);
                os << std::endl;
            }
            return os;
        }

    private:
        typedef detail::control_lattice<NDim>        lattice;
        typedef detail::initial_approximation<NDim>  initial_approximation;
        typedef detail::control_lattice_dense<NDim>  dense_lattice;
        typedef detail::control_lattice_sparse<NDim> sparse_lattice;

        std::list< std::shared_ptr<lattice> > cl;

        template <class CooIter, class ValIter>
        void init(
                const point &cmin, const point &cmax, index grid,
                CooIter coo_begin, CooIter coo_end, ValIter val_begin,
                unsigned max_levels, double tol, double min_fill,
                std::function<double(point)> initial
                )
        {
            using namespace mba::detail;

            const ptrdiff_t n = std::distance(coo_begin, coo_end);
            std::vector<double> val(val_begin, val_begin + n);

            double res, eps = 0.0;
            for(ptrdiff_t i = 0; i < n; ++i)
                eps = std::max(eps, std::abs(val[i]));
            eps *= tol;

            if (initial) {
                // Start with the given approximation.
                cl.push_back(std::make_shared<initial_approximation>(initial));
                res = cl.back()->residual(coo_begin, coo_end, val.begin());
                if (res <= eps) return;
            }

            size_t lev = 1;
            // Create dense head of the hierarchy.
            {
                auto psi = std::make_shared<dense_lattice>(
                        cmin, cmax, grid, coo_begin, coo_end, val.begin());

                res = psi->residual(coo_begin, coo_end, val.begin());
                double fill = psi->fill_ratio();

                for(; (lev < max_levels) && (res > eps) && (fill > min_fill); ++lev) {
                    grid = grid * 2ul - 1ul;

                    auto f = std::make_shared<dense_lattice>(
                            cmin, cmax, grid, coo_begin, coo_end, val.begin());

                    res = f->residual(coo_begin, coo_end, val.begin());
                    fill = f->fill_ratio();

                    f->append_refined(*psi);
                    psi.swap(f);
                }

                cl.push_back(psi);
            }

            // Create sparse tail of the hierrchy.
            for(; (lev < max_levels) && (res > eps); ++lev) {
                grid = grid * 2ul - 1ul;

                cl.push_back(std::make_shared<sparse_lattice>(
                        cmin, cmax, grid, coo_begin, coo_end, val.begin()));

                res = cl.back()->residual(coo_begin, coo_end, val.begin());
            }
        }
};

} // namespace mba

#endif


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

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

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

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

#include <boost/make_shared.hpp>
#include <ored/portfolio/builders/equityoption.hpp>
#include <ored/portfolio/enginefactory.hpp>
#include <ored/portfolio/equityoption.hpp>
#include <ored/utilities/log.hpp>
#include <ql/errors.hpp>
#include <ql/exercise.hpp>
#include <ql/instruments/compositeinstrument.hpp>
#include <ql/instruments/vanillaoption.hpp>

using namespace QuantLib;

namespace ore {
namespace data {

void EquityOption::build(const boost::shared_ptr<EngineFactory>& engineFactory) {

    Currency ccy = parseCurrency(currency_);

    QL_REQUIRE(tradeActions().empty(), "TradeActions not supported for EquityOption");

    // Payoff
    Option::Type type = parseOptionType(option_.callPut());
    boost::shared_ptr<StrikedTypePayoff> payoff(new PlainVanillaPayoff(type, strike_));

    // Only European Vanilla supported for now
    QL_REQUIRE(option_.style() == "European", "Option Style unknown: " << option_.style());
    QL_REQUIRE(option_.exerciseDates().size() == 1, "Invalid number of excercise dates");
    Date expiryDate = parseDate(option_.exerciseDates().front());

    // Exercise
    boost::shared_ptr<Exercise> exercise = boost::make_shared<EuropeanExercise>(expiryDate);

    // Vanilla European/American.
    // If price adjustment is necessary we build a simple EU Option
    boost::shared_ptr<QuantLib::Instrument> instrument;

    // QL does not have an EquityOption, so we add a vanilla one here and wrap
    // it in a composite to get the notional in.
    boost::shared_ptr<Instrument> vanilla = boost::make_shared<VanillaOption>(payoff, exercise);

    // we buy foriegn with domestic(=sold ccy).
    boost::shared_ptr<EngineBuilder> builder = engineFactory->builder(tradeType_);
    QL_REQUIRE(builder, "No builder found for " << tradeType_);
    boost::shared_ptr<EquityOptionEngineBuilder> eqOptBuilder =
        boost::dynamic_pointer_cast<EquityOptionEngineBuilder>(builder);

    vanilla->setPricingEngine(eqOptBuilder->engine(eqName_, ccy));

    Position::Type positionType = parsePositionType(option_.longShort());
    Real bsInd = (positionType == QuantLib::Position::Long ? 1.0 : -1.0);
    Real mult = quantity_ * bsInd;

    // If premium data is provided
    // 1) build the fee trade and pass it to the instrument wrapper for pricing
    // 2) add fee payment as additional trade leg for cash flow reporting
    std::vector<boost::shared_ptr<Instrument>> additionalInstruments;
    std::vector<Real> additionalMultipliers;
    if (option_.premiumPayDate() != "" && option_.premiumCcy() != "") {
        Real premiumAmount = -bsInd * option_.premium(); // pay if long, receive if short
        Currency premiumCurrency = parseCurrency(option_.premiumCcy());
        Date premiumDate = parseDate(option_.premiumPayDate());
        addPayment(additionalInstruments, additionalMultipliers, premiumDate, premiumAmount, premiumCurrency, ccy,
                   engineFactory, eqOptBuilder->configuration(MarketContext::pricing));
        DLOG("option premium added for eq option " << id());
    }

    instrument_ = boost::shared_ptr<InstrumentWrapper>(
        new VanillaInstrument(vanilla, mult, additionalInstruments, additionalMultipliers));

    npvCurrency_ = currency_;
    maturity_ = expiryDate;

    // Notional - we really need todays spot to get the correct notional.
    // But rather than having it move around we use strike * quantity
    notional_ = strike_ * quantity_;

    Handle<BlackVolTermStructure> blackVol = engineFactory->market()->equityVol(eqName_);
    LOG("Implied vol for EquityOption on " << eqName_ << " with maturity " << maturity_ << " and strike " << strike_
                                           << " is " << blackVol->blackVol(maturity_, strike_));
}

void EquityOption::fromXML(XMLNode* node) {
    Trade::fromXML(node);
    XMLNode* eqNode = XMLUtils::getChildNode(node, "EquityOptionData");
    QL_REQUIRE(eqNode, "No EquityOptionData Node");
    option_.fromXML(XMLUtils::getChildNode(eqNode, "OptionData"));
    eqName_ = XMLUtils::getChildValue(eqNode, "Name", true);
    currency_ = XMLUtils::getChildValue(eqNode, "Currency", true);
    strike_ = XMLUtils::getChildValueAsDouble(eqNode, "Strike", true);
    quantity_ = XMLUtils::getChildValueAsDouble(eqNode, "Quantity", true);
}

XMLNode* EquityOption::toXML(XMLDocument& doc) {
    XMLNode* node = Trade::toXML(doc);
    XMLNode* eqNode = doc.allocNode("EquityOptionData");
    XMLUtils::appendNode(node, eqNode);

    XMLUtils::appendNode(eqNode, option_.toXML(doc));
    XMLUtils::addChild(doc, eqNode, "Name", eqName_);
    XMLUtils::addChild(doc, eqNode, "Currency", currency_);
    XMLUtils::addChild(doc, eqNode, "Strike", strike_);
    XMLUtils::addChild(doc, eqNode, "Quantity", quantity_);

    return node;
}
} // namespace data
} // namespace ore


#pragma once
#ifndef BODYESTIMATOR_H
#define BODYESTIMATOR_H

#include <Eigen/Dense>
#include <vector>
#include "ros/ros.h"
#include "utils/cheetah_data_t.hpp"
#include "system/cheetah_state.hpp"
#include "InEKF.h"
#include <lcm/lcm-cpp.hpp>
#include "communication/lcm-types/cheetah_inekf_lcm/pose_t.hpp"
#include "geometry_msgs/Point.h"
#include "nav_msgs/Path.h"
// #include "visualization_msgs/MarkerArray.h"
// #include "inekf_msgs/State.h"

class BodyEstimator {

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        BodyEstimator(lcm::LCM* lcm);
        bool enabled();
        void enableFilter();
        void disable();
        bool biasInitialized();
        void initBias(cheetah_lcm_packet_t& cheetah_data);
        // void initState();
        void initState(const double t, const cheetah_lcm_packet_t& cheetah_data, const CheetahState& state);
        void setContacts(CheetahState& state);
        void update(cheetah_lcm_packet_t& cheetah_data, CheetahState& state);
        void correctKinematics(CheetahState& state);
        inekf::InEKF getFilter() const;
        inekf::RobotState getState() const;
        void publishMarkers(double time, std::string map_frame_id, uint32_t seq);
        void publishPath();
        void publishPose(double time, std::string map_frame_id, uint32_t seq);

    private:
        // LCM related
        lcm::LCM* lcm_;
        std::string LCM_POSE_CHANNEL;
        // ROS related
        ros::Publisher visualization_pub_;
        std::vector<geometry_msgs::PoseStamped> poses_;
        // inekf related
        inekf::InEKF filter_;
        bool enabled_ = false;
        bool bias_initialized_ = false;
        bool static_bias_initialization_ = false;
        bool estimator_debug_enabled_ = false;
        bool lcm_publish_visualization_markers_ = false;
        std::vector<Eigen::Matrix<double,6,1>,Eigen::aligned_allocator<Eigen::Matrix<double,6,1>>> bias_init_vec_;
        Eigen::Vector3d bg0_ = Eigen::Vector3d::Zero();
        Eigen::Vector3d ba0_ = Eigen::Vector3d::Zero();
        double t_prev_;
        uint32_t seq_;
        Eigen::Matrix<double,6,1> imu_prev_;
        const Eigen::Matrix<double,12,12> encoder_cov_ = 0.0174533*0.0174533 * Eigen::Matrix<double,12,12>::Identity(); // 1 deg std dev
        const Eigen::Matrix<double,3,3> prior_kinematics_cov_ = 0.05*0.05 * Eigen::Matrix<double,3,3>::Identity(); // 5 cm std Adds to FK covariance
};

#endif // BODYESTIMATOR_H