// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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)
//
/// \file type_check_tree_test.cpp
/// \brief Add your file description here.

#include "mcrl2/data/data_specification.h"
#include "mcrl2/data/experimental/type_check_tree.h"
#include "mcrl2/data/parse.h"
#include "mcrl2/data/print.h"
#include "mcrl2/data/experimental/type_checker.h"
#include "mcrl2/data/typecheck.h"
#include "mcrl2/data/untyped_sort.h"
#include "mcrl2/utilities/text_utility.h"
#include <boost/test/included/unit_test_framework.hpp>
#include <iostream>
#include <sstream>

using namespace mcrl2;

void test_data_expression(const std::string& text, const std::string& variable_context = "", const std::string& expected_sort = "", bool expected_result = true)
{
  std::cout << "--------------------------------------------------------------------" << std::endl;
  std::cout << "text            = " << text << std::endl;
  std::cout << "variables       = " << variable_context << std::endl;
  std::cout << "expected sort   = " << expected_sort << std::endl;
  std::cout << "expected result = " << std::boolalpha << expected_result << std::endl;
  data::data_specification dataspec;
  core::parser p(parser_tables_mcrl2, core::detail::ambiguity_fn, core::detail::syntax_error_fn);
  unsigned int start_symbol_index = p.start_symbol_index("DataExpr");
  bool partial_parses = false;
  core::parse_node node = p.parse(text, start_symbol_index, partial_parses);
  core::warn_and_or(node);
  data::type_check_context context;
  data::variable_list variables = data::parse_variables(variable_context);
  data::type_check_node_ptr tnode = data::type_check_tree_generator(context, p).parse_DataExpr(node);
  context.add_context_variables(variables);
  tnode->set_constraint(context);
  data::print_node(tnode);
  context.remove_context_variables(variables);
  p.destroy_parse_node(node);
}

void test_data_expression_fail(const std::string& text, const std::string& variable_context = "", const std::string& expected_sort = "")
{
  test_data_expression(text, variable_context, expected_sort, false);
}

BOOST_AUTO_TEST_CASE(data_expressions_test)
{
  test_data_expression("x"                                                     , "x: Pos;"                            , "Pos"                        );
  test_data_expression("true"                                                  , ""                                   , "Bool"                       );
  test_data_expression("if(true, true, false)"                                 , ""                                   , "Bool"                       );
  test_data_expression("!true"                                                 , ""                                   , "Bool"                       );
  test_data_expression("true && false"                                         , ""                                   , "Bool"                       );
  test_data_expression("0"                                                     , ""                                   , "Nat"                        );
  test_data_expression("-1"                                                    , ""                                   , "Int"                        );
  test_data_expression("0 + 1"                                                 , ""                                   , "Pos"                        );
  test_data_expression("1 + 0"                                                 , ""                                   , "Pos"                        );
  test_data_expression("0 + 0"                                                 , ""                                   , "Nat"                        );
  test_data_expression("1 + 1"                                                 , ""                                   , "Pos"                        );
  test_data_expression("1 * 2 + 3"                                             , ""                                   , "Pos"                        );
  test_data_expression("[]"                                                    , ""                                   , ""                           );
  test_data_expression("[] ++ []"                                              , ""                                   , ""                           );
  test_data_expression("#[]"                                                   , ""                                   , "Nat"                        );
  test_data_expression("true in []"                                            , ""                                   , "Bool"                       );
  test_data_expression("[true, false]"                                         , ""                                   , "List(Bool)"                 );
  test_data_expression("[0]"                                                   , ""                                   , "List(Nat)"                  );
  test_data_expression("[1, 2]"                                                , ""                                   , "List(Pos)"                  );
  test_data_expression("[0] ++ [1, 2]"                                         , ""                                   , "List(Nat)"                  );
  test_data_expression("[0, 1, 2]"                                             , ""                                   , "List(Nat)"                  );
  test_data_expression("[1, 0, 2]"                                             , ""                                   , "List(Nat)"                  );
  test_data_expression("l ++ [1, 2]"                                           , "l: List(Nat);"                      , "List(Nat)"                  );
  test_data_expression("l ++ [1, 2]"                                           , "l: List(Pos);"                      , "List(Pos)"                  );
  test_data_expression("[0] ++ l"                                              , "l: List(Nat);"                      , "List(Nat)"                  );
  test_data_expression("{}"                                                    , ""                                   , ""                           );
  test_data_expression("!{}"                                                   , ""                                   , ""                           );
  test_data_expression("!{} <= {}"                                             , ""                                   , ""                           );
  test_data_expression("{} <= !{}"                                             , ""                                   , ""                           );
  test_data_expression("{ true, false }"                                       , ""                                   , "FSet(Bool)"                 );
  test_data_expression("{ 1, 2, -7 }"                                          , ""                                   , "FSet(Int)"                  );
  test_data_expression("{ x: Nat | x mod 2 == 0 }"                             , ""                                   , "Set(Nat)"                   );
  test_data_expression("{:}"                                                   , ""                                   , ""                           );
  test_data_expression("{ true: 1, false: 2 }"                                 , ""                                   , "FBag(Bool)"                 );
  test_data_expression("{ 1: 1, 2: 2, -8: 8 }"                                 , ""                                   , "FBag(Int)"                  );
  test_data_expression("{ x: Nat | (lambda y: Nat. y * y)(x) }"                , ""                                   , "Bag(Nat)"                   );
  test_data_expression("(lambda x: Bool. x)[true -> false]"                    , ""                                   , ""                           );
  test_data_expression("(lambda x: Bool. x)[true -> false][false -> true]"     , ""                                   , ""                           );
  test_data_expression("(lambda n: Nat. n mod 2 == 0)[0 -> false]"             , ""                                   , ""                           );
  test_data_expression("lambda x,y: struct t. x == y"                          , ""                                   , "struct t # struct t -> Bool");
  test_data_expression("forall x,y: struct t. x == y"                          , ""                                   , "Bool"                       );
  test_data_expression("forall n: Nat. n >= 0"                                 , ""                                   , "Bool"                       );
  test_data_expression("forall n: Nat. n > -1"                                 , ""                                   , "Bool"                       );
  test_data_expression("exists x,y: struct t. x == y"                          , ""                                   , "Bool"                       );
  test_data_expression("exists n: Nat. n > 481"                                , ""                                   , "Bool"                       );
  test_data_expression("x == y"                                                , "x: struct t?is_t; y: struct t?is_t;", "Bool"                       );
  test_data_expression("x whr x = 3 end"                                       , "x: Nat;"                            , "Nat"                        );
  test_data_expression("x + y whr x = 3, y = 10 end"                           , ""                                   , "Pos"                        );
  test_data_expression("x + y whr x = 3, y = 10 end"                           , "x: Pos; y: Nat;"                    , "Pos"                        );
  test_data_expression("x + y whr x = 3, y = x + 10 end"                       , "x: Pos; y: Nat;"                    , "Pos"                        );
  test_data_expression("x + y whr x = 3, y = x + y + 10 end"                   , "x: Pos; y: Nat;"                    , "Pos"                        );
  test_data_expression("x + y whr x = y + 10, y = 3 end"                       , "x: Pos; y: Nat;"                    , "Pos"                        );
  test_data_expression("x + y whr x = y + 10, y = x + 3 end"                   , "x: Pos; y: Nat;"                    , "Pos"                        );
  test_data_expression("x ++ y whr x = [0, y], y = [x] end"                    , "x: Nat; y: Nat;"                    , "List(Nat)"                  );
  test_data_expression("x + y"                                                 , "x: Pos; y: Nat;"                    , "Pos"                        );
  test_data_expression("x == y"                                                , "x: Pos; y: Nat;"                    , "Bool"                       );

  test_data_expression_fail("[0] ++ l"                                         , "l: List(Pos);"                 );
  test_data_expression_fail("x ++ y"                                           , "x: List(Pos); y: List(Pos);"   );
  test_data_expression_fail("x == y"                                           , "x: List(Pos); y: List(Nat);"   );
  test_data_expression_fail("!{:}"                                             , ""                              );
  test_data_expression_fail("(lambda x: Bool. x)[0 -> false]"                  , ""                              );
  test_data_expression_fail("lambda x: struct t?is_t. x == t"                  , ""                              );
  test_data_expression_fail("lambda x: struct t. x == t"                       , ""                              );
  test_data_expression_fail("lambda x: struct t?is_t, y: struct t. x == y"     , ""                              );
  test_data_expression_fail("lambda f: Nat. lambda f: Nat -> Bool. f(f)"       , ""                              );
  test_data_expression_fail("x == y"                                           , "x: struct t; y: struct t?is_t;");
  test_data_expression_fail("x + y whr x = 3, y = x + 10 end"                  , ""                              );
  test_data_expression_fail("x + y whr x = 3, y = x + y + 10 end"              , ""                              );
  test_data_expression_fail("x + y whr x = y + 10, y = 3 end"                  , ""                              );
  test_data_expression_fail("x + y whr x = y + 10, y = x + 3 end"              , ""                              );
  test_data_expression_fail("x ++ y whr x = [0, y], y = [x] end"               , "x: Pos; y: Nat;"               );
}

BOOST_AUTO_TEST_CASE(replace_untyped_sort_test)
{
  using namespace mcrl2::data;
  const sort_expression& B = sort_bool::bool_();
  const sort_expression& U = untyped_sort();
  data_expression s1 = sort_set::intersection(U, sort_fset::fset(U), sort_fset::fset(U));
  data_expression s2 = sort_set::intersection(B, sort_fset::fset(B), sort_fset::fset(B));
  BOOST_CHECK(has_untyped_sort(s1));
  BOOST_CHECK(!has_untyped_sort(s2));
  BOOST_CHECK(replace_untyped_sort(s1, B) == s2);
}

boost::unit_test::test_suite* init_unit_test_suite(int argc, char* argv[])
{
  return nullptr;
}


#ifndef SPARSEBENCH_EIGENUTILS_HPP
#define SPARSEBENCH_EIGENUTILS_HPP

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include "BenchmarkUtils.hpp"
#include "IO.hpp"
#include <chrono>
#include <iostream>
#include <stdexcept>

namespace sparsebench {
namespace eigenutils {

using EigenSparseMatrix = Eigen::SparseMatrix<double, Eigen::RowMajor, int32_t>;

EigenSparseMatrix readMatrix(const std::string path) {
  FILE *f = fopen(path.c_str(), "r");
  int m, n, nz, *ai, *aj;
  double *aval;
  MM_typecode matcode;
  mm_read_mtx_crd((char *) path.c_str(), &m, &n, &nz, &ai, &aj, &aval, &matcode);

  EigenSparseMatrix A(n, n);
  std::vector <Eigen::Triplet<double>> trips;

  for (size_t i = 0; i < nz; i++) {
    int row = ai[i] - 1;
    int col = aj[i] - 1;
    trips.push_back(Eigen::Triplet<double>(row, col, aval[i]));
    if (col != row)
      // TODO must check if matrix is symmetric
      trips.push_back(Eigen::Triplet<double>(col, row, aval[i]));
  }
  A.setFromTriplets(trips.begin(), trips.end());
  fclose(f);
  return A;
}

Eigen::VectorXd readVector(std::string path) {
  std::vector<double> v = sparsebench::io::readVector(path);
  Eigen::Map<Eigen::VectorXd> vec(v.data(), v.size());
  return vec;
}

template<typename SolverType>
void runSolver(int argc, char** argv) {
  using namespace std::chrono;

  sparsebench::benchmarkutils::parseArgs(argc, argv);

  auto A = sparsebench::eigenutils::readMatrix(argv[2]);
  auto b = sparsebench::eigenutils::readVector(argv[4]);
  auto exp = sparsebench::eigenutils::readVector(argv[6]);

  SolverType solver;

  auto t1 = high_resolution_clock::now();
  solver.compute(A);
  auto t2 = high_resolution_clock::now();
  auto setupSeconds = duration_cast<duration<double>>(t2 - t1).count();

  t1 = high_resolution_clock::now();
  int iterations = 2;
  Eigen::VectorXd x;
  for (int i = 0; i < iterations; i++) {
    x = solver.solve(b);
  }
  t2 = high_resolution_clock::now();
  auto solverPerIterationSeconds = duration_cast<duration<double>>(t2 - t1).count() / iterations;

  sparsebench::benchmarkutils::printSummary(
      setupSeconds,
      solver.iterations(),
      solverPerIterationSeconds,
      solver.error(),
      (x - exp).norm(),
      iterations
  );

}

}
}
#endif //SPARSEBENCH_EIGENUTILS_HPP


