// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <boost/preprocessor/arithmetic/dec.hpp>
#include <boost/preprocessor/arithmetic/inc.hpp>
#include <boost/preprocessor/control/expr_iif.hpp>
#include <boost/preprocessor/list/adt.hpp>
#include <boost/preprocessor/repetition/for.hpp>
#include <boost/preprocessor/repetition/repeat.hpp>
#include <boost/preprocessor/tuple/to_list.hpp>
#include <limits>
#include <pup.h>
#include <vector>

#include "DataStructures/Tensor/TypeAliases.hpp"
#include "Options/Options.hpp"
#include "Parallel/CharmPupable.hpp"
#include "PointwiseFunctions/Hydro/EquationsOfState/EquationOfState.hpp"  // IWYU pragma: keep
#include "Utilities/Math.hpp"
#include "Utilities/TMPL.hpp"

/// \cond
class DataVector;
/// \endcond

// IWYU pragma: no_forward_declare Tensor

namespace EquationsOfState {
/*!
 * \ingroup EquationsOfStateGroup
 * \brief A spectral equation of state
 *
 * This equation of state is determined as a function of \f$x =
 * \ln(\rho/\rho_0)\f$ where \f$\rho\f$ is the rest mass density and
 * \f$\rho_0\f$ is the provided reference density.  The adiabatic
 * index \f$\Gamma(x)\f$ is defined such that
 * \f{equation}{
 * \frac{d \ln p}{dx} = \Gamma(x) = \sum_{n=0}^N
 * \gamma_n x^n
 * \f}
 *
 * for the set of spectral coefficinets \f$\gamma_n\f$ when
 * \f$0 < x < x_u = \ln(\rho_u/\rho_0)\f$, where \f$\rho_u\f$ is the provided
 * upper density.
 *
 * For \f$ x < 0 \f$, \f$ \Gamma(x) = \gamma_0 \f$.
 *
 * For \f$ x > x_u \f$, \f$ \Gamma(x) = \Gamma(x_u) \f$
 *
 *
 */
class Spectral : public EquationOfState<true, 1> {
 public:
  static constexpr size_t thermodynamic_dim = 1;
  static constexpr bool is_relativistic = true;

  struct ReferenceDensity {
    using type = double;
    static constexpr Options::String help = {"Reference density rho_0"};
    static double lower_bound() { return 0.0; }
  };

  struct ReferencePressure {
    using type = double;
    static constexpr Options::String help = {"Reference pressure p_0"};
    static double lower_bound() { return 0.0; }
  };

  struct Coefficients {
    using type = std::vector<double>;
    static constexpr Options::String help = {"Spectral coefficients gamma_i"};
  };

  struct UpperDensity {
    using type = double;
    static constexpr Options::String help = {"Upper density rho_u"};
    static double lower_bound() { return 0.0; }
  };

  static constexpr Options::String help = {
      "A spectral equation of state.  Defining x = log(rho/rho_0), Gamma(x) = "
      "Sum_i gamma_i x^i, then the pressure is determined from d(log P)/dx = "
      "Gamma(x) for x > 0.  For x < 0 the EOS is a polytrope with "
      "Gamma(x)=Gamma(0).  For x > x_u = log(rho_u/rho_0), Gamma(x) = "
      "Gamma(x_u).\n"
      "To get smooth equations of state, it is recommended that the second "
      "and third supplied coefficient should be 0. It is up to the user to "
      "choose coefficients that are physically reasonable, e.g. that "
      "satisfy causality."};

  using options = tmpl::list<ReferenceDensity, ReferencePressure, Coefficients,
                             UpperDensity>;

  Spectral() = default;
  Spectral(const Spectral&) = default;
  Spectral& operator=(const Spectral&) = default;
  Spectral(Spectral&&) = default;
  Spectral& operator=(Spectral&&) = default;
  ~Spectral() override = default;

  Spectral(double reference_density, double reference_pressure,
           std::vector<double> coefficients, double upper_density);

  EQUATION_OF_STATE_FORWARD_DECLARE_MEMBERS(Spectral, 1)

  WRAPPED_PUPable_decl_base_template(  // NOLINT
      SINGLE_ARG(EquationOfState<true, 1>), Spectral);

  /// The lower bound of the rest mass density that is valid for this EOS
  double rest_mass_density_lower_bound() const override { return 0.0; }

  /// The upper bound of the rest mass density that is valid for this EOS
  double rest_mass_density_upper_bound() const override {
    return std::numeric_limits<double>::max();
  }

  /// The lower bound of the specific internal energy that is valid for this EOS
  /// at the given rest mass density \f$\rho\f$
  double specific_internal_energy_lower_bound(
      const double /* rest_mass_density */) const override {
    return 0.0;
  }

  /// The upper bound of the specific internal energy that is valid for this EOS
  /// at the given rest mass density \f$\rho\f$
  double specific_internal_energy_upper_bound(
      const double /* rest_mass_density */) const override {
    return std::numeric_limits<double>::max();
  }

  /// The lower bound of the specific enthalpy that is valid for this EOS
  double specific_enthalpy_lower_bound() const override { return 1.0; }

 private:
  EQUATION_OF_STATE_FORWARD_DECLARE_MEMBER_IMPLS(1)

  double gamma(const double x) const;
  double integral_of_gamma(const double x) const;
  double chi_from_density(const double density) const;
  double specific_internal_energy_from_density(const double density) const;
  double specific_enthalpy_from_density(const double density) const;
  double pressure_from_density(const double density) const;
  double pressure_from_log_density(const double x) const;
  double rest_mass_density_from_enthalpy(const double specific_enthalpy) const;

  double reference_density_ = std::numeric_limits<double>::signaling_NaN();
  double reference_pressure_ = std::numeric_limits<double>::signaling_NaN();
  std::vector<double> integral_coefficients_{};
  std::vector<double> gamma_coefficients_{};
  double x_max_ = std::numeric_limits<double>::signaling_NaN();
  double gamma_of_x_max_ = std::numeric_limits<double>::signaling_NaN();
  double integral_of_gamma_of_x_max_ =
      std::numeric_limits<double>::signaling_NaN();
  std::vector<double> table_of_specific_energies_{};
  // Information for Gaussian quadrature
  size_t number_of_quadrature_coefs_ =
      std::numeric_limits<size_t>::signaling_NaN();
  std::vector<double> quadrature_weights_{};
  std::vector<double> quadrature_points_{};
};

}  // namespace EquationsOfState


#include "exception.hh"
#include "network.hh"
#include "timer.hh"

#include <Eigen/Dense>
#include <iostream>
#include <utility>

#include <sys/resource.h>
#include <sys/time.h>

using namespace std;
using namespace Eigen;

constexpr size_t batch_size = 1;
constexpr size_t input_size = 3;

void program_body()
{
  /* remove limit on stack size */
  const rlimit limits { RLIM_INFINITY, RLIM_INFINITY };
  CheckSystemCall( "setrlimit", setrlimit( RLIMIT_STACK, &limits ) );

  /* seed C RNG for Eigen random weight initialization */
  // srand( Timer::timestamp_ns() );
  srand( 0 );

  /* construct neural network on heap */
  auto nn = make_unique<Network<float, batch_size, input_size, 4, 4, 2, 1>>();
  nn->initializeWeightsRandomly();

  srand( 10 );
  /* initialize inputs */
  Matrix<float, batch_size, input_size> input = Matrix<float, batch_size, input_size>::Random();

  /* forward prop */
  nn->apply( input );

  /* back prop */
  nn->computeDeltas();
  nn->evaluateGradients( input );

  /* print */
  const IOFormat CleanFmt( 4, 0, ", ", "\n", "[", "]" );
  cout << "input:" << endl << input.format( CleanFmt ) << endl << endl;
  nn->print();

  unsigned int numLayers = nn->getNumLayers();

  for ( unsigned int layerNum = 0; layerNum < numLayers; layerNum++ ) {
    cout << "Layer " << layerNum << "\n";
    const unsigned int input_size_ = nn->getLayerInputSize( layerNum );
    const unsigned int output_size_ = nn->getLayerOutputSize( layerNum );
    cout << " input size: " << input_size_ << " -> "
         << "output_size: " << output_size_ << endl
         << endl;

    unsigned int numParams = nn->getNumParams( layerNum );
    vector<float> gradients( numParams, 0 );

    for ( unsigned int paramNum = 0; paramNum < numParams; paramNum++ ) {
      float gradient = nn->getEvaluatedGradient( layerNum, paramNum );
      gradients[paramNum] = gradient;
    }

    cout << "  weightGradients ";
    for ( unsigned int paramNum = 0; paramNum < numParams; paramNum++ ) {
      if ( paramNum % output_size_ == 0 )
        cout << endl << "   ";
      if ( paramNum == input_size_ * output_size_ ) {
        cout << endl;
        cout << "  biasGradients" << endl << "   ";
      }
      cout << gradients[paramNum] << " ";
    }
    cout << endl;
    cout << endl << endl;
  }
}

int main()
{
  try {
    program_body();
    return EXIT_SUCCESS;
  } catch ( const exception& e ) {
    cerr << e.what() << "\n";
    return EXIT_FAILURE;
  }
}


#include <Eigen/Core>
#include <iostream>

#include <Python.h> // for Py_ssize_t

typedef Eigen::Matrix<float, Eigen::Dynamic, 1> NumpyVecF;

static Eigen::VectorXf buf2vecf(float *mem, Py_ssize_t n)
{
    return Eigen::Map<NumpyVecF>(mem, n);
}

static void vecf2buf(const Eigen::VectorXf& vec, float *mem)
{
    Eigen::Map<NumpyVecF>(mem, vec.size()) = vec;
}

// In Python, the default is row-major (C) while in Eigen it's ColMajor (F).
typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> NumpyMatF;

static Eigen::MatrixXf buf2matf(float *mem, Py_ssize_t h, Py_ssize_t w)
{
    // This does the conversion, so very likely makes a copy.
    return Eigen::Map<NumpyMatF>(mem, h, w);
}

static void matf2buf(const Eigen::MatrixXf& mat, float *mem)
{
    Eigen::Map<NumpyMatF>(mem, mat.rows(), mat.cols()) = mat;
}


#ifndef MI_MATH_HPP
#define MI_MATH_HPP 1
#include <Eigen/Dense>
namespace mi
{
        typedef Eigen::Vector3d Vector3d;
        typedef Eigen::Vector3f Vector3f;
        typedef Eigen::Matrix< short , 3 , 1> Vector3s;
        typedef Eigen::Vector3d Point3d;
        typedef Eigen::Vector3f Point3f;
        typedef Eigen::Vector3i Point3i;
        typedef Eigen::Vector3f Color3f;
};
#endif//f MI_MATH_HPP


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

#include <boost/hana/sandbox/searchable_set.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <test/auto/base.hpp>
#include <test/cnumeric.hpp>
#include <test/injection.hpp>
#include <test/numeric.hpp>

// instances
#include <test/auto/applicative.hpp>
#include <test/auto/comparable.hpp>
#include <test/auto/functor.hpp>
#include <test/auto/monad.hpp>
#include <test/auto/searchable.hpp>
using namespace boost::hana;

namespace boost { namespace hana { namespace test {
    template <>
    auto instances<SearchableSet> = make<Tuple>(
          type<Comparable>
        , type<Functor>
        , type<Applicative>
        , type<Monad>
        , type<Searchable>
    );

    template <>
    auto objects<SearchableSet> = make<Tuple>(
        singleton(numeric(0)),
        singleton(numeric(1)),
        doubleton(numeric(0), numeric(1)),
        doubleton(numeric(0), numeric(3)),
        doubleton(numeric(0), numeric(0))
    );
}}}

template <int i>
constexpr auto n = test::numeric(i);

template <int i>
constexpr auto c = test::cnumeric<int, i>;

