/*
Copyright (C) 2015-present CompatibL

Performance test results and finance-specific examples are available at:

http://www.tapescript.org

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#ifndef cl_tape_examples_impl_performance_utils_hpp
#define cl_tape_examples_impl_performance_utils_hpp

#include <assert.h>
#include <random>
#include <boost/timer.hpp>

#define CL_BASE_SERIALIZER_OPEN
#include <cl/tape/tape.hpp>
#include <cl/tape/util/testoutput.hpp>

#include "impl/utils.hpp"

namespace cl
{
    template <class Func>
    inline double test_performance(size_t repeat_count, Func func)
    {
        assert(repeat_count != 0);
        boost::timer timer;
        for (size_t i = 0; i < repeat_count; i++)
        {
            func();
        }
        return timer.elapsed() / repeat_count;
    }

    struct test_statistic
    {
        // Input size (used for plot imformation only).
        size_t size_;
        // Tape recording time.
        double rec_;
        // Initial Forward(0) sweep time.
        double init_;
        // Forward sweep calculations time.
        double forw_;
        // Reverse sweep calculations time.
        double rev_;
        // tfunc memory.
        size_t mem_;

        double total_time() const
        {
            return rec_ + init_ + forw_ + rev_;
        }
    };

    inline std::ostream& operator<<(std::ostream& os, test_statistic const& stat)
    {
        os << "Tape recording time:                     " << stat.rec_  << std::endl;
        os << "Initial Forward(0) sweep time:           " << stat.init_ << std::endl;
        os << "Forward sweep calculations time:         " << stat.forw_ << std::endl;
        os << "Reverse sweep calculations time:         " << stat.rev_  << std::endl;
        os << "Total time for derivatives calculation:  " << stat.total_time() << std::endl;
        os << "tfunc memory (B): " << stat.mem_  << std::endl;
        return os;
    }

    struct time_columns
    {
        static std::deque<std::string> get_columns()
        {
            static std::deque<std::string> const& columns =
            {
                "Input size"
                , "Tape recording time"
                , "Initial Forward(0) sweep time"
                , "Forward mode derivatives calculation time"
                , "Reverse mode derivatives calculation time"
            };

            return columns;
        }

        static std::deque<double> get_y(test_statistic const& stat)
        {
            return{ stat.rec_, stat.init_, stat.forw_, stat.rev_ };
        }
    };

    struct mem_columns
    {
        static std::deque<std::string> get_columns()
        {
            static std::deque<std::string> const& columns =
            {
                "Input size"
                , "Optimazed tape memory"
                , "Non-optimazed tape memory"
            };

            return columns;
        }

        static std::deque<double> get_y(test_statistic const& array_stat, test_statistic const& double_stat)
        {
            return{ (double)array_stat.mem_, (double)double_stat.mem_ };
        }
    };

    struct total_time_columns
    {
        static std::deque<std::string> get_columns()
        {
            static std::deque<std::string> const& columns =
            {
                "Input size"
                , "Optimazed"
                , "Non-optimazed"
            };

            return columns;
        }

        static std::deque<double> get_y(test_statistic const& array_stat, test_statistic const& double_stat)
        {
            return{ array_stat.total_time(), double_stat.total_time() };
        }
    };

    template <class Columns = time_columns>
    struct plot_struct
    {
        plot_struct()
            : y_()
            , x_()
        {}

        template <class ... Args>
        plot_struct(test_statistic const& stat, Args... args)
            : y_(Columns::get_y(stat, args...))
            , x_(stat.size_)
        {}

        static std::deque<std::string> get_columns()
        {
            return Columns::get_columns();
        }

        template <typename stream_type>
        friend inline stream_type& operator<<(stream_type& stm, plot_struct& v)
        {
            stm << v.x_;
            for (auto y : v.y_)
            {
                stm << ";" << y;
            }
            stm << std::endl;
            return stm;
        }

        std::deque<double> y_;
        size_t x_;
    };

    inline std::ostream& fake_stream()
    {
        static std::ostream stream(nullptr);
        static tape_serializer_base fake(stream);
        return fake;
    }

    template <class Inner>
    struct adjoint_task
    {
        typedef Inner inner_type;
        typedef tape_wrapper<Inner> tape_type;

        // Input size (used for plot imformation only).
        size_t size_;
        // Input values.
        std::vector<inner_type> X_;
        // Forward mode directions.
        std::vector<inner_type> dx_;
        // Revers mode weights.
        std::vector<inner_type> w_;
        // Numbers of times to repeat task for performance measurement.
        size_t repeat_;
        // Target function.
        std::function<std::vector<tape_type>(std::vector<tape_type> const&)> func_;
    };

    template <class AdjointTask>
    inline test_statistic adjoint_performance(AdjointTask const& task, std::ostream& out_stream = fake_stream())
    {
        typedef typename AdjointTask::inner_type inner_type;
        typedef typename AdjointTask::tape_type tape_type;
        test_statistic result;
        result.size_ = task.size_;

        // Input values initialization.
        std::vector<inner_type> x_val = task.X_;
        std::vector<tape_type> X(x_val.begin(), x_val.end());
        out_str << "Input vector: " << X << "\n";

        // Output vector.
        std::vector<tape_type> Y;
        // Tape function.
        tfunc<inner_type> f;

        result.rec_ = test_performance(task.repeat_, [&X, &Y, &f, &task]()
        {
            // Declare the X vector as independent and start a tape recording.
            tape_start(X);
            // Output calculations.
            Y = task.func_(X);
            //Stop tape recording.
            f.tape_read(X, Y);
        });

        out_str << "Output vector: " << Y << "\n\n";

        out_str << "Initial Forward(0) sweep...\n\n";
        result.init_ = test_performance(task.repeat_, [&f, &x_val]()
        {
            f.forward(0, x_val);
        });

        // Forward sweep calculations.
        std::vector<inner_type> dx = task.dx_;
        out_str << "Forward(1, dx) sweep for dx = " << dx << "..." << std::endl;
        std::vector<inner_type> forw = f.forward(1, dx, out_stream);
        out_str << "Forward sweep result: " << forw << "\n\n";

        result.forw_ = test_performance(task.repeat_, [&f, &dx]()
        {
            f.forward(1, dx);
        });

        // Reverse sweep calculations.
        std::vector<inner_type> w = task.w_;
        out_str << "Reverse(1, w) sweep for w = " << w << "..." << std::endl;
        std::vector<inner_type> rev = f.reverse(1, w);
        out_str << "Reverse sweep result: " << rev << "\n\n\n";

        result.rev_ = test_performance(task.repeat_, [&f, &w]()
        {
            f.reverse(1, w);
        });

        result.mem_ = f.Memory();

        return result;
    }

    template <class Vector>
    inline Vector gen_vector(size_t n, std::mt19937& gen)
    {
        std::uniform_real_distribution<> dis(-10, 10);
        Vector vec;
        vec.resize(n);
        for (auto& x : vec)
        {
            x = dis(gen);
        }
        return vec;
    }

    template <class TaskFactory>
    inline void compare_tape(TaskFactory factory, size_t n, std::ostream& out_stream = fake_stream())
    {
        auto array_task = factory.get_array_task(n);
        auto double_task = factory.get_double_task(n);

        out_str << "Arrays:\n\n";
        test_statistic array_stat = adjoint_performance(array_task, out_stream);

        out_str << "Doubles:\n\n";
        test_statistic double_stat = adjoint_performance(double_task, out_stream);
    }

    template <class TaskFactory>
    inline void performance_plot(std::string const& folder_name, TaskFactory factory, std::ostream& out_stream = fake_stream())
    {
        tape_empty_test_output outMemory(folder_name, {
            { "title", "Tape size dependence on number of variables" }
            , { "filename", "TapeSize" }
            , { "not_clear", "Not" }
            , { "ylabel", "Memory (B)" }
            , { "cleanlog", "false" }
        });

        compare_tape(factory, 10, out_stream);

#if defined CL_GRAPH_GEN
        for (size_t i = 1; i <= 50; i++)
        {
            auto array_task = factory.get_array_task(i);
            auto double_task = factory.get_double_task(i);
            test_statistic array_stat = adjoint_performance(array_task, out_stream);
            test_statistic double_stat = adjoint_performance(double_task, out_stream);
            plot_struct<mem_columns> memory(array_stat, double_stat);
            outMemory << memory;
        }
#endif
    }

}

#endif // cl_tape_examples_impl_performance_utils_hpp


//==================================================================================================
/*!
  @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_FUNCTION_LOGICAL_XOR_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_LOGICAL_XOR_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-boolean
    Function object implementing logical_xor capabilities

    return the logical xor of the first parameter and of the second parameter.

    The operands must share the same @ref cardinal_of value.

    The result type is logical type associated to the first parameter.

    @par semantic:

    @code
    auto r = logical_xor(x,y);
    @endcode

    is similar to:

    @code
    auto r = !x != !y;
    @endcode

    @see  logical_or, logical_ornot, logical_notand,
    logical_andnot, logical_notor, logical_and, logical_not

  **/
  as_logical_t<Value0> logical_xor(Value0 const& x, Value1 const& y);
} }
#endif

#include <boost/simd/function/scalar/logical_xor.hpp>
#include <boost/simd/function/simd/logical_xor.hpp>

#endif


#pragma once
#include <iostream>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <stdexcept>
#include <unordered_map>
#include <boost/graph/graph_traits.hpp>

namespace ariel {
        struct Unit{
        std::string sign;
        // std::shared_ptr<Unit> smaller_unit = nullptr;
        // std::shared_ptr<Unit> bigger_unit = nullptr;
        Unit* smaller_unit = nullptr;
        Unit* bigger_unit = nullptr;
        double convert_smaller;
        double convert_bigger;
    };
    class NumberWithUnits {
    protected:

    public:
        double value;
        std::string number_sign;
        static std::unordered_map <std::string, Unit> units;
        NumberWithUnits(double value, std::string sign);
        static void read_units(std::ifstream &units_file);

        static double validateConversion(const std::string& s1, const std::string& s2);
        friend std::ostream& operator<< (std::ostream &out, const NumberWithUnits &num);
        friend std::istream& operator>>(std::istringstream &is,NumberWithUnits &num);

        friend NumberWithUnits operator+( const NumberWithUnits &o1, const NumberWithUnits &o2);
        friend NumberWithUnits operator+( const NumberWithUnits &o1);

        friend NumberWithUnits operator-( const NumberWithUnits &o1, const NumberWithUnits &o2);
        friend NumberWithUnits operator-(const NumberWithUnits &o1);

        friend bool operator<( const NumberWithUnits &o1, const NumberWithUnits &o2);
        friend bool operator<=( const NumberWithUnits &o1, const NumberWithUnits &o2);
        friend bool operator>( const NumberWithUnits &o1, const NumberWithUnits &o2);
        friend bool operator>=( const NumberWithUnits &o1, const NumberWithUnits &o2);

        friend NumberWithUnits operator*(double ot, const NumberWithUnits &other);
        friend NumberWithUnits operator*(const NumberWithUnits &other, double ot);

        friend bool operator==( const NumberWithUnits &o1, const NumberWithUnits &o2);
        friend bool operator!=( const NumberWithUnits &o1, const NumberWithUnits &o2);

        friend NumberWithUnits operator+=( NumberWithUnits &o1, const NumberWithUnits &o2);
        friend NumberWithUnits operator-=( NumberWithUnits &o1, const NumberWithUnits &o2);

        NumberWithUnits& operator++();
        NumberWithUnits operator++(int);
        NumberWithUnits& operator--();
        NumberWithUnits operator--(int);
    };

}


/*****************************************************************************
 * This file is huge, it contains everything from parsing yaml files, linear
 * algebra functions to networking code used for robotics.
 *
 * Contents:
 * - Data Type
 * - Macros
 * - Data
 * - Filesystem
 * - Algebra
 * - Linear Algebra
 * - Geometry
 * - Differential Geometry
 * - Statistics
 * - Transform
 * - Time
 *
 ****************************************************************************/
#pragma once
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <sys/time.h>
#include <inttypes.h>
#include <dirent.h>
#include <execinfo.h>
#include <signal.h>
#include <unistd.h>
#include <errno.h>
#include <pthread.h>
#include <netdb.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/poll.h>

#include <iostream>
#include <sstream>
#include <fstream>
#include <string>
#include <random>
#include <set>
#include <list>
#include <deque>
#include <vector>
#include <map>
#include <unordered_map>
#include <unordered_set>
#include <type_traits>

#include <Eigen/Dense>

/******************************************************************************
 *                                DATA TYPE
 *****************************************************************************/

/* PRECISION TYPE */
#define PRECISION 1 // Single Precision
// #define PRECISION 2 // Double Precision

#if PRECISION == 1
  #define real_t float