#include <iostream>
#include <iomanip>
#include <map>
#include <string>
#include <fstream>
#include <locale>
#include <vector>
#include <thread>
#include <boost/program_options.hpp>
#include <boost/program_options/errors.hpp>
#include <boost/log/core.hpp>
#include <boost/log/trivial.hpp>
#include <boost/log/expressions.hpp>
//#include <boost/log/utility/setup/file.hpp>
#include <boost/dynamic_bitset.hpp>
#include <iostream>
#include "timer.h"

namespace po = boost::program_options;
namespace logging = boost::log;

uint8_t* A;
uint8_t* x;
int* Ax;

int main(int argc, const char *argv[]) {
    unsigned int totalCores = std::thread::hardware_concurrency();
    double start, finish;
    int seed = 0;
    int mod = 10;
    int dim_size = 5;
    BOOST_LOG_TRIVIAL(info) << "Matrix-Vector Multiplication (Serial)";
    BOOST_LOG_TRIVIAL(info) << "Cores available:  " << totalCores;

    // Set log level
	logging::core::get()->set_filter(logging::trivial::severity >= logging::trivial::severity_level::info);

    // let's get some of the data from the users
    try
    {
        po::options_description desc("Allowed options");
        desc.add_options()
            ("help,h", "Show the usage/help screen")
            ("seed,s", po::value<int>()->default_value(0), "Seed used to fill matrix and array")
            ("mod,m", po::value<int>()->default_value(10), "Mod used to bound upper size of values in matrix/array")
            ("size,n", po::value<int>()->default_value(5), "Size length of matrix sizes and vector length");

        po::positional_options_description pos;
        pos.add("seed", 1);
        pos.add("mod", 1);
        pos.add("size", 1);

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

        if (vm.count("help")) {
            std::cout << desc << '\n';
            return EXIT_SUCCESS;
        }
        else {
            if (vm.count("seed")){
                BOOST_LOG_TRIVIAL(info) << "Using Seed Value: " << vm["seed"].as<int>();
                seed = vm["seed"].as<int>();
            }
            if (vm.count("mod")) {
                BOOST_LOG_TRIVIAL(info) << "Using Mod Value: " << vm["mod"].as<int>();
                mod = vm["mod"].as<int>();
            }
            if (vm.count("size")) {
                BOOST_LOG_TRIVIAL(info) << "Using Size Value: " << vm["size"].as<int>();
                dim_size = vm["size"].as<int>();
            }
        }
    }
    catch (const po::error &ex)
    {
        std::cerr << ex.what() << '\n';
    }

    BOOST_LOG_TRIVIAL(info) << "Creating Matrix...";
    int total_size = dim_size * dim_size;
    A = (uint8_t*)malloc(sizeof(uint8_t)*total_size);
    int current = seed;
    for (int i = 0; i < total_size; ++i) {
        current++;
        A[i] = current % mod;
    }
    // std::cout << "Printing Matrix (SOURCE):" << std::endl;
    // for (int i = 0; i < dim_size; ++i) {
    //     for (int j = 0; j < dim_size; ++j) {
    //         std::cout << (int)A[(i*dim_size)+j] << " ";
    //     }
    //     std::cout << std::endl;
    // }

    BOOST_LOG_TRIVIAL(info) << "Creating Vector...";
    x = (uint8_t*)malloc(sizeof(uint8_t)*dim_size);
    current = seed;
    for (int i = 0; i < dim_size; ++i) {
        current++;
        x[i] = current % mod;
    }

    // std::cout << "Printing Vector (SOURCE):" << std::endl;
    // for (int i = 0; i < dim_size; ++i) {
    //     std::cout << (int)x[i] << std::endl;
    // }

    BOOST_LOG_TRIVIAL(info) << "Performing Muliplication...";
    GET_TIME(start);
    Ax = (int*)malloc(sizeof(int)*dim_size);
    for (int i = 0; i < dim_size; ++i) {
        int cell_ans = 0;
        for (int j = 0; j < dim_size; ++j) {
            cell_ans += (A[(i*dim_size)+j] * x[j]);
        }
        Ax[i] = cell_ans;
    }

    BOOST_LOG_TRIVIAL(info) << "Summing Results...";
    long summed_answer = 0;
    for (int i = 0; i < dim_size; ++i) {
        summed_answer += Ax[i];
    }

    GET_TIME(finish);
    BOOST_LOG_TRIVIAL(info) << "Displaying Results:";
    std::cout << "Printing Vector (Result):" << std::endl;
    for (int i = 0; i < dim_size; ++i) {
        std::cout << (int)Ax[i] << std::endl;
    }

    BOOST_LOG_TRIVIAL(info) << "Summed Result: " << summed_answer;

    BOOST_LOG_TRIVIAL(info) << "Time Elapsed...";
    printf("Elapsed time = %e seconds\n", finish - start);

    // clean up
    free(A);
    free(x);
    free(Ax);

    return EXIT_SUCCESS;
}


#include <vector>
#include <algorithm>
#include <boost/compute.hpp>

namespace compute = boost::compute;

int main()
{
    // get the default compute device
    compute::device gpu = compute::system::default_device();

    std::cout << "device name: " << gpu.name() << std::endl;

    // create a compute context and command queue
    compute::context ctx(gpu);
    compute::command_queue queue(ctx, gpu);

    const int n = 1000;
    // generate random numbers on the host
    std::vector<float> host_vector(n);
    std::generate(host_vector.begin(), host_vector.end(), rand);

    // create vector on the device
    compute::vector<float> device_vector(n, ctx);

    // copy data to the device
    compute::copy(
        host_vector.begin(), host_vector.end(), device_vector.begin(), queue
    );

    // sort data on the device
    compute::sort(
        device_vector.begin(), device_vector.end(), queue
    );

    // copy data back to the host
    compute::copy(
        device_vector.begin(), device_vector.end(), host_vector.begin(), queue
    );

    return 0;
}


#define BOOST_TEST_MODULE JTest
#include <boost/test/unit_test.hpp>
#include "J.h"

BOOST_AUTO_TEST_CASE(JTest) {
    std::ios_base::sync_with_stdio(false);
    std::cin.tie(NULL);
    std::stringstream iss(
            "5 2\n"
                    "1 5 4\n"
                    "1 4 5\n"
                    "1 3 2\n"
                    "4 1 2\n"
                    "5 6 1"
    );
    std::streambuf* cinbuf = std::cin.rdbuf();
    std::cin.rdbuf(iss.rdbuf());

    std::stringstream oss;
    std::streambuf* coutbuf = std::cout.rdbuf();
    std::cout.rdbuf(oss.rdbuf());
    J j;
    j.solve();

    BOOST_TEST_MESSAGE(oss.str());
    BOOST_TEST(oss.str() == "5");

    std::cin.rdbuf(cinbuf);

}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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 <boost/config.hpp>
#include <iostream>
#include <vector>
#include <utility>
#include <algorithm>

#include <boost/graph/adjacency_list.hpp>

using namespace boost;
using namespace std;

typedef property<vertex_color_t, default_color_type,
    property<vertex_distance_t,int,
      property<vertex_degree_t,int,
        property<vertex_in_degree_t, int,
          property<vertex_out_degree_t,int> > > > > VertexProperty;
typedef property<edge_weight_t,int> EdgeProperty;
typedef adjacency_list<vecS, vecS, bidirectionalS,
                       VertexProperty, EdgeProperty> Graph;

template <class Graph>
void print(Graph& g) {
  typename Graph::vertex_iterator i, end;
  typename Graph::out_edge_iterator ei, edge_end;
  for(boost::tie(i,end) = vertices(g); i != end; ++i) {
    cout << *i << " --> ";
    for (boost::tie(ei,edge_end) = out_edges(*i, g); ei != edge_end; ++ei)
      cout << target(*ei, g) << "  ";
    cout << endl;
  }
}

std::size_t myrand(std::size_t N) {
  std::size_t ret = rand() % N;
  //  cout << "N = " << N << "  rand = " << ret << endl;
  return ret;
}

template <class Graph>
bool check_edge(Graph& g, std::size_t a, std::size_t b) {
  typename Graph::adjacency_iterator vi, viend, found;
  boost::tie(vi, viend) = adjacent_vertices(vertex(a,g), g);

  found = find(vi, viend, vertex(b, g));
  if ( found == viend )
    return false;

  return true;
}

int main(int, char*[])
{
  std::size_t N = 5;

  Graph g(N);
  int i;

  bool is_failed = false;

  for (i=0; i<6; ++i) {
    std::size_t a = myrand(N), b = myrand(N);
    while ( a == b ) b = myrand(N);
    cout << "edge edge (" << a << "," << b <<")" << endl;
    //add edges
    add_edge(a, b, g);
    is_failed =  is_failed || (! check_edge(g, a, b) );
  }

  if ( is_failed )
    cerr << "    Failed."<< endl;
  else
    cerr << "           Passed."<< endl;

  print(g);

  //remove_edge
  for (i = 0; i<2; ++i) {
    std::size_t a = myrand(N), b = myrand(N);
    while ( a == b ) b = myrand(N);
    cout << "remove edge (" << a << "," << b <<")" << endl;
    remove_edge(a, b, g);
    is_failed = is_failed || check_edge(g, a, b);
  }
  if ( is_failed )
    cerr << "    Failed."<< endl;
  else
    cerr << "           Passed."<< endl;

  print(g);

  //add_vertex
  is_failed = false;
  std::size_t old_N = N;
  std::size_t vid   = add_vertex(g);
  std::size_t vidp1 = add_vertex(g);

  N = num_vertices(g);
  if ( (N - 2) != old_N )
    cerr << "    Failed."<< endl;
  else
    cerr << "           Passed."<< endl;

  is_failed = false;
  for (i=0; i<2; ++i) {
    std::size_t a = myrand(N), b = myrand(N);
    while ( a == vid ) a = myrand(N);
    while ( b == vidp1 ) b = myrand(N);
    cout << "add edge (" << vid << "," << a <<")" << endl;
    cout << "add edge (" << vid << "," << vidp1 <<")" << endl;
    add_edge(vid, a, g);
    add_edge(b, vidp1, g);
    is_failed = is_failed || ! check_edge(g, vid, a);
    is_failed = is_failed || ! check_edge(g, b, vidp1);
  }
  if ( is_failed )
    cerr << "    Failed."<< endl;
  else
    cerr << "           Passed."<< endl;
  print(g);

  // clear_vertex
  std::size_t c = myrand(N);
  is_failed = false;
  clear_vertex(c, g);

  if ( out_degree(c, g) != 0 )
    is_failed = true;

  cout << "Removing vertex " << c << endl;
  remove_vertex(c, g);

  old_N = N;
  N = num_vertices(g);

  if ( (N + 1) != old_N )
    is_failed = true;

  if ( is_failed )
    cerr << "    Failed."<< endl;
  else
    cerr << "           Passed."<< endl;

  print(g);

  return 0;
}


#include <boost/test/unit_test.hpp>
#include <base/Time.hpp>
#include <numeric/Combinatorics.hpp>
#include <templ/solvers/temporal/point_algebra/QualitativeTimePointConstraint.hpp>
#include <templ/solvers/temporal/point_algebra/QualitativeTimePoint.hpp>
#include <templ/solvers/temporal/QualitativeTemporalConstraintNetwork.hpp>

using namespace templ::solvers;
using namespace templ::solvers::temporal;

typedef point_algebra::QualitativeTimePointConstraint QTPC;

BOOST_AUTO_TEST_SUITE(point_algebra)