int main() {
    test::check_datatype<SearchableSet>();
    using test::x;
    auto f = test::injection([]{});
    auto g = test::injection([]{});

    // union_
    {
        BOOST_HANA_CONSTANT_CHECK(equal(
            union_(singleton(c<0>), singleton(c<0>)),
            singleton(c<0>)
        ));
        BOOST_HANA_CONSTANT_CHECK(equal(
            union_(singleton(c<0>), singleton(c<1>)),
            doubleton(c<0>, c<1>)
        ));
        BOOST_HANA_CONSTANT_CHECK(equal(
            union_(singleton(c<0>), doubleton(c<0>, c<1>)),
            doubleton(c<0>, c<1>)
        ));
    }

    // Comparable
    {
        // equal
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(singleton(n<0>), singleton(n<0>)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(equal(singleton(n<0>), singleton(n<1>))));

            BOOST_HANA_CONSTEXPR_CHECK(equal(singleton(n<0>), doubleton(n<0>, n<0>)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(equal(singleton(n<0>), doubleton(n<0>, n<1>))));
            BOOST_HANA_CONSTEXPR_CHECK(not_(equal(singleton(n<0>), doubleton(n<1>, n<1>))));

            BOOST_HANA_CONSTEXPR_CHECK(equal(doubleton(n<0>, n<1>), doubleton(n<0>, n<1>)));
            BOOST_HANA_CONSTEXPR_CHECK(equal(doubleton(n<0>, n<1>), doubleton(n<1>, n<0>)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(equal(doubleton(n<0>, n<1>), doubleton(n<0>, n<0>))));
            BOOST_HANA_CONSTEXPR_CHECK(not_(equal(doubleton(n<0>, n<1>), doubleton(n<3>, n<4>))));
        }
    }

    // Functor
    {
        // transform
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                transform(singleton(n<0>), f),
                singleton(f(n<0>))
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                transform(doubleton(n<0>, n<1>), f),
                doubleton(f(n<0>), f(n<1>))
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                transform(doubleton(n<0>, n<0>), f),
                singleton(f(n<0>))
            ));
        }
    }

    // Applicative
    {
        // ap
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                ap(singleton(f), singleton(x<0>)),
                singleton(f(x<0>))
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                ap(singleton(f), doubleton(x<0>, x<1>)),
                doubleton(f(x<0>), f(x<1>))
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                ap(doubleton(f, g), singleton(x<0>)),
                doubleton(f(x<0>), g(x<0>))
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                ap(doubleton(f, g), doubleton(x<0>, x<1>)),
                union_(doubleton(f(x<0>), f(x<1>)), doubleton(g(x<0>), g(x<1>)))
            ));
        }

        // lift
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                lift<SearchableSet>(x<0>),
                singleton(x<0>)
            ));
        }
    }

    // Monad
    {
        // flatten
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                flatten(singleton(singleton(c<0>))),
                singleton(c<0>)
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                flatten(singleton(doubleton(c<0>, c<1>))),
                doubleton(c<0>, c<1>)
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                flatten(doubleton(singleton(c<0>), singleton(c<1>))),
                doubleton(c<0>, c<1>)
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                flatten(doubleton(doubleton(c<0>, c<1>), singleton(c<2>))),
                union_(doubleton(c<0>, c<1>), singleton(c<2>))
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                flatten(doubleton(singleton(c<0>), doubleton(c<1>, c<2>))),
                union_(doubleton(c<0>, c<1>), singleton(c<2>))
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                flatten(doubleton(doubleton(c<0>, c<1>), doubleton(c<2>, c<3>))),
                union_(doubleton(c<0>, c<1>), doubleton(c<2>, c<3>))
            ));
        }
    }

    // Searchable
    {

        BOOST_HANA_CONSTEXPR_LAMBDA auto is = [](auto x) {
            return [=](auto y) { return equal(x, y); };
        };

        // any_of
        {
            BOOST_HANA_CONSTEXPR_CHECK(any_of(singleton(n<0>), is(n<0>)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(any_of(singleton(n<0>), is(n<1>))));
            BOOST_HANA_CONSTEXPR_CHECK(any_of(doubleton(n<0>, n<1>), is(n<0>)));
            BOOST_HANA_CONSTEXPR_CHECK(any_of(doubleton(n<0>, n<1>), is(n<1>)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(any_of(doubleton(n<0>, n<1>), is(n<2>))));
        }

        // find_if
        {
            BOOST_HANA_CONSTANT_CHECK(find_if(singleton(c<0>), is(c<0>)) == just(c<0>));
            BOOST_HANA_CONSTANT_CHECK(find_if(singleton(c<1>), is(c<0>)) == nothing);

            BOOST_HANA_CONSTANT_CHECK(find_if(doubleton(c<0>, c<1>), is(c<0>)) == just(c<0>));
            BOOST_HANA_CONSTANT_CHECK(find_if(doubleton(c<0>, c<1>), is(c<1>)) == just(c<1>));
            BOOST_HANA_CONSTANT_CHECK(find_if(doubleton(c<0>, c<1>), is(c<2>)) == nothing);
        }

        // subset
        {
            BOOST_HANA_CONSTEXPR_CHECK(subset(singleton(n<0>), singleton(n<0>)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(subset(singleton(n<1>), singleton(n<0>))));

            BOOST_HANA_CONSTEXPR_CHECK(subset(singleton(n<0>), doubleton(n<0>, n<1>)));
            BOOST_HANA_CONSTEXPR_CHECK(subset(singleton(n<1>), doubleton(n<0>, n<1>)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(subset(singleton(n<2>), doubleton(n<0>, n<1>))));

            BOOST_HANA_CONSTEXPR_CHECK(subset(doubleton(n<0>, n<1>), doubleton(n<0>, n<1>)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(subset(doubleton(n<0>, n<2>), doubleton(n<0>, n<1>))));
            BOOST_HANA_CONSTEXPR_CHECK(not_(subset(doubleton(n<2>, n<3>), doubleton(n<0>, n<1>))));
        }
    }
}


//=======================================================================
// Copyright 2009 Trustees of Indiana University.
// Authors: Michael Hansen, Andrew Lumsdaine
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <fstream>
#include <iostream>
#include <set>

#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/graph/grid_graph.hpp>
#include <boost/random.hpp>
#include <boost/test/minimal.hpp>

using namespace boost;

// Function that prints a vertex to std::cout
template <typename Vertex>
void print_vertex(Vertex vertex_to_print) {

  std::cout << "(";

  for (std::size_t dimension_index = 0;
       dimension_index < vertex_to_print.size();
       ++dimension_index) {
    std::cout << vertex_to_print[dimension_index];

    if (dimension_index != (vertex_to_print.size() - 1)) {
      std::cout << ", ";
    }
  }

  std::cout << ")";
}

template <unsigned int Dims>
void do_test(minstd_rand& generator) {
  typedef grid_graph<Dims> Graph;
  typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
  typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

  typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;

  std::cout << "Dimensions: " << Dims << ", lengths: ";

  // Randomly generate the dimension lengths (3-10) and wrapping
  boost::array<vertices_size_type, Dims> lengths;
  boost::array<bool, Dims> wrapped;

  for (unsigned int dimension_index = 0;
       dimension_index < Dims;
       ++dimension_index) {
    lengths[dimension_index] = 3 + (generator() % 8);
    wrapped[dimension_index] = ((generator() % 2) == 0);

    std::cout << lengths[dimension_index] <<
      (wrapped[dimension_index] ? " [W]" : " [U]") << ", ";
  }

  std::cout << std::endl;

  Graph graph(lengths, wrapped);

  // Verify dimension lengths and wrapping
  for (unsigned int dimension_index = 0;
       dimension_index < Dims;
       ++dimension_index) {
    BOOST_REQUIRE(graph.length(dimension_index) == lengths[dimension_index]);
    BOOST_REQUIRE(graph.wrapped(dimension_index) == wrapped[dimension_index]);
  }

  // Verify matching indices
  for (vertices_size_type vertex_index = 0;
       vertex_index < num_vertices(graph);
       ++vertex_index) {
    BOOST_REQUIRE(get(boost::vertex_index, graph, vertex(vertex_index, graph)) == vertex_index);
  }

  for (edges_size_type edge_index = 0;
       edge_index < num_edges(graph);
       ++edge_index) {

    edge_descriptor current_edge = edge_at(edge_index, graph);
    BOOST_REQUIRE(get(boost::edge_index, graph, current_edge) == edge_index);
  }

  // Verify all vertices are within bounds
  vertices_size_type vertex_count = 0;
  BOOST_FOREACH(vertex_descriptor current_vertex, vertices(graph)) {

    vertices_size_type current_index =
      get(boost::vertex_index, graph, current_vertex);

    for (unsigned int dimension_index = 0;
         dimension_index < Dims;
         ++dimension_index) {
      BOOST_REQUIRE(/*(current_vertex[dimension_index] >= 0) && */ // Always true
                   (current_vertex[dimension_index] < lengths[dimension_index]));
    }

    // Verify out-edges of this vertex
    edges_size_type out_edge_count = 0;
    std::set<vertices_size_type> target_vertices;

    BOOST_FOREACH(edge_descriptor out_edge,
                  out_edges(current_vertex, graph)) {

      target_vertices.insert
        (get(boost::vertex_index, graph, target(out_edge, graph)));

      ++out_edge_count;
    }

    BOOST_REQUIRE(out_edge_count == out_degree(current_vertex, graph));

    // Verify in-edges of this vertex
    edges_size_type in_edge_count = 0;

    BOOST_FOREACH(edge_descriptor in_edge,
                  in_edges(current_vertex, graph)) {

      BOOST_REQUIRE(target_vertices.count
                   (get(boost::vertex_index, graph, source(in_edge, graph))) > 0);

      ++in_edge_count;
    }

    BOOST_REQUIRE(in_edge_count == in_degree(current_vertex, graph));

    // The number of out-edges and in-edges should be the same
    BOOST_REQUIRE(degree(current_vertex, graph) ==
                 out_degree(current_vertex, graph) +
                 in_degree(current_vertex, graph));

    // Verify adjacent vertices to this vertex
    vertices_size_type adjacent_count = 0;

    BOOST_FOREACH(vertex_descriptor adjacent_vertex,
                  adjacent_vertices(current_vertex, graph)) {

      BOOST_REQUIRE(target_vertices.count
                   (get(boost::vertex_index, graph, adjacent_vertex)) > 0);

      ++adjacent_count;
    }

    BOOST_REQUIRE(adjacent_count == out_degree(current_vertex, graph));

    // Verify that this vertex is not listed as connected to any
    // vertices outside of its adjacent vertices.
    BOOST_FOREACH(vertex_descriptor unconnected_vertex, vertices(graph)) {

      vertices_size_type unconnected_index =
        get(boost::vertex_index, graph, unconnected_vertex);

      if ((unconnected_index == current_index) ||
          (target_vertices.count(unconnected_index) > 0)) {
        continue;
      }

      BOOST_REQUIRE(!edge(current_vertex, unconnected_vertex, graph).second);
      BOOST_REQUIRE(!edge(unconnected_vertex, current_vertex, graph).second);
    }

    ++vertex_count;
  }

  BOOST_REQUIRE(vertex_count == num_vertices(graph));

  // Verify all edges are within bounds
  edges_size_type edge_count = 0;
  BOOST_FOREACH(edge_descriptor current_edge, edges(graph)) {

    vertices_size_type source_index =
      get(boost::vertex_index, graph, source(current_edge, graph));

    vertices_size_type target_index =
      get(boost::vertex_index, graph, target(current_edge, graph));

    BOOST_REQUIRE(source_index != target_index);
    BOOST_REQUIRE(/* (source_index >= 0) : always true && */ (source_index < num_vertices(graph)));
    BOOST_REQUIRE(/* (target_index >= 0) : always true && */ (target_index < num_vertices(graph)));

    // Verify that the edge is listed as existing in both directions
    BOOST_REQUIRE(edge(source(current_edge, graph), target(current_edge, graph), graph).second);
    BOOST_REQUIRE(edge(target(current_edge, graph), source(current_edge, graph), graph).second);

    ++edge_count;
  }

  BOOST_REQUIRE(edge_count == num_edges(graph));
}

int test_main(int argc, char* argv[]) {

  std::size_t random_seed = time(0);

  if (argc > 1) {
    random_seed = lexical_cast<std::size_t>(argv[1]);
  }

  minstd_rand generator(random_seed);

  do_test<0>(generator);
  do_test<1>(generator);
  do_test<2>(generator);
  do_test<3>(generator);
  do_test<4>(generator);

  return (0);
}


#include <iostream>

#include <cuda.h>
#include <cuda_runtime_api.h>

#include <blitz.h>

using namespace blitz;
// M N
Shape input_shape(2);
// N M
Shape output_shape(2);

void compare_cpu_gpu(size_t size, float* output_cpu, float* output_gpu) {
  for (size_t i = 0; i < size; ++i) {
    if (output_cpu[i] > output_gpu[i] + 1e-3 ||
      output_cpu[i] < output_gpu[i] - 1e-3) {
      std::cout << "Index: " << i << ", CPU: " << output_cpu[i] <<
        ", GPU: " << output_gpu[i] << std::endl;
    }
  }
}

void transpose(size_t m, size_t n) {
  // set up cpu
  CPUTensor<float> input_cpu(input_shape);
  CPUTensor<float> output_cpu(output_shape);
  // set up gpu
  GPUTensor<float> input_gpu(input_shape);
  GPUTensor<float> output_gpu(output_shape);
  CPUTensor<float> output_copy(output_shape);
  // init values
  Backend<CPUTensor, float>::UniformDistributionFunc(&input_cpu, 0.0, 1.0);
  cudaMemcpy(input_gpu.data(), input_cpu.data(),
    input_cpu.size() * sizeof(float), cudaMemcpyHostToDevice);
  // transpose
  Backend<CPUTensor, float>::Transpose2DFunc(&input_cpu, &output_cpu);
  Backend<GPUTensor, float>::Transpose2DFunc(&input_gpu, &output_gpu);
  // copy from gpu to cpu
  cudaMemcpy(output_copy.data(), output_gpu.data(),
    output_gpu.size() * sizeof(float), cudaMemcpyDeviceToHost);
  compare_cpu_gpu(output_cpu.size(), output_cpu.data(), output_copy.data());
}

int main(int argc, char** argv) {
  const size_t NUM_ARGS = 2;
  FLAGS_logtostderr = true;
  google::InitGoogleLogging(argv[0]);
  // M N
  if (argc != NUM_ARGS + 1) {
    LOG(FATAL) << "Not enough args!" << " argc " << argc;
  }
  const size_t M = atoi(argv[1]);
  const size_t N = atoi(argv[2]);
  // set shapes
  input_shape[0] = M;
  input_shape[1] = N;
  output_shape[0] = N;
  output_shape[1] = M;
  // run
  transpose(M, N);
  return 0;
}


// Copyright (c) 2012 Christopher Lux <christopherlux@gmail.com>
// Distributed under the Modified BSD License, see license.txt.

#include <exception>
#include <stdexcept>

#include <boost/utility.hpp>

namespace scm {
namespace data {
namespace detail {

/*

template<typename val_type>
bool build_lookup_table<val_type>(boost::scoped_array<val_type>& dst,
                                  const piecewise_function_weighted_1d<unsigned char, val_type>& scal_trafu,
                                  unsigned size)
{
    if (size < 1) {
        return (false);
    }

    float    dst_ind_scal_factor = float(size - 1) / 255.0f;

    unsigned dst_ind_begin;
    unsigned dst_ind_end;

    float    dst_ind_begin_weight;
    val_type dst_ind_begin_value;
    val_type dst_ind_end_value;

    float lerp_factor;
    float lerp_factor_w;
    float part_step_size;

    // clear beginning
    if (scal_trafu.empty()) {
        dst_ind_begin = 0;
        dst_ind_end   = size - 1;
    }
    else {
        dst_ind_begin   = 0;
        dst_ind_end     = unsigned(math::floor(float(scal_trafu.stops_begin()->first) * dst_ind_scal_factor));
    }

    for (unsigned dst_ind = dst_ind_begin; dst_ind < dst_ind_end; ++dst_ind) {
        dst[dst_ind] = val_type(0);
    }

    // fill lookup table
    for (scm::piecewise_function_weighted_1d<unsigned char, val_type>::const_stop_iterator it_left = scal_trafu.stops_begin();
         it_left  != scal_trafu.stops_end();
         ++it_left) {

        dst_ind_begin           = unsigned(math::floor(float(it_left->first) * dst_ind_scal_factor));
        dst_ind_begin_value     = it_left->second._value;
        dst_ind_begin_weight    = it_left->second._weight;

        scm::piecewise_function_weighted_1d<unsigned char, val_type>::const_stop_iterator it_right = boost::next(it_left);
        if (it_right != scal_trafu.stops_end()) {
            dst_ind_end         = unsigned(math::floor(float(it_right->first) * dst_ind_scal_factor));
            dst_ind_end_value   = it_right->second._value;
        }
        else {
            dst_ind_end         = dst_ind_begin + 1;
            dst_ind_end_value   = dst_ind_begin_value;
        }

        part_step_size = 1.0f / float(dst_ind_end - dst_ind_begin);
        lerp_factor = 0.0f;
        lerp_factor_w = 0.0f;

        for (unsigned dst_ind = dst_ind_begin; dst_ind < dst_ind_end; ++dst_ind) {
            //lerp_factor = math::shoothstep(dst_ind_begin, dst_ind_end, dst_ind);
            //// smoothstep
            //float s = math::clamp(float(dst_ind-dst_ind_begin)/float(dst_ind_end-dst_ind_begin), 0.0f, 1.0f);
            //s = scm::detail::non_linear_value_weight<float>(s, dst_ind_begin_weight);
            //lerp_factor_w = (s*s*(3.0f-2.0f*s));

            lerp_factor_w = scm::detail::non_linear_value_weight<float>(lerp_factor, dst_ind_begin_weight);
            dst[dst_ind] = math::lerp(dst_ind_begin_value, dst_ind_end_value, lerp_factor_w);
            lerp_factor += part_step_size;
        }
    }

    // clear end
    for (unsigned dst_ind = dst_ind_end; dst_ind < size; ++dst_ind) {
        dst[dst_ind] = val_type(0);
    }

    // original code
    //float a;
    //float step = 255.0f / float(size - 1);
    //for (unsigned i = 0; i < size; i++) {
    //    a = float(i) * step;
    //    dst[i] = scal_trafu[a];
    //}

    return (true);
}

*/

template<typename val_type>
struct build_lookup_table_impl<val_type, unsigned char>
{
    static bool build_table(boost::scoped_array<val_type>& dst,
                            const piecewise_function_1d<unsigned char, val_type>& scal_trafu,
                            unsigned size)
    {
        using namespace scm::math;

        if (size < 1) {
            return (false);
        }

        float    dst_ind_scal_factor = float(size - 1) / 255.0f;

        unsigned dst_ind_begin;
        unsigned dst_ind_end;

        val_type dst_ind_begin_value;
        val_type dst_ind_end_value;

        float lerp_factor;
        float part_step_size;

        // clear beginning
        if (scal_trafu.empty()) {
            dst_ind_begin = 0;
            dst_ind_end   = size - 1;
        }
        else {
            dst_ind_begin   = 0;
            dst_ind_end     = unsigned(floor(float(scal_trafu.stops_begin()->first) * dst_ind_scal_factor));
        }

        for (unsigned dst_ind = dst_ind_begin; dst_ind < dst_ind_end; ++dst_ind) {
            dst[dst_ind] = val_type(0);
        }

        // fill lookup table
        for (typename scm::data::piecewise_function_1d<unsigned char, val_type>::const_stop_iterator it_left = scal_trafu.stops_begin();
            it_left  != scal_trafu.stops_end();
            ++it_left) {

            dst_ind_begin       = unsigned(floor(float(it_left->first) * dst_ind_scal_factor));
            dst_ind_begin_value = it_left->second;

            typename scm::data::piecewise_function_1d<unsigned char, val_type>::const_stop_iterator it_right = boost::next(it_left);
            if (it_right != scal_trafu.stops_end()) {
            dst_ind_end         = unsigned(floor(float(it_right->first) * dst_ind_scal_factor));
            dst_ind_end_value   = it_right->second;
            }
            else {
            dst_ind_end         = dst_ind_begin + 1;
            dst_ind_end_value   = dst_ind_begin_value;
            }

            part_step_size = 1.0f / float(dst_ind_end - dst_ind_begin);
            lerp_factor = 0.0f;

            for (unsigned dst_ind = dst_ind_begin; dst_ind < dst_ind_end; ++dst_ind) {
            //lerp_factor = math::shoothstep(dst_ind_begin, dst_ind_end, dst_ind);
            dst[dst_ind] = lerp(dst_ind_begin_value, dst_ind_end_value, lerp_factor);
            lerp_factor += part_step_size;
            }
        }

        // clear end
        for (unsigned dst_ind = dst_ind_end; dst_ind < size; ++dst_ind) {
            dst[dst_ind] = val_type(0);
        }

        // original code
        //float a;
        //float step = 255.0f / float(size - 1);
        //for (unsigned i = 0; i < size; i++) {
        //    a = float(i) * step;
        //    dst[i] = scal_trafu[a];
        //}

        return (true);
    }
}; // struct_look_uptable_impl

template<typename val_type>
struct build_lookup_table_impl<val_type, float>
{
    static bool build_table(boost::scoped_array<val_type>&                dst,
                            const piecewise_function_1d<float, val_type>& scal_trafu,
                            unsigned                                      size)
    {
        using namespace scm::math;

        if (size < 1) {
            return (false);
        }

        float    dst_ind_scal_factor = float(size - 1);

        unsigned dst_ind_begin;
        unsigned dst_ind_end;

        val_type dst_ind_begin_value;
        val_type dst_ind_end_value;

        float lerp_factor;
        float part_step_size;

        // clear beginning
        if (scal_trafu.empty()) {
            dst_ind_begin = 0;
            dst_ind_end   = size - 1;
        }
        else {
            dst_ind_begin   = 0;
            dst_ind_end     = unsigned(floor(scal_trafu.stops_begin()->first * dst_ind_scal_factor));
        }

        for (unsigned dst_ind = dst_ind_begin; dst_ind < dst_ind_end; ++dst_ind) {
            dst[dst_ind] = val_type(0);
        }

        // fill lookup table
        for (typename scm::data::piecewise_function_1d<float, val_type>::const_stop_iterator it_left = scal_trafu.stops_begin();
            it_left  != scal_trafu.stops_end();
            ++it_left) {

            dst_ind_begin       = unsigned(floor(it_left->first * dst_ind_scal_factor));
            dst_ind_begin_value = it_left->second;

            typename scm::data::piecewise_function_1d<float, val_type>::const_stop_iterator it_right = boost::next(it_left);
            if (it_right != scal_trafu.stops_end()) {
            dst_ind_end         = unsigned(floor(it_right->first * dst_ind_scal_factor));
            dst_ind_end_value   = it_right->second;
            }
            else {
            dst_ind_end         = dst_ind_begin + 1;
            dst_ind_end_value   = dst_ind_begin_value;
            }

            part_step_size = 1.0f / float(dst_ind_end - dst_ind_begin);
            lerp_factor = 0.0f;

            for (unsigned dst_ind = dst_ind_begin; dst_ind < dst_ind_end; ++dst_ind) {
            //lerp_factor = math::shoothstep(dst_ind_begin, dst_ind_end, dst_ind);
            dst[dst_ind] = lerp(dst_ind_begin_value, dst_ind_end_value, lerp_factor);
            lerp_factor += part_step_size;
            }
        }

        // clear end
        for (unsigned dst_ind = dst_ind_end; dst_ind < size; ++dst_ind) {
            dst[dst_ind] = val_type(0);
        }

        // original code
        //float a;
        //float step = 255.0f / float(size - 1);
        //for (unsigned i = 0; i < size; i++) {
        //    a = float(i) * step;
        //    dst[i] = scal_trafu[a];
        //}

        return (true);
    }
}; // struct_look_uptable_impl

} // namespace detail
} // namespace data
} // namespace scm


#ifndef _MSC_VER
#include "update_ops_cpp.hpp"
extern "C"{
#include "utility.h"
}
#else
#include "update_ops_cpp.hpp"
#include "utility.h"
#endif
#include <Eigen/Core>
#include <functional>

void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list, UINT target_qubit_index_count, const CTYPE* matrix, CTYPE* state, ITYPE dim) {

    // matrix dim, mask, buffer
    const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
    const ITYPE* matrix_mask_list = create_matrix_mask_list(target_qubit_index_list, target_qubit_index_count);
    Eigen::Map<const Eigen::Matrix<std::complex<double>,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>, Eigen::Aligned> eigen_matrix((std::complex<double>*)matrix,matrix_dim,matrix_dim);
    Eigen::VectorXcd buffer(matrix_dim);
    std::complex<double>* eigen_state = reinterpret_cast<std::complex<double>*>(state);

    // insert index
    const UINT* sorted_insert_index_list = create_sorted_ui_list(target_qubit_index_list, target_qubit_index_count);

    // loop variables
    const ITYPE loop_dim = dim >> target_qubit_index_count;

    ITYPE state_index;
    for(state_index = 0 ; state_index < loop_dim ; ++state_index ){
        // create base index
        ITYPE basis_0 = state_index;
        for(UINT cursor=0; cursor < target_qubit_index_count ; cursor++){
            UINT insert_index = sorted_insert_index_list[cursor];
            basis_0 = insert_zero_to_basis_index(basis_0, 1ULL << insert_index , insert_index );
        }

        // fetch vector
        for(ITYPE y = 0 ; y < matrix_dim ; ++y){
            buffer[y] = eigen_state[basis_0 ^ matrix_mask_list[y]];
        }

        buffer = eigen_matrix * buffer;

        // set result
        for(ITYPE y = 0 ; y < matrix_dim ; ++y){
            eigen_state[basis_0 ^ matrix_mask_list[y]] = buffer[y];
        }
    }
    free((UINT*)sorted_insert_index_list);
    free((ITYPE*)matrix_mask_list);
}

void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list, UINT target_qubit_index_count, const Eigen::Matrix<std::complex<double>,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>& eigen_matrix, CTYPE* state, ITYPE dim){
    // matrix dim, mask, buffer
    const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
    const ITYPE* matrix_mask_list = create_matrix_mask_list(target_qubit_index_list, target_qubit_index_count);
    Eigen::VectorXcd buffer(matrix_dim);
    std::complex<double>* eigen_state = reinterpret_cast<std::complex<double>*>(state);

    // insert index
    const UINT* sorted_insert_index_list = create_sorted_ui_list(target_qubit_index_list, target_qubit_index_count);

    // loop variables
    const ITYPE loop_dim = dim >> target_qubit_index_count;

    ITYPE state_index;
    for(state_index = 0 ; state_index < loop_dim ; ++state_index ){
        // create base index
        ITYPE basis_0 = state_index;
        for(UINT cursor=0; cursor < target_qubit_index_count ; cursor++){
            UINT insert_index = sorted_insert_index_list[cursor];
            basis_0 = insert_zero_to_basis_index(basis_0, 1ULL << insert_index , insert_index );
        }

        // fetch vector
        for(ITYPE y = 0 ; y < matrix_dim ; ++y){
            buffer[y] = eigen_state[basis_0 ^ matrix_mask_list[y]];
        }

        buffer = eigen_matrix * buffer;

        // set result
        for(ITYPE y = 0 ; y < matrix_dim ; ++y){
            eigen_state[basis_0 ^ matrix_mask_list[y]] = buffer[y];
        }
    }
    free((UINT*)sorted_insert_index_list);
    free((ITYPE*)matrix_mask_list);
}

void multi_qubit_dense_matrix_gate_eigen(const UINT* target_qubit_index_list, UINT target_qubit_index_count, const Eigen::MatrixXcd& eigen_matrix, CTYPE* state, ITYPE dim) {

    // matrix dim, mask, buffer
    const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
    const ITYPE* matrix_mask_list = create_matrix_mask_list(target_qubit_index_list, target_qubit_index_count);
    std::complex<double>* cppstate = reinterpret_cast<std::complex<double>*>(state);
    Eigen::VectorXcd buffer(matrix_dim);

    // insert index
    const UINT* sorted_insert_index_list = create_sorted_ui_list(target_qubit_index_list, target_qubit_index_count);

    // loop variables
    const ITYPE loop_dim = dim >> target_qubit_index_count;

    ITYPE state_index;
    for(state_index = 0 ; state_index < loop_dim ; ++state_index ){
        // create base index
        ITYPE basis_0 = state_index;
        for(UINT cursor=0; cursor < target_qubit_index_count ; cursor++){
            UINT insert_index = sorted_insert_index_list[cursor];
            basis_0 = insert_zero_to_basis_index(basis_0, 1ULL << insert_index , insert_index );
        }

        // fetch vector
        for(ITYPE y = 0 ; y < matrix_dim ; ++y){
            buffer[y] = cppstate[basis_0 ^ matrix_mask_list[y]];
        }

        buffer = eigen_matrix * buffer;

        // set result
        for(ITYPE y = 0 ; y < matrix_dim ; ++y){
            cppstate[basis_0 ^ matrix_mask_list[y]] = buffer[y];
        }
    }
    free((UINT*)sorted_insert_index_list);
    free((ITYPE*)matrix_mask_list);
}

void multi_qubit_sparse_matrix_gate_eigen(const UINT* target_qubit_index_list, UINT target_qubit_index_count, const Eigen::SparseMatrix<std::complex<double>>& eigen_matrix, CTYPE* state, ITYPE dim) {
	// matrix dim, mask, buffer
	const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
	const ITYPE* matrix_mask_list = create_matrix_mask_list(target_qubit_index_list, target_qubit_index_count);
	Eigen::VectorXcd buffer(matrix_dim);
	std::complex<double>* eigen_state = reinterpret_cast<std::complex<double>*>(state);

	// insert index
	const UINT* sorted_insert_index_list = create_sorted_ui_list(target_qubit_index_list, target_qubit_index_count);

	// loop variables
	const ITYPE loop_dim = dim >> target_qubit_index_count;

	ITYPE state_index;
	for (state_index = 0; state_index < loop_dim; ++state_index) {
		// create base index
		ITYPE basis_0 = state_index;
		for (UINT cursor = 0; cursor < target_qubit_index_count; cursor++) {
			UINT insert_index = sorted_insert_index_list[cursor];
			basis_0 = insert_zero_to_basis_index(basis_0, 1ULL << insert_index, insert_index);
		}

		// fetch vector
		for (ITYPE y = 0; y < matrix_dim; ++y) {
			buffer[y] = eigen_state[basis_0 ^ matrix_mask_list[y]];
		}

		buffer = eigen_matrix * buffer;

		// set result
		for (ITYPE y = 0; y < matrix_dim; ++y) {
			eigen_state[basis_0 ^ matrix_mask_list[y]] = buffer[y];
		}
	}
	free((UINT*)sorted_insert_index_list);
	free((ITYPE*)matrix_mask_list);
}

void reversible_boolean_gate(const UINT* target_qubit_index_list, UINT target_qubit_index_count, std::function<ITYPE(ITYPE,ITYPE)> function_ptr, CTYPE* state, ITYPE dim) {

	// matrix dim, mask, buffer
	const ITYPE matrix_dim = 1ULL << target_qubit_index_count;
	const ITYPE* matrix_mask_list = create_matrix_mask_list(target_qubit_index_list, target_qubit_index_count);

	// insert index
	const UINT* sorted_insert_index_list = create_sorted_ui_list(target_qubit_index_list, target_qubit_index_count);

	// loop variables
	const ITYPE loop_dim = dim >> target_qubit_index_count;

	CTYPE* buffer = (CTYPE*)malloc((size_t)(sizeof(CTYPE)*matrix_dim));
	ITYPE state_index;
	for (state_index = 0; state_index < loop_dim; ++state_index) {
		// create base index
		ITYPE basis_0 = state_index;
		for (UINT cursor = 0; cursor < target_qubit_index_count; cursor++) {
			UINT insert_index = sorted_insert_index_list[cursor];
			basis_0 = insert_zero_to_basis_index(basis_0, 1ULL << insert_index, insert_index);
		}

		// compute matrix-vector multiply

		for (ITYPE x = 0; x < matrix_dim; ++x) {
			ITYPE y = function_ptr(x, matrix_dim);
			buffer[y] = state[basis_0 ^ matrix_mask_list[x]];
		}

		// set result
		for (ITYPE y = 0; y < matrix_dim; ++y) {
			state[basis_0 ^ matrix_mask_list[y]] = buffer[y];
		}
	}
	free(buffer);
	free((UINT*)sorted_insert_index_list);
	free((ITYPE*)matrix_mask_list);
}


#include <fstream>
#include <iostream>
#include <vector>

#include <Eigen/Geometry>
#include "GINS.hpp"

using namespace std;

// comment to turn off gnss interrupt
#define INTERRUPT_ON
const double CONVERGENCE_TIME = 500;
const double GNSS_TIME = 180;
const double INTERRUPT_TIME = 60;
const double MOTION_TIME = 456370;

string read_path = "/home/ubuntu/Dataset/";
string imu_data_name = "A15_imu.bin";
string gnss_data_name = "GNSS_RTK.txt";
string truth_name = "truth.nav";

#ifdef INTERRUPT_ON
string output_name = "result_interrupt.nav";
#else
string output_name = "result.nav";
#endif

class Truth {
public:
  double week;
  double second;
  Eigen::Vector3d pos;
  Eigen::Vector3d vel;
  Eigen::Vector3d att;
};

int main(int argc, char const *argv[]) {
  // read files
  ifstream imu_data((read_path + imu_data_name).c_str(), ios::in | ios::binary);
  ifstream gnss_data((read_path + gnss_data_name).c_str());
  ifstream truth_data((read_path + truth_name).c_str());
  ofstream result_data((read_path + output_name).c_str(), ios::trunc);

  if (!imu_data || !gnss_data || !truth_data || !result_data) exit(-1);

  // parse data and save
  iNav::IMUData imu_frame;
  iNav::GnssData gnss_frame;
  Truth truth_frame;
  vector<iNav::IMUData> imu_vec;
  vector<iNav::GnssData> gnss_vec;
  vector<Truth> truth_vec;

  iNav::NavData initial_state;
  initial_state.timestamp = 456300.0;
  initial_state.pos = Eigen::Vector3d(30.444787369, 114.471863247, 20.910);
  initial_state.pos_std = Eigen::Vector3d(0.005, 0.004, 0.008);
  initial_state.vel = Eigen::Vector3d(0, 0, 0);
  initial_state.vel_std = Eigen::Vector3d(0.003, 0.004, 0.004);
  initial_state.att = Eigen::Vector3d(0.854, -2.0345, 185.696);
  initial_state.att_std = Eigen::Vector3d(0.003, 0.003, 0.023);

  iNav::IMUParam imu_param;
  imu_param.ARW = 0.003;
  imu_param.VRW = 0.03;
  imu_param.gyro_bias_std = 0.027;
  imu_param.T_gyro_bias = 4;
  imu_param.acc_bias_std = 15;
  imu_param.T_acc_bias = 4;
  imu_param.gyro_scalar_std = 300;
  imu_param.T_gyro_scalar = 4;
  imu_param.acc_scalar_std = 300;
  imu_param.T_acc_scalar = 4;

  Eigen::Vector3d l_b(0.136, -0.301, -0.184);

  // save data in vector
  while (fabs(imu_frame.timestamp - initial_state.timestamp) > 0.001)
    imu_data.read((char *)&imu_frame, sizeof(imu_frame));
  imu_vec.push_back(imu_frame);

  while (truth_frame.second < initial_state.timestamp) {
    truth_data >> truth_frame.week >> truth_frame.second >>
        truth_frame.pos[0] >> truth_frame.pos[1] >> truth_frame.pos[2] >>
        truth_frame.vel[0] >> truth_frame.vel[1] >> truth_frame.vel[2] >>
        truth_frame.att[0] >> truth_frame.att[1] >> truth_frame.att[2];
  }
  truth_vec.push_back(truth_frame);

  while (gnss_frame.timestamp < initial_state.timestamp) {
    gnss_data >> gnss_frame.timestamp >> gnss_frame.pos[0] >>
        gnss_frame.pos[1] >> gnss_frame.pos[2] >> gnss_frame.pos_std[0] >>
        gnss_frame.pos_std[1] >> gnss_frame.pos_std[2];
  }
  gnss_vec.push_back(gnss_frame);

  while (imu_data.read((char *)&imu_frame, sizeof(imu_frame)))
    imu_vec.push_back(imu_frame);

  while (!gnss_data.eof()) {
    gnss_data >> gnss_frame.timestamp >> gnss_frame.pos[0] >>
        gnss_frame.pos[1] >> gnss_frame.pos[2] >> gnss_frame.pos_std[0] >>
        gnss_frame.pos_std[1] >> gnss_frame.pos_std[2];
    gnss_vec.push_back(gnss_frame);
  }

  while (!truth_data.eof()) {
    truth_data >> truth_frame.week >> truth_frame.second >>
        truth_frame.pos[0] >> truth_frame.pos[1] >> truth_frame.pos[2] >>
        truth_frame.vel[0] >> truth_frame.vel[1] >> truth_frame.vel[2] >>
        truth_frame.att[0] >> truth_frame.att[1] >> truth_frame.att[2];
    truth_vec.push_back(truth_frame);
  }

  iNav::GINS gnss_ins(initial_state, imu_param, l_b, imu_vec[0]);

  iNav::NavData nav_output;
  bool interrupt_flag = false;
  double record_time = initial_state.timestamp;
  int i = 1;
  for (int j = 1; j < imu_vec.size(); j++) {
    if (i < gnss_vec.size()) {
      if (imu_vec[j].timestamp < gnss_vec[i].timestamp) {
        nav_output = gnss_ins.Mechanization(imu_vec[j]);
        gnss_ins.Prediction();
      } else if (!interrupt_flag) {
        nav_output = gnss_ins.GNSSUpdate(gnss_vec[i++], imu_vec[j]);
      } else {
        i++;
        nav_output = gnss_ins.Mechanization(imu_vec[j]);
        gnss_ins.Prediction();
      }
    } else {
      nav_output = gnss_ins.Mechanization(imu_vec[j]);
      gnss_ins.Prediction();
    }

#ifdef INTERRUPT_ON
    if (imu_vec[j].timestamp - initial_state.timestamp < CONVERGENCE_TIME) {
      // wait for convergence
    } else {
      if (imu_vec[j].timestamp - record_time > GNSS_TIME && !interrupt_flag) {
        interrupt_flag = true;
        record_time = imu_vec[j].timestamp;
      }

      if (imu_vec[j].timestamp - record_time > INTERRUPT_TIME &&
          interrupt_flag) {
        interrupt_flag = false;
        record_time = imu_vec[j].timestamp;
      }
    }
#endif

    // nav files output
    if (nav_output.timestamp > MOTION_TIME) {
      result_data.precision(12);
      result_data << 2017 << " " << nav_output.timestamp << " ";
      result_data << nav_output.pos[0] << " " << nav_output.pos[1] << " "
                  << nav_output.pos[2] << " ";
      result_data << nav_output.vel[0] << " " << nav_output.vel[1] << " "
                  << nav_output.vel[2] << " ";
      auto yaw =
          nav_output.att[2] >= 0 ? nav_output.att[2] : nav_output.att[2] + 360;
      result_data << nav_output.att[0] << " " << nav_output.att[1] << " " << yaw
                  << endl;
    }

    // terminal output
    cout.precision(12);
    cout << "---" << endl;
    cout << "my position: " << endl;
    cout << nav_output.timestamp << endl;
    cout << nav_output.pos.transpose() << endl;

    cout << "truth: " << endl;
    cout << truth_vec[j].second << endl;
    cout << truth_vec[j].pos.transpose() << endl;
  }

  return 0;
}


// Declares State class and model hyperparameters

#ifndef STATE_HPP
#define STATE_HPP

#include <Eigen/Dense>
#include <gsl/gsl_randist.h>

///////////////////////////
// Model hyperparameters //
///////////////////////////

// Vague gamma prior on CRP concentration
#define VG_SHAPE 1.0
#define VG_RATE 0.05
#define VG_INIT 1.0

// Normal Gamma prior on cluster-level action parameters
#define NG_SHAPE 1.0
#define NG_RATE 0.05
#define NG_NU 0.5
#define NG_MU 0.0

///////////////////////
// Class declaration //
///////////////////////

class State
{

public:
    // Random number generator
    const gsl_rng* rng;

    // Model variables               // Shape
    double conc; // DP concentration
    Eigen::ArrayXi  z;               // (NUM_OBJECTS)
    Eigen::ArrayXXi events_pos;      // (NUM_ACTIONS,NUM_OBJECTS)
    Eigen::ArrayXXi events_neg;      // (NUM_ACTIONS,NUM_OBJECTS)
    Eigen::ArrayXXd strengths;       // (NUM_ACTIONS,NUM_OBJECTS)
    Eigen::ArrayXi  counts;          // (NUM_CLUSTERS)
    Eigen::ArrayXXd action_means;    // (NUM_ACTIONS,NUM_CLUSTERS)
    Eigen::ArrayXXd action_precs;    // (NUM_ACTIONS,NUM_CLUSTERS)

    // Constructor
    State(const gsl_rng *r,
          Eigen::Ref<Eigen::ArrayXXi> events_pos,
          Eigen::Ref<Eigen::ArrayXXi> events_neg);

    // Resample the state
    void update(int num_proposals);

    // For debugging
    void print_vars(bool show_events = false);

private:
    // Internal update functions
    void update_conc();
    void update_z(int num_proposals);
    void update_action_params();
    void update_strengths(int num_proposals);
};

#endif // End include guard


#include <unordered_map>
#include <iostream>

#include <Eigen/Core>

#include "DBSCAN.h"

#include "LineGroup.h"

#include "LineDetection.h"

namespace localization
{

void LineDetection::filterLineCluster(const std::vector<Line>& cluster)
{
    if (cluster.empty()) return;

    std::vector<LineGroup> orientationFiltered;
    filterByOrientation(cluster, orientationFiltered);

    std::vector<LineGroup> proximityFiltered;
    for (int i = 0; i < orientationFiltered.size(); ++i)
    {
        filterByProximity(orientationFiltered[i], proximityFiltered);
    }

    filteredLines_.clear();
    for (int i = 0; i < proximityFiltered.size(); ++i)
    {
        filteredLines_.push_back(proximityFiltered[i].convertToLine());
    }
}

void LineDetection::filterByOrientation(const std::vector<Line>& cluster,
                                        std::vector<LineGroup>& filtered)
{
    for (int i = 0; i < cluster.size(); ++i)
    {
        groupByOrientation(cluster[i], filtered);
    }
}

void LineDetection::groupByOrientation(const Line& line, std::vector<LineGroup>& groups)
{
    bool groupFound = false;
    for (int i = 0; i < groups.size() && !groupFound; ++i)
    {
        if (groups[i].isCollateral(line, 0.80))
        {
            groups[i].add(line);
            groupFound = true;
        }
    }

    if (!groupFound)
    {
        groups.push_back(LineGroup(line));
    }
}

void LineDetection::filterByProximity(const LineGroup& group,
                                      std::vector<LineGroup>& filtered)
{
    std::vector<const Line*> lines = group.getLines();
    std::vector<Vector> centroids(lines.size());
    for (int i = 0; i < lines.size(); ++i)
    {
        centroids[i] = lines[i]->getCentroid();
    }

    std::vector<int> clusterMemberships;
    DBSCAN::DBSCAN(centroids, 50, 1, clusterMemberships);

    parseClusterMemberships(clusterMemberships, lines, filtered);
}

void LineDetection::parseClusterMemberships(const std::vector<int>& clusterMemberships,
                                            const std::vector<const Line*>& cluster,
                                                  std::vector<LineGroup>& filtered)
{
    if (cluster.size() != clusterMemberships.size()) return;

    std::unordered_map<int, LineGroup> groups;

    std::unordered_map<int, LineGroup>::iterator it = groups.end();
    for (int i = 0; i < clusterMemberships.size(); ++i)
    {
        int groupId = clusterMemberships[i];
        it = groups.find(groupId);
        if (it != groups.end())
        {
            it->second.add(*cluster[i]);
        }
        else
        {
            groups.insert({ groupId, LineGroup(*cluster[i]) });
        }
    }

    for (it = groups.begin(); it != groups.end(); it++)
    {
        filtered.push_back(it->second);
    }
}

}


#include <Eigen/Geometry>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <tf_conversions/tf_eigen.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <robot_kf/WheelOdometry.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <robot_kf/robot_kf.h>

using Eigen::Vector3d;
using Eigen::Matrix3d;
typedef Eigen::Matrix<double, 6, 6> Matrix6d;

static double const big = 99999.0;

static boost::shared_ptr<tf::TransformListener> sub_tf;
static boost::shared_ptr<tf::TransformBroadcaster> pub_tf;
static ros::Subscriber sub_compass, sub_encoders, sub_gps;
static ros::Publisher pub_fused;

static robot_kf::KalmanFilter kf;
static geometry_msgs::Twist velocity;

static std::string global_frame_id, odom_frame_id, base_frame_id, offset_frame_id;
static boost::shared_ptr<tf::Transform> offset_tf;

static ros::Time prev_odom_time;
static ros::Time prev_compass_time;

static void publish(ros::Time stamp)
{
    Vector3d const state = kf.getState();
    Matrix3d const cov = kf.getCovariance();

    // Wrap the fused state estimate in a ROS message.
    geometry_msgs::PoseStamped fused_base;
    fused_base.header.stamp    = stamp;
    fused_base.header.frame_id = odom_frame_id;
    fused_base.pose.position.x = state[0];
    fused_base.pose.position.y = state[1];
    fused_base.pose.position.z = 0.0;
    fused_base.pose.orientation = tf::createQuaternionMsgFromYaw(state[2]);

    /*
     * We actually want to publish the /map to /base_footprint transform, but
     * this would cause /base_footprint to have two parents. Instead, we need
     * to publish the /map to /odom transform. See this chart:
     *
     * /map --[T1]--> /odom --[T2]--> /base_footprint
     * /map ----------[T3]----------> /base_footprint
     *
     * The output of the Kalman filter is T3, but we want T1. We find T1 by
     * computing T1 = T3 * inv(T2), where T2 is provided by the odometry
     * source.
     */
    tf::Vector3 const pos(state[0], state[1], 0);
    tf::Quaternion const ori = tf::createQuaternionFromYaw(state[2]);
    tf::Transform t3(ori, pos);

    tf::StampedTransform t2;
    try {
        sub_tf->waitForTransform(odom_frame_id, base_frame_id, stamp, ros::Duration(1.0));
        sub_tf->lookupTransform(odom_frame_id, base_frame_id, stamp, t2);
    } catch (tf::TransformException const &e) {
        ROS_WARN("%s", e.what());
        return;
    }

    tf::Transform const t1 = t3 * t2.inverse();
    tf::StampedTransform transform(t1, stamp, global_frame_id, odom_frame_id);
    pub_tf->sendTransform(transform);

    // Relative frame to fix RViz numerical stability issues.
    if (offset_tf) {
        tf::StampedTransform offset_stamped(*offset_tf, stamp, global_frame_id, offset_frame_id);
        pub_tf->sendTransform(offset_stamped);
    }

    // Publish the odometry message.
    nav_msgs::Odometry msg;
    msg.header.stamp = stamp;
    msg.header.frame_id = global_frame_id;
    msg.child_frame_id = base_frame_id;
    msg.pose.pose =  fused_base.pose;

    // Use the covariance estimated by the filter.
    Eigen::Map<Matrix6d> cov_out(&msg.pose.covariance.front());
    cov_out.topLeftCorner<2, 2>() = cov.topLeftCorner<2, 2>();
    cov_out(5, 5) = cov(2, 2);

    // TODO: Estimate the covariance of the encoders.
    msg.twist.twist = velocity;
    msg.twist.covariance[0] = -1;
    pub_fused.publish(msg);
}

static void updateCompass(sensor_msgs::Imu const &msg)
{
    if (msg.header.frame_id != base_frame_id) {
        ROS_ERROR_THROTTLE(10, "Imu message must have frame_id '%s'",
                           base_frame_id.c_str());
        return;
    }

    double const yaw = tf::getYaw(msg.orientation);
    Eigen::Map<Eigen::Matrix3d const> cov_raw(&msg.orientation_covariance.front());

    kf.update_compass(yaw, cov_raw(2, 2));
}

static void updateEncoders(robot_kf::WheelOdometry const &msg)
{
    if (msg.header.frame_id != base_frame_id) {
        ROS_ERROR_THROTTLE(10, "WheelOdometry message must have frame_id '%s'", base_frame_id.c_str());
        return;
    } else if (msg.separation <= 0) {
        ROS_ERROR_THROTTLE(10, "Wheel separation in WheelOdometry message must be positive.");
        return;
    }

    Eigen::Vector2d const z_raw = (Eigen::Vector2d() <<
        msg.left.movement, msg.right.movement).finished();
    Eigen::Matrix2d const cov_z_raw = (Eigen::Matrix2d() <<
        msg.left.variance, 0.0, 0.0, msg.right.variance).finished();

    // Prevent the encoders from double-counting rotation in conjunction with
    // the compass.
    Eigen::Vector2d z;
    Eigen::Matrix2d cov_z;
    if (prev_odom_time < prev_compass_time) {
        ros::Duration const dt_encoders = msg.header.stamp - prev_odom_time;
        ros::Duration const dt_compass  = prev_odom_time - prev_compass_time;
        ros::Duration const dt_ignore = dt_encoders - dt_compass;
        double const ratio = 1.0 - dt_ignore.toSec() / dt_encoders.toSec();
        z = ratio * z_raw;
        cov_z = ratio * cov_z_raw;
    } else {
        z = z_raw;
        cov_z = cov_z_raw;
    }
    // FIXME: Correct the GPS in the same way.

    // Compute velocity using the wheel encoders. Theoretically the GPS and
    // compass could also be used to estimate these velocities, but those
    // estimates are too noisy to justify the complexity of adding two more
    // state variables.
    double const movement_linear = (z[1] + z[0]) / 2;
    double const movement_angular = (z[1] - z[0]) / msg.separation;
    double const delta_time = msg.timestep.toSec();
    velocity.linear.x = movement_linear / delta_time;
    velocity.angular.z = movement_angular / delta_time;

    kf.update_encoders(z, cov_z, msg.separation);
    publish(msg.header.stamp);
}

static void updateGps(nav_msgs::Odometry const &msg)
{
    if (msg.header.frame_id != global_frame_id) {
        ROS_ERROR_THROTTLE(10, "GPS message must have frame_id '%s'",
                           global_frame_id.c_str());
        return;
    }

    Eigen::Vector2d const z_raw = (Eigen::Vector2d() <<
        msg.pose.pose.position.x, msg.pose.pose.position.y).finished();
    Eigen::Map<Eigen::Matrix<double, 6, 6> const> cov6_raw(
        &msg.pose.covariance.front()
    );
    Eigen::Matrix2d const cov_raw = cov6_raw.topLeftCorner<2, 2>();

    /* The GPS gives a position relative to child_frame_id, which is located at
     * the center of the GPS antennna. However, the Kalman filter only manipulates
     * quantities in base_frame_id. So the situation is:
     *
     *     /map --[ T1 ]--> /gps <--[ T2 ]-- /base_footprint
     *     /map -----------[ T3 ]----------> /base_footprint
     *
     * Transformation T1 is provided by the GPS, T2 is a static transformation
     * provided by the URDF, and T3 is the desired translation. Therefore, we
     * can find T3 by: T3 = T1 * T2^{-1}.
     */
    tf::StampedTransform t2_inv;
    try {
        sub_tf->lookupTransform(msg.child_frame_id, base_frame_id, ros::Time(0), t2_inv);
    } catch (tf::TransformException const &e) {
        ROS_WARN("%s", e.what());
        return;
    }

    tf::Vector3 const z_bt_raw(z_raw[0], z_raw[1], 0);
    tf::Vector3 const z_bt = t2_inv * z_bt_raw;
    Eigen::Vector2d const z = (Eigen::Vector2d() << z_bt[0], z_bt[1]).finished();
    // TODO: Transform the covariance matrix using the rotation matrix.
    Eigen::Matrix2d const cov = cov_raw;

    if (!offset_tf) {
        tf::Vector3 const pos_offset(z[0], z[1], 0.0);
        tf::Quaternion const ori_offset(0.0, 0.0, 0.0, 1.0);
        offset_tf = boost::make_shared<tf::Transform>(ori_offset, pos_offset);
        ROS_INFO("Initialized %s to (%f, %f)", offset_frame_id.c_str(), z[0], z[1]);
    }

    kf.update_gps(z, cov);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "robot_kf_node");

    // Initialize the skeleton twist message.
    velocity.linear.y = 0;
    velocity.linear.z = 0;
    velocity.angular.x = 0;
    velocity.angular.y = 0;

    ros::NodeHandle nh, nh_node("~");
    nh_node.param<std::string>("global_frame_id", global_frame_id, "/map");
    nh_node.param<std::string>("odom_frame_id", odom_frame_id, "/odom");
    nh_node.param<std::string>("base_frame_id", base_frame_id, "/base_footprint");
    nh_node.param<std::string>("offset_frame_id", offset_frame_id, "/map_offset");

    sub_tf = boost::make_shared<tf::TransformListener>();
    pub_tf = boost::make_shared<tf::TransformBroadcaster>();
    sub_compass  = nh.subscribe("compass", 1, &updateCompass);
    sub_encoders = nh.subscribe("wheel_odom", 1, &updateEncoders);
    sub_gps      = nh.subscribe("gps", 1, &updateGps);
    pub_fused    = nh.advertise<nav_msgs::Odometry>("odom_fused", 100);

    ros::spin();
    return 0;
}


// Copyright (C) 2018 Thanaphon Chavengsaksongkram <as12production@gmail.com>, He Sun <he.sun@ed.ac.uk>
// This file is subject to the license terms in the LICENSE file
// found in the top-level directory of this distribution.

#ifndef GSPARSE_ER_APPROXIMATEER_HPP
#define GSPARSE_ER_APPROXIMATEER_HPP

#include "../Interface/EffectiveResistance.hpp"
#include "../Config.hpp"
#include "../Util/JL.hpp"  // Building Random Projection

// Approximate ER Policies
#include "Policy/AproxERSLMJacobiCG.hpp"

#include <Eigen/Dense>
#include <Eigen/Sparse>
namespace gSparse
{
    namespace ER
    {

        /// \ingroup EffectiveResistance
        ///
        /// This class calculates Approxmation of Graph's Effective Resistance.
        /// There are many approach to this problem. Such approach is set via class Policy.
        template <typename Policy>
        class _ApproximateER : public IEffectiveResistance, private Policy
        {
        public:
            using Policy::_calculateER;
            inline gSparse::COMPUTE_INFO CalculateER( gSparse::PrecisionRowMatrix & er,
                const gSparse::Graph & graph)
            {
                return _calculateER(er, graph);
            }
        };
        typedef _ApproximateER<Policy::AproxERSLMJacobiCG> ApproximateER; //<! ApproximateER class to calculate effective resistance
    }
}
#endif


//============================================================================
//
// This file is part of the Thea toolkit.
//
// This software is distributed under the BSD license, as detailed in the
// accompanying LICENSE.txt file. Portions are derived from other works:
// their respective licenses and copyright information are reproduced in
// LICENSE.txt and/or in the relevant source files.
//
// Author: Siddhartha Chaudhuri
// First version: 2014
//
//============================================================================

#ifndef __Thea_Algorithms_Icp3_hpp__
#define __Thea_Algorithms_Icp3_hpp__

#include "../Common.hpp"
#include "BvhN.hpp"
#include "MetricL2.hpp"
#include "PointTraitsN.hpp"
#include "../AffineTransformN.hpp"
#include "../Math.hpp"
#include "../MatVec.hpp"
#include "../HyperplaneN.hpp"
#include <Eigen/SVD>

namespace Thea {
namespace Algorithms {

/** Align two sets of points in 3D using the Iterative Closest Point (ICP) algorithm. */
template <typename ScalarT = Real>
class Icp3
{
  private:
    typedef Vector<3, ScalarT>       VectorT;  ///< 3D vector.
    typedef Matrix<3, 3, ScalarT>    MatrixT;  ///< 3x3 matrix.
    typedef HyperplaneN<3, ScalarT>  PlaneT;   ///< Plane in 3-space.

    /** The default weight per point. */
    template <typename T> struct DefaultWeightFunc
    {
      double getTranslationWeight(T const & t) const { return 1; }
      double getRotationWeight(T const & t) const { return 1; }
    };

  public:
    typedef AffineTransformN<3, ScalarT> AffineTransformT;  ///< Affine transform in 3 dimensions.

    /**
     * Constructor.
     *
     * @param fractional_error_threshold_ The maximum fractional change in error to determine convergence (negative for
     *   default).
     * @param min_iterations_ The minimum number of iterations (negative for default).
     * @param max_iterations_ The maximum number of iterations (negative for default).
     * @param verbose_ If true, print extra debugging information.
     */
    Icp3(ScalarT fractional_error_threshold_ = -1, intx min_iterations_ = -1, intx max_iterations_ = -1, bool verbose_ = false)
    : fractional_error_threshold(fractional_error_threshold_), min_iterations(min_iterations_), max_iterations(max_iterations_),
      has_up(false), verbose(verbose_)
    {
      if (fractional_error_threshold < 0) fractional_error_threshold = 0.0001;
      if (min_iterations < 0) min_iterations = 3;
      if (max_iterations < 0) max_iterations = 10;
    }

    /** Set the up vector. Subsequent alignments will only rotate around the up vector. */
    void setUpVector(VectorT const & up_) { up = up_.normalized(); has_up = true; }

    /** Check if the up vector has been set. */
    bool hasUpVector() const { return has_up; }

    /**
     * Get the up vector, if it has been set.
     *
     * @see hasUpVector();
     */
    VectorT const & getUpVector() const { return up; }

    /** Clear the up vector. Subsequent alignments will be unconstrained. */
    void clearUpVector() { has_up = false; }

    /** Find the transform that best aligns the point set \a from to the point set \a to. */
    template <typename FromT, typename ToT>
    AffineTransformT align(intx from_num_pts, FromT const * from, intx to_num_pts, ToT const * to, ScalarT * error = nullptr)
                     const
    {
      return align(from_num_pts, from, (DefaultWeightFunc<FromT> *)nullptr, to_num_pts, to, error);
    }

    /**
     * Find the transform that best aligns the point set \a from to the point set \a to, with a per-point weight assigned to the
     * cost of aligning each element of \a from.
     */
    template <typename FromT, typename ToT, typename FromWeightFuncT>
    AffineTransformT align(intx from_num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                           intx to_num_pts, ToT const * to, ScalarT * error = nullptr) const
    {
      if (from_num_pts <= 0 || to_num_pts <= 0)
        return AffineTransformT::identity();

      BvhN<ToT, 3, ScalarT> to_bvh(to, to + to_num_pts);
      to_bvh.enableNearestNeighborAcceleration();

      return align(from_num_pts, from, from_weight_func, to_bvh, error);
    }

    /** Find the transform that best aligns the point set \a from to the proximity query structure \a to. */
    template <typename FromT, typename ToProximityQueryStructureT>
    AffineTransformT align(intx from_num_pts, FromT const * from, ToProximityQueryStructureT const & to,
                           ScalarT * error = nullptr) const
    {
      return align(from_num_pts, from, (DefaultWeightFunc<FromT> *)nullptr, to, error);
    }

    /**
     * Find the transform that best aligns the point set \a from to the proximity query structure \a to, with a per-point weight
     * assigned to the cost of aligning each element of \a from.
     */
    template <typename FromT, typename ToProximityQueryStructureT, typename FromWeightFuncT>
    AffineTransformT align(intx from_num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                           ToProximityQueryStructureT const & to, ScalarT * error = nullptr) const
    {
      return align(from_num_pts, from, from_weight_func, nullptr, to, nullptr, error);
    }

    /**
     * Find the transform that best aligns the point set \a from to the point set \a to, assuming both sets have known symmetry
     * planes. The computed alignment will ensure the symmetry planes coincide.
     */
    template <typename FromT, typename ToT>
    AffineTransformT alignSymmetric(intx from_num_pts, FromT const * from, PlaneT const & from_symmetry_plane,
                                    intx to_num_pts, ToT const * to, PlaneT const & to_symmetry_plane,
                                    ScalarT * error = nullptr) const
    {
      return alignSymmetric(from_num_pts, from, (DefaultWeightFunc<FromT> *)nullptr, from_symmetry_plane,
                            to_num_pts, to, to_symmetry_plane, error);
    }

    /**
     * Find the transform that best aligns the point set \a from to the point set \a to, with a per-point weight assigned to the
     * cost of aligning each element of \a from, and assuming both sets have known symmetry planes. The computed alignment will
     * ensure the symmetry planes coincide.
     */
    template <typename FromT, typename ToT, typename FromWeightFuncT>
    AffineTransformT alignSymmetric(intx from_num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                                    PlaneT const & from_symmetry_plane,
                                    intx to_num_pts, ToT const * to, PlaneT const & to_symmetry_plane,
                                    ScalarT * error = nullptr) const
    {
      if (from_num_pts <= 0 || to_num_pts <= 0)
        return AffineTransformT::identity();

      BvhN<ToT, 3, ScalarT> to_bvh(to, to + to_num_pts);
      to_bvh.enableNearestNeighborAcceleration();

      return alignSymmetric(from_num_pts, from, from_weight_func, from_symmetry_plane, to_bvh, to_symmetry_plane, error);
    }

    /**
     * Find the transform that best aligns the point set \a from to the proximity query structure \a to, assuming both \a from
     * and \a to have known symmetry planes. The computed alignment will ensure the symmetry planes coincide.
     */
    template <typename FromT, typename ToProximityQueryStructureT>
    AffineTransformT alignSymmetric(intx from_num_pts, FromT const * from, PlaneT const & from_symmetry_plane,
                                    ToProximityQueryStructureT const & to, PlaneT const & to_symmetry_plane,
                                    ScalarT * error = nullptr) const
    {
      return alignSymmetric(from_num_pts, from, (DefaultWeightFunc<FromT> *)nullptr, from_symmetry_plane, to, to_symmetry_plane,
                            error);
    }

    /**
     * Find the transform that best aligns the point set \a from to the proximity query structure \a to, with a per-point weight
     * assigned to the cost of aligning each element of \a from, and assuming both \a from and \a to have known symmetry planes.
     * The computed alignment will ensure the symmetry planes coincide.
     */
    template <typename FromT, typename ToProximityQueryStructureT, typename FromWeightFuncT>
    AffineTransformT alignSymmetric(intx from_num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                                    PlaneT const & from_symmetry_plane,
                                    ToProximityQueryStructureT const & to, PlaneT const & to_symmetry_plane,
                                    ScalarT * error = nullptr) const
    {
      return align(from_num_pts, from, from_weight_func, &from_symmetry_plane, to, &to_symmetry_plane, error);
    }

  private:
    /**
     * Find the transform that best aligns the point set \a from to the proximity query structure \a to, with a per-point weight
     * assigned to the cost of aligning each element of \a from.
     */
    template <typename FromT, typename ToProximityQueryStructureT, typename FromWeightFuncT>
    AffineTransformT align(intx from_num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                           PlaneT const * from_symmetry_plane, ToProximityQueryStructureT const & to,
                           PlaneT const * to_symmetry_plane, ScalarT * error = nullptr) const
    {
      if (verbose)
        THEA_CONSOLE << "Icp3(fractional_error_threshold = " << fractional_error_threshold
                     <<    ", min_iterations = " << min_iterations
                     <<    ", max_iterations = " << max_iterations << ')';

      if (from_num_pts <= 0)
      {
        if (error) *error = 0;
        return AffineTransformT::identity();
      }

      Array<VectorT> from_points((size_t)from_num_pts);
      Array<VectorT> to_points((size_t)from_num_pts);
      for (size_t i = 0; i < from_points.size(); ++i)
        from_points[i] = PointTraitsN<FromT, 3, ScalarT>::getPosition(from[i]);

      AffineTransformT old_tr = AffineTransformT::identity();
      ScalarT old_error = measureError(old_tr, from_num_pts, &from_points[0], from_weight_func, to, &to_points[0]);
      if (verbose) THEA_CONSOLE << "[Icp3] Initial error: " << old_error;

      if (old_error <= std::numeric_limits<ScalarT>::min())
      {
        if (error) *error = old_error;
        return old_tr;
      }

      ScalarT new_error = old_error;
      AffineTransformT new_tr = old_tr;
      for (intx i = 0; i < max_iterations; ++i)
      {
        // Align using the point mapping created by the last call to measureError()
        AffineTransformT inc_tr = alignOneStep(from_num_pts, &from_points[0], from_weight_func, from_symmetry_plane,
                                               &to_points[0], to_symmetry_plane);

        // Update the overall transform
        new_tr = inc_tr * old_tr;

        // Compute the new error and the new mapping between points
        if (i < max_iterations - 1 || error)
          new_error = measureError(inc_tr, from_num_pts, &from_points[0], from_weight_func, to, &to_points[0]);

        if (i < max_iterations - 1)
        {
          ScalarT frac_change = (old_error - new_error) / old_error;

          if (verbose)
            THEA_CONSOLE << "[Icp3] Iteration " << i << " error: " << new_error << " (fractional change: " << frac_change << ')';

          if (i >= min_iterations && frac_change < fractional_error_threshold)
          {
            if (frac_change > 0)  // we improved slightly
            {
              if (error) *error = new_error;
              return new_tr;
            }
            else  // the previous alignment was better
            {
              if (error) *error = old_error;
              return old_tr;
            }
          }

          for (size_t j = 0; j < from_points.size(); ++j)  // transform original points to prevent drift
            from_points[j] = new_tr * PointTraitsN<FromT, 3, ScalarT>::getPosition(from[j]);
        }
        else
        {
          if (verbose)
            THEA_CONSOLE << "[Icp3] Iteration " << i << " error: " << new_error;
        }

        old_error = new_error;
        old_tr = new_tr;
      }

      if (error) *error = new_error;
      return new_tr;
    }

    /**
     * Align a point set to a proximity query structure, in one ICP step, after finding the nearest neighbor of each point in
     * the first set in the structure.
     */
    template <typename FromT, typename ToProximityQueryStructureT, typename FromWeightFuncT>
    AffineTransformT alignOneStep(intx from_num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                                  PlaneT const * from_sym_plane, ToProximityQueryStructureT const & to,
                                  PlaneT const * to_sym_plane, VectorT * to_points) const
    {
      for (intx i = 0; i < from_num_pts; ++i)
      {
        intx index = to.template closestElement<MetricL2>(PointTraitsN<FromT, 3, ScalarT>::getPosition(from[i]), -1,
                                                          UniversalCompatibility(), nullptr, &to_points[i]);
        if (index < 0)
          throw Error(format("Icp3: Couldn't get nearest neighbor of source point %ld", i));
      }

      return alignOneStep(from_num_pts, from, from_weight_func, from_sym_plane, to_points, to_sym_plane);
    }

    /** Align one point set to another, in one ICP step, with a known bijective mapping between the points. */
    template <typename FromT, typename FromWeightFuncT>
    AffineTransformT alignOneStep(intx num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                                  PlaneT const * from_sym_plane, VectorT const * to, PlaneT const * to_sym_plane) const
    {
      // When both point sets have symmetry planes, we'll align the projections. This does *NOT* minimize the correct error
      // function, but provides a simple approximation to the true solution which is ok for now.
      bool use_symmetry = from_sym_plane && to_sym_plane;

      // Find centroids of the two sets
      VectorT p_mean = VectorT::Zero();
      VectorT q_mean = VectorT::Zero();
      ScalarT sum_weights = 0;
      for (intx i = 0; i < num_pts; ++i)
      {
        VectorT p = PointTraitsN<FromT, 3, ScalarT>::getPosition(from[i]);
        VectorT q = to[i];
        if (use_symmetry)
        {
          p = from_sym_plane->closestPoint(p);
          q = to_sym_plane->closestPoint(q);
        }

        if (from_weight_func)
        {
          ScalarT weight = (ScalarT)from_weight_func->getTranslationWeight(from[i]);
          p_mean += weight * p;
          q_mean += weight * q;
          sum_weights += weight;
        }
        else
        {
          p_mean += p;
          q_mean += q;
          sum_weights += 1;
        }
      }

      p_mean /= sum_weights;
      q_mean /= sum_weights;

      // Find cross-covariance matrix
      MatrixT cov; cov.setZero();
      for (intx i = 0; i < num_pts; ++i)
      {
        VectorT p = PointTraitsN<FromT, 3, ScalarT>::getPosition(from[i]);
        VectorT q = to[i];
        if (use_symmetry)
        {
          p = from_sym_plane->closestPoint(p);
          q = to_sym_plane->closestPoint(q);
        }

        VectorT dp = p - p_mean;
        VectorT dq = q - q_mean;

        if (has_up)  // remove the component in the up direction
        {
          dp = dp - (dp.dot(up) * up);
          dq = dq - (dq.dot(up) * up);
        }

        if (from_weight_func)
        {
          ScalarT weight = (ScalarT)from_weight_func->getRotationWeight(from[i]);
          dp *= weight;
          dq *= weight;
        }

        cov += (dp * dq.transpose());  // outer product
      }

      Eigen::JacobiSVD<MatrixT> svd(cov, Eigen::ComputeFullU | Eigen::ComputeFullV);
      MatrixT rot = svd.matrixU() * svd.matrixV().transpose();

      if (use_symmetry && rot.determinant() < 0)  // matching two planar projections can cause flips in the symmetry plane
      {
        // Generate the transformation that flips in the (origin-centered) target symmetry plane
        VectorT n = to_sym_plane->getNormal();
        MatrixT nn = n * n.transpose();  // outer product
        MatrixT flip = MatrixT::Identity() - static_cast<ScalarT>(2) * nn;
        rot = flip * rot;
      }

      VectorT trans = q_mean - rot * p_mean;

      return AffineTransformT(rot, trans);
    }

    /** Measure the alignment error of a mapping from each point of \a from to its nearest neighbor in \a to. */
    template <typename FromT, typename ToProximityQueryStructureT, typename FromWeightFuncT>
    static ScalarT measureError(AffineTransformT const & tr,
                                intx from_num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                                ToProximityQueryStructureT const & to, VectorT * to_points)
    {
      if (from_num_pts <= 0)
        return 0;

      for (intx i = 0; i < from_num_pts; ++i)
      {
        intx index = to.template closestElement<MetricL2>(PointTraitsN<FromT, 3, ScalarT>::getPosition(from[i]), -1,
                                                          UniversalCompatibility(), nullptr, &to_points[i]);
        if (index < 0)
          throw Error(format("Icp3: Couldn't get nearest neighbor of source point %ld", i));
      }

      return measureError(tr, from_num_pts, from, from_weight_func, to_points);
    }

    /**
     * Measure the alignment error of a mapping from each point of \a from to each point of \a to, with a known bijective
     * mapping between them.
     */
    template <typename FromT, typename FromWeightFuncT>
    static ScalarT measureError(AffineTransformT const & tr,
                                intx num_pts, FromT const * from, FromWeightFuncT * from_weight_func,
                                VectorT const * to)
    {
      if (num_pts <= 0)
        return 0;

      VectorT p_mean = VectorT::Zero();
      VectorT q_mean = VectorT::Zero();

      ScalarT sum_weights = 0;
      for (intx i = 0; i < num_pts; ++i)
      {
        VectorT p = PointTraitsN<FromT, 3, ScalarT>::getPosition(from[i]);
        VectorT const & q = to[i];

        if (from_weight_func)
        {
          ScalarT weight = (ScalarT)from_weight_func->getTranslationWeight(from[i]);
          p_mean += weight * p;
          q_mean += weight * q;
          sum_weights += weight;
        }
        else
        {
          p_mean += p;
          q_mean += q;
          sum_weights += 1;
        }
      }

      p_mean /= sum_weights;
      q_mean /= sum_weights;

      ScalarT err = (tr * p_mean - q_mean).squaredNorm();

      for (intx i = 0; i < num_pts; ++i)
      {
        VectorT p = PointTraitsN<FromT, 3, ScalarT>::getPosition(from[i]);
        VectorT q = to[i];

        VectorT dp = p - p_mean;
        VectorT dq = q - q_mean;
        if (from_weight_func)
        {
          ScalarT weight = (ScalarT)from_weight_func->getRotationWeight(from[i]);
          err += weight * weight * (tr.getLinear() * dp - dq).squaredNorm();
        }
        else
          err += (tr.getLinear() * dp - dq).squaredNorm();
      }

      return err;
    }

    double fractional_error_threshold;  ///< Maximum fractional change in error to determine convergence.
    intx min_iterations;  ///< Minimum number of iterations.
    intx max_iterations;  ///< Maximum number of iterations.
    bool has_up;
    VectorT up;
    bool verbose;  ///< Print lots of debugging information?

}; // class Icp3

} // namespace Algorithms
} // namespace Thea

#endif


#include "opttmp/vectorization/register_tiling.hpp"

#include <cassert>

#include <boost/align/aligned_allocator.hpp>
using boost::alignment::aligned_allocator;

#include <Vc/Vc>
using Vc::double_v;
constexpr size_t total_width = 16;
constexpr size_t blocking = total_width / double_v::size();

using namespace opttmp::vectorization;

constexpr size_t array_length = 8192 * 2 * 2 * 2;
constexpr size_t iterations = array_length / total_width;

int main(void) {
  std::vector<double, aligned_allocator<double, 32>> A(array_length);
  std::vector<double, aligned_allocator<double, 32>> B(array_length);
  for (size_t i = 0; i < array_length; i++) {
    A[i] = static_cast<double>(i);
  }
  for (size_t i = 0; i < array_length; i++) {
    B[i] = static_cast<double>(i);
  }

  using reg_array = register_array<double_v, blocking>;
  reg_array r = 0.0;
  for (size_t i = 0; i < iterations; i++) {
    reg_array a_reg(A.data() + i * total_width, Vc::flags::vector_aligned);
    reg_array b_reg(B.data() + i * total_width, Vc::flags::vector_aligned);
    r += a_reg * b_reg;
  }
  for (size_t i = 0; i < r.size(); i++) {
    std::cout << "r[" << i << "]: " << r[i] << std::endl;
  }
}

  // reg_array a(data_up.data(), Vc::flags::vector_aligned);
  // reg_array b = 2.0;
  // reg_array c = 3.141;
	  // for (size_t i = 0; i < 1000000; i++) {
  //   // r += c * (a - b);
  //   r = c * (r - a);
  // }


//
// Copyright (c) 2020 INRIA
//

#ifndef __pinocchio_math_mutliprecision_mpfr_hpp__
#define __pinocchio_math_mutliprecision_mpfr_hpp__

#include "pinocchio/math/multiprecision.hpp"
#include "pinocchio/math/sincos.hpp"

#include <boost/serialization/nvp.hpp>
#include <boost/multiprecision/mpfr.hpp>

namespace pinocchio
{
template <
    unsigned S_digits10, boost::multiprecision::mpfr_allocation_type S_alloc,
    boost::multiprecision::expression_template_option S_et, unsigned C_digits10,
    boost::multiprecision::mpfr_allocation_type C_alloc,
    boost::multiprecision::expression_template_option C_et, unsigned X_digits10,
    boost::multiprecision::mpfr_allocation_type X_alloc,
    boost::multiprecision::expression_template_option X_et>
struct SINCOSAlgo<
    boost::multiprecision::number<
        boost::multiprecision::mpfr_float_backend<X_digits10, X_alloc>, X_et>,
    boost::multiprecision::number<
        boost::multiprecision::mpfr_float_backend<S_digits10, S_alloc>, S_et>,
    boost::multiprecision::number<
        boost::multiprecision::mpfr_float_backend<C_digits10, C_alloc>, C_et>>
{
  static void run(
      boost::multiprecision::number<
          boost::multiprecision::mpfr_float_backend<X_digits10, X_alloc>,
          X_et> const& a,
      boost::multiprecision::number<
          boost::multiprecision::mpfr_float_backend<S_digits10, S_alloc>, S_et>*
          sa,
      boost::multiprecision::number<
          boost::multiprecision::mpfr_float_backend<C_digits10, C_alloc>, C_et>*
          ca)
  {
    mpfr_srcptr x_mpfr((a.backend().data()));
    mpfr_ptr s_mpfr(sa->backend().data());
    mpfr_ptr c_mpfr(ca->backend().data());
    mpfr_sin_cos(s_mpfr, c_mpfr, x_mpfr, MPFR_RNDN);
  }
};
}  // namespace pinocchio

#endif  // ifndef __pinocchio_math_mutliprecision_hpp__


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#define _USE_MATH_DEFINES
#include <cmath>

#include <exception>
#include <map>

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>

#include "rttbTCPLQModel.h"
#include "rttbDvhBasedModels.h"
#include "rttbIntegration.h"
#include "rttbInvalidParameterException.h"
#include "rttbExceptionMacros.h"

namespace rttb
{

	namespace models
	{

		TCPLQModel::TCPLQModel(): TCPModel() {
			_name = "TCPLQModel";
			fillParameterMap();
		}

		TCPLQModel::TCPLQModel(DVHPointer aDVH, BioModelParamType aAlphaMean, BioModelParamType aBeta,
		                       BioModelParamType aRho,
		                       int aNumberOfFractions): TCPModel(aDVH, aNumberOfFractions), _alphaMean(aAlphaMean),
			_alphaVariance(0),
			_alpha_beta(aAlphaMean / aBeta), _rho(aRho)  {
			_name = "TCPLQModel";
			fillParameterMap();
		}

		TCPLQModel::TCPLQModel(DVHPointer aDVH, BioModelParamType aRho, int aNumberOfFractions,
		                       BioModelParamType aAlpha_Beta,
		                       BioModelParamType aAlphaMean, BioModelParamType aAlphaVariance): TCPModel(aDVH, aNumberOfFractions),
			_alphaMean(aAlphaMean),
			_alphaVariance(aAlphaVariance), _alpha_beta(aAlpha_Beta), _rho(aRho) {
			fillParameterMap();
			_name = "TCPLQModel";
		}

		void TCPLQModel::setParameters(const BioModelParamType aAlphaMean,
		                               const BioModelParamType aAlpha_Beta,
		                               const BioModelParamType aRho, const BioModelParamType aAlphaVariance)
		{
			_alphaMean = aAlphaMean;
			_alphaVariance = aAlphaVariance;
			_alpha_beta = aAlpha_Beta;
			_rho = aRho;

			//reset _value, because parameters have changed.
			_value = 0;
		}

		void TCPLQModel::setAlpha(const BioModelParamType aAlphaMean,
		                          const BioModelParamType aAlphaVariance)
		{
			_alphaVariance = aAlphaVariance;
			_alphaMean = aAlphaMean;
		}

		void TCPLQModel::setAlphaBeta(const BioModelParamType aAlpha_Beta)
		{
			_alpha_beta = aAlpha_Beta;
		}

		void TCPLQModel::setRho(const BioModelParamType aRho)
		{
			_rho = aRho;
		}

		const BioModelParamType TCPLQModel::getAlphaBeta()
		{
			return _alpha_beta;
		}

		const BioModelParamType TCPLQModel::getAlphaMean()
		{
			return _alphaMean;
		}

		const BioModelParamType TCPLQModel::getAlphaVariance()
		{
			return _alphaVariance;
		}

		const BioModelParamType TCPLQModel::getRho()
		{
			return _rho;
		}

		long double TCPLQModel::calcTCPi(BioModelParamType aRho, BioModelParamType aAlphaMean, double vj,
		                                 double bedj)
		{
			return exp(-aRho * vj * exp(-aAlphaMean * bedj));
		}

		long double TCPLQModel::calcTCP(std::map<rttb::DoseTypeGy, rttb::DoseCalcType> aBEDDVH,
		                                BioModelParamType aRho,
		                                BioModelParamType aAlphaMean, double aDeltaV)
		{
			std::map<rttb::DoseTypeGy, rttb::DoseCalcType>::iterator it;
			long double tcp = 1;

			for (it = aBEDDVH.begin(); it != aBEDDVH.end(); ++it)
			{
				long double tcpi = this->calcTCPi(aRho, aAlphaMean, (*it).second * aDeltaV, (*it).first);
				tcp = tcp * tcpi;
			}

			return tcp;
		}

		long double TCPLQModel::calcTCPAlphaNormalDistribution(
		    std::map<rttb::DoseTypeGy, rttb::DoseCalcType> aBEDDVH,
		    BioModelParamType aRho, BioModelParamType aAlphaMean,
		    BioModelParamType aAlphaVariance, double aDeltaV)
		{

			std::map<rttb::DoseTypeGy, rttb::DoseCalcType>::iterator it;
			std::vector<DoseCalcType> volumeV2;
			std::vector<DoseTypeGy> bedV2;
			int i = 0;

			for (it = aBEDDVH.begin(); it != aBEDDVH.end(); ++it)
			{
				volumeV2.push_back((*it).second * aDeltaV);
				bedV2.push_back((*it).first);
				i++;
			}

			struct TcpParams params = {aAlphaMean, aAlphaVariance, aRho, volumeV2, bedV2};

			double result = integrateTCP(0, params);

			if (result == -100)
			{
				std::cerr << "Integration error!\n";
				return -1;
			}
			else
			{
				long double tcp = 1 / (pow(2 * M_PI, 0.5) * _alphaVariance) * result;

				return tcp;
			}
		}

		BioModelValueType TCPLQModel::calcModel(const double doseFactor)
		{
			core::DVH variantDVH = core::DVH(_dvh->getDataDifferential(),
			                                 (DoseTypeGy)(_dvh->getDeltaD() * doseFactor),
			                                 _dvh->getDeltaV(), "temporary", "temporary");
			auto spDVH = boost::make_shared<core::DVH>(variantDVH);

			BioModelValueType value = 0;

			if (_alphaVariance == 0)
			{
				if (_alphaMean <= 0 || _alpha_beta <= 0 || _rho <= 0)
				{
					throw core::InvalidParameterException("Parameter invalid: alpha, alpha/beta, rho and number of fractions must >0!");
				}

				if (_numberOfFractions <= 1)
				{
					throw core::InvalidParameterException("Parameter invalid: numberOfFractions must be >1! The dvh should be an accumulated-dvh of all fractions, not a single fraction-dvh!");
				}

				std::map<rttb::DoseTypeGy, rttb::DoseCalcType> dataBED = calcBEDDVH(spDVH, _numberOfFractions,
				        _alpha_beta);

				value = (BioModelValueType)this->calcTCP(dataBED, _rho, _alphaMean, variantDVH.getDeltaV());
				return value;
			}

			//if alpha normal distribution
			else
			{
				if (this->_alpha_beta <= 0 || this->_alphaMean <= 0 || this->_alphaVariance < 0 || _rho <= 0)
				{
					throw core::InvalidParameterException("Parameter invalid: alpha/beta, alphaMean, rho and number of fractions must >0!");
				}

				if (_numberOfFractions <= 1)
				{
					throw core::InvalidParameterException("Parameter invalid: numberOfFractions must be >1! The dvh should be an accumulated-dvh of all fractions, not a single fraction-dvh!");
				}

				std::map<rttb::DoseTypeGy, rttb::DoseCalcType> dataBED = calcBEDDVH(spDVH, _numberOfFractions,
				        _alpha_beta);
				value = (BioModelValueType)(this->calcTCPAlphaNormalDistribution(dataBED, _rho, _alphaMean,
				                            _alphaVariance,
				                            variantDVH.getDeltaV()));
				return value;
			}
		}

		void TCPLQModel::setParameterVector(const ParamVectorType& aParameterVector)
		{
			if (aParameterVector.size() != 4)
			{
				throw core::InvalidParameterException("Parameter invalid: aParameterVector.size must be 4! ");
			}
			else
			{
				_alphaMean = aParameterVector.at(0);
				_alphaVariance = aParameterVector.at(1);
				_alpha_beta = aParameterVector.at(2);
				_rho = aParameterVector.at(3);
			}
		}

		void TCPLQModel::setParameterByID(const int aParamId, const BioModelParamType aValue)
		{
			if (aParamId == 0)
			{
				_alphaMean = aValue;
			}
			else if (aParamId == 1)
			{
				_alphaVariance = aValue;
			}
			else if (aParamId == 2)
			{
				_alpha_beta = aValue;
			}
			else if (aParamId == 3)
			{
				_rho = aValue;
			}
			else
			{
				throw core::InvalidParameterException("Parameter invalid: aParamID must be 0(alphaMean) or 1(alphaVariance) or 2(alpha_beta) or 3(rho)! ");
			}

		}

		const int TCPLQModel::getParameterID(const std::string& aParamName) const
		{
			if (aParamName == "alphaMean")
			{
				return 0;
			}
			else if (aParamName == "alphaVariance")
			{
				return 1;
			}
			else if (aParamName == "alpha_beta")
			{
				return 2;
			}
			else if (aParamName == "rho")
			{
				return 3;
			}
			else
			{
				rttbExceptionMacro(core::InvalidParameterException,
				                   << "Parameter name " << aParamName <<
				                   " invalid: it should be alphaMean or alphaVariance or alpha_beta or rho!");

			}
		}

		std::map<std::string, double> TCPLQModel::getParameterMap() const{
			return parameterMap;
		}

		void TCPLQModel::fillParameterMap(){
			parameterMap["numberOfFraction"] = getNumberOfFractions();
			parameterMap["alphaMean"] = getAlphaMean();
			parameterMap["alphaVariance"] = getAlphaVariance();
			parameterMap["alpha_beta"] = getAlphaBeta();
			parameterMap["rho"] = getRho();
		}

		std::string TCPLQModel::getModelType() const{
			return _name;
		}

	}//end namespace models
}//end namespace rttb


// Boost.Geometry Index
// Unit Test

// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.

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

#include <geometry_index_test_common.hpp>

#include <boost/geometry/index/detail/algorithms/path_intersection.hpp>

#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/segment.hpp>

//#include <boost/geometry/io/wkt/read.hpp>

template <typename Box, typename Linestring>
void test_path_intersection(Box const& box, Linestring const& path,
                            bool expected_result,
                            typename bg::default_length_result<Linestring>::type expected_dist)
{
    typename bgi::detail::default_path_intersection_distance_type<Box, Linestring>::type dist;

    bool value = bgi::detail::path_intersection(box, path, dist);
    BOOST_CHECK(value == expected_result);
    if ( value && expected_result )
        BOOST_CHECK_CLOSE(dist, expected_dist, 0.0001);

    if ( ::boost::size(path) == 2 )
    {
        typedef typename ::boost::range_value<Linestring>::type P;
        typedef bg::model::segment<P> Seg;
        typename bgi::detail::default_path_intersection_distance_type<Box, Seg>::type dist;
        Seg seg(*::boost::begin(path), *(::boost::begin(path)+1));
        bool value = bgi::detail::path_intersection(box, seg, dist);
        BOOST_CHECK(value == expected_result);
        if ( value && expected_result )
            BOOST_CHECK_CLOSE(dist, expected_dist, 0.0001);
    }
}

template <typename Box, typename Linestring>
void test_geometry(std::string const& wkt_g, std::string const& wkt_path,
                   bool expected_result,
                   typename bg::default_length_result<Linestring>::type expected_dist)
{
    Box box;
    bg::read_wkt(wkt_g, box);
    Linestring path;
    bg::read_wkt(wkt_path, path);
    test_path_intersection(box, path, expected_result, expected_dist);
}

void test_large_integers()
{
    typedef bg::model::point<int, 2, bg::cs::cartesian> int_point_type;
    typedef bg::model::point<double, 2, bg::cs::cartesian> double_point_type;

    bg::model::box<int_point_type> int_box;
    bg::model::box<double_point_type> double_box;
    typedef bg::model::linestring<int_point_type> IP;
    IP int_path;
    typedef bg::model::linestring<double_point_type> DP;
    DP double_path;

    std::string const str_box = "POLYGON((1536119 192000, 1872000 528000))";
    std::string const str_path = "LINESTRING(1535000 191000, 1873000 191000, 1873000 300000, 1536119 300000)";
    bg::read_wkt(str_box, int_box);
    bg::read_wkt(str_box, double_box);
    bg::read_wkt(str_path, int_path);
    bg::read_wkt(str_path, double_path);

    bg::default_length_result<IP>::type int_value;
    bool int_result = bgi::detail::path_intersection(int_box, int_path, int_value);
    bg::default_length_result<DP>::type double_value;
    bool double_result = bgi::detail::path_intersection(double_box, double_path, double_value);

    BOOST_CHECK(int_result == double_result);
    if ( int_result && double_result )
        BOOST_CHECK_CLOSE(int_value, double_value, 0.0001);
}

int test_main(int, char* [])
{
    typedef bg::model::point<int, 2, bg::cs::cartesian> P2ic;
    typedef bg::model::point<float, 2, bg::cs::cartesian> P2fc;
    typedef bg::model::point<double, 2, bg::cs::cartesian> P2dc;

    typedef bg::model::point<int, 3, bg::cs::cartesian> P3ic;
    typedef bg::model::point<float, 3, bg::cs::cartesian> P3fc;
    typedef bg::model::point<double, 3, bg::cs::cartesian> P3dc;

    typedef bg::model::linestring<P2ic> L2ic;
    typedef bg::model::linestring<P2fc> L2fc;
    typedef bg::model::linestring<P2dc> L2dc;

    typedef bg::model::linestring<P3ic> L3ic;
    typedef bg::model::linestring<P3fc> L3fc;
    typedef bg::model::linestring<P3dc> L3dc;

    // IMPORTANT! For 2-point linestrings comparable distance optimization is enabled!

    test_geometry<bg::model::box<P2ic>, L2ic>("POLYGON((0 1,2 4))", "LINESTRING(0 0, 2 5)", true, 1.0f/5);
    test_geometry<bg::model::box<P2fc>, L2fc>("POLYGON((0 1,2 4))", "LINESTRING(0 0, 2 5)", true, 1.0f/5);
    test_geometry<bg::model::box<P2dc>, L2dc>("POLYGON((0 1,2 4))", "LINESTRING(0 0, 2 5)", true, 1.0/5);
    test_geometry<bg::model::box<P3ic>, L3ic>("POLYGON((0 1 2,2 4 6))", "LINESTRING(0 0 0, 2 5 7)", true, 2.0f/7);
    test_geometry<bg::model::box<P3fc>, L3fc>("POLYGON((0 1 2,2 4 6))", "LINESTRING(0 0 0, 2 5 7)", true, 2.0f/7);
    test_geometry<bg::model::box<P3dc>, L3dc>("POLYGON((0 1 2,2 4 6))", "LINESTRING(0 0 0, 2 5 7)", true, 2.0/7);

    test_geometry<bg::model::box<P2fc>, L2fc>("POLYGON((0 1,2 4))", "LINESTRING(0 0, 1 0, 1 5)", true, 2);
    test_geometry<bg::model::box<P2fc>, L2fc>("POLYGON((0 1,2 4))", "LINESTRING(0 0, 3 0, 3 2, 0 2)", true, 6);
    test_geometry<bg::model::box<P2fc>, L2fc>("POLYGON((0 1,2 4))", "LINESTRING(1 2, 3 3, 0 3)", true, 0);

    test_large_integers();

    return 0;
}