#elif PRECISION == 2
  #define real_t double
#else
  #define real_t double
#endif

#define col_major_t Eigen::ColMajor
#define row_major_t Eigen::RowMajor

typedef Eigen::Matrix<real_t, 2, 1> vec2_t;
typedef Eigen::Matrix<real_t, 3, 1> vec3_t;
typedef Eigen::Matrix<real_t, 4, 1> vec4_t;
typedef Eigen::Matrix<real_t, 5, 1> vec5_t;
typedef Eigen::Matrix<real_t, 6, 1> vec6_t;
typedef Eigen::Matrix<real_t, Eigen::Dynamic, 1> vecx_t;
typedef Eigen::Matrix<real_t, 2, 2> mat2_t;
typedef Eigen::Matrix<real_t, 3, 3> mat3_t;
typedef Eigen::Matrix<real_t, 4, 4> mat4_t;
typedef Eigen::Matrix<real_t, Eigen::Dynamic, Eigen::Dynamic> matx_t;
typedef Eigen::Matrix<real_t, 3, 4> mat34_t;
typedef Eigen::Quaternion<real_t> quat_t;
typedef Eigen::AngleAxis<real_t> angle_axis_t;
typedef Eigen::Matrix<real_t, 1, Eigen::Dynamic> row_vector_t;
typedef Eigen::Matrix<real_t, Eigen::Dynamic, 1> col_vector_t;
typedef Eigen::Array<real_t, Eigen::Dynamic, 1> arrayx_t;

typedef std::vector<vec2_t, Eigen::aligned_allocator<vec2_t>> vec2s_t;
typedef std::vector<vec3_t, Eigen::aligned_allocator<vec3_t>> vec3s_t;
typedef std::vector<vec4_t, Eigen::aligned_allocator<vec4_t>> vec4s_t;
typedef std::vector<vec5_t, Eigen::aligned_allocator<vec5_t>> vec5s_t;
typedef std::vector<vec6_t, Eigen::aligned_allocator<vec6_t>> vec6s_t;
typedef std::vector<vecx_t> vecxs_t;
typedef std::vector<mat2_t, Eigen::aligned_allocator<mat2_t>> mat2s_t;
typedef std::vector<mat3_t, Eigen::aligned_allocator<mat3_t>> mat3s_t;
typedef std::vector<mat4_t, Eigen::aligned_allocator<mat4_t>> mat4s_t;
typedef std::vector<matx_t, Eigen::aligned_allocator<matx_t>> matxs_t;
typedef std::vector<quat_t, Eigen::aligned_allocator<quat_t>> quats_t;

template <int LENGTH, Eigen::StorageOptions STRIDE_TYPE = Eigen::ColMajor>
using vec_t = Eigen::Matrix<real_t, LENGTH, 1, STRIDE_TYPE>;

template <int ROWS,
          int COLS,
          Eigen::StorageOptions STRIDE_TYPE = Eigen::ColMajor>
using mat_t = Eigen::Matrix<real_t, ROWS, COLS, STRIDE_TYPE>;

template <int ROWS,
          int COLS,
          Eigen::StorageOptions STRIDE_TYPE = Eigen::ColMajor>
using map_mat_t = Eigen::Map<Eigen::Matrix<real_t, ROWS, COLS, STRIDE_TYPE>>;

template <int ROWS>
using map_vec_t = Eigen::Map<Eigen::Matrix<real_t, ROWS, 1>>;

typedef uint64_t timestamp_t;
typedef std::vector<timestamp_t> timestamps_t;

/******************************************************************************
 *                                MACROS
 *****************************************************************************/

#define __FILENAME__                                                           \
  (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__)