BOOST_AUTO_TEST_CASE(composition)
{
    using namespace templ::solvers::temporal::point_algebra;

    base::Time start = base::Time::now();
    int i = 0;
    for(; i < 1000; ++i)
    {
        QTPC::Type type = QualitativeTimePointConstraint::getComposition(QTPC::Greater, QTPC::GreaterOrEqual);
    }
    base::Time stop = base::Time::now();

    BOOST_TEST_MESSAGE("getComposition requires: " << (stop-start).toSeconds()/(i*1.0) << "s/call");
}

BOOST_AUTO_TEST_CASE(symmetric)
{
    using namespace templ::solvers::temporal::point_algebra;

    BOOST_REQUIRE_MESSAGE(QualitativeTimePointConstraint::getSymmetric(QTPC::Greater) == QualitativeTimePointConstraint::Less, "Greater symmetric to less");
    BOOST_REQUIRE_MESSAGE(QualitativeTimePointConstraint::getSymmetric(QTPC::Less) == QualitativeTimePointConstraint::Greater, "Greater symmetric to less");

    base::Time start = base::Time::now();
    int i = 0;
    for(; i < 1000; ++i)
    {
        QTPC::Type type = QualitativeTimePointConstraint::getSymmetric(QTPC::Greater);
    }
    base::Time stop = base::Time::now();
    BOOST_TEST_MESSAGE("getSymmetry requires: " << (stop-start).toSeconds()/(i*1.0) << "s/call");
}

BOOST_AUTO_TEST_CASE(intersection)
{
    using namespace templ::solvers::temporal::point_algebra;

    base::Time start = base::Time::now();
    int i = 0;
    for(; i < 1000; ++i)
    {
        QTPC::Type type = QualitativeTimePointConstraint::getIntersection(QTPC::GreaterOrEqual, QTPC::Equal);
    }
    base::Time stop = base::Time::now();

    BOOST_TEST_MESSAGE("getIntersection requires: " << (stop-start).toSeconds()/(i*1.0) << "s/call");
}

BOOST_AUTO_TEST_CASE(bidirectional_constraint_type)
{
    using namespace templ::solvers::temporal::point_algebra;
    using namespace graph_analysis;

    QualitativeTemporalConstraintNetwork::Ptr tcn(new QualitativeTemporalConstraintNetwork());
    {
        QualitativeTimePoint::Ptr lastTp;
        int i = 0;
        for(; i < 100; ++i)
        {
            std::stringstream ss;
            ss << "t" << i;
            QualitativeTimePoint::Ptr tp(new QualitativeTimePoint(ss.str()));
            if(lastTp)
            {
                tcn->addQualitativeConstraint(lastTp, tp, QTPC::Greater);
                tcn->addQualitativeConstraint(tp, lastTp, QTPC::Less);
            }

            lastTp = tp;
        }
    }

    base::Time start = base::Time::now();
    std::vector<Vertex::Ptr> vertices = tcn->getGraph()->getAllVertices();

    Vertex::Ptr source = vertices[10];
    Vertex::Ptr target = vertices[50];

    int i = 0;
    for(; i < 1000; ++i)
    {
        QTPC::Type type = tcn->getBidirectionalConstraintType(source, target);
    }
    base::Time stop = base::Time::now();

    BOOST_TEST_MESSAGE("getBidirectionalConstraintType requires: " << (stop-start).toSeconds()/(i*1.0) << "s/call");
}

BOOST_AUTO_TEST_CASE(qualitative_constraints)
{
    using namespace templ::solvers::temporal::point_algebra;

    BOOST_REQUIRE_MESSAGE( QualitativeTimePointConstraint::isConsistent(QTPC::Universal, QTPC::Equal), "Consistency: P and =" );
    BOOST_REQUIRE_MESSAGE( QualitativeTimePointConstraint::isConsistent(QTPC::Universal, QTPC::Less), "Consistency: P and <" );
    BOOST_REQUIRE_MESSAGE( QualitativeTimePointConstraint::isConsistent(QTPC::Universal, QTPC::Greater), "Consistency: P and >" );
    BOOST_REQUIRE_MESSAGE( QualitativeTimePointConstraint::isConsistent(QTPC::Universal, QTPC::GreaterOrEqual), "Consistency: P and >= " );
    BOOST_REQUIRE_MESSAGE( QualitativeTimePointConstraint::isConsistent(QTPC::Universal, QTPC::LessOrEqual), "Consistency: P and <=" );

    BOOST_REQUIRE_MESSAGE(  QualitativeTimePointConstraint::isConsistent(QTPC::Equal, QTPC::Equal), "Consistency: = and =" );
    BOOST_REQUIRE_MESSAGE(  QualitativeTimePointConstraint::isConsistent(QTPC::Equal, QTPC::LessOrEqual), "Consistency: = and <=" );
    BOOST_REQUIRE_MESSAGE(  QualitativeTimePointConstraint::isConsistent(QTPC::Equal, QTPC::GreaterOrEqual), "Consistency: = and >=" );
    BOOST_REQUIRE_MESSAGE(! QualitativeTimePointConstraint::isConsistent(QTPC::Equal, QTPC::Less),    "Consistency: = and <" );
    BOOST_REQUIRE_MESSAGE(! QualitativeTimePointConstraint::isConsistent(QTPC::Equal, QTPC::Greater), "Consistency: = and >" );

    BOOST_REQUIRE_MESSAGE(  QualitativeTimePointConstraint::isConsistent(QTPC::Less, QTPC::LessOrEqual),       "Consistency: < and <=" );
    BOOST_REQUIRE_MESSAGE(! QualitativeTimePointConstraint::isConsistent(QTPC::Less, QTPC::GreaterOrEqual),    "Consistency: < and >=" );
    BOOST_REQUIRE_MESSAGE(  QualitativeTimePointConstraint::isConsistent(QTPC::Less, QTPC::Less),              "Consistency: < and <" );
    BOOST_REQUIRE_MESSAGE(! QualitativeTimePointConstraint::isConsistent(QTPC::Less, QTPC::Greater),           "Consistency: < and >" );

    BOOST_REQUIRE_MESSAGE(! QualitativeTimePointConstraint::isConsistent(QTPC::Greater, QTPC::LessOrEqual),       "Consistency: > and <=" );
    BOOST_REQUIRE_MESSAGE(  QualitativeTimePointConstraint::isConsistent(QTPC::Greater, QTPC::GreaterOrEqual),    "Consistency: > and >=" );
    BOOST_REQUIRE_MESSAGE(  QualitativeTimePointConstraint::isConsistent(QTPC::Greater, QTPC::Greater),           "Consistency: > and >" );

    BOOST_REQUIRE_MESSAGE(  QualitativeTimePointConstraint::isConsistent(QTPC::LessOrEqual, QTPC::GreaterOrEqual),    "Consistency: <= and >=" );
    BOOST_REQUIRE_MESSAGE(  QualitativeTimePointConstraint::isConsistent(QTPC::LessOrEqual, QTPC::LessOrEqual),       "Consistency: <= and <=" );

    BOOST_REQUIRE_MESSAGE(  QualitativeTimePointConstraint::isConsistent(QTPC::GreaterOrEqual, QTPC::GreaterOrEqual),       "Consistency: >= and >=" );

    std::vector<QualitativeTimePointConstraint::Type> types = QualitativeTimePointConstraint::getAllConstraintTypes();
    numeric::Combination<QualitativeTimePointConstraint::Type> combination(types, 2, numeric::EXACT);
    do {
        std::vector<QualitativeTimePointConstraint::Type> testPair = combination.current();
        BOOST_REQUIRE_MESSAGE( QualitativeTimePointConstraint::isConsistent(testPair[0], testPair[1]) == QualitativeTimePointConstraint::isConsistent(testPair[1], testPair[0]), "Check commutative constraint: " << QualitativeTimePointConstraint::TypeSymbol[ testPair[0] ] << " and " << QualitativeTimePointConstraint::TypeSymbol[ testPair[1] ] );

        try {
            BOOST_CHECK_MESSAGE( QualitativeTimePointConstraint::getComposition(testPair[0], testPair[1]) == QualitativeTimePointConstraint::getComposition(testPair[1], testPair[0]), "Check composition constraint: " << QualitativeTimePointConstraint::TypeSymbol[ testPair[0] ] << " and " << QualitativeTimePointConstraint::TypeSymbol[ testPair[1] ] );
        } catch(const std::runtime_error& e)
        {
            BOOST_TEST_MESSAGE(e.what());
        }

    } while(combination.next());
}

BOOST_AUTO_TEST_CASE(qualitative_timepoints)
{
    using namespace templ::solvers::temporal::point_algebra;
    QualitativeTimePoint tp0("tp0");
    QualitativeTimePoint tp1("tp1");
    QualitativeTimePoint tp2("tp2");

    BOOST_REQUIRE_MESSAGE(tp0 != tp1, "Timepoints tp0 and tp1 are different");
    BOOST_REQUIRE_MESSAGE(tp0 != tp2, "Timepoints tp0 and tp2 are different");
    BOOST_REQUIRE_MESSAGE(tp1 != tp2, "Timepoints tp1 and tp2 are different");

    tp1.addAlias("tp3");
    tp2.addAlias("tp3");

    BOOST_REQUIRE_MESSAGE(tp0 != tp1, "Timepoints tp0 and tp1 are different");
    BOOST_REQUIRE_MESSAGE(tp0 != tp2, "Timepoints tp0 and tp2 are different");
    BOOST_REQUIRE_MESSAGE(tp1 == tp2, "Timepoints tp1 and tp2 are same due to same alias 'tp3'");
}

BOOST_AUTO_TEST_CASE(complete)
{
    using namespace templ::solvers::temporal::point_algebra;

    QualitativeTemporalConstraintNetwork::Ptr tcn(new QualitativeTemporalConstraintNetwork());

    QualitativeTimePoint::Ptr lastTp;
    int i = 0;
    for(; i < 5; ++i)
    {
        std::stringstream ss;
        ss << "t" << i;
        QualitativeTimePoint::Ptr tp(new QualitativeTimePoint(ss.str()));
        if(lastTp)
        {
            tcn->addQualitativeConstraint(lastTp, tp, QTPC::Greater);
        }

        lastTp = tp;
    }
    BOOST_TEST_MESSAGE("Network constructed");

    {
        base::Time start = base::Time::now();
        bool consistent = tcn->isConsistent();
        base::Time stop = base::Time::now() - start;
        BOOST_REQUIRE_MESSAGE(consistent, "1. Temporal constraint network is consistent: time " << stop.toSeconds() );
    }
    {
        base::Time start = base::Time::now();
        bool consistent = tcn->isConsistent();
        base::Time stop = base::Time::now() - start;
        BOOST_REQUIRE_MESSAGE(consistent, "2. Temporal constraint network is consistent: time " << stop.toSeconds() );
    }
    tcn->save("/tmp/thomas-test-network");

    QualitativeTimePoint::Ptr t0(new QualitativeTimePoint("t0"));
    QualitativeTimePoint::Ptr t1(new QualitativeTimePoint("t1"));
    QualitativeTimePoint::Ptr t2(new QualitativeTimePoint("t2"));

    tcn->addQualitativeConstraint(t0, t1, QTPC::Greater);
    tcn->addQualitativeConstraint(t1, t2, QTPC::Greater);
    tcn->addQualitativeConstraint(t2, t1, QTPC::Greater);

    BOOST_REQUIRE_THROW(tcn->incrementalPathConsistency(), std::runtime_error);
}

BOOST_AUTO_TEST_CASE(consistency_check_performance)
{
    using namespace templ::solvers::temporal::point_algebra;

    QualitativeTemporalConstraintNetwork::Ptr tcn(new QualitativeTemporalConstraintNetwork());

    QualitativeTimePoint::Ptr lastTp;
    int i = 0;
    for(; i < 100; ++i)
    {
        std::stringstream ss;
        ss << "t" << i;
        QualitativeTimePoint::Ptr tp(new QualitativeTimePoint(ss.str()));
        if(lastTp)
        {
            tcn->addQualitativeConstraint(lastTp, tp, QTPC::Greater);
            tcn->addQualitativeConstraint(tp, lastTp, QTPC::Less);
        }

        lastTp = tp;
    }

    {
        base::Time start = base::Time::now();
        bool consistent = tcn->isConsistent();
        base::Time stop = base::Time::now() - start;
        BOOST_REQUIRE_MESSAGE(consistent, "Temporal constraint network with " << i << " timepoints is consistent: computing time: " << stop.toSeconds());
    }
}

BOOST_AUTO_TEST_CASE(inconsistency_0)
{
    using namespace templ::solvers::temporal::point_algebra;
    TemporalConstraintNetwork::Ptr tcn(new QualitativeTemporalConstraintNetwork());

    QualitativeTimePoint::Ptr t0(new QualitativeTimePoint("t0"));
    QualitativeTimePoint::Ptr t1(new QualitativeTimePoint("t1"));
    QualitativeTimePoint::Ptr t2(new QualitativeTimePoint("t2"));

    tcn->addQualitativeConstraint(t0, t1, QTPC::Greater);
    tcn->addQualitativeConstraint(t1, t2, QTPC::Greater);
    tcn->addQualitativeConstraint(t2, t1, QTPC::Greater);

    bool consistent = tcn->isConsistent();
    tcn->save("/tmp/templ-test-point_algebra-inconsistency_0-test");
    BOOST_REQUIRE_MESSAGE(!consistent, "Temporal constraint network is inconsistent");
}

BOOST_AUTO_TEST_CASE(inconsistency_1)
{
    using namespace templ::solvers::temporal::point_algebra;
    QualitativeTemporalConstraintNetwork::Ptr tcn(new QualitativeTemporalConstraintNetwork());

    QualitativeTimePoint::Ptr t0(new QualitativeTimePoint("t0"));
    QualitativeTimePoint::Ptr t1(new QualitativeTimePoint("t1"));
    QualitativeTimePoint::Ptr t2(new QualitativeTimePoint("t2"));
    QualitativeTimePoint::Ptr t3(new QualitativeTimePoint("t3"));
    QualitativeTimePoint::Ptr t4(new QualitativeTimePoint("t4"));
    QualitativeTimePoint::Ptr t5(new QualitativeTimePoint("t5"));
    QualitativeTimePoint::Ptr t6(new QualitativeTimePoint("t6"));
    QualitativeTimePoint::Ptr t7(new QualitativeTimePoint("t7"));
    QualitativeTimePoint::Ptr t8(new QualitativeTimePoint("t8"));

    tcn->addQualitativeConstraint(t1, t0, QTPC::Greater);
    tcn->addQualitativeConstraint(t2, t1, QTPC::Greater);
    tcn->addQualitativeConstraint(t3, t2, QTPC::Greater);
    tcn->addQualitativeConstraint(t4, t3, QTPC::Greater);
    tcn->addQualitativeConstraint(t5, t4, QTPC::Greater);
    tcn->addQualitativeConstraint(t6, t5, QTPC::Greater);
    tcn->addQualitativeConstraint(t7, t6, QTPC::Greater);
    tcn->addQualitativeConstraint(t8, t7, QTPC::Greater);
    tcn->addQualitativeConstraint(t5, t1, QTPC::Less);

    tcn->save("/tmp/test-point_algebra-consistency-test-pre");

    bool consistent = tcn->isConsistent();
    BOOST_REQUIRE_MESSAGE(!consistent, "Temporal constraint network is inconsistent");
}

BOOST_AUTO_TEST_CASE(inconsistency_benchmark)
{
    using namespace templ::solvers::temporal::point_algebra;
    QualitativeTemporalConstraintNetwork::Ptr tcn(new QualitativeTemporalConstraintNetwork());

    QualitativeTimePoint::Ptr t0(new QualitativeTimePoint("t0"));
    QualitativeTimePoint::Ptr t1(new QualitativeTimePoint("t1"));
    QualitativeTimePoint::Ptr t2(new QualitativeTimePoint("t2"));
    QualitativeTimePoint::Ptr t3(new QualitativeTimePoint("t3"));
    QualitativeTimePoint::Ptr t4(new QualitativeTimePoint("t4"));
    QualitativeTimePoint::Ptr t5(new QualitativeTimePoint("t5"));
    QualitativeTimePoint::Ptr t6(new QualitativeTimePoint("t6"));
    QualitativeTimePoint::Ptr t7(new QualitativeTimePoint("t7"));
    QualitativeTimePoint::Ptr t8(new QualitativeTimePoint("t8"));

    tcn->addQualitativeConstraint(t1, t0, QTPC::Greater);
    tcn->addQualitativeConstraint(t2, t1, QTPC::Greater);
    tcn->addQualitativeConstraint(t3, t2, QTPC::Greater);
    tcn->addQualitativeConstraint(t4, t3, QTPC::Greater);
    tcn->addQualitativeConstraint(t5, t4, QTPC::Greater);
    tcn->addQualitativeConstraint(t6, t5, QTPC::Greater);
    tcn->addQualitativeConstraint(t7, t6, QTPC::Greater);
    tcn->addQualitativeConstraint(t8, t7, QTPC::Greater);
    tcn->addQualitativeConstraint(t5, t1, QTPC::Less);

    tcn->save("/tmp/test-point_algebra-consistency-test-pre");

    bool consistent = tcn->isConsistent();
    BOOST_REQUIRE_MESSAGE(!consistent, "Temporal constraint network is inconsistent");
}

BOOST_AUTO_TEST_SUITE_END()


#include "drake/systems/primitives/integrator.h"

#include <memory>
#include <stdexcept>
#include <string>

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include "drake/systems/framework/basic_vector.h"
#include "drake/systems/framework/fixed_input_port_value.h"
#include "drake/systems/framework/test_utilities/scalar_conversion.h"

namespace drake {
namespace systems {
namespace {

const int kLength = 3;

class IntegratorTest : public ::testing::Test {
 protected:
  void SetUp() override {
    integrator_.reset(new Integrator<double>(kLength));
    context_ = integrator_->CreateDefaultContext();
    derivatives_ = integrator_->AllocateTimeDerivatives();

    // Set the state to zero initially.
    ContinuousState<double>& xc = continuous_state();
    EXPECT_EQ(3, xc.size());
    EXPECT_EQ(3, xc.get_misc_continuous_state().size());
    xc.SetFromVector(Eigen::VectorXd::Zero(kLength));
  }

  ContinuousState<double>& continuous_state() {
    return context_->get_mutable_continuous_state();
  }

  std::unique_ptr<System<double>> integrator_;
  std::unique_ptr<Context<double>> context_;
  std::unique_ptr<ContinuousState<double>> derivatives_;
};

// Tests that the system exports the correct topology.
TEST_F(IntegratorTest, Topology) {
  ASSERT_EQ(1, integrator_->num_input_ports());
  const auto& input_port = integrator_->get_input_port(0);
  EXPECT_EQ(kVectorValued, input_port.get_data_type());
  EXPECT_EQ(kLength, input_port.size());

  ASSERT_EQ(1, integrator_->num_output_ports());
  const auto& output_port = integrator_->get_output_port(0);
  EXPECT_EQ(kVectorValued, output_port.get_data_type());
  EXPECT_EQ(kLength, output_port.size());
}

// Tests that the output of an integrator is its state.
TEST_F(IntegratorTest, Output) {
  ASSERT_EQ(1, context_->num_input_ports());
  integrator_->get_input_port(0).FixValue(context_.get(),
      Eigen::Vector3d{1.0, 2.0, 3.0});

  Eigen::Vector3d expected = Eigen::Vector3d::Zero();
  EXPECT_EQ(expected, integrator_->get_output_port(0).Eval(*context_));

  continuous_state().get_mutable_vector().SetAtIndex(1, 42.0);
  expected << 0.0, 42.0, 0.0;
  EXPECT_EQ(expected, integrator_->get_output_port(0).Eval(*context_));
}

// Tests that the derivatives of an integrator's state are its input.
TEST_F(IntegratorTest, Derivatives) {
  ASSERT_EQ(1, context_->num_input_ports());
  integrator_->get_input_port(0).FixValue(context_.get(),
                                          Eigen::Vector3d{1.0, 2.0, 3.0});

  integrator_->CalcTimeDerivatives(*context_, derivatives_.get());
  Eigen::Vector3d expected(1.0, 2.0, 3.0);
  EXPECT_EQ(expected, derivatives_->CopyToVector());
}

// Asserts that integrators do not have any direct feedthrough inputs.
TEST_F(IntegratorTest, IntegratorIsNotDirectFeedthrough) {
  EXPECT_FALSE(integrator_->HasAnyDirectFeedthrough());
}

class SymbolicIntegratorTest : public IntegratorTest {
 protected:
  void SetUp() override {
    IntegratorTest::SetUp();
    symbolic_integrator_ = integrator_->ToSymbolic();
    symbolic_context_ = symbolic_integrator_->CreateDefaultContext();
    symbolic_derivatives_ = symbolic_integrator_->AllocateTimeDerivatives();

    ASSERT_EQ(1, symbolic_context_->num_input_ports());
    symbolic_integrator_->get_input_port(0).FixValue(symbolic_context_.get(),
        Vector3<symbolic::Expression>{symbolic::Variable("u0"),
                                      symbolic::Variable("u1"),
                                      symbolic::Variable("u2")});

    auto& xc = symbolic_context_->get_mutable_continuous_state_vector();
    xc[0] = symbolic::Variable("x0");
    xc[1] = symbolic::Variable("x1");
    xc[2] = symbolic::Variable("x2");
  }

  std::unique_ptr<System<symbolic::Expression>> symbolic_integrator_;
  std::unique_ptr<Context<symbolic::Expression>> symbolic_context_;
  std::unique_ptr<ContinuousState<symbolic::Expression>> symbolic_derivatives_;
};

TEST_F(SymbolicIntegratorTest, Output) {
  const auto& out =
      symbolic_integrator_->get_output_port(0).Eval(*symbolic_context_);
  EXPECT_EQ("x0", out[0].to_string());
  EXPECT_EQ("x1", out[1].to_string());
  EXPECT_EQ("x2", out[2].to_string());
}

TEST_F(SymbolicIntegratorTest, Derivatives) {
  symbolic_integrator_->CalcTimeDerivatives(*symbolic_context_,
                                            symbolic_derivatives_.get());
  const auto& xcdot = *symbolic_derivatives_;
  EXPECT_EQ("u0", xcdot[0].to_string());
  EXPECT_EQ("u1", xcdot[1].to_string());
  EXPECT_EQ("u2", xcdot[2].to_string());
}

TEST_F(IntegratorTest, ToAutoDiff) {
  EXPECT_TRUE(is_autodiffxd_convertible(*integrator_));
}

TEST_F(IntegratorTest, ToSymbolic) {
  EXPECT_TRUE(is_symbolic_convertible(*integrator_));
}

}  // namespace
}  // namespace systems
}  // namespace drake


#pragma once

#include <boost/optional.hpp>

#include <stack>
#include <stdexcept>
#include <utility>
#include <vector>

///
/// \brief A Trie datastructure used to remove prefixes in a set of words
///
/// The datastructure only works for words over integral unsigned types. In principle the symbols
/// can be unbounded, however having very large symbols degrades the performance a lot. Some random
/// testing shows that for symbols <= 50 the performance is similar to std::set (which is solving a
/// different problem).
///
/// Tests : 1M words, avg words length 4 (geometric dist.), alphabet 50 symbols
/// trie reduction 58% in 1.15s
/// set  reduction 49% in 0.92s
///
/// I did not implement any iterators, as those are quite hard to get right.
/// There are, however, "internal iterators" exposed as a for_each() member
/// function (if only we had coroutines already...)
///
template <typename T> struct trie {
	static_assert(std::is_integral<T>::value && std::is_unsigned<T>::value, "");

	/// \brief Inserts a word (given by iterators \p begin and \p end)
	/// \returns true if the element was inserted, false if already there
	template <typename Iterator> bool insert(Iterator && begin, Iterator && end) {
		if (begin == end) return false;

		size_t i = *begin++;
		if (i >= branches.size()) branches.resize(i + 1);

		auto & b = branches[i];
		if (b) return b->insert(begin, end);

		b = trie();
		b->insert(begin, end);
		return true;
	}

	/// \brief Inserts a word given as range \p r
	/// \returns true if the element was inserted, false if already there
	template <typename Range> bool insert(Range const & r) { return insert(begin(r), end(r)); }

	/// \brief Applies \p function to all word (not to the prefixes)
	template <typename Fun> void for_each(Fun && function) const {
		std::vector<T> word;
		return for_each_impl(std::forward<Fun>(function), word);
	}

	/// \brief Empties the complete set
	void clear() { branches.clear(); }

  private:
	template <typename Fun> void for_each_impl(Fun && function, std::vector<T> & word) const {
		size_t count = 0;
		for (T i = 0; i < branches.size(); ++i) {
			auto const & b = branches[i];
			if (b) {
				++count;
				word.push_back(i);
				b->for_each_impl(function, word);
				word.resize(word.size() - 1);
			}
		}

		if (count == 0) {
			const auto & cword = word;
			function(cword); // we don't want function to modify word
			return;
		}
	}

	std::vector<boost::optional<trie>> branches;
};