#define LOG_ERROR(M, ...)                                                      \
  fprintf(stderr,                                                              \
          "\033[31m[ERROR] [%s:%d] " M "\033[0m\n",                            \
          __FILENAME__,                                                        \
          __LINE__,                                                            \
          ##__VA_ARGS__)

#define LOG_INFO(M, ...) fprintf(stdout, "[INFO] " M "\n", ##__VA_ARGS__)
#define LOG_WARN(M, ...)                                                       \
  fprintf(stdout, "\033[33m[WARN] " M "\033[0m\n", ##__VA_ARGS__)

#define FATAL(M, ...)                                                          \
  fprintf(stdout,                                                              \
          "\033[31m[FATAL] [%s:%d] " M "\033[0m\n",                            \
          __FILENAME__,                                                        \
          __LINE__,                                                            \
          ##__VA_ARGS__);                                                      \
  exit(-1)

#ifdef NDEBUG
#define DEBUG(M, ...)
#else
#define DEBUG(M, ...) fprintf(stdout, "[DEBUG] " M "\n", ##__VA_ARGS__)
#endif

#define UNUSED(expr)                                                           \
  do {                                                                         \
    (void) (expr);                                                             \
  } while (0)

#ifndef CHECK
#define CHECK(A, M, ...)                                                       \
  if (!(A)) {                                                                  \
    LOG_ERROR(M, ##__VA_ARGS__);                                               \
    goto error;                                                                \
  }
#endif

/******************************************************************************
 *                                  DATA
 *****************************************************************************/

/**
 * Convert bytes to signed 8bit number
 */
int8_t int8(const uint8_t *data, const size_t offset);

/**
 * Convert bytes to unsigned 8bit number
 */
uint8_t uint8(const uint8_t *data, const size_t offset);

/**
 * Convert bytes to signed 16bit number
 */
int16_t int16(const uint8_t *data, const size_t offset);

/**
 * Convert bytes to unsigned 16bit number
 */
uint16_t uint16(const uint8_t *data, const size_t offset);

/**
 * Convert bytes to signed 32bit number
 */
int32_t int32(const uint8_t *data, const size_t offset);

/**
 * Convert bytes to unsigned 32bit number
 */
uint32_t uint32(const uint8_t *data, const size_t offset);

/**
 * Allocate memory for a C-style string
 */
char *malloc_string(const char *s);

/**
 * Get number of rows in CSV file.
 * @returns Number of rows in CSV file else -1 for failure.
 */
int csv_rows(const char *fp);

/**
 * Get number of cols in CSV file.
 * @returns Number of cols in CSV file else -1 for failure.
 */
int csv_cols(const char *fp);

/**
 * Return csv fields as strings and number of fields in csv file.
 */
char **csv_fields(const char *fp, int *nb_fields);

/**
 * Load data in csv file `fp`. Assumming the data are real_ts. Also returns
 * number of rows and cols in `nb_rows` and `nb_cols` respectively.
 */
real_t **csv_data(const char *fp, int *nb_rows, int *nb_cols);

/**
 * Load integer arrays in csv file located at `csv_path`. The number of arrays
 * is returned in `nb_arrays`.
 */
int **load_iarrays(const char *csv_path, int *nb_arrays);

/**
 * Load real_t arrays in csv file located at `csv_path`. The number of arrays
 * is returned in `nb_arrays`.
 */
real_t **load_darrays(const char *csv_path, int *nb_arrays);

/**
 * Get number of rows in CSV file.
 * @returns Number of rows in CSV file else -1 for failure.
 */
int csv_rows(const std::string &file_path);

/**
 * Get number of columns in CSV file.
 * @returns Number of columns in CSV file else -1 for failure.
 */
int csv_cols(const std::string &file_path);

/**
 * Convert CSV file to matrix.
 * @returns 0 for success, -1 for failure
 */
int csv2mat(const std::string &file_path, const bool header, matx_t &data);

/**
 * Convert matrix to csv file.
 * @returns 0 for success, -1 for failure
 */
int mat2csv(const std::string &file_path, const matx_t &data);

/**
 * Convert vector to csv file.
 * @returns 0 for success, -1 for failure
 */
int vec2csv(const std::string &file_path, const std::deque<vec3_t> &data);

/**
 * Convert timestamps to csv file.
 * @returns 0 for success, -1 for failure
 */
int ts2csv(const std::string &file_path, const std::deque<timestamp_t> &data);

/**
 * Print progress to screen
 */
void print_progress(const real_t percentage);

/**
 * Check if vector `x` is all true.
 */
bool all_true(const std::vector<bool> x);

int check_jacobian(const std::string &jac_name,
                   const matx_t &fdiff,
                   const matx_t &jac,
                   const real_t threshold,
                   const bool print = false);

/******************************************************************************
 *                                FILESYSTEM
 *****************************************************************************/

/**
 * Open file in `path` with `mode` and set `nb_rows`.
 * @returns File pointer on success, nullptr on failure.
 */
FILE *file_open(const std::string &path,
                const std::string &mode,
                int *nb_rows = nullptr);

/**
 * Skip file line in file `fp`.
 */
void skip_line(FILE *fp);

/**
 * Get number of rows in file.
 * @returns Number of rows in file else -1 for failure.
 */
int file_rows(const std::string &file_path);

/**
 * Copy file from path `src` to path `dest.
 *
 * @returns 0 for success else -1 if `src` file could not be opend, or -2 if
 * `dest` file could not be opened.
 */
int file_copy(const std::string &src, const std::string &dest);

/**
 * Return file extension in `path`.
 */
std::string parse_fext(const std::string &path);

/**
 * Return basename
 */
std::string parse_fname(const std::string &path);

/**
 * Check if file exists
 *
 * @param path Path to file
 * @returns true or false
 */
bool file_exists(const std::string &path);

/**
 * Check if path exists
 *
 * @param path Path
 * @returns true or false
 */
bool dir_exists(const std::string &path);

/**
 * Create directory
 *
 * @param path Path
 * @returns 0 for success, -1 for failure
 */
int dir_create(const std::string &path);

/**
 * Return directory name
 *
 * @param path Path
 * @returns directory name
 */
std::string dir_name(const std::string &path);

/**
 * Strips a target character from the start and end of a string
 *
 * @param s String to strip
 * @param target Target character to strip
 * @returns Stripped string
 */
std::string strip(const std::string &s, const std::string &target = " ");

/**
 * Strips a target character from the end of a string
 *
 * @param s String to strip
 * @param target Target character to strip
 * @returns Stripped string
 */
std::string strip_end(const std::string &s, const std::string &target = " ");

/**
 * Create directory
 *
 * @param path Path to directory
 * @returns 0 for success, -1 for failure
 */
int create_dir(const std::string &path);

/**
 * Remove directory
 *
 * @param path Path to directory
 * @returns 0 for success, -1 for failure
 */
int remove_dir(const std::string &path);

/**
 * Remove file extension
 *
 * @param path Path to directory
 * @returns File path without extension
 */
std::string remove_ext(const std::string &path);

/**
 * List directory
 *
 * @param path Path to directory
 * @param results List of files and directories
 * @returns 0 for success, -1 for failure
 */
int list_dir(const std::string &path, std::vector<std::string> &results);

/**
 * Split path into a number of elements
 *
 * @param path Path
 * @returns List of path elements
 */
std::vector<std::string> path_split(const std::string path);

/**
 * Combine `path1` and `path2`
 *
 * @param path1 Path 1
 * @param path2 Path 22
 * @returns Combined path
 */
std::string paths_combine(const std::string path1, const std::string path2);

/******************************************************************************
 *                                  ALGEBRA
 *****************************************************************************/

/**
 * Sign of number
 *
 * @param[in] x Number to check sign
 * @return
 *    - 0: Number is zero
 *    - 1: Positive number
 *    - -1: Negative number
 */
int sign(const real_t x);

/**
 * Floating point comparator
 *
 * @param[in] f1 First value
 * @param[in] f2 Second value
 * @return
 *    - 0: if equal
 *    - 1: if f1 > f2
 *    - -1: if f1 < f2
 */
int fltcmp(const real_t f1, const real_t f2);

/**
 * Calculate binomial coefficient
 *
 * @param[in] n
 * @param[in] k
 * @returns Binomial coefficient
 */
real_t binomial(const real_t n, const real_t k);

/**
 * Return evenly spaced numbers over a specified interval.
 */
template <typename T>
std::vector<T> linspace(const T start, const T end, const int num) {
  std::vector<T> linspaced;

  if (num == 0) {
    return linspaced;
  }
  if (num == 1) {
    linspaced.push_back(start);
    return linspaced;
  }

  const real_t diff = static_cast<real_t>(end - start);
  const real_t delta = diff / static_cast<real_t>(num - 1);
  for (int i = 0; i < num - 1; ++i) {
    linspaced.push_back(start + delta * i);
  }
  linspaced.push_back(end);
  return linspaced;
}

/******************************************************************************
 *                              LINEAR ALGEBRA
 *****************************************************************************/

/**
 * Print shape of a matrix
 *
 * @param[in] name Name of matrix
 * @param[in] A Matrix
 */
void print_shape(const std::string &name, const matx_t &A);

/**
 * Print shape of a vector
 *
 * @param[in] name Name of vector
 * @param[in] v Vector
 */
void print_shape(const std::string &name, const vecx_t &v);

/**
 * Print array
 *
 * @param[in] name Name of array
 * @param[in] array Target array
 * @param[in] size Size of target array
 */
void print_array(const std::string &name,
                 const real_t *array,
                 const size_t size);

/**
 * Print vector `v` with a `name`.
 */
void print_vector(const std::string &name, const vecx_t &v);

/**
 * Print matrix `m` with a `name`.
 */
void print_matrix(const std::string &name, const matx_t &m);

/**
 * Print quaternion `q` with a `name`.
 */
void print_quaternion(const std::string &name, const quat_t &q);

/**
 * Array to string
 *
 * @param[in] array Target array
 * @param[in] size Size of target array
 * @returns String of array
 */
std::string array2str(const real_t *array, const size_t size);

/**
 * Convert real_t array to Eigen::Vector
 *
 * @param[in] x Input array
 * @param[in] size Size of input array
 * @param[out] y Output vector
 */
void array2vec(const real_t *x, const size_t size, vecx_t &y);

/**
 * Vector to array
 *
 * @param[in] v Vector
 * @returns Array
 */
real_t *vec2array(const vecx_t &v);

/**
 * Matrix to array
 *
 * @param[in] m Matrix
 * @returns Array
 */
real_t *mat2array(const matx_t &m);

/**
 * Quaternion to array
 *
 * *VERY IMPORTANT*: The returned array is (x, y, z, w).
 *
 * @param[in] q Quaternion
 * @returns Array
 */
real_t *quat2array(const quat_t &q);

/**
 * Vector to array
 *
 * @param[in] v Vector
 * @param[out] out Output array
 */
void vec2array(const vecx_t &v, real_t *out);

/**
 * Matrix to array
 *
 * @param[in] m Matrix
 * @param[in] out Output array
 */
void mat2array(const matx_t &m, real_t *out);

/**
 * Matrix to list of vectors
 *
 * @param[in] m Matrix
 * @param[in] row_wise Row wise
 * @returns Vectors
 */
std::vector<vecx_t> mat2vec(const matx_t &m, const bool row_wise = true);

/**
 * Matrix to list of vectors of size 3
 *
 * @param[in] m Matrix
 * @param[in] row_wise Row wise
 * @returns Vectors
 */
vec3s_t mat2vec3(const matx_t &m, const bool row_wise = true);

/**
 * Matrix to list of vectors of size 3
 *
 * @param[in] m Matrix
 * @param[in] row_wise Row wise
 * @returns Vectors
 */
vec2s_t mat2vec2(const matx_t &m, const bool row_wise = true);

/**
 * Vectors to matrix
 */
matx_t vecs2mat(const vec3s_t &vs);

/**
 * Vector to string
 *
 * @param[in] v Vector
 * @param[in] brackets Brakcets around vector string
 * @returns Vector as a string
 */
std::string vec2str(const vecx_t &v, const bool brackets = true);

/**
 * Array to string
 *
 * @param[in] arr Array
 * @param[in] len Length of array
 * @param[in] brackets Brakcets around vector string
 * @returns Array as a string
 */
std::string arr2str(const real_t *arr, const size_t len, bool brackets = true);

/**
 * Matrix to string
 *
 * @param[in] m Matrix
 * @param[in] indent Indent string
 * @returns Array as a string
 */
std::string mat2str(const matx_t &m, const std::string &indent = "  ");

/**
 * Normalize vector x
 */
vec2_t normalize(const vec2_t &x);

/**
 * Normalize vector `v`.
 */
vec3_t normalize(const vec3_t &v);

/**
 * Condition number of `A`.
 */
real_t cond(const matx_t &A);

/**
 * Zeros-matrix
 *
 * @param rows Number of rows
 * @param cols Number of cols
 * @returns Zeros matrix
 */
matx_t zeros(const int rows, const int cols);

/**
 * Zeros square matrix
 *
 * @param size Square size of matrix
 * @returns Zeros matrix
 */
matx_t zeros(const int size);

/**
 * Identity-matrix
 *
 * @param rows Number of rows
 * @param cols Number of cols
 * @returns Identity matrix
 */
matx_t I(const int rows, const int cols);

/**
 * Identity square matrix
 *
 * @param size Square size of matrix
 * @returns Identity square matrix
 */
matx_t I(const int size);

/**
 * Ones-matrix
 *
 * @param rows Number of rows
 * @param cols Number of cols
 * @returns Ones square matrix
 */
matx_t ones(const int rows, const int cols);

/**
 * Ones square matrix
 *
 * @param size Square size of matrix
 * @returns Ones square matrix
 */
matx_t ones(const int size);

/**
 * Horizontally stack matrices A and B
 *
 * @param A Matrix A
 * @param B Matrix B
 * @returns Stacked matrix
 */
matx_t hstack(const matx_t &A, const matx_t &B);

/**
 * Vertically stack matrices A and B
 *
 * @param A Matrix A
 * @param B Matrix B
 * @returns Stacked matrix
 */
matx_t vstack(const matx_t &A, const matx_t &B);

/**
 * Diagonally stack matrices A and B
 *
 * @param A Matrix A
 * @param B Matrix B
 * @returns Stacked matrix
 */
matx_t dstack(const matx_t &A, const matx_t &B);

/**
 * Skew symmetric-matrix
 *
 * @param w Input vector
 * @returns Skew symmetric matrix
 */
mat3_t skew(const vec3_t &w);

/**
 * Skew symmetric-matrix squared
 *
 * @param w Input vector
 * @returns Skew symmetric matrix squared
 */
mat3_t skewsq(const vec3_t &w);

/**
 * Enforce Positive Semi-Definite
 *
 * @param A Input matrix
 * @returns Positive semi-definite matrix
 */
matx_t enforce_psd(const matx_t &A);

/**
 * Null-space of A
 *
 * @param A Input matrix
 * @returns Null space of A
 */
matx_t nullspace(const matx_t &A);

/**
 * Load std::vector of real_ts to an Eigen::Matrix
 *
 * @param[in] x Matrix values
 * @param[in] rows Number of matrix rows
 * @param[in] cols Number of matrix colums
 * @param[out] y Output matrix
 */
void load_matrix(const std::vector<real_t> &x,
                 const int rows,
                 const int cols,
                 matx_t &y);

/**
 * Load an Eigen::Matrix into a std::vector of real_ts
 *
 * @param[in] A Matrix
 * @param[out] x Output vector of matrix values
 */
void load_matrix(const matx_t A, std::vector<real_t> &x);

/**
 * Perform Schur's Complement
 */
void schurs_complement(const matx_t &H, const vecx_t &b,
                       const size_t m, const size_t r,
                       matx_t &H_marg, vecx_t &b_marg,
                       const bool precond=false, const bool debug=false);

/******************************************************************************
 *                                 Geometry
 *****************************************************************************/

/**
 * Sinc function.
 */
real_t sinc(const real_t x);

/**
 * Degrees to radians
 *
 * @param[in] d Degree to be converted
 * @return Degree in radians
 */
real_t deg2rad(const real_t d);

/**
 * Degrees to radians
 *
 * @param[in] d Degree to be converted
 * @return Degree in radians
 */
vec3_t deg2rad(const vec3_t d);

/**
 * Radians to degree
 *
 * @param[in] r Radian to be converted
 * @return Radian in degrees
 */
real_t rad2deg(const real_t r);

/**
 * Radians to degree
 *
 * @param[in] r Radian to be converted
 * @return Radian in degrees
 */
vec3_t rad2deg(const vec3_t &r);

/**
 * Wrap angle in degrees to 180
 *
 * @param[in] d Degrees
 * @return Angle wraped to 180
 */
real_t wrap180(const real_t d);

/**
 * Wrap angle in degrees to 360
 *
 * @param[in] d Degrees
 * @return Angle wraped to 360
 */
real_t wrap360(const real_t d);

/**
 * Wrap angle in radians to PI
 *
 * @param[in] r Radians
 * @return Angle wraped to PI
 */
real_t wrapPi(const real_t r);

/**
 * Wrap angle in radians to 2 PI
 *
 * @param[in] r Radians
 * @return Angle wraped to 2 PI
 */
real_t wrap2Pi(const real_t r);

/**
 * Create a circle point of radius `r` at angle `theta` radians.
 */
vec2_t circle(const real_t r, const real_t theta);

/**
 * Create the sphere point with sphere radius `rho` at longitude `theta`
 * [radians] and latitude `phi` [radians].
 */
vec3_t sphere(const real_t rho, const real_t theta, const real_t phi);

/**
 * Create look at matrix.
 */
mat4_t lookat(const vec3_t &cam_pos,
              const vec3_t &target,
              const vec3_t &up_axis = vec3_t{0.0, -1.0, 0.0});

/**
 * Cross-Track error based on waypoint line between p1, p2, and robot position
 *
 * @param[in] p1 Waypoint 1
 * @param[in] p2 Waypoint 2
 * @param[in] pos Robot position
 * @return Cross track error
 */
real_t cross_track_error(const vec2_t &p1, const vec2_t &p2, const vec2_t &pos);

/**
 * Check if point `pos` is left or right of line formed by `p1` and `p2`
 *
 * @param[in] p1 Waypoint 1
 * @param[in] p2 Waypoint 2
 * @param[in] pos Robot position
 * @returns
 *    - 1: Point is left of waypoint line formed by `p1` and `p2`
 *    - 2: Point is right of waypoint line formed by `p1` and `p2`
 *    - 0: Point is colinear with waypoint line formed by `p1` and `p2`
 */
int point_left_right(const vec2_t &p1, const vec2_t &p2, const vec2_t &pos);

/**
 * Calculate closest point given waypoint line between `p1`, `p2` and robot
 * position
 *
 * @param[in] p1 Waypoint 1
 * @param[in] p2 Waypoint 2
 * @param[in] p3 Robot position
 * @param[out] closest Closest point
 * @returns
 *    Unit number denoting where the closest point is on waypoint line. For
 *    example, a return value of 0.5 denotes the closest point is half-way
 *    (50%) of the waypoint line, alternatively a negative number denotes the
 *    closest point is behind the first waypoint.
 */
real_t closest_point(const vec2_t &p1,
                     const vec2_t &p2,
                     const vec2_t &p3,
                     vec2_t &closest);

#define EARTH_RADIUS_M 6378137.0

/**
 * Calculate new latitude and logitude coordinates with an offset in North and
 * East direction.
 *
 * IMPORTANT NOTE: This function is only an approximation. As such do not rely
 * on this function for precise latitude, longitude offsets.
 *
 * @param lat_ref Latitude of origin (decimal format)
 * @param lon_ref Longitude of origin (decimal format)
 * @param offset_N Offset in North direction (meters)
 * @param offset_E Offset in East direction (meters)
 * @param lat_new New latitude (decimal format)
 * @param lon_new New longitude (decimal format)
 */
void latlon_offset(real_t lat_ref,
                   real_t lon_ref,
                   real_t offset_N,
                   real_t offset_E,
                   real_t *lat_new,
                   real_t *lon_new);

/**
 * Calculate difference in distance in North and East from two GPS coordinates
 *
 * IMPORTANT NOTE: This function is only an approximation. As such do not rely
 * on this function for precise latitude, longitude diffs.
 *
 * @param lat_ref Latitude of origin (decimal format)
 * @param lon_ref Longitude of origin (decimal format)
 * @param lat Latitude of point of interest (decimal format)
 * @param lon Longitude of point of interest (decimal format)
 * @param dist_N Distance of point of interest in North axis [m]
 * @param dist_E Distance of point of interest in East axis [m]
 */
void latlon_diff(real_t lat_ref,
                 real_t lon_ref,
                 real_t lat,
                 real_t lon,
                 real_t *dist_N,
                 real_t *dist_E);

/**
 * Calculate Euclidean distance between two GPS coordintes
 *
 * IMPORTANT NOTE: This function is only an approximation. As such do not rely
 * on this function for precise latitude, longitude distance.
 *
 * @param lat_ref Latitude of origin (decimal format)
 * @param lon_ref Longitude of origin (decimal format)
 * @param lat Latitude of point of interest (decimal format)
 * @param lon Longitude of point of interest (decimal format)
 *
 * @returns Euclidean distance between two GPS coordinates [m]
 */
real_t latlon_dist(real_t lat_ref, real_t lon_ref, real_t lat, real_t lon);

/*****************************************************************************
 *                         DIFFERENTIAL GEOMETRY
 *****************************************************************************/

namespace lie {

mat3_t Exp(const vec3_t &phi);
vec3_t Log(const mat3_t &C);
mat3_t Jr(const vec3_t &psi);

} // namespace lie

/******************************************************************************
 *                                STATISTICS
 *****************************************************************************/

/**
 * Create random integer
 *
 * @param[in] ub Upper bound
 * @param[in] lb Lower bound
 * @return Random integer
 */
int randi(const int ub, const int lb);

/**
 * Create random real_t
 *
 * @param[in] ub Upper bound
 * @param[in] lb Lower bound
 * @return Random floating point
 */
real_t randf(const real_t ub, const real_t lb);

/**
 * Calculate median given an array of numbers
 *
 * @param[in] v Array of numbers
 * @return Median of given array
 */
real_t median(const std::vector<real_t> &v);

/**
 * Mean vector
 *
 * @param[in] x List of vectors
 * @return Mean vector
 */
vec3_t mean(const vec3s_t &x);

/**
 * Shannon Entropy of a given covariance matrix `covar`.
 */
real_t shannon_entropy(const matx_t &covar);

/**
 * Multivariate normal.
 */
vec3_t mvn(std::default_random_engine &engine,
           const vec3_t &mu = vec3_t{0.0, 0.0, 0.0},
           const vec3_t &stdev = vec3_t{1.0, 1.0, 1.0});

/**
 * Gassian normal.
 * http://c-faq.com/lib/gaussian.html
 */
real_t gauss_normal();

/*****************************************************************************
 *                               TRANSFORM
 *****************************************************************************/

/**
 * Form a 4x4 homogeneous transformation matrix from a
 * rotation matrix `C` and translation vector `r`.
 */
template <typename T>
Eigen::Matrix<T, 4, 4> tf(const Eigen::Matrix<T, 3, 3> &C,
                          const Eigen::Matrix<T, 3, 1> &r) {
  Eigen::Matrix<T, 4, 4> transform = Eigen::Matrix<T, 4, 4>::Identity();
  transform.block(0, 0, 3, 3) = C;
  transform.block(0, 3, 3, 1) = r;
  return transform;
}

/**
 * Form a 4x4 homogeneous transformation matrix from a pointer to real_t array
 * containing (quaternion + translation) 7 elements: (qw, qx, qy, qz, x, y, z)
 */
mat4_t tf(const vecx_t &params);

/**
 * Form a 4x4 homogeneous transformation matrix from a
 * rotation matrix `C` and translation vector `r`.
 */
mat4_t tf(const mat3_t &C, const vec3_t &r);

/**
 * Form a 4x4 homogeneous transformation matrix from a
 * Hamiltonian quaternion `q` and translation vector `r`.
 */
mat4_t tf(const quat_t &q, const vec3_t &r);

/**
 * Extract rotation from transform
 */
inline mat3_t tf_rot(const mat4_t &tf) { return tf.block<3, 3>(0, 0); }

/**
 * Extract rotation and convert to quaternion from transform
 */
inline quat_t tf_quat(const mat4_t &tf) { return quat_t{tf.block<3, 3>(0, 0)}; }

/**
 * Extract translation from transform
 */
inline vec3_t tf_trans(const mat4_t &tf) { return tf.block<3, 1>(0, 3); }

/**
 * Perturb the rotation element in the tranform `T` by `step_size` at index
 * `i`. Where i = 0 for x-axis, i = 1 for y-axis, and i = 2 for z-axis.
 */
mat4_t tf_perturb_rot(const mat4_t &T, real_t step_size, const int i);

/**
 * Perturb the translation element in the tranform `T` by `step_size` at index
 * `i`. Where i = 0 for x-axis, i = 1 for y-axis, and i = 2 for z-axis.
 */
mat4_t tf_perturb_trans(const mat4_t &T, real_t step_size, const int i);

/**
 * Transform point `p` with transform `T`.
 */
vec3_t tf_point(const mat4_t &T, const vec3_t &p);

/**
 * Rotation matrix around x-axis (counter-clockwise, right-handed).
 * @returns Rotation matrix
 */
mat3_t rotx(const real_t theta);

/**
 * Rotation matrix around y-axis (counter-clockwise, right-handed).
 * @returns Rotation matrix
 */
mat3_t roty(const real_t theta);

/**
 * Rotation matrix around z-axis (counter-clockwise, right-handed).
 * @returns Rotation matrix
 */
mat3_t rotz(const real_t theta);

/**
 * Convert euler sequence 123 to rotation matrix R
 * This function assumes we are performing a body fixed intrinsic rotation.
 *
 * Source:
 *
 *     Kuipers, Jack B. Quaternions and Rotation Sequences: A Primer with
 *     Applications to Orbits, Aerospace, and Virtual Reality. Princeton, N.J:
 *     Princeton University Press, 1999. Print.
 *
 *     Page 86.
 *
 * @returns Rotation matrix
 */
mat3_t euler123(const vec3_t &euler);

/**
 * Convert euler sequence 321 to rotation matrix R
 * This function assumes we are performing a body fixed intrinsic rotation.
 *
 * Source:
 *
 *     Kuipers, Jack B. Quaternions and Rotation Sequences: A Primer with
 *     Applications to Orbits, Aerospace, and Virtual Reality. Princeton, N.J:
 *     Princeton University Press, 1999. Print.
 *
 *     Page 86.
 *
 * @returns Rotation matrix
 */
mat3_t euler321(const vec3_t &euler);

/**
 * Convert roll, pitch and yaw to quaternion.
 */
quat_t euler2quat(const vec3_t &euler);

/**
 * Convert rotation vectors to rotation matrix using measured acceleration
 * `a_m` from an IMU and gravity vector `g`.
 */
mat3_t vecs2rot(const vec3_t &a_m, const vec3_t &g);

/**
 * Convert rotation vector `rvec` to rotation matrix.
 */
mat3_t rvec2rot(const vec3_t &rvec, const real_t eps = 1e-5);

/**
 * Convert quaternion to euler angles.
 */
vec3_t quat2euler(const quat_t &q);

/**
 * Convert quaternion to rotation matrix.
 */
mat3_t quat2rot(const quat_t &q);

/**
 * Convert small angle euler angle to quaternion.
 */
quat_t quat_delta(const vec3_t &dalpha);

/**
 * Return left quaternion product matrix.
 */
mat4_t quat_lmul(const quat_t &q);

/**
 * Return left quaternion product matrix (but only for x, y, z components).
 */
mat3_t quat_lmul_xyz(const quat_t &q);

/**
 * Return right quaternion product matrix.
 */
mat4_t quat_rmul(const quat_t &q);

/**
 * Return right quaternion product matrix (but only for x, y, z components).
 */
mat3_t quat_rmul_xyz(const quat_t &q);

/**
 * Return only the x, y, z, components of a quaternion matrix.
 */
mat3_t quat_mat_xyz(const mat4_t &Q);

/**
 * Initialize attitude using IMU gyroscope `w_m` and accelerometer `a_m`
 * measurements. The calculated attitude outputted into to `C_WS`. Note: this
 * function does not calculate initial yaw angle in the world frame. Only the
 * roll, and pitch are inferred from IMU measurements.
 */
void imu_init_attitude(const vec3s_t w_m,
                       const vec3s_t a_m,
                       mat3_t &C_WS,
                       const size_t buffer_size = 50);

/*****************************************************************************
 *                                    TIME
 *****************************************************************************/

/**
 * Print timestamp.
 */
void timestamp_print(const timestamp_t &ts, const std::string &prefix = "");

/**
 * Convert ts to second.
 */
real_t ts2sec(const timestamp_t &ts);

/**
 * Convert nano-second to second.
 */
real_t ns2sec(const uint64_t ns);

/**
 * Start timer.
 */
struct timespec tic();

/**
 * Stop timer and return number of seconds.
 */
float toc(struct timespec *tic);

/**
 * Stop timer and return miliseconds elasped.
 */
float mtoc(struct timespec *tic);

/**
 * Get time now in milliseconds since epoch
 */
real_t time_now();

/*****************************************************************************
 *                                BA DATA
 ****************************************************************************/

struct pose_t {
  vec_t<7> param;

  pose_t();
  pose_t(const mat4_t &T);

  quat_t rot() const;
  vec3_t trans() const;
  mat4_t T() const;

  quat_t rot();
  vec3_t trans();
  mat4_t T();

  void set_trans(const vec3_t &r);
  void set_rot(const quat_t &q);
  void set_rot(const mat3_t &C);
};
typedef std::vector<pose_t> poses_t;
typedef std::vector<vec2_t> keypoints_t;

void pose_print(const std::string &prefix, const pose_t &pose);

poses_t load_poses(const std::string &csv_path);
keypoints_t parse_keypoints_line(const char *line);
std::vector<keypoints_t> load_keypoints(const std::string &data_path);
void keypoints_print(const keypoints_t &keypoints);
poses_t load_poses(const std::string &csv_path);
keypoints_t parse_keypoints_line(const char *line);
std::vector<keypoints_t> load_keypoints(const std::string &data_path);
void keypoints_print(const keypoints_t &keypoints);
mat3_t load_camera(const std::string &data_path);
poses_t load_camera_poses(const std::string &data_path);
poses_t load_target_pose(const std::string &data_path);
real_t **load_points(const std::string &data_path, int *nb_points);
int **load_point_ids(const std::string &data_path, int *nb_points);


#define BOOST_TEST_MODULE "test_config"

#include <boost/test/included/unit_test.hpp> // for BOOST_PP_IIF_1, BOOST_PP_IIF_0, BOOST_PP_BO...
#include <cstdint>                           // for int32_t
#include <exception>                         // for exception
#include <memory>                            // for allocator_traits<>::value_type
#include <regex>                             // for match_results<>::_Base_type
#include <stdexcept>                         // for runtime_error
#include <vector>                            // for vector

// the code to test:
#include "Config.hpp" // for Config

#include "json.hpp" // for json_ref, basic_json<>::object_t, json

using kotekan::Config;

using json = nlohmann::json;

BOOST_AUTO_TEST_CASE(_get_value_recursive) {
    json json_config = {
        {"pi", 3.141},
        {"truth",
         {
             {"not", true},
             {"but", false},
         }},
        {"lie", {{"not", false}, {"but", true}}},
        {"happy", true},
        {"name", "Niels"},
        {"nothing", nullptr},
        {"answer", {{"everything", 42}}},
        {"list", {1, 0, 2}},
        {"object", {{"currency", "USD"}, {"value", 42.99}, {"hidden", {{"treasure", "found"}}}}},
        {"add", "2 + pi"},
        {"one_var", "pi"},
        {"num_as_string", "-5.01"},
        {"int_as_string", "50"},
        {"int_as_string2", "-50"},
        {"multiply", "2 * pi"},
        {"subtract", "25.5 - 0.5"},
        {"divide", "10 / 4"},
        {"scientific", "-0.5e2"},
        {"scientific2", "-0.5e2 + -0.5e+4 + 1.2E-1 - 5.32e-2"},
        {"scientific3", "1.22e20"},
        {"scientific4", "1.258e-20"},
        {"scientific5", "190.2508e+0"},
        {"broken", "5 + e3"},
        {"broken2", "5 +"},
        {"broken3", "5 +* 8"},
        {"broken4", "5 + (8"},
        {"broken5", "*5"},
        {"missing", "5 + not_set"},
        {"complicated", "1e4 + divide * 10 /(4 + 1) - 20 * (6-pi) + -1.2e2"}};
    Config config;
    config.update_config(json_config);

    BOOST_CHECK(
        (config.get_value("not").at(0).get<bool>() && config.get_value("not").at(1).get<bool>())
        == false);
    BOOST_CHECK(
        (config.get_value("not").at(0).get<bool>() || config.get_value("not").at(1).get<bool>())
        == true);
    BOOST_CHECK(
        (config.get_value("but").at(0).get<bool>() && config.get_value("but").at(1).get<bool>())
        == false);
    BOOST_CHECK(
        (config.get_value("but").at(0).get<bool>() || config.get_value("but").at(1).get<bool>())
        == true);

    std::vector<json> results;

    json j = "found";
    results.push_back(j);
    BOOST_CHECK_EQUAL(config.get_value("treasure"), results);

    j = {{"treasure", "found"}};
    results.clear();
    results.push_back(j);
    BOOST_CHECK_EQUAL(config.get_value("hidden"), results);

    j = 3.141;
    results.clear();
    results.push_back(j);
    BOOST_CHECK_EQUAL(config.get_value("pi"), results);

    j = {{"currency", "USD"}, {"value", 42.99}, {"hidden", {{"treasure", "found"}}}};
    results.clear();
    results.push_back(j);
    BOOST_CHECK_EQUAL(config.get_value("object"), results);

    j = 42.99;
    results.clear();
    results.push_back(j);
    BOOST_CHECK_EQUAL(config.get_value("value"), results);

    j = 42;
    results.clear();
    results.push_back(j);
    BOOST_CHECK_EQUAL(config.get_value("everything"), results);

    // Check the arithmetic parser
    BOOST_CHECK_EQUAL(config.get<double>("/", "add"), 5.141);
    BOOST_CHECK_EQUAL(config.get<double>("/", "one_var"), 3.141);
    BOOST_CHECK_EQUAL(config.get<double>("/", "num_as_string"), -5.01);
    BOOST_CHECK_EQUAL(config.get<int32_t>("/", "int_as_string"), 50);
    BOOST_CHECK_EQUAL(config.get<int32_t>("/", "int_as_string2"), -50);
    BOOST_CHECK_EQUAL(config.get<double>("/", "subtract"), 25);
    BOOST_CHECK_EQUAL(config.get<double>("/", "divide"), 2.5);
    BOOST_CHECK_EQUAL(config.get<double>("/", "scientific"), -0.5e2);
    BOOST_CHECK_EQUAL(config.get<double>("/", "scientific2"), -0.5e2 + -0.5e+4 + 1.2E-1 - 5.32e-2);
    BOOST_CHECK_EQUAL(config.get<double>("/", "scientific3"), 1.22e20);
    BOOST_CHECK_EQUAL(config.get<double>("/", "scientific4"), 1.258e-20);
    BOOST_CHECK_EQUAL(config.get<double>("/", "scientific5"), 190.2508e+0);
    BOOST_CHECK_THROW(config.get<double>("/", "broken"), std::runtime_error);
    BOOST_CHECK_THROW(config.get<double>("/", "broken2"), std::runtime_error);
    BOOST_CHECK_THROW(config.get<double>("/", "broken3"), std::runtime_error);
    BOOST_CHECK_THROW(config.get<double>("/", "broken4"), std::runtime_error);
    BOOST_CHECK_THROW(config.get<double>("/", "broken5"), std::runtime_error);
    BOOST_CHECK_THROW(config.get<double>("/", "missing"), std::runtime_error);
    BOOST_CHECK_EQUAL(config.get<double>("/", "complicated"),
                      1e4 + (2.5) * 10 / (4 + 1) - 20 * (6 - 3.141) + -1.2e2);

    BOOST_CHECK(config.get_value("Niels").empty());
    BOOST_CHECK(config.get_value("found").empty());
    BOOST_CHECK(config.get_value("3.141").empty());
    BOOST_CHECK(config.get_value("USD").empty());
    BOOST_CHECK(config.get_value("treasures").empty());
    BOOST_CHECK(config.get_value("").empty());
    BOOST_CHECK(config.get_value(" ").empty());
    BOOST_CHECK(config.get_value("/").empty());
}


/*!
 * @file
 * This file defines `boost::mpl::set_equal`.
 */

#ifndef BOOST_MPL_SET_EQUAL_HPP
#define BOOST_MPL_SET_EQUAL_HPP

#include <boost/mpl/all_of.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/apply_wrap.hpp>
#include <boost/mpl/at.hpp>
#include <boost/mpl/has_key.hpp>
#include <boost/mpl/key_view.hpp>
#include <boost/mpl/lambda.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/quote.hpp>
#include <boost/mpl/set.hpp>
#include <boost/mpl/set_insert_range.hpp>
#include <boost/mpl/set_union.hpp>
#include <boost/type_traits/is_same.hpp>

namespace boost { namespace mpl {
namespace assoc_equal_detail {
    template <typename Sequence>
    using as_set = typename set_insert_range<
        typename set<>::type, Sequence
    >::type;

    template <typename Map1, typename Map2, typename Pred>
    struct make_assoc_equal {
        using Keys = typename set_union<
            as_set<key_view<Map1>>, as_set<key_view<Map2>>
        >::type;

        using type = all_of<
            Keys,
            and_<
                has_key<Map1, _1>,
                has_key<Map2, _1>,
                apply_wrap2<
                    typename lambda<Pred>::type,
                    at<Map1, _1>,
                    at<Map2, _1>
                >
            >
        >;
    };
} // end namespace assoc_equal_detail

template <typename Map1, typename Map2, typename Pred = quote2<is_same>>
struct assoc_equal
    : assoc_equal_detail::make_assoc_equal<Map1, Map2, Pred>::type
{ };
}} // end namespace boost::mpl

#endif // !BOOST_MPL_SET_EQUAL_HPP


#include "expr.hpp"

#include "version.hpp"
#include "env.hpp"

#include <string_view>
#include <iostream>
#include <cmath>
#include <numeric>
#ifdef ENABLE_NIVALIS_BOOST_MATH
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/trigamma.hpp>
#include <boost/math/special_functions/polygamma.hpp>
#include <boost/math/special_functions/zeta.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/special_functions/factorials.hpp>
#endif
#include "opcodes.hpp"
#include "util.hpp"

namespace nivalis {

namespace {
const double NONE = std::numeric_limits<double>::quiet_NaN();
using ::nivalis::OpCode::repr;
#ifndef ENABLE_NIVALIS_BOOST_MATH
void print_boost_warning(uint32_t opcode) {
    std::cerr << "Function " << OpCode::repr(opcode)
        << " requires Nivalis to be compiled with Boost" << std::endl;
}

// Falling factorial
double fa_fact (size_t x, size_t to_exc = 1) {
    if (x >= to_exc + 175) // too expensive
        return std::numeric_limits<double>::quiet_NaN();
    double result = 1.;
    while (x > to_exc) result *= x--;
    return result;
}
#endif
}  // namespace

namespace detail {
double eval_ast(Environment& env, const Expr::AST& ast,
        const std::vector<double>& arg_vals) {
    // Main AST evaluation stack
    thread_local std::vector<double> stk;
    // Stack of thunks available:
    // contains positions of thunk_jmp encountered
    thread_local std::vector<size_t> thunks;
    // Thunk call stack: contains positions to return to
    // after reaching thunk_ret
    thread_local std::vector<size_t> thunks_stk;

    // Max function call stack height
    static const size_t MAX_CALL_STK_HEIGHT = 256;
    // Curr function call stack height
    thread_local size_t call_stk_height = 0;

    thread_local size_t top = -1;
    size_t init_top = top;

    // Make sure there is enough space
    stk.resize(top + ast.size() + 1);

#ifdef ENABLE_NIVALIS_BOOST_MATH
    using namespace boost;
    using namespace boost::math;
#endif
    using namespace nivalis::OpCode;

    bool _is_thunk_ret = false;

    // Shorthands for first, 2nd, 3rd arguments to operator
#define ARG3 stk[top-2]
#define ARG2 stk[top-1]
#define ARG1 stk[top]
// Return value from thunk or func call
#define RET_VAL stk[top+1]
// Quit without messing up stack
#define FAIL_AND_QUIT do {top = init_top; return NONE; } while(0)

    for (size_t cidx = ast.size() - 1; ~cidx; --cidx) {
        const auto& node = ast[cidx];
        switch(node.opcode) {
            case null: stk[++top] = NONE; break;
            case val:
                stk[++top] = node.val;  break;
            case ref:
                stk[++top] = env.vars[node.ref]; break;
            case arg:
                stk[++top] = arg_vals[node.ref]; break;
            case thunk_jmp:
                thunks.push_back(cidx);
                cidx -= ast[cidx].ref;
                break;
            case thunk_ret:
                cidx = thunks_stk.back() + 1;
                thunks_stk.pop_back();
                _is_thunk_ret = true;
                break;
            case call:
                {
                    size_t n_args = node.call_info[1];
                    auto& func = env.funcs[node.call_info[0]];
                    std::vector<double> f_args(n_args);
                    for (size_t i = 0; i < n_args; ++i) {
                        f_args[i] = stk[top--];
                    }
                    if (n_args != func.n_args ||               // Should not happen
                        &func.expr.ast[0] == &ast[0] ||        // Disallow recursion
                        call_stk_height > MAX_CALL_STK_HEIGHT) // Too many nested calls
                            FAIL_AND_QUIT;
                    ++call_stk_height;
                    eval_ast(env, func.expr.ast, f_args);
                    --call_stk_height;
                    ++top;
                }
                break;
            case bnz:
                if (_is_thunk_ret) {
                    --top; _is_thunk_ret = false;
                    ARG1 = RET_VAL;
                } else {
                    thunks_stk.push_back(cidx);
                    cidx = thunks[thunks.size() - (ARG1 == 0.) - 1];
                    thunks.resize(thunks.size() - 2);
                }
                break;

            case sums: case prods:
                {
                    if (_is_thunk_ret) {
                        --top; _is_thunk_ret = false;
                        // update arg3 (the output)
                        if (node.opcode == prods)
                            ARG3 *= RET_VAL;
                        else
                            ARG3 += RET_VAL; // arg3 is output
                    } else {
                        // Move over the arguments and use arg3
                        // as output
                        ++top; ARG1 = ARG2; ARG2 = ARG3;
                        ARG3 = node.opcode == prods ? 1. : 0.;
                    }
                    uint64_t var_id = node.ref;
                    int64_t a = static_cast<int64_t>(ARG1),
                            b = static_cast<int64_t>(ARG2);
                    int64_t step = (a <= b) ? 1 : -1;
                    if (std::isnan(ARG1)) {
                        top -= 2; thunks.pop_back();
                    } else {
                        env.vars[var_id] = static_cast<double>(a);
                        a += step;
                        if (a == b + step) ARG1 = NONE;
                        else ARG1 = static_cast<double>(a);
                        thunks_stk.push_back(cidx);
                        cidx = thunks.back();
                    }
                }
                break;

            case bsel: --top; break;
            case add: ARG2 += ARG1; --top; break;
            case sub: ARG2 = ARG1 - ARG2; --top; break;
            case mul: ARG2 *= ARG1;  --top; break;
            case divi: ARG2 = ARG1 / ARG2; --top; break;
            case mod: ARG2 = std::fmod(ARG1, ARG2); --top; break;
            case power: ARG2 = std::pow(ARG1, ARG2); --top; break;
            case logbase: ARG2 = log(ARG1) / log(ARG2); --top; break;
            case max: ARG2 = std::max(ARG1, ARG2); --top; break;
            case min: ARG2 = std::min(ARG1, ARG2); --top; break;
            case land: ARG2 = static_cast<double>(ARG1 && ARG2); --top; break;
            case lor: ARG2 = static_cast<double>(ARG1 || ARG2); --top; break;
            case lxor: ARG2 = static_cast<double>(
                               (ARG1 != 0.) ^ (ARG2 != 0.)); --top; break;
            case gcd: ARG2 = static_cast<double>(
                              std::gcd((int64_t) ARG1,
                                       (int64_t) ARG2)); --top; break;
            case lcm: ARG2 = ARG1 * ARG2 /
                      static_cast<double>(std::gcd(
                          (int64_t) ARG1, (int64_t) ARG2)); --top; break;
#ifdef ENABLE_NIVALIS_BOOST_MATH
            case choose: ARG2 = binomial_coefficient<double>(
                            (uint32_t)ARG1, (uint32_t)ARG2); --top; break;
            case fafact: ARG2 = falling_factorial<double>(
                            (uint32_t)ARG1, (uint32_t)ARG2); --top; break;
            case rifact: ARG2 = rising_factorial<double>(
                            (uint32_t)ARG1, (uint32_t)ARG2); --top; break;
            case betab: ARG2 = beta<double>(ARG1, ARG2); --top; break;
            case polygammab:
                        ARG2 = polygamma<double>((int)ARG1, ARG2); --top;
                        break;
#else
            case choose: {
                             double ad = std::round(ARG1);
                             double bd = std::round(ARG2);
                             if (ad < 0 || bd < 0) {
                                 ARG2 = std::numeric_limits<double>::quiet_NaN(); --top; break;
                             }
                             size_t a = static_cast<size_t>(ad), b = static_cast<size_t>(bd);
                             b = std::min(b, a-b);
                             ARG2 = fa_fact(a, a-b) / fa_fact(b);
                             --top; break;
                         }
            case fafact: {
                             double ad = std::round(ARG1);
                             double bd = std::round(ARG2);
                             if (ad < 0 || bd < 0) {
                                 ARG2 = std::numeric_limits<double>::quiet_NaN(); --top; break;
                             }
                             size_t a = static_cast<size_t>(ad), b = static_cast<size_t>(bd);
                             ARG2 = fa_fact(a, a-b);
                             --top; break;
                         }
            case rifact: {
                             double ad = std::round(ARG1);
                             double bd = std::round(ARG2);
                             if (ad < 0 || bd < 0) {
                                 ARG2 = std::numeric_limits<double>::quiet_NaN(); --top; break;
                             }
                             size_t a = static_cast<size_t>(ad), b = static_cast<size_t>(bd);
                             ARG2 = fa_fact(a+b-1, a-1);
                             --top; break;
                         }
            case betab: case polygammab:
                ARG1 = NONE; print_boost_warning(node.opcode); break;
#endif
            case lt: ARG2 = static_cast<double>(ARG1 < ARG2); --top; break;
            case le: ARG2 = static_cast<double>(ARG1 <= ARG2); --top; break;
            case eq: ARG2 = static_cast<double>(ARG1 == ARG2); --top; break;
            case ne: ARG2 = static_cast<double>(ARG1 != ARG2); --top; break;
            case ge: ARG2 = static_cast<double>(ARG1 >= ARG2); --top; break;
            case gt: ARG2 = static_cast<double>(ARG1 > ARG2); --top; break;

            case unaryminus: ARG1 = -ARG1; break;
            case lnot: ARG1 = static_cast<double>(!(ARG1)); break;
            case absb: ARG1 = std::fabs(ARG1); break;
            case sqrtb: ARG1 = std::sqrt(ARG1); break;
            case sqrb: ARG1 *= ARG1; break;
            case sgn: ARG1 = ARG1 > 0 ? 1 : (ARG1 == 0 ? 0 : -1); break;
            case floorb: ARG1 = floor(ARG1); break;
            case ceilb: ARG1 = ceil(ARG1); break; case roundb: ARG1 = round(ARG1); break;

            case expb:   ARG1 = exp(ARG1); break; case exp2b: ARG1 = exp2(ARG1); break;
            case logb:   ARG1 = log(ARG1); break;
            case factb:
                        {
                            unsigned n = static_cast<unsigned>(std::max(
                                        ARG1, 0.));
#ifdef ENABLE_NIVALIS_BOOST_MATH
                            ARG1 = factorial<double>(n);
#else
                            ARG1 = fa_fact(n, 1);
#endif
                        }
                        break;
            case log2b: ARG1 = log2(ARG1); break;  case log10b: ARG1 = log10(ARG1); break;
            case sinb: ARG1 = sin(ARG1); break;   case cosb:   ARG1 = cos(ARG1); break;
            case tanb: ARG1 = tan(ARG1); break;   case asinb: ARG1 = asin(ARG1); break;
            case acosb: ARG1 = acos(ARG1); break;  case atanb: ARG1 = atan(ARG1); break;
            case sinhb: ARG1 = sinh(ARG1); break;  case coshb: ARG1 = cosh(ARG1); break;
            case tanhb: ARG1 = tanh(ARG1); break;
            case tgammab: ARG1 = std::tgamma(ARG1); break;
            case lgammab: ARG1 = std::lgamma(ARG1); break;
#ifdef ENABLE_NIVALIS_BOOST_MATH
            case digammab: ARG1 = digamma<double>(ARG1); break;
            case trigammab: ARG1 = trigamma<double>(ARG1); break;
            case zetab: ARG1 = zeta<double>(ARG1); break;
#else
           // The following functions are unavailable without Boost
            case digammab: case trigammab: case zetab:
                ARG1 = NONE; print_boost_warning(node.opcode); break;
#endif
            case erfb: ARG1 = erf(ARG1); break; break;
        }
    }
    return stk[top--];
}
}  // namespace detail

// Interface for evaluating expression
double Expr::operator()(Environment& env) const {
    return detail::eval_ast(env, ast);
}
double Expr::operator()(double arg, Environment& env) const {
    return detail::eval_ast(env, ast, {arg});
}
double Expr::operator()(const std::vector<double>& args,
        Environment& env) const {

    return detail::eval_ast(env, ast, args);
}

}  // namespace nivalis


#include <stan/math/torsten/test/unit/test_fixture_twocpt_effcpt.hpp>
#include <stan/math/torsten/test/unit/test_functors.hpp>
#include <stan/math/torsten/pmx_twocpt_model.hpp>
#include <stan/math/torsten/pmx_ode_model.hpp>
#include <boost/mp11.hpp>

TYPED_TEST_SUITE_P(test_twocpt_effcpt);
TYPED_TEST_P(test_twocpt_effcpt, multiple_bolus) {
  this -> compare_solvers_val();
  this -> compare_solvers_adj(this -> amt, 1.e-8, "AMT");
  this -> compare_solvers_adj(this -> rate, 1.e-8, "RATE");
  this -> compare_solvers_adj(this -> ii, 1.e-8, "II");
  this -> compare_solvers_adj(this -> theta[0], 5.e-6, "theta");
  this -> compare_solvers_adj(this -> biovar[0], 1.e-6, "bioavailability");
  this -> compare_solvers_adj(this -> tlag[0], 1.e-8, "lag time");
}

TYPED_TEST_P(test_twocpt_effcpt, multiple_bolus_addl) {
  this -> ii[0] = 1.55;
  this -> compare_solvers_val();
  this -> compare_solvers_adj(this -> amt, 1.e-8, "AMT");
  this -> compare_solvers_adj(this -> rate, 1.e-8, "RATE");
  this -> compare_solvers_adj(this -> ii, 5.e-6, "II");
  this -> compare_solvers_adj(this -> theta[0], 5.e-6, "theta");
  this -> compare_solvers_adj(this -> biovar[0], 1.e-6, "bioavailability");
  this -> compare_solvers_adj(this -> tlag[0], 1.e-8, "lag time");
}

REGISTER_TYPED_TEST_SUITE_P(test_twocpt_effcpt,
                            multiple_bolus,
                            multiple_bolus_addl);

using test_types = boost::mp11::mp_product<
  std::tuple,
  ::testing::Types<pmx_solve_twocpt_effcpt_functor>, // solver 1
  ::testing::Types<pmx_solve_linode_functor,
                   pmx_solve_rk45_functor>, // solver 2
  ::testing::Types<double>,  // TIME
  ::testing::Types<double, stan::math::var_value<double>>,  // AMT
  ::testing::Types<double, stan::math::var_value<double>> , // RATE
  ::testing::Types<double> , // II
  ::testing::Types<double, stan::math::var_value<double>> , // PARAM
  ::testing::Types<double> , // BIOVAR
  ::testing::Types<double> , // TLAG
  ::testing::Types<torsten::PMXTwocptEffCptODE>             // ODE
    >;

INSTANTIATE_TYPED_TEST_SUITE_P(PMX, test_twocpt_effcpt, test_types);


/*
 * Copyright 2020 California  Institute  of Technology (“Caltech”)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <x/vio/slam_update.h>
#include <x/vio/tools.h>
#include <x/ekf/state.h>
#include <boost/math/distributions.hpp>

using namespace x;
using namespace Eigen;

SlamUpdate::SlamUpdate(const x::TrackList& trks,
                       const x::AttitudeList& quats,
                       const x::TranslationList& poss,
                       const MatrixXd& feature_states,
                       const std::vector<int>& anchor_idxs,
                       const MatrixXd& cov_s,
                       const int n_poses_max,
                       const double sigma_img)
{
  // Number of features
  const size_t n_trks = trks.size();

  // Initialize Kalman update matrices
  const size_t rows = 2 * n_trks;
  const size_t cols = cov_s.cols();
  jac_ = MatrixXd::Zero(rows, cols);
  cov_m_diag_ = VectorXd::Ones(rows);
  res_ = MatrixXd::Zero(rows, 1);

  // For each track, compute residual, Jacobian and covariance block
  const double var_img = sigma_img * sigma_img;
  for (size_t i = 0, row_h = 0; i < n_trks; ++i) {
    processOneTrack(trks[i],
                    quats,
                    poss,
                    feature_states,
                    anchor_idxs,
                    cov_s,
                    n_poses_max,
                    var_img,
                    i,
                    row_h);
  }
}

void SlamUpdate::processOneTrack(const x::Track& track,
                                 const x::AttitudeList& C_q_G,
                                 const x::TranslationList& G_p_C,
                                 const MatrixXd& feature_states,
                                 const std::vector<int>& anchor_idxs,
                                 const MatrixXd& P,
                                 const int n_poses_max,
                                 const double var_img,
                                 const size_t& j,
                                 size_t& row_h)
{
  const size_t cols = P.cols();
  MatrixXd h_j(MatrixXd::Zero(2, cols));
  MatrixXd Hf_j(MatrixXd::Zero(2, cols));
  MatrixXd res_j(MatrixXd::Zero(2, 1));

  //==========================================================================
  // Feature information
  //==========================================================================
  // A-priori inverse-depth parameters in last observation frame
  double alpha = feature_states(j * 3, 0);
  double beta = feature_states(j * 3 + 1, 0);
  double rho = feature_states(j * 3 + 2, 0);

  // Anchor pose
  unsigned int anchor_idx = anchor_idxs[j];
  x::Quaternion Ca_q_G;
  Ca_q_G.x() = C_q_G[anchor_idx].ax;
  Ca_q_G.y() = C_q_G[anchor_idx].ay;
  Ca_q_G.z() = C_q_G[anchor_idx].az;
  Ca_q_G.w() = C_q_G[anchor_idx].aw;

  Vector3d G_p_Ca(G_p_C[anchor_idx].tx, G_p_C[anchor_idx].ty, G_p_C[anchor_idx].tz);

  // Coordinate of feature in global frame
  Vector3d G_p_fj = 1 / (rho)*Ca_q_G.normalized().toRotationMatrix() * Vector3d(alpha, beta, 1) + G_p_Ca;

  // FOR LAST FEATURE OBSERVATION
  x::Translation G_p_Cn(G_p_C.back());
  x::Attitude Cn_q_G(C_q_G.back());

  x::Quatern attitude_to_quaternion;
  Quaterniond Ci_q_G_ = attitude_to_quaternion(Cn_q_G);
  Vector3d G_p_Ci_(G_p_Cn.tx, G_p_Cn.ty, G_p_Cn.tz);

  // Feature position expressed in camera frame.
  Vector3d Ci_p_fj;
  Ci_p_fj << Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_);

  // eq. 20(a)
  Vector2d z;
  const size_t track_size(track.size());
  const unsigned int i = track_size - 1;
  z(0) = track[i].getX();
  z(1) = track[i].getY();

  Vector2d z_hat(z);
  assert(Ci_p_fj(2));
  z_hat(0) = Ci_p_fj(0) / Ci_p_fj(2);
  z_hat(1) = Ci_p_fj(1) / Ci_p_fj(2);

  // eq. 20(b)
  res_j(0, 0) = z(0) - z_hat(0);
  res_j(1, 0) = z(1) - z_hat(1);

  //============================
  // Measurement Jacobian matrix
  //============================

  const unsigned int pos = C_q_G.size() - 1;
  if (anchor_idx == pos)  // Handle special case
  {
    // Inverse-depth feature coordinates jacobian
    MatrixXd mat(MatrixXd::Zero(2, 3));
    mat(0, 0) = 1.0;
    mat(1, 1) = 1.0;

    // Update stacked Jacobian matrices associated to the current feature
    unsigned int row = 0;
    unsigned int col = (n_poses_max * 2 + j) * kJacCols;
    h_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = mat;
  }
  else
  {
    // Set Jacobian of pose for i'th measurement of feature j (eq.22, 23)
    VisJacBlock J_i(VisJacBlock::Zero());
    // first row
    J_i(0, 0) = 1.0 / Ci_p_fj(2);
    J_i(0, 1) = 0.0;
    J_i(0, 2) = -Ci_p_fj(0) / std::pow((double)Ci_p_fj(2), 2);
    // second row
    J_i(1, 0) = 0.0;
    J_i(1, 1) = 1.0 / Ci_p_fj(2);
    J_i(1, 2) = -Ci_p_fj(1) / std::pow((double)Ci_p_fj(2), 2);

    // Attitude
    Vector3d skew_vector = Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_);
    VisJacBlock J_attitude = J_i * x::Skew(skew_vector(0), skew_vector(1), skew_vector(2)).matrix;

    // Position
    VisJacBlock J_position = -J_i * Ci_q_G_.normalized().toRotationMatrix().transpose();

    // Anchor attitude
    VisJacBlock J_anchor_att = -1 / rho * J_i * Ci_q_G_.normalized().toRotationMatrix().transpose() *
                                    Ca_q_G.normalized().toRotationMatrix() * x::Skew(alpha, beta, 1).matrix;

    // Anchor position
    VisJacBlock J_anchor_pos = -J_position;

    // Inverse-depth feature coordinates
    MatrixXd mat(MatrixXd::Identity(3, 3));
    mat(0, 2) = -alpha / rho;
    mat(1, 2) = -beta / rho;
    mat(2, 2) = -1 / rho;
    VisJacBlock Hf_j1 = 1 / rho * J_i * Ci_q_G_.normalized().toRotationMatrix().transpose() *
                             Ca_q_G.normalized().toRotationMatrix() * mat;

    // Update stacked Jacobian matrices associated to the current feature
    unsigned int row = 0;
    const unsigned int pos = C_q_G.size() - 1;
    unsigned int col = pos * kJacCols;
    h_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = J_position;

    col += n_poses_max * kJacCols;
    h_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = J_attitude;

    col = anchor_idx * kJacCols;
    h_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = J_anchor_pos;

    col += n_poses_max * kJacCols;
    h_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = J_anchor_att;

    col = (n_poses_max * 2 + j) * kJacCols;
    h_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = Hf_j1;
  }

  //==========================================================================
  // Outlier rejection
  //==========================================================================
  const VectorXd r_j_diag = var_img * VectorXd::Ones(2);
  MatrixXd r_j = var_img * MatrixXd::Identity(2, 2);
  MatrixXd S_inv = (h_j * P * h_j.transpose() + r_j).inverse();
  MatrixXd gamma = res_j.transpose() * S_inv * res_j;
  boost::math::chi_squared_distribution<> my_chisqr(2 * track_size);
  double chi = quantile(my_chisqr, 0.9);  // 95-th percentile

  if (gamma(0, 0) < chi)  // Inlier
  {
    jac_.block(row_h,          // startRow
             0,               // startCol
             2,               // numRows
             cols) = h_j;  // numCols

    // Residual vector (one feature)
    res_.block(row_h, 0, 2, 1) = res_j;

    // Measurement covariance matrix
    cov_m_diag_.segment(row_h, 2) = r_j_diag;

    row_h += 2;
  }
}

void SlamUpdate::computeInverseDepthsNew(const x::TrackList& new_trks,
                                         const double rho_0,
                                         MatrixXd& ivds) const
{
  const size_t n_new_slam_std_trks = new_trks.size();
  ivds = MatrixXd::Zero(n_new_slam_std_trks * 3, 1);

  // For each standard SLAM feature to init
  for (size_t j = 0; j < n_new_slam_std_trks; ++j) {
    // Compute inverse-depth coordinates of new feature state
    // (last observation in track)
    computeOneInverseDepthNew(new_trks[j].back(), rho_0, j, ivds);
  }
}

void SlamUpdate::computeOneInverseDepthNew(const x::Feature& feature,
                                           const double rho_0,
                                           const unsigned int& idx,
                                           MatrixXd& ivds) const
{
  // Inverse-depth parameters anchored in last observation frame
  const double alpha = feature.getX();
  const double beta  = feature.getY();
  // const double rho = 1.0 / G_p_Ci.back().tz; // height-based init:

  ivds(3*idx)     = alpha;
  ivds(3*idx + 1) = beta;
  ivds(3*idx + 2) = rho_0;
}


//==================================================================================================
/*!
  @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_SCALAR_FUNCTION_DIV_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_DIV_HPP_INCLUDED

#include <boost/simd/function/divides.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/hierarchy.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/hierarchy.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{

  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bd::scalar_<bd::arithmetic_<T>>
                          , bd::scalar_<bd::arithmetic_<T>>
                          )
  {
    BOOST_MAYBEINLINE T operator()(T  a, T  b ) const BOOST_NOEXCEPT
    {
      return divides(a, b);
    }
  };

#ifdef BOOST_MSVC
  #pragma warning(push)
  #pragma warning(disable: 4723) // potential divide by 0
#endif

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

} } }

#endif


#include "MatchingMesh.h"
#include "loader.hpp"
#include <Eigen/Dense>

using namespace std;
using namespace chai3d;
using namespace Eigen;

cVector3d prevPos = cVector3d(0,0,0);

MatchingMesh::MatchingMesh(cWorld* world, string s)
{
	mesh = new cMesh();

	vector<cVector3d> vertices;
	vector<cVector3d> normals;
	vector<cVector3d> uvs;
	vector<unsigned int> indices;

	loadOBJ(s, &vertices, &normals, &uvs, &indices);

	for (unsigned int i = 0; i < vertices.size(); i++)
	{
		mesh->newVertex(vertices[i], normals[i]);
	}
	mesh->setVertexColor(cColorf(1, 1, 1, 1));

	for (unsigned int i = 0; i < indices.size() / 3; i++)
	{
		mesh->newTriangle(indices[i * 3], indices[i * 3 + 1], indices[i * 3 + 2]);
		vector<int> sorted_idx; sorted_idx.push_back(indices[i * 3 + 0]); sorted_idx.push_back(indices[i * 3 + 1]); sorted_idx.push_back(indices[i * 3 + 2]);
		sort(sorted_idx.begin(), sorted_idx.end());
		// ensures no duplicated/directional edges by making sure edge.first < edge.second
		edges.insert(pair<unsigned int, unsigned int>(sorted_idx[0], sorted_idx[1]));
		edges.insert(pair<unsigned int, unsigned int>(sorted_idx[0], sorted_idx[2]));
		edges.insert(pair<unsigned int, unsigned int>(sorted_idx[1], sorted_idx[2]));
	}
	mesh->computeAllNormals();

	mesh->m_material = cMaterial::create();
	mesh->m_material->setWhiteGhost();
	mesh->m_material->setUseHapticShading(true);

	world->addChild(mesh);

	originalPoints = vector<cVector3d>(mesh->m_vertices->getNumElements());
	forces = vector<cVector3d>(mesh->m_vertices->getNumElements());
	velocities = vector<cVector3d>(mesh->m_vertices->getNumElements());

	for (unsigned int i = 0; i < mesh->m_vertices->getNumElements(); i++)
	{
		originalPoints[i] = mesh->m_vertices->getLocalPos(i);
		forces[i] = cVector3d(0, 0, 0);
		velocities[i] = cVector3d(0, 0, 0);
	}
}

MatchingMesh::~MatchingMesh()
{
}

void MatchingMesh::update(cVector3d &force, cVector3d &position, cVector3d &cursorPosition)
{
	forceComputed.assign(mesh->m_vertices->getNumElements(), 0);
	cVector3d collisionPoint = cVector3d(0,0,0);
	bool collided = false;
	float count = 0;

	affectedVertices.clear();
	bool lContact = contact;

	contact = false;
	for (unsigned int i = 0; i < mesh->getNumTriangles(); i++) {
		if (updateTriangle(force, position, cursorPosition, i))
		{
			contact = true;
		}
	}
	for (auto& e : edges) {
		if (updateLine(e.first, e.second, position, cursorPosition))
		{
			contact = true;
		}
	}
	for (unsigned int i = 0; i < mesh->m_vertices->getNumElements(); i++)
	{
		cVector3d cPos = cursorPosition;
		if (updatePoint(i, position, cursorPosition))
		{
			contact = true;
		}
	}
	if (contact && !lContact) {
		lCursorPosition = cursorPosition;
	}

	/*for (unsigned int i = 0; i < mesh->getNumTriangles(); i++) {
		cVector3d cPos = cursorPosition;
		if (updateTriangle(force, position, cPos, i))
		{
			collided = true;
			count += 1.f;
			collisionPoint += cPos;
		}
	}
	for (auto& e : edges) {
		cVector3d cPos = cursorPosition;
		if (updateLine(e.first, e.second, position, cPos))
		{
			count += 1.f;
			collided = true;
			collisionPoint += cPos;
		}
	}
	for (unsigned int i = 0; i < mesh->m_vertices->getNumElements(); i++)
	{
		cVector3d cPos = cursorPosition;
		if (updatePoint(i, position, cPos))
		{
			count += 1.f;
			collided = true;
			collisionPoint += cPos;
		}
	}

	if (collided)
		cursorPosition = (collisionPoint)/count;*/

	if (contact&&lContact) {
		auto d = cursorPosition - lCursorPosition;
		if (cDynamicFriction) {
			if (d.length() > dynamicFriction) {
				d.normalize();
				lCursorPosition = cursorPosition - d * (dynamicFriction+0.0001);
			}
			else {
				cDynamicFriction = 0;
			}
		}
		else {
			if (d.length() > staticFriction) {
				d.normalize();
				lCursorPosition = cursorPosition - d * (staticFriction+0.0001);
			}
		}

		cursorPosition = lCursorPosition;
	}

	force = (cursorPosition - position) * 2000;

	for (auto i : affectedVertices) {
		forces[i] += -force * 0.1;
	}

	for (unsigned int i = 0; i < mesh->m_vertices->getNumElements(); i++)
	{
		movePoint(i);
	}

	prevPos = cursorPosition;
}

float planeIntersection(cVector3d ray, cVector3d origin, cVector3d n, cVector3d q)
{
	n.normalize();
	if (cDot(ray, n) != 0)
		return (cDot(q, n) - cDot(n, origin)) / cDot(ray, n);

	return -1;
}

//candidate 1
cVector3d calc_barycentric_coords(cVector3d p0, cVector3d p1, cVector3d p2, cVector3d point)
{
	// calculating barycentric
	cVector3d barycentric;
	{ auto d = p1 - p0; d.cross(point - p0); barycentric.z(d.length() * (d.z() / abs(d.z()))); }
	{ auto d = p2 - p1; d.cross(point - p1); barycentric.x(d.length() * (d.z() / abs(d.z()))); }
	{ auto d = p0 - p2; d.cross(point - p2); barycentric.y(d.length() * (d.z() / abs(d.z()))); }
	barycentric.normalize();

	return barycentric;
}

cVector3d triangleIntersection(cVector3d p0, cVector3d p1, cVector3d p2, cVector3d origin, cVector3d ray, float &t)
{
	cVector3d s = origin - p0;
	cVector3d e1 = p1 - p0;
	cVector3d e2 = p2 - p0;

	Matrix3f mt;// = mat3(s, e1, e2);
	mt << s.x(), e1.x(), e2.x(),  s.y(), e1.y(), e2.y(),  s.z(), e1.z(), e2.z();

	Matrix3f mu; //= mat3(-ray, s, e2);
	mu << -ray.x(), s.x(), e2.x(), -ray.y(), s.y(), e2.y(), -ray.z(), s.z(), e2.z();

	Matrix3f mv; // = mat3(-ray, e1, s);
	mv << -ray.x(), e1.x(), s.x(), -ray.y(), e1.y(), s.y(), -ray.z(), e1.z(), s.z() ;
	Matrix3f md;// = mat3(-ray, e1, e2);
	md << -ray.x(), e1.x(), e2.x(), -ray.y(), e1.y(), e2.y(), -ray.z(), e1.z(), e2.z();

	t = mt.determinant() / md.determinant();
	float u = mu.determinant() / md.determinant();
	float v = mv.determinant() / md.determinant();
	float w = 1 - u - v;

	return cVector3d(w,u,v);

}

bool barycentric_test(cVector3d bar)
{
	if (bar.x() >= 0 && bar.y() >= 0 && bar.z() >= 0)
	{
		return true;
	}

	return false;
}

bool MatchingMesh::updateTriangle(chai3d::cVector3d &force, chai3d::cVector3d &position, chai3d::cVector3d &cursorPosition, unsigned int index) {

	unsigned int index0 = (mesh->m_triangles->getVertexIndex0(index));
	cVector3d p0 = mesh->m_vertices->getLocalPos(index0);

	unsigned int index1 = (mesh->m_triangles->getVertexIndex1(index));
	cVector3d p1 = mesh->m_vertices->getLocalPos(index1);

	unsigned int index2 = (mesh->m_triangles->getVertexIndex2(index));
	cVector3d p2 = mesh->m_vertices->getLocalPos(index2);

	if (!forceComputed[index0])	forces[index0] = cVector3d(0, 0, 0);
	if (!forceComputed[index1]) forces[index1] = cVector3d(0, 0, 0);
	if (!forceComputed[index2])	forces[index2] = cVector3d(0, 0, 0);

	auto normal = (p1 - p0); normal.cross(p2 - p0); normal.normalize();
	// triangle between tool and avatar
	cVector3d toolToAvatar = prevPos - position;
	toolToAvatar.normalize();
	/*float t = planeIntersection(toolToAvatar, position, normal, p0);
	cVector3d intersection = position + toolToAvatar * t;
	cVector3d bar_intersection = calc_barycentric_coords(p0, p1, p2, intersection);*/

	float t;
	cVector3d bar_intersection = triangleIntersection(p0,p1,p2, position, toolToAvatar, t);
	if (t >= 0 && t <= (prevPos - position).length())
	{
		if (barycentric_test(bar_intersection))
		{
			cursorPosition = position + toolToAvatar * (t+0.1);
		}
	}

	bool contact = 0;
	auto proj = (cursorPosition - p0);
	auto proj_dot_normal = proj.dot(normal);
	auto proj_sign = (proj_dot_normal > 0) ? 1 : -1;
	proj = proj_dot_normal * normal;
	{
		// check cursor sphere against the face
		auto proj_face = cursorPosition - proj;

		//cVector3d barycentric = calc_barycentric_coords(p0,p1,p2, proj_face);

		cVector3d barycentric = triangleIntersection(p0,p1,p2, cursorPosition, cNormalize(cursorPosition-proj_face), t);

		// cursor sphere touching triangle face
		if (abs(proj_dot_normal) < radius && barycentric.x() >= 0 && barycentric.y() >= 0 && barycentric.z() >= 0
			&& barycentric.x() <= 1 && barycentric.y() <= 1 && barycentric.z() <= 1) {
			cursorPosition = (proj_face + normal * radius * proj_dot_normal / abs(proj_dot_normal));
			contact = 1;

			// direction * barycentric coordinate (weight) * penetration depth * sign * stiffness
			forces[index0] += -normal * barycentric.x() * (radius - abs(proj_dot_normal)) * proj_sign * 1000;
			forces[index1] += -normal * barycentric.y() * (radius - abs(proj_dot_normal)) * proj_sign * 1000;
			forces[index2] += -normal * barycentric.z() * (radius - abs(proj_dot_normal)) * proj_sign * 1000;
			forceComputed[index0] = forceComputed[index1] = forceComputed[index2] = 1;
		}
	}

	if (contact) {
		affectedVertices.insert(index0);
		affectedVertices.insert(index1);
		affectedVertices.insert(index2);
	}

	return contact;
}

cVector3d solveCylinderCollisions(cVector3d c1, cVector3d c2, float radius, cVector3d l1, cVector3d l2)
{
	cVector3d cylinderDir = c2 - c1;
	cylinderDir.normalize();
	cVector3d lineDir = l2-l1;
	lineDir.normalize();

	//calcuate vector from one line to the other
	cVector3d line_distance = l1-c1;
	//attemtp to get normal
	cVector3d normal = cCross(cylinderDir, lineDir);
	float distance = 0;
	//If no normal is found then the vectors are colinear
	if (normal.length() == 0)
	{
		//The following calculates the orthogonal direction from one parallel line onto the other
		cVector3d lineV = cProject(line_distance, lineDir);
		lineV = line_distance-lineV;
		distance = lineV.length();
	}
	else
	{
		//project line onto the normal (orthogonal to both lines)
		//This yields the orthogonal distance, the shortest distance between the lines
		line_distance = cProject(line_distance, normal);
		distance = line_distance.length();
	}

	if (distance > radius)
	{
		return c1;
	}

	cVector3d c1p = c1- line_distance;
	cVector3d c2p = c2 - line_distance;

	Matrix3f A;
	//Construct the matrix [v1, v2, 0]
	A << cylinderDir.x(), -lineDir.x(), 0, cylinderDir.y(), -lineDir.y(), 0, cylinderDir.z(), -lineDir.z(), 0;

	cVector3d offset = l2 - c1p;

	Vector3f b;
	//Create the result vector p2-p1
	b << offset.x(), offset.y(), offset.z();

	//Solve the system of equations
	Vector3f x = A.colPivHouseholderQr().solve(b);
	//cout << "test: "<< x << endl;

	bool inside = false;
	cVector3d c3 = c1 + cylinderDir * x[0];
	cVector3d l3 = l1 + lineDir * x[1];

	//cout << c1 + cylinderDir * x[0] << endl;
	return c1+cylinderDir*x[0];
}

bool MatchingMesh::updateLine(unsigned int indexP0, unsigned int indexP1, chai3d::cVector3d &position, chai3d::cVector3d &cursorPosition) {
	auto p0 = mesh->m_vertices->getLocalPos(indexP0);
	auto p1 = mesh->m_vertices->getLocalPos(indexP1);

	//cursorPosition = solveCylinderCollisions(prevPos, position, radius, p0, p1);

	bool contact = false;
	// cursor sphere touching line segment p0 p1
	auto d = cursorPosition - p0;
	auto l01 = p1 - p0;
	auto lerp_amount = d.dot(l01) / l01.dot(l01);
	auto proj = l01 * lerp_amount;

	if (lerp_amount >= 0 && lerp_amount <= 1) {
		auto o = d - proj;
		auto penetration_depth = radius - o.length();
		o.normalize();
		if (penetration_depth > 0) {
			contact = 1;
			// compute cursor position
			auto target_contact = p0 + proj;
			cursorPosition = (target_contact + o * radius);
			// apply forces to the triangle
			auto fd = -(cursorPosition - position); fd.normalize();
			forces[indexP0] += fd * (1 - lerp_amount) * penetration_depth * 1000;
			forces[indexP1] += fd * (lerp_amount)* penetration_depth * 1000;
		}
	}
	if (contact) {
		affectedVertices.insert(indexP0);
		affectedVertices.insert(indexP1);
	}
	return contact;
}

bool MatchingMesh::updatePoint(unsigned int index, chai3d::cVector3d &position, chai3d::cVector3d &cursorPosition) {
	bool contact = false;
	auto p0 = mesh->m_vertices->getLocalPos(index);

	auto d = cursorPosition - p0;
	auto f = d.length() - radius;
	if (f < 0) {
		d.normalize();
		cursorPosition = (p0 + d * (radius));
		contact = 1;
		auto fd = -(cursorPosition - position); fd.normalize();
		forces[index] = -f * fd * 1000;
	}
	if (contact) {
		affectedVertices.insert(index);
	}
	return contact;
}

void MatchingMesh::movePoint(unsigned int index)
{
	double delta = 0.001;
	auto p = mesh->m_vertices->getLocalPos(index);

	forces[index] += (originalPoints[index] - p) * k - velocities[index] * b;

	velocities[index] += forces[index] * delta / m;

	p += velocities[index] * delta;

	mesh->m_vertices->setLocalPos(index, p);
}


/* 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.
 */

#define BOOST_TEST_MODULE ThermalOperator

#include <Geometry.hh>
#include <ThermalOperator.hh>

#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/numerics/matrix_tools.h>

#include <boost/property_tree/ptree.hpp>

#include "main.cc"

BOOST_AUTO_TEST_CASE(thermal_operator)
{
  MPI_Comm communicator = MPI_COMM_WORLD;

  // Create the Geometry
  boost::property_tree::ptree geometry_database;
  geometry_database.put("length", 12);
  geometry_database.put("length_divisions", 4);
  geometry_database.put("height", 6);
  geometry_database.put("height_divisions", 5);
  adamantine::Geometry<2> geometry(communicator, geometry_database);
  // Create the DoFHandler
  dealii::FE_Q<2> fe(2);
  dealii::DoFHandler<2> dof_handler(geometry.get_triangulation());
  dof_handler.distribute_dofs(fe);
  dealii::AffineConstraints<double> affine_constraints;
  affine_constraints.close();
  dealii::QGauss<1> quad(3);

  // Create the MaterialProperty
  boost::property_tree::ptree mat_prop_database;
  mat_prop_database.put("property_format", "polynomial");
  mat_prop_database.put("n_materials", 1);
  mat_prop_database.put("material_0.solid.density", 1.);
  mat_prop_database.put("material_0.powder.density", 1.);
  mat_prop_database.put("material_0.liquid.density", 1.);
  mat_prop_database.put("material_0.solid.specific_heat", 1.);
  mat_prop_database.put("material_0.powder.specific_heat", 1.);
  mat_prop_database.put("material_0.liquid.specific_heat", 1.);
  mat_prop_database.put("material_0.solid.thermal_conductivity", 10.);
  mat_prop_database.put("material_0.powder.thermal_conductivity", 10.);
  mat_prop_database.put("material_0.liquid.thermal_conductivity", 10.);
  std::shared_ptr<adamantine::MaterialProperty<2>> mat_properties(
      new adamantine::MaterialProperty<2>(
          communicator, geometry.get_triangulation(), mat_prop_database));

  // Initialize the ThermalOperator
  adamantine::ThermalOperator<2, 2, dealii::MemorySpace::Host> thermal_operator(
      communicator, mat_properties);
  thermal_operator.reinit(dof_handler, affine_constraints, quad);
  thermal_operator.compute_inverse_mass_matrix(dof_handler, affine_constraints);
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> dummy(
      thermal_operator.m());
  thermal_operator.evaluate_material_properties(dummy);
  BOOST_CHECK(thermal_operator.m() == 99);
  BOOST_CHECK(thermal_operator.m() == thermal_operator.n());

  // Check matrix-vector multiplications
  double const tolerance = 1e-15;
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> src;
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> dst_1;
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> dst_2;

  dealii::MatrixFree<2, double> const &matrix_free =
      thermal_operator.get_matrix_free();
  matrix_free.initialize_dof_vector(src);
  matrix_free.initialize_dof_vector(dst_1);
  matrix_free.initialize_dof_vector(dst_2);

  src = 1.;
  thermal_operator.vmult(dst_1, src);
  BOOST_CHECK_CLOSE(dst_1.l1_norm(), 0., tolerance);

  thermal_operator.Tvmult(dst_2, src);
  BOOST_CHECK_CLOSE(dst_2.l1_norm(), dst_1.l1_norm(), tolerance);

  dst_2 = 1.;
  thermal_operator.vmult_add(dst_2, src);
  thermal_operator.vmult(dst_1, src);
  dst_1 += src;
  BOOST_CHECK_CLOSE(dst_1.l1_norm(), dst_2.l1_norm(), tolerance);

  dst_1 = 1.;
  thermal_operator.Tvmult_add(dst_1, src);
  BOOST_CHECK_CLOSE(dst_1.l1_norm(), dst_2.l1_norm(), tolerance);
}

BOOST_AUTO_TEST_CASE(spmv)
{
  MPI_Comm communicator = MPI_COMM_WORLD;

  // Create the Geometry
  boost::property_tree::ptree geometry_database;
  geometry_database.put("length", 12);
  geometry_database.put("length_divisions", 4);
  geometry_database.put("height", 6);
  geometry_database.put("height_divisions", 5);
  adamantine::Geometry<2> geometry(communicator, geometry_database);
  // Create the DoFHandler
  dealii::FE_Q<2> fe(2);
  dealii::DoFHandler<2> dof_handler(geometry.get_triangulation());
  dof_handler.distribute_dofs(fe);
  dealii::AffineConstraints<double> affine_constraints;
  affine_constraints.close();
  dealii::QGauss<1> quad(3);

  // Create the MaterialProperty
  boost::property_tree::ptree mat_prop_database;
  mat_prop_database.put("property_format", "polynomial");
  mat_prop_database.put("n_materials", 1);
  mat_prop_database.put("material_0.solid.density", 1.);
  mat_prop_database.put("material_0.powder.density", 1.);
  mat_prop_database.put("material_0.liquid.density", 1.);
  mat_prop_database.put("material_0.solid.specific_heat", 1.);
  mat_prop_database.put("material_0.powder.specific_heat", 1.);
  mat_prop_database.put("material_0.liquid.specific_heat", 1.);
  mat_prop_database.put("material_0.solid.thermal_conductivity", 1.);
  mat_prop_database.put("material_0.powder.thermal_conductivity", 1.);
  mat_prop_database.put("material_0.liquid.thermal_conductivity", 1.);
  std::shared_ptr<adamantine::MaterialProperty<2>> mat_properties(
      new adamantine::MaterialProperty<2>(
          communicator, geometry.get_triangulation(), mat_prop_database));

  // Initialize the ThermalOperator
  adamantine::ThermalOperator<2, 2, dealii::MemorySpace::Host> thermal_operator(
      communicator, mat_properties);
  thermal_operator.reinit(dof_handler, affine_constraints, quad);
  thermal_operator.compute_inverse_mass_matrix(dof_handler, affine_constraints);
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> dummy(
      thermal_operator.m());
  thermal_operator.evaluate_material_properties(dummy);
  BOOST_CHECK(thermal_operator.m() == 99);
  BOOST_CHECK(thermal_operator.m() == thermal_operator.n());

  // Build the matrix. This only works in serial.
  dealii::DynamicSparsityPattern dsp(dof_handler.n_dofs());
  dealii::DoFTools::make_sparsity_pattern(dof_handler, dsp, affine_constraints);
  dealii::SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from(dsp);
  dealii::SparseMatrix<double> sparse_matrix(sparsity_pattern);
  dealii::MatrixCreator::create_laplace_matrix(
      dof_handler, dealii::QGauss<2>(3), sparse_matrix);

  // Compare vmult using matrix free and building the matrix
  double const tolerance = 1e-12;
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> src;
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> dst_1;
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> dst_2;

  dealii::MatrixFree<2, double> const &matrix_free =
      thermal_operator.get_matrix_free();
  matrix_free.initialize_dof_vector(src);
  matrix_free.initialize_dof_vector(dst_1);
  matrix_free.initialize_dof_vector(dst_2);

  for (unsigned int i = 0; i < thermal_operator.m(); ++i)
  {
    src = 0.;
    src[i] = 1;
    thermal_operator.vmult(dst_1, src);
    sparse_matrix.vmult(dst_2, src);
    for (unsigned int j = 0; j < thermal_operator.m(); ++j)
      BOOST_CHECK_CLOSE(dst_1[j], -dst_2[j], tolerance);
  }
}