/// \brief Flattens a trie \p t
/// \returns an array of words (without the prefixes)
template <typename T> std::vector<std::vector<T>> flatten(trie<T> const & t) {
	std::vector<std::vector<T>> ret;
	t.for_each([&ret](std::vector<T> const & w) { ret.push_back(w); });
	return ret;
}

/// \brief Returns size and total sum of symbols
template <typename T> std::pair<size_t, size_t> total_size(trie<T> const & t) {
	size_t count = 0;
	size_t total_count = 0;
	t.for_each([&count, &total_count](std::vector<T> const &  w) {
		++count;
		total_count += w.size();
	});
	return {count, total_count};
}


#include "WSPRLog.hxx"
#include "SolarTime.hxx"
#include "TimeCorr.hxx"

#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/copy.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <string>
#include <iostream>
#include <fstream>
#include <list>
#include <math.h>
#include <set>

// plot distance, azimuth pairs.
const int NUM_TIME_BUCKETS = (24 * 10);
const int NUM_AZ_BUCKETS = (360 / 5);

class myWSPRLog : public WSPRLog {
public:
  myWSPRLog() : WSPRLog() {
    total_reports = 0;
    for(int tbucket = 0; tbucket < NUM_TIME_BUCKETS; tbucket++) {
      for(int azbucket = 0; azbucket < NUM_AZ_BUCKETS; azbucket++) {
	histo[tbucket][azbucket] = 0;
      }
    }
  }

  bool processEntry(WSPRLogEntry * ent) {
    int x, y;

    if(ent == NULL) return false;

    SolarTime tx_time(ent->dtime, ent->txgrid);
    SolarTime rx_time(ent->dtime, ent->rxgrid);
    float mid_hour = TimeCorr::circularMean(24.0, tx_time.getFHour(), rx_time.getFHour());
    bump(mid_hour, ent->az);
    return false;
  }

  void bump(float t_hour, float az) {
    // quantize into 10 minute buckets
    int time_bucket = ((int) floor(t_hour * 6.0));
    // quantize into 5 degree segments
    int az_bucket = (int) (az / 5.0);
    if(time_bucket >= NUM_TIME_BUCKETS) time_bucket = (NUM_TIME_BUCKETS - 1);
    if(az_bucket >= NUM_AZ_BUCKETS) az_bucket = (NUM_AZ_BUCKETS - 1);
    histo[time_bucket][az_bucket]++;
    total_reports++;
  }

  void writeReport(const std::string & out_name) {
    std::ofstream os(out_name);
    float rscale = 1.0 / ((float) total_reports);

    for(int tbucket = 0; tbucket < NUM_TIME_BUCKETS; tbucket++) {
      for(int azbucket = 0; azbucket < NUM_AZ_BUCKETS; azbucket++) {
	float val = (float) histo[tbucket][azbucket];
	val = val * rscale;
	os << boost::format("%d %d %f\n")
	  % (tbucket * 10) % (azbucket * 5) % val;
      }
    }
    os.close();
  }

private:
  int histo[NUM_TIME_BUCKETS][NUM_AZ_BUCKETS];
  int total_reports;
};

int main(int argc, char * argv[])
{
  std::string in_name, out_name, x_field_selector, y_field_selector;
  bool input_gzipped;
  namespace po = boost::program_options;

  po::options_description desc("Options:");

  desc.add_options()
    ("help", "help message")
    ("in", po::value<std::string>(&in_name)->required(), "Input log file (csv) in WSPR log format")
    ("out", po::value<std::string>(&out_name)->required(), "Output data file")
    ("igz", po::value<bool>(&input_gzipped)->default_value(false), "if true, input file is gzip compressed");

  po::positional_options_description pos_opts ;
  pos_opts.add("in", 1);
  pos_opts.add("out", 1);

  po::variables_map vm;

  std::string what_am_i("Write 2D histogram file (heat map) for count vs. solar time and azimuth\n\t");
  try {
    po::store(po::command_line_parser(argc, argv).options(desc)
	      .positional(pos_opts).run(), vm);

    if(vm.count("help")) {
      std::cout << what_am_i
		<< desc << std::endl;
      exit(-1);
    }

    po::notify(vm);
  }
  catch(po::required_option & e) {
    std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
    std::cerr << what_am_i
	      << desc << std::endl;
    exit(-1);
  }
  catch(po::error & e) {
    std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
    std::cerr << what_am_i
	      << desc << std::endl;
    exit(-1);
  }

  myWSPRLog wlog;

  wlog.readLog(in_name, input_gzipped);

  wlog.writeReport(out_name);
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/log2_em1.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/detail/constant/powlargelim.hpp>
#include <boost/simd/detail/constant/powlowlim.hpp>
#include <boost/simd/constant/sixteen.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_else_zero.hpp>
#include <boost/simd/function/if_one_else_zero.hpp>
#include <boost/simd/function/if_zero_else.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/is_gtz.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/is_odd.hpp>
#include <boost/simd/function/ldexp.hpp>
#include <boost/simd/function/log.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/pow2.hpp>
#include <boost/simd/function/if_inc.hpp>
#include <boost/simd/function/if_minus.hpp>
#include <boost/simd/function/shift_right.hpp>
#include <boost/simd/function/signnz.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/function/unary_minus.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <utility>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/if_zero_else_one.hpp>
#include <boost/simd/function/is_equal.hpp>
#include <boost/simd/function/is_gtz.hpp>
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/is_ltz.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF( pow_abs_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::pack_<bd::floating_<A0>, X>
                            , bs::pack_<bd::floating_<A0>, X>
                            )
  {
    inline A0 operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      using iA0 = bd::as_integer_t<A0>;
      iA0 e;
      A0 ax = bs::abs(a0);
      A0 x;
      std::tie(x, e) = frexp(ax);
      iA0 i  = detail::pow_kernel<A0>::select(x);
      A0 z = sqr(x);
      A0 w = detail::pow_kernel<A0>::pow1(x, z);
      w = fma(Mhalf<A0>(), z, w);
      w = fma(Log2_em1<A0>(), w, w);
      z = fma(Log2_em1<A0>(), x, w);
      z += x;
      w =  fma(tofloat(-i), Ratio<A0, 1, 16>(), tofloat(e));
      A0 ya = reduc(a1);
      A0 yb = a1 - ya;
      A0 W = fma(z, a1, w*yb);
      A0 Wa = reduc(W);
      A0 Wb = W - Wa;
      W = fma(w, ya, Wa);
      Wa = reduc(W);
      A0 u = W - Wa;
      W = Wb + u;
      Wb = reduc(W);
      w = Sixteen<A0>()*(Wa + Wb);
      // Test the power of 2 for over/underflow
      auto inf_ret = is_greater(w, Powlargelim<A0>());
      auto zer_ret = is_less(w, Powlowlim<A0>());
      e = toint(w);
      Wb = W - Wb;
      auto test = is_gtz(Wb);
      e = if_inc(test, e);
      Wb = if_minus(test, Wb, Ratio<A0, 1, 16>());
      z = detail::pow_kernel<A0>::pow2(Wb)*Wb;
      i = if_inc(is_gtz(e), (e/Sixteen<iA0>()));
      e = (i*Sixteen<iA0>()) - e;
      w =  detail::pow_kernel<A0>::twomio16(e);
      z = fma(w, z, w);
      z = ldexp( z, i );
#ifndef BOOST_SIMD_NO_INFINITIES
      auto gtax1 = is_greater(ax,One<A0>());
      z =  if_else(is_equal(a1,  Inf<A0>()),if_else_zero(gtax1, Inf<A0>()), z);
      z =  if_else(is_equal(a1, Minf<A0>()),if_zero_else(gtax1, Inf<A0>()), z);
      z =  if_else(is_equal(ax, Inf<A0>()),
                   if_else(is_gtz(a1),
                           Inf<A0>(),
                           if_zero_else_one(is_ltz(a1))),
                   z);
#endif
      z = if_zero_else(zer_ret, z);
      z = if_else(inf_ret, Inf<A0>(), z);
      z = if_else(is_equal(ax, One<A0>()), ax, z);
      return if_else(is_eqz(a0), if_one_else_zero(is_eqz(a1)), z);
    }
  private :
    static BOOST_FORCEINLINE A0 reduc(const A0& x)
    {
      // Find a multiple of 1/16 that is within 1/16 of x.
      return Ratio<A0, 1, 16>()*floor(Sixteen<A0>()*x);
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF ( pow_abs_
                             , (typename A0, typename A1, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::fast_tag
                             , bs::pack_< bd::floating_<A0>, X>
                             , bs::pack_< bd::floating_<A1>, X>
                             )
  {
    BOOST_FORCEINLINE A0 operator()(const fast_tag &,
                                    A0 const& a0, A1 const& a1) const BOOST_NOEXCEPT
    {
      return bs::exp(a1*bs::log(bs::abs(a0)));
    }
  };
} } }

#endif


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/unit_test.hpp>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/Ephemerides/keplerEphemeris.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/sphericalBodyShapeModel.h"
#include "Tudat/SimulationSetup/tudatEstimationHeader.h"

namespace tudat
{
namespace unit_tests
{

//Using declarations.
using namespace tudat::ephemerides;
using namespace tudat::interpolators;
using namespace tudat::numerical_integrators;
using namespace tudat::spice_interface;
using namespace tudat::simulation_setup;
using namespace tudat::basic_astrodynamics;
using namespace tudat::orbital_element_conversions;
using namespace tudat::propagators;
using namespace tudat::coordinate_conversions;
using namespace tudat::reference_frames;
using namespace tudat::observation_models;
using namespace tudat::estimatable_parameters;
using namespace tudat::orbit_determination;

BOOST_AUTO_TEST_SUITE( test_rotational_dynamics_estimation )

//! Create body map to be used in estimation test
NamedBodyMap getTestBodyMap( const double phobosSemiMajorAxis,
                             const bool useSymmetricEquator = 0 )
{
    NamedBodyMap bodyMap;

    // Create Mars object
    bodyMap[ "Mars" ] = std::make_shared< Body >( );
    bodyMap[ "Mars" ]->setEphemeris( createBodyEphemeris(
                                         getDefaultEphemerisSettings( "Mars" ), "Mars" ) );
    std::shared_ptr< SphericalHarmonicsGravityFieldSettings > marsGravityFieldSettings =
            std::dynamic_pointer_cast< SphericalHarmonicsGravityFieldSettings >(
                getDefaultGravityFieldSettings( "Mars", TUDAT_NAN, TUDAT_NAN ) );
    marsGravityFieldSettings->resetGravitationalParameter( spice_interface::getBodyGravitationalParameter( "Mars" ) );
    bodyMap[ "Mars" ]->setGravityFieldModel(
                createGravityFieldModel( marsGravityFieldSettings, "Mars", bodyMap ) );
    bodyMap[ "Mars" ]->setRotationalEphemeris(
                createRotationModel( getDefaultRotationModelSettings( "Mars", TUDAT_NAN, TUDAT_NAN ), "Mars" ) );

    // Create Mars object
    bodyMap[ "Earth" ] = std::make_shared< Body >( );
    bodyMap[ "Earth" ]->setEphemeris( createBodyEphemeris(
                                          getDefaultEphemerisSettings( "Earth" ), "Earth" ) );

    // Create Phobos object
    bodyMap[ "Phobos" ] = std::make_shared< Body >( );

    // Set Phobos inertia
    Eigen::Matrix3d phobosInertiaTensor = Eigen::Matrix3d::Zero( );
    phobosInertiaTensor( 0, 0 ) = 0.3615;
    phobosInertiaTensor( 1, 1 ) = 0.4265;
    phobosInertiaTensor( 2, 2 ) = 0.5024;
    if( useSymmetricEquator )
    {
        phobosInertiaTensor( 0, 0 ) = phobosInertiaTensor( 1, 1 );
    }
    double phobosReferenceRadius = 11.27E3;
    double phobosMass = 1.0659E16;
    phobosInertiaTensor *= (phobosReferenceRadius * phobosReferenceRadius * phobosMass );
    bodyMap[ "Phobos" ]->setBodyInertiaTensor( phobosInertiaTensor );

    // Set Phobos shape
    bodyMap[ "Phobos" ]->setShapeModel(
                std::make_shared< SphericalBodyShapeModel >( 15.0E3 ) );

    // Compute and set Phobos gravity field
    double phobosGravitationalParameter = phobosMass * physical_constants::GRAVITATIONAL_CONSTANT;
    Eigen::MatrixXd phobosCosineGravityFieldCoefficients = Eigen::Matrix3d::Zero( ),
            phobosSineGravityFieldCoefficients = Eigen::Matrix3d::Zero( );
    double phobosScaledMeanMomentOfInertia;
    gravitation::getDegreeTwoSphericalHarmonicCoefficients(
                phobosInertiaTensor, phobosGravitationalParameter, phobosReferenceRadius, true,
                phobosCosineGravityFieldCoefficients, phobosSineGravityFieldCoefficients, phobosScaledMeanMomentOfInertia );
    bodyMap[ "Phobos" ]->setGravityFieldModel(
                std::make_shared< gravitation::SphericalHarmonicsGravityField >(
                    phobosGravitationalParameter, phobosReferenceRadius, phobosCosineGravityFieldCoefficients,
                    phobosSineGravityFieldCoefficients, "Phobos_Fixed",
                    std::bind( &Body::setBodyInertiaTensorFromGravityFieldAndExistingMeanMoment, bodyMap.at( "Phobos" ), true ) ) );

    // Set Phobos dummy rotational ephemeris
    Eigen::Quaterniond noRotationQuaternion = Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) );
    Eigen::Matrix< double, 7, 1 > unitRotationState = Eigen::Matrix< double, 7, 1 >::Zero( );
    unitRotationState( 0 ) = noRotationQuaternion.w( );
    unitRotationState( 1 ) = noRotationQuaternion.x( );
    unitRotationState( 2 ) = noRotationQuaternion.y( );
    unitRotationState( 3 ) = noRotationQuaternion.z( );

    std::map< double, Eigen::Matrix< double, 7, 1 > > dummyRotationMap;
    dummyRotationMap[ -1.0E100 ] = unitRotationState;
    dummyRotationMap[ 1.0E100 ] = unitRotationState;

    std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Matrix< double, 7, 1 > > > dummyInterpolator =
            std::make_shared< interpolators::LinearInterpolator< double, Eigen::Matrix< double, 7, 1 > > >( dummyRotationMap );
    bodyMap[ "Phobos" ]->setRotationalEphemeris( std::make_shared< TabulatedRotationalEphemeris< double, double > >(
                                                     dummyInterpolator, "ECLIPJ2000", "Phobos_Fixed" ) );

    // Set Phobos on Kepler orbit
    Eigen::Vector6d phobosKeplerElements = Eigen::Vector6d::Zero( );
    phobosKeplerElements( 0 ) = phobosSemiMajorAxis;
    bodyMap[ "Phobos" ]->setEphemeris(
                ephemerides::getTabulatedEphemeris(
                    std::make_shared< ephemerides::KeplerEphemeris >(
                        phobosKeplerElements, 0.0, spice_interface::getBodyGravitationalParameter( "Mars" ),
                        "Mars", "ECLIPJ2000" ), -3600.0, 120.0 * 86400.0 + 3600.0, 120.0,
                    std::make_shared< interpolators::LagrangeInterpolatorSettings >( 8 ) ) );

    return bodyMap;
}

//! Test if Phobos rotational dynamics is correctly estimation from lander tracking data
BOOST_AUTO_TEST_CASE( test_RotationalDynamicsEstimationFromLanderData )
{
    //Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Retrieve list of body objects.
    NamedBodyMap bodyMap = getTestBodyMap( 9376.0E3, 0 );
    createGroundStation( bodyMap.at( "Phobos" ), "Lander", ( Eigen::Vector3d( ) << 0.1, 0.35, 0.0 ).finished( ), geodetic_position );
    setGlobalFrameBodyEphemerides( bodyMap, "Mars", "ECLIPJ2000" );

    // Define time range of test.
    double initialEphemerisTime = 0.0;
    double finalEphemerisTime = initialEphemerisTime + 60.0 * 86400.0;

    // Define mean motion (equal to rotation rate).
    double phobosSemiMajorAxis = 9376.0E3;
    double meanMotion = std::sqrt( getBodyGravitationalParameter( "Mars" ) /
                                   std::pow( phobosSemiMajorAxis, 3.0 ) );

    // Define initial rotational state (slight-perturbation from tidally locked)
    double initialAnglePerturbation = 1.0E-3;
    double initialRotationRatePerturbation = 1.0E-2;
    Eigen::Quaterniond nominalInitialRotation =
            Eigen::Quaterniond( Eigen::AngleAxisd( -initialAnglePerturbation, Eigen::Vector3d::UnitX( ) ) );
    Eigen::VectorXd systemInitialState = Eigen::VectorXd::Zero( 7 );
    systemInitialState.segment( 0, 4 ) = linear_algebra::convertQuaternionToVectorFormat( nominalInitialRotation );
    systemInitialState( 6 ) = meanMotion * ( 1.0 + initialRotationRatePerturbation );
//    Eigen::Matrix3d phobosInertiaTensor = bodyMap.at( "Phobos" )->getBodyInertiaTensor( );

    // Create torque models
    std::vector< std::string > bodiesToIntegrate;
    bodiesToIntegrate.push_back( "Phobos" );
    SelectedTorqueMap torqueMap;
    torqueMap[ "Phobos" ][ "Mars" ].push_back( std::make_shared< TorqueSettings >( second_order_gravitational_torque ) );
    basic_astrodynamics::TorqueModelMap torqueModelMap = createTorqueModelsMap(
                bodyMap, torqueMap, bodiesToIntegrate );

    // Define integrator settings.
    double timeStep = 240.0;
    std::shared_ptr< IntegratorSettings< > > integratorSettings =
            std::make_shared< IntegratorSettings< > >
            ( rungeKutta4, initialEphemerisTime, timeStep );

    // Define propagator settings.
    std::shared_ptr< RotationalStatePropagatorSettings< double > > propagatorSettings =
            std::make_shared< RotationalStatePropagatorSettings< double > >
            ( torqueModelMap, bodiesToIntegrate, systemInitialState, std::make_shared< PropagationTimeTerminationSettings >(
                  finalEphemerisTime ) );

    // Define link ends and observables
    std::vector< LinkEnds > linkEndsList;
    LinkEnds currentLinkEnds;
    currentLinkEnds[ transmitter ] = std::make_pair( "Earth", "" );
    currentLinkEnds[ receiver ] = std::make_pair( "Phobos", "Lander" );
    linkEndsList.push_back( currentLinkEnds );
    std::map< ObservableType, std::vector< LinkEnds > > linkEndsPerObservable;
    //linkEndsPerObservable[ one_way_range ].push_back( linkEndsList[ 0 ] );

    LinkEnds currentLinkEnds2;
    currentLinkEnds2[ observed_body ] = std::make_pair( "Phobos", "" );
    linkEndsPerObservable[ euler_angle_313_observable ].push_back( currentLinkEnds2 );

    // Create parameters to estimate
    std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames;
    parameterNames.push_back(
                std::make_shared< InitialRotationalStateEstimatableParameterSettings< double > >(
                    "Phobos", systemInitialState ) );
    std::vector< std::pair< int, int > > blockIndices;
    blockIndices.push_back( std::make_pair( 2, 2 ) );
    parameterNames.push_back( std::make_shared< SphericalHarmonicEstimatableParameterSettings >(
                                  blockIndices, "Phobos", spherical_harmonics_cosine_coefficient_block ) );
    std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parametersToEstimate =
            createParametersToEstimate( parameterNames, bodyMap );
    printEstimatableParameterEntries( parametersToEstimate );

    // Create observation settings
    observation_models::ObservationSettingsMap observationSettingsMap;
    for( std::map< ObservableType, std::vector< LinkEnds > >::iterator linkEndIterator = linkEndsPerObservable.begin( );
         linkEndIterator != linkEndsPerObservable.end( ); linkEndIterator++ )
    {
        ObservableType currentObservable = linkEndIterator->first;

        std::vector< LinkEnds > currentLinkEndsList = linkEndIterator->second;
        for( unsigned int i = 0; i < currentLinkEndsList.size( ); i++ )
        {
            observationSettingsMap.insert( std::make_pair( currentLinkEndsList.at( i ),
                                                           std::make_shared< ObservationSettings >( currentObservable ) ) );
        }
    }

    // Create orbit determination object
    OrbitDeterminationManager< double, double > orbitDeterminationManager =
            OrbitDeterminationManager< double, double >(
                bodyMap, parametersToEstimate, observationSettingsMap,
                integratorSettings, propagatorSettings );

    // Deifne observation times
    std::vector< double > observationTimes;
    double currentTime = initialEphemerisTime + 1800.0;
    double observationTimeStep = 60.0;
    while( currentTime < finalEphemerisTime - 1800.0 )
    {
        observationTimes.push_back( currentTime );
        currentTime += observationTimeStep;
    }
    std::map< ObservableType, std::map< LinkEnds, std::pair< std::vector< double >, LinkEndType > > > measurementSimulationInput;
    for( std::map< ObservableType, std::vector< LinkEnds > >::iterator linkEndIterator = linkEndsPerObservable.begin( );
         linkEndIterator != linkEndsPerObservable.end( ); linkEndIterator++ )
    {
        ObservableType currentObservable = linkEndIterator->first;
        std::vector< LinkEnds > currentLinkEndsList = linkEndIterator->second;
        for( unsigned int i = 0; i < currentLinkEndsList.size( ); i++ )
        {
            measurementSimulationInput[ currentObservable ][ currentLinkEndsList.at( i ) ] =
                    std::make_pair( observationTimes, observed_body );
        }
    }

    typedef Eigen::Matrix< double, Eigen::Dynamic, 1 > ObservationVectorType;
    typedef std::map< LinkEnds, std::pair< ObservationVectorType, std::pair< std::vector< double >, LinkEndType > > > SingleObservablePodInputType;
    typedef std::map< ObservableType, SingleObservablePodInputType > PodInputDataType;

    // Simulate observations
    PodInputDataType observationsAndTimes = simulateObservations< double, double >(
                measurementSimulationInput, orbitDeterminationManager.getObservationSimulators( ) );

    // Perturb parameter estimate
    Eigen::Matrix< double, Eigen::Dynamic, 1 > initialParameterEstimate =
            parametersToEstimate->template getFullParameterValues< double >( );
    Eigen::Matrix< double, Eigen::Dynamic, 1 > truthParameters = initialParameterEstimate;
    int numberOfParameters = initialParameterEstimate.rows( );
    initialParameterEstimate( 1 ) -= 1.0E-7;
    initialParameterEstimate( 2 ) -= 1.0E-7;
    initialParameterEstimate( 3 ) += 1.0E-7;

    initialParameterEstimate.segment( 0, 4 ).normalize( );
    initialParameterEstimate( 5 ) += 1.0E-9;
    initialParameterEstimate( 7 ) += 1.0E-12;

    // Define estimation input
    std::shared_ptr< PodInput< double, double  > > podInput =
            std::make_shared< PodInput< double, double > >(
                observationsAndTimes, numberOfParameters,
                Eigen::MatrixXd::Zero( numberOfParameters, numberOfParameters ), initialParameterEstimate - truthParameters );

    // Perform estimation
    std::shared_ptr< PodOutput< double > > podOutput = orbitDeterminationManager.estimateParameters(
                podInput, std::make_shared< EstimationConvergenceChecker >( 6 ) );

    // Check residual size (sub-mm over >1 AU)
    BOOST_CHECK_SMALL( std::fabs( podOutput->residualStandardDeviation_ ), 1.0E-3 );

    // Check parameter errors
    for( unsigned int i = 0; i < 4; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( i ) - truthParameters( i ) ), 1.0E-13 );
    }
    for( unsigned int i = 0; i < 3; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( i + 4 ) - truthParameters( i + 4 ) ), 1.0E-17 );
    }
    for( unsigned int i = 0; i < 1; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( i + 7 ) - truthParameters( i + 7 ) ), 1.0E-16 );

    }
    std::cout<<"True error: "<<( podOutput->parameterEstimate_ - truthParameters ).transpose( )<<std::endl;
    std::cout<<"Formal error: "<<podOutput->getFormalErrorVector( ).transpose( )<<std::endl;
    std::cout<<"Error ratio: "<<( ( 1.0E-3 * podOutput->getFormalErrorVector( ).segment( 0, numberOfParameters ) ).cwiseQuotient(
                                      podOutput->parameterEstimate_ - truthParameters ) ).transpose( )<<std::endl;

//    input_output::writeMatrixToFile( podOutput->normalizedInformationMatrix_,
//                                     "rotationTestEstimationInformationMatrix.dat", 16,
//                                     input_output::getTudatRootPath( ) );
//    input_output::writeMatrixToFile( podOutput->informationMatrixTransformationDiagonal_,
//                                     "rotationTestEstimationInformationMatrixNormalization.dat", 16,
//                                     input_output::getTudatRootPath( ) );
//    input_output::writeMatrixToFile( podOutput->weightsMatrixDiagonal_,
//                                     "rotationTestEstimationWeightsDiagonal.dat", 16,
//                                     input_output::getTudatRootPath( ) );
//    input_output::writeMatrixToFile( podOutput->residuals_,
//                                     "rotationTestEstimationResiduals.dat", 16,
//                                     input_output::getTudatRootPath( ) );
//    input_output::writeMatrixToFile( podOutput->getCorrelationMatrix( ),
//                                     "rotationTestEstimationCorrelations.dat", 16,
//                                     input_output::getTudatRootPath( ) );
//    input_output::writeMatrixToFile( podOutput->getResidualHistoryMatrix( ),
//                                     "rotationTestResidualHistory.dat", 16,
//                                     input_output::getTudatRootPath( ) );
//    input_output::writeMatrixToFile( podOutput->getParameterHistoryMatrix( ),
//                                     "rotationTestParameterHistory.dat", 16,
//                                     input_output::getTudatRootPath( ) );
//    input_output::writeMatrixToFile( getConcatenatedMeasurementVector( podInput->getObservationsAndTimes( ) ),
//                                     "rotationTestObservationMeasurements.dat", 16,
//                                     input_output::getTudatRootPath( ) );
//    input_output::writeMatrixToFile( utilities::convertStlVectorToEigenVector(
//                                         getConcatenatedTimeVector( podInput->getObservationsAndTimes( ) ) ),
//                                     "rotationTestObservationTimes.dat", 16,
//                                     input_output::getTudatRootPath( ) );
//    input_output::writeMatrixToFile( utilities::convertStlVectorToEigenVector(
//                                         getConcatenatedGroundStationIndex( podInput->getObservationsAndTimes( ) ).first ),
//                                     "rotationTestObservationLinkEnds.dat", 16,
//                                     input_output::getTudatRootPath( ) );
//    input_output::writeMatrixToFile( getConcatenatedMeasurementVector( podInput->getObservationsAndTimes( ) ),
//                                     "rotationTestObservationMeasurements.dat", 16,
//                                     input_output::getTudatRootPath( ) );

//    input_output::writeMatrixToFile( podOutput->getFormalErrorVector( ),
//                                     "rotationTestObservationFormalEstimationError.dat", 16,
//                                     input_output::getTudatRootPath( ) );
}

//! Test if Phobos coupled translational-rotational dynamics is correctly estimation from lander tracking data
BOOST_AUTO_TEST_CASE( test_RotationalTranslationalDynamicsEstimationFromLanderData )
{
    //Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Retrieve list of body objects.
    NamedBodyMap bodyMap = getTestBodyMap( 9376.0E3, 0 );
    createGroundStation( bodyMap.at( "Phobos" ), "Lander", ( Eigen::Vector3d( ) << 0.1, 0.35, 0.0 ).finished( ), geodetic_position );
    setGlobalFrameBodyEphemerides( bodyMap, "Mars", "ECLIPJ2000" );

    // Define time range of test.
    double initialEphemerisTime = 0.0;
    double finalEphemerisTime = initialEphemerisTime + 60.0 * 86400.0;

    // Define mean motion (equal to rotation rate).
    double phobosSemiMajorAxis = 9376.0E3;
    double meanMotion = std::sqrt( getBodyGravitationalParameter( "Mars" ) /
                                   std::pow( phobosSemiMajorAxis, 3.0 ) );

    // Define initial rotational state
    double initialAnglePerturbation = 1.0E-2;
    double initialRotationRatePerturbation = 1.0E-2;

    Eigen::Quaterniond nominalInitialRotation =
            Eigen::Quaterniond( Eigen::AngleAxisd( -initialAnglePerturbation, Eigen::Vector3d::UnitZ( ) ) );
    Eigen::VectorXd systemInitialState = Eigen::VectorXd::Zero( 7 );
    systemInitialState.segment( 0, 4 ) = linear_algebra::convertQuaternionToVectorFormat( nominalInitialRotation );
    systemInitialState( 6 ) = meanMotion * ( 1.0 + initialRotationRatePerturbation );

    Eigen::Matrix3d phobosInertiaTensor = bodyMap.at( "Phobos" )->getBodyInertiaTensor( );

    // Create torque models
    SelectedTorqueMap torqueMap;
    torqueMap[ "Phobos" ][ "Mars" ].push_back( std::make_shared< TorqueSettings >( second_order_gravitational_torque ) );

    std::vector< std::string > bodiesToIntegrate;
    bodiesToIntegrate.push_back( "Phobos" );

    basic_astrodynamics::TorqueModelMap torqueModelMap = createTorqueModelsMap(
                bodyMap, torqueMap, bodiesToIntegrate );

    // Define acceleration model settings.
    SelectedAccelerationMap accelerationMap;
    std::vector< std::string > translationalBodiesToPropagate;
    std::vector< std::string > translationalCentralBodies;
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfPhobos;
    accelerationsOfPhobos[ "Mars" ].push_back( std::make_shared< MutualSphericalHarmonicAccelerationSettings >( 2, 2, 2, 2 ) );
    accelerationMap[ "Phobos" ] = accelerationsOfPhobos;
    translationalBodiesToPropagate.push_back( "Phobos" );
    translationalCentralBodies.push_back( "Mars" );

    // Create acceleration models
    basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                bodyMap, accelerationMap, translationalBodiesToPropagate, translationalCentralBodies );

    // Define integrator settings.
    double timeStep = 240.0;
    std::shared_ptr< IntegratorSettings< > > integratorSettings =
            std::make_shared< RungeKuttaVariableStepSizeSettings< > >
            ( initialEphemerisTime, timeStep,
              RungeKuttaCoefficients::rungeKuttaFehlberg78, timeStep, timeStep, 1.0, 1.0 );

    // Define propagator settings.
    std::shared_ptr< RotationalStatePropagatorSettings< double > > rotationalPropagatorSettings =
            std::make_shared< RotationalStatePropagatorSettings< double > >
            ( torqueModelMap, bodiesToIntegrate, systemInitialState, std::make_shared< PropagationTimeTerminationSettings >(
                  finalEphemerisTime ) );

    Eigen::VectorXd initialTranslationalState =
            propagators::getInitialStatesOfBodies(
                translationalBodiesToPropagate, translationalCentralBodies, bodyMap, initialEphemerisTime );
    std::shared_ptr< TranslationalStatePropagatorSettings< double > > translationalPropagatorSettings =
            std::make_shared< TranslationalStatePropagatorSettings< double > >
            ( translationalCentralBodies, accelerationModelMap, translationalBodiesToPropagate,
              initialTranslationalState, finalEphemerisTime );

    std::vector< std::shared_ptr< SingleArcPropagatorSettings< double > > >  propagatorSettingsList;
    propagatorSettingsList.push_back( translationalPropagatorSettings );
    propagatorSettingsList.push_back( rotationalPropagatorSettings );

    std::shared_ptr< PropagatorSettings< double > > propagatorSettings =
            std::make_shared< MultiTypePropagatorSettings< double > >(
                propagatorSettingsList,
                std::make_shared< PropagationTimeTerminationSettings >( finalEphemerisTime ) );

    // Define link ends and observables
    std::vector< LinkEnds > linkEndsList;
    LinkEnds currentLinkEnds;
    currentLinkEnds[ transmitter ] = std::make_pair( "Earth", "" );
    currentLinkEnds[ receiver ] = std::make_pair( "Phobos", "Lander" );
    linkEndsList.push_back( currentLinkEnds );

    std::map< ObservableType, std::vector< LinkEnds > > linkEndsPerObservable;
    linkEndsPerObservable[ one_way_range ].push_back( linkEndsList[ 0 ] );

    // Create parameters to estimate
    std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames;
    parameterNames.push_back(
                std::make_shared< InitialTranslationalStateEstimatableParameterSettings< double > >(
                    "Phobos", initialTranslationalState, "Mars" ) );
    parameterNames.push_back(
                std::make_shared< InitialRotationalStateEstimatableParameterSettings< double > >(
                    "Phobos", systemInitialState ) );
    std::vector< std::pair< int, int > > blockIndices;
    blockIndices.push_back( std::make_pair( 2, 0 ) );
    blockIndices.push_back( std::make_pair( 2, 2 ) );
    parameterNames.push_back( std::make_shared< SphericalHarmonicEstimatableParameterSettings >(
                                  blockIndices, "Phobos", spherical_harmonics_cosine_coefficient_block ) );

    std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parametersToEstimate =
            createParametersToEstimate( parameterNames, bodyMap );
    printEstimatableParameterEntries( parametersToEstimate );

    // Create observation settings
    observation_models::ObservationSettingsMap observationSettingsMap;
    for( std::map< ObservableType, std::vector< LinkEnds > >::iterator linkEndIterator = linkEndsPerObservable.begin( );
         linkEndIterator != linkEndsPerObservable.end( ); linkEndIterator++ )
    {
        ObservableType currentObservable = linkEndIterator->first;

        std::vector< LinkEnds > currentLinkEndsList = linkEndIterator->second;
        for( unsigned int i = 0; i < currentLinkEndsList.size( ); i++ )
        {
            observationSettingsMap.insert( std::make_pair( currentLinkEndsList.at( i ),
                                                           std::make_shared< ObservationSettings >( currentObservable ) ) );
        }
    }

    // Create orbit determination object
    OrbitDeterminationManager< double, double > orbitDeterminationManager =
            OrbitDeterminationManager< double, double >(
                bodyMap, parametersToEstimate, observationSettingsMap,
                integratorSettings, propagatorSettings );

    // Deifne observation times
    std::vector< double > observationTimes;
    double currentTime = initialEphemerisTime + 1800.0;
    double observationTimeStep = 1200.0;
    while( currentTime < finalEphemerisTime - 1800.0 )
    {
        observationTimes.push_back( currentTime );
        currentTime += observationTimeStep;
    }

    std::map< ObservableType, std::map< LinkEnds, std::pair< std::vector< double >, LinkEndType > > > measurementSimulationInput;
    for( std::map< ObservableType, std::vector< LinkEnds > >::iterator linkEndIterator = linkEndsPerObservable.begin( );
         linkEndIterator != linkEndsPerObservable.end( ); linkEndIterator++ )
    {
        ObservableType currentObservable = linkEndIterator->first;
        std::vector< LinkEnds > currentLinkEndsList = linkEndIterator->second;
        for( unsigned int i = 0; i < currentLinkEndsList.size( ); i++ )
        {
            measurementSimulationInput[ currentObservable ][ currentLinkEndsList.at( i ) ] =
                    std::make_pair( observationTimes, receiver );
        }
    }

    typedef Eigen::Matrix< double, Eigen::Dynamic, 1 > ObservationVectorType;
    typedef std::map< LinkEnds, std::pair< ObservationVectorType, std::pair< std::vector< double >, LinkEndType > > > SingleObservablePodInputType;
    typedef std::map< ObservableType, SingleObservablePodInputType > PodInputDataType;

    // Simulate observations
    PodInputDataType observationsAndTimes = simulateObservations< double, double >(
                measurementSimulationInput, orbitDeterminationManager.getObservationSimulators( ) );

    // Perturb parameter estimate
    Eigen::Matrix< double, Eigen::Dynamic, 1 > initialParameterEstimate =
            parametersToEstimate->template getFullParameterValues< double >( );
    int numberOfParameters = initialParameterEstimate.rows( );

    Eigen::Matrix< double, Eigen::Dynamic, 1 > truthParameters = initialParameterEstimate;
    initialParameterEstimate( 7 ) -= 1.0E-5;
    initialParameterEstimate( 8 ) += 1.0E-5;
    initialParameterEstimate( 9 ) += 1.0E-5;

    initialParameterEstimate.segment( 6, 4 ).normalize( );
    initialParameterEstimate( 10 ) += 1.0E-7;

    initialParameterEstimate( 0 ) += 1.0;
    initialParameterEstimate( 1 ) += 1.0;
    initialParameterEstimate( 2 ) += 1.0;
    int parameterSize = initialParameterEstimate.rows( );

    // Define estimation input
    std::shared_ptr< PodInput< double, double  > > podInput =
            std::make_shared< PodInput< double, double > >(
                observationsAndTimes, parameterSize,
                Eigen::MatrixXd::Zero( parameterSize, parameterSize ), initialParameterEstimate - truthParameters );
    podInput->defineEstimationSettings( true, false, true, true, true, true );

    // Perform estimation
    std::shared_ptr< PodOutput< double > > podOutput = orbitDeterminationManager.estimateParameters(
                podInput, std::make_shared< EstimationConvergenceChecker >( 6 ) );

    // Check residual size (sub-mm over >1 AU)
    BOOST_CHECK_SMALL( std::fabs( podOutput->residualStandardDeviation_ ), 1.0E-3 );

    // Check parameter errors
    BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( 0 ) - truthParameters( 0 ) ), 1.0E-4 );
    BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( 1 ) - truthParameters( 1 ) ), 1.0E-4 );
    BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( 2 ) - truthParameters( 2 ) ), 1.0E-2 );

    BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( 3 ) - truthParameters( 3 ) ), 2.0E-8 );
    BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( 4 ) - truthParameters( 4 ) ), 2.0E-8 );
    BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( 5 ) - truthParameters( 5 ) ), 2.0E-6 );

    BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( 6 ) - truthParameters( 6 ) ), 1.0E-12 );
    BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( 7 ) - truthParameters( 7 ) ), 1.0E-9 );
    BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( 8 ) - truthParameters( 8 ) ), 1.0E-9 );
    BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( 9 ) - truthParameters( 9 ) ), 1.0E-9 );

    BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( 10 ) - truthParameters( 10 ) ), 5.0E-13 );
    BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( 11 ) - truthParameters( 11 ) ), 5.0E-13 );
    BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( 12 ) - truthParameters( 12 ) ), 5.0E-13 );

    BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( 13 ) - truthParameters( 13 ) ), 1.0E-10 );
    BOOST_CHECK_SMALL( std::fabs( podOutput->parameterEstimate_( 14 ) - truthParameters( 14 ) ), 1.0E-11 );
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


/* Copyright (c) 2016 - 2020, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#include <ThermalOperator.hh>
#include <instantiation.hh>

#include <deal.II/base/index_set.h>
#include <deal.II/grid/filtered_iterator.h>
#include <deal.II/matrix_free/fe_evaluation.h>

namespace adamantine
{

template <int dim, int fe_degree, typename MemorySpaceType>
ThermalOperator<dim, fe_degree, MemorySpaceType>::ThermalOperator(
    MPI_Comm const &communicator,
    std::shared_ptr<MaterialProperty<dim>> material_properties)
    : _communicator(communicator), _material_properties(material_properties),
      _inverse_mass_matrix(
          new dealii::LA::distributed::Vector<double, MemorySpaceType>())
{
  _matrix_free_data.tasks_parallel_scheme =
      dealii::MatrixFree<dim, double>::AdditionalData::partition_color;
  _matrix_free_data.mapping_update_flags = dealii::update_gradients |
                                           dealii::update_JxW_values |
                                           dealii::update_quadrature_points;
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::reinit(
    dealii::DoFHandler<dim> const &dof_handler,
    dealii::AffineConstraints<double> const &affine_constraints,
    dealii::QGaussLobatto<1> const &quad)
{
  _matrix_free.reinit(dof_handler, affine_constraints, quad, _matrix_free_data);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::reinit(
    dealii::DoFHandler<dim> const &dof_handler,
    dealii::AffineConstraints<double> const &affine_constraints,
    dealii::QGauss<1> const &quad)
{
  _matrix_free.reinit(dof_handler, affine_constraints, quad, _matrix_free_data);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::
    compute_inverse_mass_matrix(
        dealii::DoFHandler<dim> const &dof_handler,
        dealii::AffineConstraints<double> const &affine_constraints)
{
  // Compute the inverse of the mass matrix
  dealii::QGaussLobatto<1> mass_matrix_quad(fe_degree + 1);
  dealii::MatrixFree<dim, double> mass_matrix_free;
  typename dealii::MatrixFree<dim, double>::AdditionalData mf_data;
  mf_data.tasks_parallel_scheme =
      dealii::MatrixFree<dim, double>::AdditionalData::partition_color;
  mf_data.mapping_update_flags = dealii::update_values |
                                 dealii::update_JxW_values |
                                 dealii::update_quadrature_points;

  mass_matrix_free.reinit(dof_handler, affine_constraints, mass_matrix_quad,
                          mf_data);
  mass_matrix_free.initialize_dof_vector(*_inverse_mass_matrix);
  dealii::VectorizedArray<double> one =
      dealii::make_vectorized_array(static_cast<double>(1.));
  dealii::FEEvaluation<dim, fe_degree, fe_degree + 1, 1, double> fe_eval(
      mass_matrix_free);
  unsigned int const n_q_points = fe_eval.n_q_points;
  for (unsigned int cell = 0; cell < mass_matrix_free.n_macro_cells(); ++cell)
  {
    fe_eval.reinit(cell);
    for (unsigned int q = 0; q < n_q_points; ++q)
      fe_eval.submit_value(one, q);
    fe_eval.integrate(true, false);
    fe_eval.distribute_local_to_global(*_inverse_mass_matrix);
  }
  _inverse_mass_matrix->compress(dealii::VectorOperation::add);
  unsigned int const local_size = _inverse_mass_matrix->local_size();
  for (unsigned int k = 0; k < local_size; ++k)
  {
    if (_inverse_mass_matrix->local_element(k) > 1e-15)
      _inverse_mass_matrix->local_element(k) =
          1. / _inverse_mass_matrix->local_element(k);
    else
      _inverse_mass_matrix->local_element(k) = 0.;
  }
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::clear()
{
  _matrix_free.clear();
  _inverse_mass_matrix->reinit(0);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::vmult(
    dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &src) const
{
  dst = 0.;
  vmult_add(dst, src);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::Tvmult(
    dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &src) const
{
  dst = 0.;
  Tvmult_add(dst, src);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::vmult_add(
    dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &src) const
{
  // Execute the matrix-free matrix-vector multiplication
  _matrix_free.cell_loop(&ThermalOperator::local_apply, this, dst, src);

  // Because cell_loop resolves the constraints, the constrained dofs are not
  // called they stay at zero. Thus, we need to force the value on the
  // constrained dofs by hand. The variable scaling is used so that we get the
  // right order of magnitude.
  // TODO: for now the value of scaling is set to 1
  double const scaling = 1.;
  std::vector<unsigned int> const &constrained_dofs =
      _matrix_free.get_constrained_dofs();
  for (auto &dof : constrained_dofs)
    dst.local_element(dof) += scaling * src.local_element(dof);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::Tvmult_add(
    dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &src) const
{
  // The system of equation is symmetric so we can use vmult_add
  vmult_add(dst, src);
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::local_apply(
    dealii::MatrixFree<dim, double> const &data,
    dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &src,
    std::pair<unsigned int, unsigned int> const &cell_range) const
{
  dealii::FEEvaluation<dim, fe_degree, fe_degree + 1, 1, double> fe_eval(data);
  dealii::Tensor<1, dim> unit_tensor;
  for (unsigned int i = 0; i < dim; ++i)
    unit_tensor[i] = 1.;

  // Loop over the "cells". Note that we don't really work on a cell but on a
  // set of quadrature point.
  for (unsigned int cell = cell_range.first; cell < cell_range.second; ++cell)
  {
    // Reinit fe_eval on the current cell
    fe_eval.reinit(cell);
    // Store in a local vector the local values of src
    fe_eval.read_dof_values(src);
    // Evaluate only the function gradients on the reference cell
    fe_eval.evaluate(false, true);
    // Apply the Jacobian of the transformation, multiply by the variable
    // coefficients and the quadrature points
    for (unsigned int q = 0; q < fe_eval.n_q_points; ++q)
      fe_eval.submit_gradient(-_inv_rho_cp(cell, q) *
                                  _thermal_conductivity(cell, q) *
                                  fe_eval.get_gradient(q),
                              q);
    // Sum over the quadrature points.
    fe_eval.integrate(false, true);
    fe_eval.distribute_local_to_global(dst);
  }
}

template <int dim, int fe_degree, typename MemorySpaceType>
void ThermalOperator<dim, fe_degree, MemorySpaceType>::
    evaluate_material_properties(
        dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> const
            &temperature)
{
  // Update the state of the materials
  _material_properties->update(_matrix_free.get_dof_handler(), temperature);

  unsigned int const n_cells = _matrix_free.n_macro_cells();
  dealii::FEEvaluation<dim, fe_degree, fe_degree + 1, 1, double> fe_eval(
      _matrix_free);
  _inv_rho_cp.reinit(n_cells, fe_eval.n_q_points);
  _thermal_conductivity.reinit(n_cells, fe_eval.n_q_points);
  for (unsigned int cell = 0; cell < n_cells; ++cell)
    for (unsigned int q = 0; q < fe_eval.n_q_points; ++q)
      for (unsigned int i = 0; i < _matrix_free.n_components_filled(cell); ++i)
      {
        typename dealii::DoFHandler<dim>::cell_iterator cell_it =
            _matrix_free.get_cell_iterator(cell, i);
        // Cast to Triangulation<dim>::cell_iterator to access the material_id
        typename dealii::Triangulation<dim>::active_cell_iterator cell_tria(
            cell_it);

        _thermal_conductivity(cell, q)[i] = _material_properties->get(
            cell_tria, StateProperty::thermal_conductivity);

        _inv_rho_cp(cell, q)[i] =
            1. / (_material_properties->get(cell_tria, StateProperty::density) *
                  _material_properties->get(cell_tria,
                                            StateProperty::specific_heat));
        _cell_it_to_mf_cell_map[cell_it] = std::make_pair(cell, i);
      }
}
} // namespace adamantine

INSTANTIATE_DIM_FEDEGREE_HOST(TUPLE(ThermalOperator))