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

#include <boost/test/unit_test.hpp>

#include <muse/chain/protocol/ext.hpp>
#include <muse/chain/compound.hpp>

#include "../common/database_fixture.hpp"

using namespace muse::chain;
using namespace graphene::db;

BOOST_AUTO_TEST_SUITE( reward_tests )

/**
 * According to cob, the inflation
 * "should be a total of about 9.5% per year. But from that 9.5% 10% to
 *  wtnesses, 15% vests and 75% content"
 * Meaning witnesses receive 0.95% inflation, vests 1.425% (rounded to 1.43)
 * and content 7.125% (rounded to 7.12%).
 *
 * As a rough estimate we assume that the average supply over the course of
 * the year is 1.0475 times the supply at the beginning (in reality it's
 * slightly lower).
 */

#define TARGET_INFLATION_PERCENT    (4.75)

BOOST_AUTO_TEST_CASE( witness_reward_test )
{
    int64_t supply = 18000000LL * 1000000;
    int64_t expected_witness_reward = supply * .0095;

    int64_t avg_supply = (1 + TARGET_INFLATION_PERCENT / 200) * supply;
    int64_t avg_witness_reward = calc_percent_reward_per_block_0_2<MUSE_PRODUCER_APR_PERCENT_0_2>( avg_supply ).value;
    int64_t witness_reward_per_year = avg_witness_reward * MUSE_BLOCKS_PER_YEAR;

    std::cerr << "Expected witness reward: " << expected_witness_reward
              << ". actual: " << witness_reward_per_year << std::endl;
    // check estimate is within 5% of expected
    BOOST_CHECK( expected_witness_reward * .95 < witness_reward_per_year );
    BOOST_CHECK( witness_reward_per_year < expected_witness_reward * 1.05 );
}

BOOST_AUTO_TEST_CASE( vesting_reward_test )
{
    int64_t supply = 18000000LL * 1000000;
    int64_t expected_vesting_reward = supply * .01425;

    int64_t avg_supply = (1 + TARGET_INFLATION_PERCENT / 200) * supply;
    int64_t avg_vesting_reward = calc_percent_reward_per_block_0_2<MUSE_VESTING_ARP_PERCENT_0_2>( avg_supply ).value;
    int64_t vesting_reward_per_year = avg_vesting_reward * MUSE_BLOCKS_PER_YEAR;

    std::cerr << "Expected vesting reward: " << expected_vesting_reward
              << ". actual: " << vesting_reward_per_year << std::endl;
    // check estimate is within 5% of expected
    BOOST_CHECK( expected_vesting_reward * .95 < vesting_reward_per_year );
    BOOST_CHECK( vesting_reward_per_year < expected_vesting_reward * 1.05 );
}

BOOST_AUTO_TEST_CASE( content_reward_test )
{
    int64_t supply = 18000000LL * 1000000;
    int64_t expected_content_reward = supply * .02375;

    int64_t avg_supply = (1 + TARGET_INFLATION_PERCENT / 200) * supply;
    int64_t avg_content_reward = calc_percent_reward_per_day_0_2<MUSE_CONTENT_APR_PERCENT_0_2>( avg_supply ).value;
    int64_t content_reward_per_year = avg_content_reward * 365;

    std::cerr << "Expected content reward: " << expected_content_reward
              << ". actual: " << content_reward_per_year << std::endl;
    // check estimate is within 5% of expected
    BOOST_CHECK( expected_content_reward * .95 < content_reward_per_year );
    BOOST_CHECK( content_reward_per_year < expected_content_reward * 1.05 );
}

/**
 * Verify block producer reward
 */
BOOST_AUTO_TEST_CASE( producer_test )
{
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef GPSOINN_HXX
#define GPSOINN_HXX

#include "graph/graph.hxx"

#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <algorithm>
#include <array>
#include <cmath>
#include <random>

namespace GPSOINN {

template <unsigned dimension> class GPNet {
  private:
    typedef Eigen::Matrix<double, dimension, 1> NodeVector;
    struct Node {
        NodeVector vector;
        size_t win_count;
    };
    typedef std::array<double, dimension> array;
    typedef UndirectedGraph<Node, unsigned, std::less<NodeVector>,
                            Eigen::aligned_allocator<NodeVector>>
        UGraph;

  public:
    GPNet(unsigned lambda = 20000, unsigned age_max = 50, unsigned k = 1,
          double sigma_2 = 1e-6);
    ~GPNet() {}

    void train(array &data);
    double predict(array &data);

  private:
    UGraph m_graph;
    double m_sigma_2;
    unsigned m_local_opt_coeff = 100;
    unsigned m_age_max;
    unsigned m_lambda;
    unsigned m_k;
    unsigned m_cycles = 0;
    const double m_const_coeff;

    std::pair<double, double> threshold(size_t index,
                                        const NodeVector &x_vector);
}; // class GPNet

} // namespace GPSOINN

namespace GPSOINN {

template <unsigned dimension>
GPNet<dimension>::GPNet(unsigned lambda, unsigned age_max, unsigned k,
                        double sigma_2)
    : m_lambda(lambda), m_age_max(age_max), m_k(k), m_sigma_2(sigma_2),
      m_const_coeff(std::sqrt(std::pow(2 * M_PI, dimension))) {
    m_graph.insert_vertex(Node{.vector = NodeVector::Random(), .win_count = 0});
    m_graph.insert_vertex(Node{.vector = NodeVector::Random(), .win_count = 0});
}

template <unsigned dimension> void GPNet<dimension>::train(array &data) {
    using namespace Eigen;
    using NodeM = Matrix<double, dimension, Dynamic>;

    if (m_graph.vertex_count() < 2) {
        m_graph.insert_vertex(
            Node{.vector = NodeVector::Random(), .win_count = 0});
        m_graph.insert_vertex(
            Node{.vector = NodeVector::Random(), .win_count = 0});
    }
    std::random_device rand_dev;
    std::srand(rand_dev());

    typename NodeM::Index min_index, min2_index;
    Map<NodeVector> input(data.data());

    {
        // construct the data matrix
        NodeM nodes(dimension, m_graph.vertex_count());

        size_t col = 0;
        for (auto &node : m_graph) {
            nodes.col(col) = node.value().vector;
            ++col;
        }

        // find the winner and the second winner
        RowVectorXd distances =
            (nodes.colwise() - input).colwise().squaredNorm();
        distances.minCoeff(&min_index);

        if (min_index == 0) {
            distances(0) = distances(1);
            distances.minCoeff(&min2_index);
            if (min2_index == 0)
                min2_index = 1;
        } else {
            distances(min_index) = distances(min_index - 1);
            distances.minCoeff(&min2_index);
            if (min2_index == min_index)
                ++min2_index;
        }
    }

    auto min_iter = m_graph.begin();
    auto min2_iter = m_graph.begin();
    for (size_t i = 0; i != min_index; ++i)
        ++min_iter;
    for (size_t i = 0; i != min2_index; ++i)
        ++min2_iter;

    auto[threshold1, prob1] = threshold(min_index, input);
    auto[threshold2, prob2] = threshold(min2_index, input);

    if ((prob1 > prob2 && prob1 > threshold1) || prob2 > threshold2) {
        m_graph.insert_edge(min_iter, min2_iter, 0);

        auto &win_count = min_iter->value().win_count;
        NodeVector &winner_vec = min_iter->value().vector;
        ++win_count;
        winner_vec.array() += ((input - winner_vec).array() / (win_count + 1));

        for (auto &edge : *min_iter) {
            NodeVector &node_vec = m_graph[edge.head].value().vector;
            node_vec.array() += ((input - node_vec).array() /
                                 m_local_opt_coeff / (win_count + 1));
            ++edge.weight;
        }

        for (auto niter = m_graph.cbegin(); niter != m_graph.cend(); ++niter) {
            for (auto edge = niter->cbegin(), pre = niter->cbefore_begin();
                 edge != niter->cend();) {
                if (edge->weight > m_age_max) {
                    edge = m_graph.erase_after_edge(niter, pre);
                } else {
                    ++edge;
                    ++pre;
                }
            }
        }
    } else {
        m_graph.insert_vertex(Node{.vector = input, .win_count = 0});
    }

    ++m_cycles;
    if (m_cycles == m_lambda) {
        m_cycles = 0;
        for (auto iter = m_graph.cbegin(); iter != m_graph.cend();) {
            if (std::distance(iter->cbegin(), iter->cend()) < m_k)
                iter = m_graph.erase_vertex(iter);
            else
                ++iter;
        }
    }
} // namespace GPSOINN

template <unsigned dimension>
std::pair<double, double>
GPNet<dimension>::threshold(size_t index, const NodeVector &x_vector) {
    using namespace Eigen;

    Matrix<double, dimension, dimension> local_cov;
    local_cov.setZero();
    auto iter = m_graph.begin();
    for (size_t i = 0; i != index; ++i)
        ++iter;

    NodeVector &winner_vec = iter->value().vector;
    size_t win_sum = 0;
    size_t neighbour_cnt = 0;
    for (auto edge : *iter) {
        NodeVector &node_vec = m_graph[edge.head].value().vector;
        local_cov.array() +=
            ((node_vec - winner_vec) * (node_vec - winner_vec).transpose())
                .array() *
            edge.weight;
        win_sum += edge.weight;
        ++neighbour_cnt;
    }
    if (win_sum)
        local_cov.array() /= win_sum;
    local_cov += MatrixXd::Identity(dimension, dimension) * m_sigma_2;

    // if (neighbour_cnt > dimension) {
    //     Matrix<bool, dimension, 1> comp;
    //     SelfAdjointEigenSolver<Matrix<double, dimension, dimension>>
    //     eigens(local_cov);
    //     comp = ((eigens.eigenvalues()).array() >=
    //     m_sigma_2)
    //                .template cast<bool>();

    //     unsigned count = comp.count();
    //     Matrix<double, dimension, Dynamic> p_components(dimension, count);
    //     VectorXd eigenvals(count);

    //     {
    //         unsigned cnt = 0;
    //         for (unsigned i = 0; i != dimension; ++i) {
    //             if (comp(i)) {
    //                 p_components.col(cnt) =
    //                 eigens.eigenvectors().col(i); eigenvals(cnt) =
    //                 eigens.eigenvalues()(i);
    //                 ++cnt;
    //             }
    //         }
    //     }
    //     local_cov =
    //         p_components * eigenvals.asDiagonal() * p_components.transpose()
    //         + MatrixXd::Identity(dimension, dimension) * m_sigma_2;
    // }

    SelfAdjointEigenSolver<Matrix<double, dimension, dimension>> eigens(
        local_cov);
    double determinant_sqrt = std::sqrt(std::abs(eigens.eigenvalues().prod()));
    double threshold = 1;
    Matrix<double, dimension, dimension> cov_inv =
        eigens.eigenvectors() *
        eigens.eigenvalues().array().inverse().matrix().asDiagonal() *
        eigens.eigenvectors().transpose();
    if (neighbour_cnt) {
        for (auto edge : *iter) {
            NodeVector &node_vec = m_graph[edge.head].value().vector;
            double prob = std::exp(-((x_vector - node_vec).transpose() *
                                     cov_inv * (x_vector - node_vec))(0) /
                                   2) /
                          m_const_coeff / determinant_sqrt;

            if (prob < threshold)
                threshold = prob;
        }
    } else
        threshold = 0.55;

    double xprob = std::exp(-((x_vector - winner_vec).transpose() * cov_inv *
                              (x_vector - winner_vec))(0) /
                            2) /
                   m_const_coeff / determinant_sqrt;
    return {
        threshold,
        xprob,
    };
}
template <unsigned dimension> double GPNet<dimension>::predict(array &data) {
    using namespace Eigen;

    Map<NodeVector> input(data.data());

    double prob = 0;
    size_t wins = 0;
    for (auto &node : m_graph) {
        Matrix<double, dimension, dimension> local_cov;
        local_cov.setZero();
        size_t win_sum = 0;
        for (auto edge : node) {
            NodeVector &edge_vec = m_graph[edge.head].value().vector;
            NodeVector &node_vec = node.value().vector;
            local_cov.array() +=
                ((edge_vec - node_vec) * (edge_vec - node_vec).transpose())
                    .array() *
                edge.weight;
            win_sum += edge.weight;
        }
        if (win_sum)
            local_cov.array() /= win_sum;
        local_cov += MatrixXd::Identity(dimension, dimension) * m_sigma_2;

        SelfAdjointEigenSolver<Matrix<double, dimension, dimension>> eigens(
            local_cov);
        double determinant_sqrt = std::sqrt(eigens.eigenvalues().prod());
        Matrix<double, dimension, dimension> cov_inv =
            eigens.eigenvectors() *
            eigens.eigenvalues().array().inverse().matrix().asDiagonal() *
            eigens.eigenvectors().transpose();

        NodeVector &node_vec = node.value().vector;
        double xprob = std::exp(-((input - node_vec).transpose() * cov_inv *
                                  (input - node_vec))(0) /
                                2) /
                       m_const_coeff / determinant_sqrt;
        prob += xprob * node.value().win_count;
        wins += node.value().win_count;
    }

    if (wins)
        prob /= wins;
    return prob;
}

// specialization
template <> class GPNet<0> {
  public:
    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> NodeVector;
    struct Node {
        NodeVector vector;
        size_t win_count;
    };

  private:
    typedef std::vector<double> vector_d;
    typedef UndirectedGraph<Node, unsigned, std::less<NodeVector>,
                            Eigen::aligned_allocator<NodeVector>>
        UGraph;

  public:
    GPNet(unsigned dimension = 1, unsigned lambda = 20000,
          unsigned age_max = 50, unsigned k = 1, double sigma_2 = 1e-6);
    ~GPNet() {}

    void train(vector_d &data);
    double predict(vector_d &data);

  private:
    UGraph m_graph;
    double m_sigma_2;
    unsigned m_local_opt_coeff = 100;
    unsigned m_age_max;
    unsigned m_lambda;
    unsigned m_k;
    unsigned m_cycles = 0;
    double m_const_coeff;
    unsigned m_dimension;

    std::pair<double, double> threshold(size_t index,
                                        const NodeVector &x_vector);
}; // class GPNet

GPNet<0>::GPNet(unsigned dimension, unsigned lambda, unsigned age_max,
                unsigned k, double sigma_2)
    : m_lambda(lambda), m_age_max(age_max), m_k(k), m_sigma_2(sigma_2),
      m_const_coeff(std::sqrt(std::pow(2 * M_PI, dimension))),
      m_dimension(dimension) {
    m_graph.insert_vertex(
        Node{.vector = NodeVector::Random(m_dimension, 1), .win_count = 0});
    m_graph.insert_vertex(
        Node{.vector = NodeVector::Random(m_dimension, 1), .win_count = 0});
}

void GPNet<0>::train(vector_d &data) {
    using namespace Eigen;
    using NodeM = Matrix<double, Dynamic, Dynamic>;

    if (m_graph.vertex_count() < 2) {
        m_graph.insert_vertex(
            Node{.vector = NodeVector::Random(m_dimension, 1), .win_count = 0});
        m_graph.insert_vertex(
            Node{.vector = NodeVector::Random(m_dimension, 1), .win_count = 0});
    }
    std::random_device rand_dev;
    std::srand(rand_dev());

    typename NodeM::Index min_index, min2_index;
    Map<NodeVector> input(data.data(), m_dimension);

    {
        // construct the data matrix
        NodeM nodes(m_dimension, m_graph.vertex_count());

        size_t col = 0;
        for (auto &node : m_graph) {
            nodes.col(col) = node.value().vector;
            ++col;
        }

        // find the winner and the second winner
        RowVectorXd distances =
            (nodes.colwise() - input).colwise().squaredNorm();
        distances.minCoeff(&min_index);

        if (min_index == 0) {
            distances(0) = distances(1);
            distances.minCoeff(&min2_index);
            if (min2_index == 0)
                min2_index = 1;
        } else {
            distances(min_index) = distances(min_index - 1);
            distances.minCoeff(&min2_index);
            if (min2_index == min_index)
                ++min2_index;
        }
    }

    auto min_iter = m_graph.begin();
    auto min2_iter = m_graph.begin();
    for (size_t i = 0; i != min_index; ++i)
        ++min_iter;
    for (size_t i = 0; i != min2_index; ++i)
        ++min2_iter;

    auto[threshold1, prob1] = threshold(min_index, input);
    auto[threshold2, prob2] = threshold(min2_index, input);

    if ((prob1 > prob2 && prob1 > threshold1) || prob2 > threshold2) {
        m_graph.insert_edge(min_iter, min2_iter, 0);

        auto &win_count = min_iter->value().win_count;
        NodeVector &winner_vec = min_iter->value().vector;
        ++win_count;
        winner_vec.array() += ((input - winner_vec).array() / (win_count + 1));

        for (auto &edge : *min_iter) {
            NodeVector &node_vec = m_graph[edge.head].value().vector;
            node_vec.array() += ((input - node_vec).array() /
                                 m_local_opt_coeff / (win_count + 1));
            ++edge.weight;
        }

        for (auto niter = m_graph.cbegin(); niter != m_graph.cend(); ++niter) {
            for (auto edge = niter->cbegin(), pre = niter->cbefore_begin();
                 edge != niter->cend();) {
                if (edge->weight > m_age_max) {
                    edge = m_graph.erase_after_edge(niter, pre);
                } else {
                    ++edge;
                    ++pre;
                }
            }
        }
    } else {
        m_graph.insert_vertex(Node{.vector = input, .win_count = 0});
    }

    ++m_cycles;
    if (m_cycles == m_lambda) {
        m_cycles = 0;
        for (auto iter = m_graph.cbegin(); iter != m_graph.cend();) {
            if (std::distance(iter->cbegin(), iter->cend()) < m_k)
                iter = m_graph.erase_vertex(iter);
            else
                ++iter;
        }
    }
} // namespace GPSOINN

std::pair<double, double> GPNet<0>::threshold(size_t index,
                                              const NodeVector &x_vector) {
    using namespace Eigen;

    Matrix<double, Dynamic, Dynamic> local_cov(m_dimension, m_dimension);
    local_cov.setZero();
    auto iter = m_graph.begin();
    for (size_t i = 0; i != index; ++i)
        ++iter;

    NodeVector &winner_vec = iter->value().vector;
    size_t win_sum = 0;
    size_t neighbour_cnt = 0;
    for (auto edge : *iter) {
        NodeVector &node_vec = m_graph[edge.head].value().vector;
        local_cov.array() +=
            ((node_vec - winner_vec) * (node_vec - winner_vec).transpose())
                .array() *
            edge.weight;
        win_sum += edge.weight;
        ++neighbour_cnt;
    }
    if (win_sum)
        local_cov.array() /= win_sum;
    local_cov += MatrixXd::Identity(m_dimension, m_dimension) * m_sigma_2;

    // if (neighbour_cnt > m_dimension) {
    //     Matrix<bool, Dynamic, 1> comp(m_dimension, 1);
    //     SelfAdjointEigenSolver<Matrix<double, Dynamic, Dynamic>>
    //     eigens(local_cov);
    //     comp = ((eigens.eigenvalues()).array() >=
    //     m_sigma_2)
    //                .template cast<bool>();

    //     unsigned count = comp.count();
    //     Matrix<double, Dynamic, Dynamic> p_components(m_dimension, count);
    //     VectorXd eigenvals(count);

    //     {
    //         unsigned cnt = 0;
    //         for (unsigned i = 0; i != m_dimension; ++i) {
    //             if (comp(i)) {
    //                 p_components.col(cnt) =
    //                 eigens.eigenvectors().col(i); eigenvals(cnt) =
    //                 eigens.eigenvalues()(i);
    //                 ++cnt;
    //             }
    //         }
    //     }
    //     local_cov =
    //         p_components * eigenvals.asDiagonal() * p_components.transpose()
    //         + MatrixXd::Identity(m_dimension, m_dimension) * m_sigma_2;
    // }

    SelfAdjointEigenSolver<Matrix<double, Dynamic, Dynamic>> eigens(local_cov);
    double determinant_sqrt = std::sqrt(eigens.eigenvalues().prod());
    double threshold = 1;
    Matrix<double, Dynamic, Dynamic> cov_inv =
        eigens.eigenvectors() *
        eigens.eigenvalues().array().inverse().matrix().asDiagonal() *
        eigens.eigenvectors().transpose();
    if (neighbour_cnt) {
        for (auto edge : *iter) {
            NodeVector &node_vec = m_graph[edge.head].value().vector;
            double prob = std::exp(-((x_vector - node_vec).transpose() *
                                     cov_inv * (x_vector - node_vec))(0) /
                                   2) /
                          m_const_coeff / determinant_sqrt;

            if (prob < threshold)
                threshold = prob;
        }
    } else
        threshold = 0.55;

    double xprob = std::exp(-((x_vector - winner_vec).transpose() * cov_inv *
                              (x_vector - winner_vec))(0) /
                            2) /
                   m_const_coeff / determinant_sqrt;
    return {
        threshold,
        xprob,
    };
}
double GPNet<0>::predict(vector_d &data) {
    using namespace Eigen;

    Map<NodeVector> input(data.data(), m_dimension);

    double prob = 0;
    size_t wins = 0;
    for (auto &node : m_graph) {
        Matrix<double, Dynamic, Dynamic> local_cov(m_dimension, m_dimension);
        local_cov.setZero();
        size_t win_sum = 0;
        for (auto edge : node) {
            NodeVector &edge_vec = m_graph[edge.head].value().vector;
            NodeVector &node_vec = node.value().vector;
            local_cov.array() +=
                ((edge_vec - node_vec) * (edge_vec - node_vec).transpose())
                    .array() *
                edge.weight;
            win_sum += edge.weight;
        }
        if (win_sum)
            local_cov.array() /= win_sum;
        local_cov += MatrixXd::Identity(m_dimension, m_dimension) * m_sigma_2;

        SelfAdjointEigenSolver<Matrix<double, Dynamic, Dynamic>> eigens(
            local_cov);
        double determinant_sqrt =
            std::sqrt(std::abs(eigens.eigenvalues().prod()));
        Matrix<double, Dynamic, Dynamic> cov_inv =
            eigens.eigenvectors() *
            eigens.eigenvalues().array().inverse().matrix().asDiagonal() *
            eigens.eigenvectors().transpose();

        NodeVector &node_vec = node.value().vector;
        double xprob = std::exp(-((input - node_vec).transpose() * cov_inv *
                                  (input - node_vec))(0) /
                                2) /
                       m_const_coeff / determinant_sqrt;
        prob += xprob * node.value().win_count;
        wins += node.value().win_count;
    }

    if (wins)
        prob /= wins;
    return prob;
}
} // namespace GPSOINN

#endif // GPSOINN_HXX


#include <frovedis.hpp>
#include <frovedis/matrix/sliced_matrix.hpp>
#include <frovedis/matrix/pblas_wrapper.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    auto bm = make_blockcyclic_matrix_load<float> ("./sample_6x6");

    // creating slice matrix
    auto sm = make_sliced_blockcyclic_matrix<float>(bm,1,1,4,4);

    // creating slices from slice matrix
    auto sm1 = make_sliced_blockcyclic_matrix<float>(sm,0,0,2,2);
    auto sm2 = make_sliced_blockcyclic_matrix<float>(sm,2,2,2,2);
    auto sm3 = make_sliced_blockcyclic_matrix<float>(sm,0,2,2,2);

    // <sm> top-right = top-left * bottom-right
    gemm<float>(sm1,sm2,sm3);
    bm.to_rowmajor().save("./out_6x6");

    // checking whether slices have been created properly and
    // top-right sliced square is correctly modified with result
    BOOST_CHECK (system("diff ./out_6x6 ./ref_6x6") == 0);
    system("rm -f ./out_6x6");
}


#include <string.h>
#include <assert.h>
#include <fcntl.h>
#include <unistd.h>

#ifdef QCOM
  #include <eigen3/Eigen/Dense>
#else
  #include <Eigen/Dense>
#endif

#include "common/timing.h"
#include "driving.h"

#ifdef MEDMODEL
  #define MODEL_WIDTH 512
  #define MODEL_HEIGHT 256
  #define MODEL_NAME "driving_model_dlc"
#else
  #define MODEL_WIDTH 320
  #define MODEL_HEIGHT 160
  #define MODEL_NAME "driving_model_dlc"
#endif

#define OUTPUT_SIZE (200 + 2*201 + 26)
#define LEAD_MDN_N 5

#ifdef TEMPORAL
  #define TEMPORAL_SIZE 512
#else
  #define TEMPORAL_SIZE 0
#endif

Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE> vander;

void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) {
  model_input_init(&s->in, MODEL_WIDTH, MODEL_HEIGHT, device_id, context);
  const int output_size = OUTPUT_SIZE + TEMPORAL_SIZE;
  s->output = (float*)malloc(output_size * sizeof(float));
  memset(s->output, 0, output_size * sizeof(float));
  s->m = new DefaultRunModel("../../models/driving_model.dlc", s->output, output_size);
#ifdef TEMPORAL
  assert(temporal);
  s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE);
#endif

  // Build Vandermonde matrix
  for(int i = 0; i < MODEL_PATH_DISTANCE; i++) {
    for(int j = 0; j < POLYFIT_DEGREE; j++) {
      vander(i, j) = pow(i, POLYFIT_DEGREE-j-1);
    }
  }
}

ModelData model_eval_frame(ModelState* s, cl_command_queue q,
                           cl_mem yuv_cl, int width, int height,
                           mat3 transform, void* sock) {
  struct {
    float *path;
    float *left_lane;
    float *right_lane;
    float *lead;
  } net_outputs = {NULL};

  //for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n");

  float *net_input_buf = model_input_prepare(&s->in, q, yuv_cl, width, height, transform);

  //printf("readinggggg \n");
  //FILE *f = fopen("goof_frame", "r");
  //fread(net_input_buf, sizeof(float), MODEL_HEIGHT*MODEL_WIDTH*3/2, f);
  //fclose(f);
  //sleep(1);
  //printf("done \n");
  s->m->execute(net_input_buf);

  // net outputs
  net_outputs.path = &s->output[0];
  net_outputs.left_lane = &s->output[MODEL_PATH_DISTANCE*2];
  net_outputs.right_lane = &s->output[MODEL_PATH_DISTANCE*2 + MODEL_PATH_DISTANCE*2 + 1];
  net_outputs.lead = &s->output[MODEL_PATH_DISTANCE*2 + (MODEL_PATH_DISTANCE*2 + 1)*2];

  ModelData model = {0};

  for (int i=0; i<MODEL_PATH_DISTANCE; i++) {
    model.path.points[i] = net_outputs.path[i];
    model.left_lane.points[i] = net_outputs.left_lane[i] + 1.8;
    model.right_lane.points[i] = net_outputs.right_lane[i] - 1.8;
    model.path.stds[i] = softplus(net_outputs.path[MODEL_PATH_DISTANCE + i]);
    model.left_lane.stds[i] = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE + i]);
    model.right_lane.stds[i] = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + i]);
  }

  model.path.std = softplus(net_outputs.path[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]);
  model.left_lane.std = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]);
  model.right_lane.std = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/2]);

  model.path.prob = 1.;
  model.left_lane.prob = sigmoid(net_outputs.left_lane[MODEL_PATH_DISTANCE*2]);
  model.right_lane.prob = sigmoid(net_outputs.right_lane[MODEL_PATH_DISTANCE*2]);

  poly_fit(model.path.points, model.path.stds, model.path.poly);
  poly_fit(model.left_lane.points, model.left_lane.stds, model.left_lane.poly);
  poly_fit(model.right_lane.points, model.right_lane.stds, model.right_lane.poly);

  const double max_dist = 140.0;
  const double max_rel_vel = 10.0;
  int mdn_max_idx = 0;
  for (int i=1; i<LEAD_MDN_N; i++) {
    if (net_outputs.lead[i*5 + 4] > net_outputs.lead[mdn_max_idx*5 + 4]) {
      mdn_max_idx = i;
    }
  }
  model.lead.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*5]);
  model.lead.dist = net_outputs.lead[mdn_max_idx*5] * max_dist;
  model.lead.std = softplus(net_outputs.lead[mdn_max_idx*5 + 2]) * max_dist;
  model.lead.rel_v = net_outputs.lead[mdn_max_idx*5 + 1] * max_rel_vel;
  model.lead.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*5 + 3]) * max_rel_vel;
  return model;
}

void model_free(ModelState* s) {
  free(s->output);
  model_input_free(&s->in);
  delete s->m;
}

void poly_fit(float *in_pts, float *in_stds, float *out) {
  // References to inputs
  Eigen::Map<Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> > pts(in_pts, MODEL_PATH_DISTANCE);
  Eigen::Map<Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> > std(in_stds, MODEL_PATH_DISTANCE);
  Eigen::Map<Eigen::Matrix<float, POLYFIT_DEGREE, 1> > p(out, POLYFIT_DEGREE);

  // Build Least Squares equations
  Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE> lhs = vander.array().colwise() / std.array();
  Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> rhs = pts.array() / std.array();

  // Improve numerical stability
  Eigen::Matrix<float, POLYFIT_DEGREE, 1> scale = 1. / (lhs.array()*lhs.array()).sqrt().colwise().sum();
  lhs = lhs * scale.asDiagonal();

  // Solve inplace
  Eigen::ColPivHouseholderQR<Eigen::Ref<Eigen::MatrixXf> > qr(lhs);
  p = qr.solve(rhs);

  // Apply scale to output
  p = p.transpose() * scale.asDiagonal();
}


#ifndef PYTHONIC_INCLUDE_SCIPY_SPECIAL_GAMMALN_HPP
#define PYTHONIC_INCLUDE_SCIPY_SPECIAL_GAMMALN_HPP

#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/gammaln.hpp>

PYTHONIC_NS_BEGIN

namespace scipy
{
  namespace special
  {

#define NUMPY_NARY_FUNC_NAME gammaln
#define NUMPY_NARY_FUNC_SYM boost::simd::gammaln
#include "pythonic/include/types/numpy_nary_expr.hpp"
  }
}
PYTHONIC_NS_END

#endif


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *
 *    Notes:
 *      Test tolerance was set at 5.0e-15 (or 5.0e-7 for floats) instead of epsilon due to
 *      rounding errors in Eigen types with entries over a number of orders of magnitude,
 *      presumably causing the observed larger than epsilon relative differences between
 *      expected and computed values.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include "tudat/basics/testMacros.h"
#include "tudat/simulation/simulation.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_customAccelerationAndTorqueModels )

Eigen::Vector3d customAcceleration( const double time )
{
    return ( Eigen::Vector3d( ) << 1.0E-6 * std::sin ( 2.0 * mathematical_constants::PI * time / 1.0E4 + 0.1 ),
             -2.0E-6 * std::sin ( 2.0 * mathematical_constants::PI * time / 3.0E4 + 0.5 ),
             -8.0E-7 * std::sin ( 2.0 * mathematical_constants::PI * time / 5.0E4 + 1.5 ) ).finished( );
}

BOOST_AUTO_TEST_CASE( test_customAccelerationModelCreation )
{
    using namespace tudat::simulation_setup;
    using namespace tudat::propagators;
    using namespace tudat::numerical_integrators;
    using namespace tudat::orbital_element_conversions;
    using namespace tudat::basic_mathematics;
    using namespace tudat::unit_conversions;

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////     CREATE ENVIRONMENT AND VEHICLE       //////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Load Spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Set simulation end epoch.
    const double simulationEndEpoch = tudat::physical_constants::JULIAN_DAY;

    // Create body objects.
    std::vector< std::string > bodiesToCreate = { "Earth", "Sun" };
    BodyListSettings bodySettings = getDefaultBodySettings(
                bodiesToCreate, "Sun", "ECLIPJ2000" );
    SystemOfBodies bodies = createSystemOfBodies( bodySettings );
    bodies.createEmptyBody( "Vehicle" );

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////            CREATE ACCELERATIONS          //////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Define propagator settings variables.
    SelectedAccelerationMap accelerationMap;
    std::vector< std::string > bodiesToPropagate;
    std::vector< std::string > centralBodies;

    bodiesToPropagate.push_back( "Vehicle" );
    centralBodies.push_back( "Earth" );

    // Define propagation settings.
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle;
    accelerationsOfVehicle[ "Earth" ].push_back(
                std::make_shared< AccelerationSettings >( basic_astrodynamics::point_mass_gravity ) );
    accelerationsOfVehicle[ "Sun" ].push_back(
                std::make_shared< AccelerationSettings >( basic_astrodynamics::point_mass_gravity ) );

    std::map< double, Eigen::Vector3d > customAccelerationMap;
    double timeStep = 30.0;
    double currentTime = -120.0;
    while( currentTime < simulationEndEpoch + 120.0 )
    {
        customAccelerationMap[ currentTime ] = customAcceleration( currentTime );
        currentTime += timeStep;
    }
    std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Vector3d > > customAccelerationInterpolator =
            interpolators::createOneDimensionalInterpolator(
                customAccelerationMap, std::make_shared< interpolators::LagrangeInterpolatorSettings >( 8 ) );

    std::function< Eigen::Vector3d( const double ) > customAccelerationFunction =
            std::bind( static_cast< Eigen::Vector3d( interpolators::OneDimensionalInterpolator< double, Eigen::Vector3d >::* )
                       ( const double ) >( &interpolators::OneDimensionalInterpolator< double, Eigen::Vector3d >::interpolate ),
                       customAccelerationInterpolator, std::placeholders::_1 );

    std::function< double( const double) > customAccelerationScalingFunction =
            tudat::simulation_setup::getOccultationFunction(
                bodies, "Sun", "Earth", "Vehicle" );

    accelerationsOfVehicle[ "Earth" ].push_back(
                std::make_shared< CustomAccelerationSettings >(
                    customAccelerationFunction, customAccelerationScalingFunction ) );

    accelerationMap[ "Vehicle" ] = accelerationsOfVehicle;

    // Create acceleration models and propagation settings.
    basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                bodies, accelerationMap, bodiesToPropagate, centralBodies );

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////             CREATE PROPAGATION SETTINGS            ////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Set initial conditions for the Vehicle satellite that will be propagated in this simulation.
    // The initial conditions are given in Keplerian elements and later on converted to Cartesian
    // elements.

    // Set Keplerian elements for Vehicle.
    Eigen::Vector6d vehicleInitialStateInKeplerianElements;
    vehicleInitialStateInKeplerianElements( semiMajorAxisIndex ) = 7500.0E3;
    vehicleInitialStateInKeplerianElements( eccentricityIndex ) = 0.1;
    vehicleInitialStateInKeplerianElements( inclinationIndex ) = convertDegreesToRadians( 0.0 );
    vehicleInitialStateInKeplerianElements( argumentOfPeriapsisIndex ) =
            convertDegreesToRadians( 235.7 );
    vehicleInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex ) =
            convertDegreesToRadians( 23.4 );
    vehicleInitialStateInKeplerianElements( trueAnomalyIndex ) = convertDegreesToRadians( 139.87 );

    // Convert Vehicle state from Keplerian elements to Cartesian elements.
    double earthGravitationalParameter = bodies.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );
    Eigen::VectorXd systemInitialState = convertKeplerianToCartesianElements(
                vehicleInitialStateInKeplerianElements,
                earthGravitationalParameter );

    // Create propagator settings.
    // Set variables to save
    std::shared_ptr< DependentVariableSaveSettings > dependentVariableSaveSettings;
    std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables;
    dependentVariables.push_back(
                std::make_shared< SingleAccelerationDependentVariableSaveSettings >(
                    basic_astrodynamics::custom_acceleration, "Vehicle", "Earth", 0 ) );
    dependentVariableSaveSettings = std::make_shared< DependentVariableSaveSettings >( dependentVariables );
    std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings =
            std::make_shared< TranslationalStatePropagatorSettings< double > >
            ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, simulationEndEpoch,
              cowell, dependentVariableSaveSettings );

    // Create numerical integrator settings.
    double simulationStartEpoch = 0.0;
    const double fixedStepSize = 10.0;
    std::shared_ptr< IntegratorSettings< > > integratorSettings =
            std::make_shared< IntegratorSettings< > >
            ( rungeKutta4, simulationStartEpoch, fixedStepSize );

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////             PROPAGATE ORBIT            ////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Create simulation object and propagate dynamics.
    SingleArcDynamicsSimulator< > dynamicsSimulator( bodies, integratorSettings, propagatorSettings );
    std::map< double, Eigen::VectorXd > dependentVariableHistory = dynamicsSimulator.getDependentVariableHistory( );
    std::map< double, Eigen::VectorXd > stateHistory = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );

    double sourceBodyRadius = bodies.at( "Sun" )->getShapeModel( )->getAverageRadius( );
    double occultingBodyRadius = bodies.at( "Earth" )->getShapeModel( )->getAverageRadius( );

    for( auto it : dependentVariableHistory )
    {
        double shadowFunction =
                mission_geometry::computeShadowFunction(
                    bodies.at( "Sun" )->getStateInBaseFrameFromEphemeris( it.first ).segment( 0, 3 ), sourceBodyRadius,
                    bodies.at( "Earth" )->getStateInBaseFrameFromEphemeris( it.first ).segment( 0, 3 ), occultingBodyRadius,
                    stateHistory.at( it.first ).segment( 0, 3 ) +
                    bodies.at( "Earth" )->getStateInBaseFrameFromEphemeris( it.first ).segment( 0, 3 ) );

        Eigen::Vector3d expectedAcceleration = shadowFunction * customAcceleration( it.first );
        Eigen::Vector3d savedAcceleration = it.second;

//        std::cout<<savedAcceleration.transpose( )<<std::endl;
//        std::cout<<expectedAcceleration.transpose( )<<std::endl<<std::endl;

        BOOST_CHECK_SMALL( savedAcceleration( 0 ) - expectedAcceleration( 0 ), 1.0E-19 );
        BOOST_CHECK_SMALL( savedAcceleration( 1 ) - expectedAcceleration( 1 ), 1.0E-19 );
        BOOST_CHECK_SMALL( savedAcceleration( 2 ) - expectedAcceleration( 2 ), 1.0E-19 );
    }
}

Eigen::Vector3d customTorque( const double time )
{
    return ( Eigen::Vector3d( ) << 1.0E-6 * std::sin ( 2.0 * mathematical_constants::PI * time / 1.0E4 + 0.1 ),
             -2.0E-6 * std::sin ( 2.0 * mathematical_constants::PI * time / 3.0E4 + 0.5 ),
             -8.0E-7 * std::sin ( 2.0 * mathematical_constants::PI * time / 5.0E4 + 1.5 ) ).finished( );
}

BOOST_AUTO_TEST_CASE( test_customTorqueModelCreation )
{
    using namespace tudat::simulation_setup;
    using namespace tudat::propagators;
    using namespace tudat::numerical_integrators;
    using namespace tudat::orbital_element_conversions;
    using namespace tudat::basic_mathematics;
    using namespace tudat::basic_astrodynamics;
    using namespace tudat::unit_conversions;

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////     CREATE ENVIRONMENT AND VEHICLE       //////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Load Spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Set simulation end epoch.
    const double simulationEndEpoch = tudat::physical_constants::JULIAN_DAY;

    // Create body objects.
    std::vector< std::string > bodiesToCreate = { "Earth", "Sun" };
    BodyListSettings bodySettings = getDefaultBodySettings(
                bodiesToCreate, "Sun", "ECLIPJ2000" );
    SystemOfBodies bodies = createSystemOfBodies( bodySettings );
    bodies.createEmptyBody( "Vehicle" );
    Eigen::Matrix3d bodyInertiaTensor = 100.0 * Eigen::Matrix3d::Identity( );
    bodyInertiaTensor( 2, 2 ) *= 2.0;
    bodyInertiaTensor( 1, 1 ) *= 1.5;

    bodies.at( "Vehicle" )->setBodyInertiaTensor( bodyInertiaTensor );

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////            CREATE ACCELERATIONS          //////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Define propagator settings variables.
    SelectedAccelerationMap accelerationMap;
    std::vector< std::string > bodiesToPropagate;
    std::vector< std::string > centralBodies;

    bodiesToPropagate.push_back( "Vehicle" );
    centralBodies.push_back( "Earth" );

    // Define propagation settings.
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle;
    accelerationsOfVehicle[ "Earth" ].push_back(
                std::make_shared< AccelerationSettings >( basic_astrodynamics::point_mass_gravity ) );
    accelerationsOfVehicle[ "Sun" ].push_back(
                std::make_shared< AccelerationSettings >( basic_astrodynamics::point_mass_gravity ) );

    accelerationMap[ "Vehicle" ] = accelerationsOfVehicle;

    // Create acceleration models and propagation settings.
    basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                bodies, accelerationMap, bodiesToPropagate, centralBodies );

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////             CREATE PROPAGATION SETTINGS            ////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Set initial conditions for the Vehicle satellite that will be propagated in this simulation.
    // The initial conditions are given in Keplerian elements and later on converted to Cartesian
    // elements.

    // Set Keplerian elements for Vehicle.
    Eigen::Vector6d vehicleInitialStateInKeplerianElements;
    vehicleInitialStateInKeplerianElements( semiMajorAxisIndex ) = 7500.0E3;
    vehicleInitialStateInKeplerianElements( eccentricityIndex ) = 0.1;
    vehicleInitialStateInKeplerianElements( inclinationIndex ) = convertDegreesToRadians( 0.0 );
    vehicleInitialStateInKeplerianElements( argumentOfPeriapsisIndex ) =
            convertDegreesToRadians( 235.7 );
    vehicleInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex ) =
            convertDegreesToRadians( 23.4 );
    vehicleInitialStateInKeplerianElements( trueAnomalyIndex ) = convertDegreesToRadians( 139.87 );

    // Convert Vehicle state from Keplerian elements to Cartesian elements.
    double earthGravitationalParameter = bodies.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );
    Eigen::VectorXd systemInitialTranslationalState = convertKeplerianToCartesianElements(
                vehicleInitialStateInKeplerianElements,
                earthGravitationalParameter );

    // Create propagator settings.
    std::shared_ptr< TranslationalStatePropagatorSettings< double > > translationalPropagatorSettings =
            std::make_shared< TranslationalStatePropagatorSettings< double > >
            ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialTranslationalState, simulationEndEpoch );

    Eigen::VectorXd systemInitialRotationalState = Eigen::VectorXd::Zero( 7 );
    systemInitialRotationalState.segment( 0, 4 ) = linear_algebra::convertQuaternionToVectorFormat(
                Eigen::Quaterniond( Eigen::Matrix3d::Identity( ) ) );
    systemInitialRotationalState.segment( 4, 3 ) = Eigen::Vector3d::Constant( 1.0E-5 );

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

    std::map< double, Eigen::Vector3d > customTorqueMap;
    double timeStep = 30.0;
    double currentTime = -120.0;
    while( currentTime < simulationEndEpoch + 120.0 )
    {
        customTorqueMap[ currentTime ] = customTorque( currentTime );
        currentTime += timeStep;
    }
    std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::Vector3d > > customTorqueInterpolator =
            interpolators::createOneDimensionalInterpolator(
                customTorqueMap, std::make_shared< interpolators::LagrangeInterpolatorSettings >( 8 ) );

    std::function< Eigen::Vector3d( const double ) > customTorqueFunction =
            std::bind( static_cast< Eigen::Vector3d( interpolators::OneDimensionalInterpolator< double, Eigen::Vector3d >::* )
                       ( const double ) >( &interpolators::OneDimensionalInterpolator< double, Eigen::Vector3d >::interpolate ),
                       customTorqueInterpolator, std::placeholders::_1 );

    std::function< double( const double) > customTorqueScalingFunction =
            tudat::simulation_setup::getOccultationFunction(
                bodies, "Sun", "Earth", "Vehicle" );

    torqueMap[ "Vehicle" ][ "Earth" ].push_back( std::make_shared< CustomTorqueSettings >(
                                                     customTorqueFunction, customTorqueScalingFunction ) );

    basic_astrodynamics::TorqueModelMap torqueModelMap = createTorqueModelsMap(
                bodies, torqueMap, bodiesToPropagate );

    // Define propagator settings.
    std::shared_ptr< RotationalStatePropagatorSettings< double > > rotationalPropagatorSettings =
            std::make_shared< RotationalStatePropagatorSettings< double > >
            ( torqueModelMap, bodiesToPropagate, systemInitialRotationalState, std::make_shared< PropagationTimeTerminationSettings >(
                  simulationEndEpoch ) );

    // Set variables to save
    std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables;
    dependentVariables.push_back(
                std::make_shared< SingleTorqueDependentVariableSaveSettings >(
                    custom_torque, "Vehicle", "Earth" ) );
    std::shared_ptr< DependentVariableSaveSettings > dependentVariablesToSave =
            std::make_shared< DependentVariableSaveSettings >( dependentVariables );

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

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

    // Create numerical integrator settings.
    double simulationStartEpoch = 0.0;
    const double fixedStepSize = 10.0;
    std::shared_ptr< IntegratorSettings< > > integratorSettings =
            std::make_shared< IntegratorSettings< > >
            ( rungeKutta4, simulationStartEpoch, fixedStepSize );

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////             PROPAGATE ORBIT            ////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Create simulation object and propagate dynamics.
    SingleArcDynamicsSimulator< > dynamicsSimulator( bodies, integratorSettings, propagatorSettings );
    std::map< double, Eigen::VectorXd > dependentVariableHistory = dynamicsSimulator.getDependentVariableHistory( );
    std::map< double, Eigen::VectorXd > stateHistory = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );

    double sourceBodyRadius = bodies.at( "Sun" )->getShapeModel( )->getAverageRadius( );
    double occultingBodyRadius = bodies.at( "Earth" )->getShapeModel( )->getAverageRadius( );

    for( auto it : dependentVariableHistory )
    {
        double shadowFunction =
                mission_geometry::computeShadowFunction(
                    bodies.at( "Sun" )->getStateInBaseFrameFromEphemeris( it.first ).segment( 0, 3 ), sourceBodyRadius,
                    bodies.at( "Earth" )->getStateInBaseFrameFromEphemeris( it.first ).segment( 0, 3 ), occultingBodyRadius,
                    stateHistory.at( it.first ).segment( 0, 3 ) +
                    bodies.at( "Earth" )->getStateInBaseFrameFromEphemeris( it.first ).segment( 0, 3 ) );

        Eigen::Vector3d expectedTorque = shadowFunction * customTorque( it.first );
        Eigen::Vector3d savedTorque = it.second;
        BOOST_CHECK_SMALL( savedTorque( 0 ) - expectedTorque( 0 ), 1.0E-19 );
        BOOST_CHECK_SMALL( savedTorque( 1 ) - expectedTorque( 1 ), 1.0E-19 );
        BOOST_CHECK_SMALL( savedTorque( 2 ) - expectedTorque( 2 ), 1.0E-19 );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include <stan/math/rev/core.hpp>
#include <stan/math/torsten/pmx_onecpt_model.hpp>
#include <stan/math/torsten/pmx_twocpt_model.hpp>
#include <stan/math/torsten/ev_manager.hpp>
#include <stan/math/torsten/test/unit/pmx_onecpt_test_fixture.hpp>
#include <stan/math/torsten/test/unit/pmx_twocpt_test_fixture.hpp>
#include <stan/math/torsten/test/unit/pmx_twocpt_mpi_test_fixture.hpp>
#include <stan/math/torsten/test/unit/test_macros.hpp>
#include <Eigen/Dense>
#include <Eigen/src/Core/NumTraits.h>
#include <gtest/gtest.h>
#include <vector>

using stan::math::var;
using torsten::EventsManager;
using torsten::NONMENEventsRecord;
using torsten::NonEventParameters;

TEST_F(TorstenOneCptTest, lag_time) {
  // 2 subjects
  nt = 10;
  resize(nt);

  // two subjects
  std::vector<int> len{6, 4};
  evid = {0, 1, 1, 1, 0, 1, 0, 1, 1, 1};
  cmt = std::vector<int>(nt, 1);
  cmt[3] = 2;
  ii = std::vector<double>(nt, 0.0);
  addl = std::vector<int>(nt, 0);
  time = {0, 12, 24, 24, 48, 36, 0, 6, 18, 30};
  ss = std::vector<int>(nt, 0);
  rate = std::vector<double>(nt, 0.0);
  amt = std::vector<double>{0, 1100.0, 1100.0, 900.0, 0.0, 1100.0,
    0, 800.0, 800.0, 800.0};

  tlag.resize(nt);
  for (auto& lag : tlag) {
    lag = std::vector<double>{0.0, 0.0};
  }

  tlag[2] = std::vector<double>{6.0, 6.0};
  tlag[8] = std::vector<double>{6.0, 6.0};

  const NONMENEventsRecord<double, double, double, double>
    events_rec(nCmt, len, time, amt, rate, ii, evid, cmt, addl, ss);

  using EM = EventsManager<NONMENEventsRecord<double, double, double, double>,
                           NonEventParameters<double, double, std::vector, std::tuple<double, double> >>;

  // subject 1
  EM em1(0, events_rec, pMatrix, biovar, tlag);
  auto ev1 = em1.events();

  // subject 2
  EM em2(1, events_rec, pMatrix, biovar, tlag);
  auto ev2 = em2.events();

  EXPECT_EQ(ev1.size(), len[0] + 1);
  EXPECT_EQ(ev2.size(), len[1] + 1);

  EXPECT_FLOAT_EQ(ev1.time(2), 24.0);
  EXPECT_FLOAT_EQ(ev1.time(3), 24.0);
  EXPECT_FLOAT_EQ(ev1.time(4), 30.0);
  EXPECT_EQ(ev1.evid(2), 9);
  EXPECT_EQ(ev1.evid(4), 1);
  EXPECT_EQ(ev1.keep(4), 0);

  EXPECT_FLOAT_EQ(ev2.time(2), 18.0);
  EXPECT_FLOAT_EQ(ev2.time(3), 24.0);
  EXPECT_EQ(ev2.evid(2), 9);
  EXPECT_EQ(ev2.evid(3), 1);
  EXPECT_EQ(ev2.keep(3), 0);

  // std::cout << "taki test: " << events_rec.has_positive_param(0, tlag) << "\n";
  int nev1 = EM::num_events(0, events_rec, pMatrix, biovar, tlag);
  int nev2 = EM::num_events(1, events_rec, pMatrix, biovar, tlag);
  EXPECT_EQ(nev1, len[0] + 1);
  EXPECT_EQ(nev2, len[1] + 1);
}


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

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

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/integral.hpp>

#include <tuple>
using namespace boost::hana;

// We need the comparison to be compile-time.
template <int i>
constexpr auto x = int_<i>;

BOOST_HANA_CONSTEXPR_LAMBDA auto f = [](auto x) {
    return x + int_<1>;
};

BOOST_HANA_CONSTEXPR_LAMBDA auto g = [](auto x) {
    return x * int_<20>;
};

BOOST_HANA_CONSTANT_ASSERT(ap(singleton(f), singleton(x<0>)) == singleton(f(x<0>)));
BOOST_HANA_CONSTANT_ASSERT(ap(singleton(f), doubleton(x<0>, x<1>)) == doubleton(f(x<0>), f(x<1>)));

BOOST_HANA_CONSTANT_ASSERT(ap(doubleton(f, g), singleton(x<0>)) == doubleton(f(x<0>), g(x<0>)));
BOOST_HANA_CONSTANT_ASSERT(ap(doubleton(f, g), doubleton(x<0>, x<1>)) == union_(doubleton(f(x<0>), f(x<1>)), doubleton(g(x<0>), g(x<1>))));

int main() {
    (void)f;
    (void)g;
}


#include <boost/simd/sdk/simd/pack.hpp>
#include <boost/simd/sdk/simd/pack/io.hpp>
#include <boost/simd/include/functions/tofloat.hpp>
#include <boost/simd/include/functions/plus.hpp>
#include <boost/simd/include/functions/multiplies.hpp>
#include <boost/simd/include/functions/sum.hpp>
#include <boost/simd/include/functions/load.hpp>
#include <boost/simd/include/functions/store.hpp>
#include <boost/simd/include/functions/splat.hpp>
#include <boost/simd/include/functions/make.hpp>
#include <nt2/include/functions/sincos.hpp>
#include <boost/simd/include/constants/ten.hpp>
#include <boost/fusion/include/deref.hpp>
#include <boost/fusion/include/begin.hpp>
#include <iostream>
#include <nt2/sdk/meta/type_id.hpp>

int main()
{
  int array[4] = {1, 2, 3, 4};

  boost::simd::pack<float, 4> r;
  boost::simd::pack<int, 4>* v = reinterpret_cast<boost::simd::pack<int, 4>*>(array);

  // testing element-wise operations, reductions, display
  r = nt2::tofloat(*v);
  std::cout << r << "\n";
  std::cout << (r + r) << "\n";
  float sum = boost::simd::sum(r + r);
  std::cout << sum << "\n";

  typedef boost::simd::native<float, boost::simd::tag::sse_> native;
  typedef boost::simd::pack<float, 4> pack;
  typedef boost::simd::pack<int, 4> packi;

  // testing functions which return a tuple (compile but doesn't do the right thing)
  boost::fusion::vector2<native, native> t = nt2::sincos(evaluate(r));
  std::cout << t << std::endl;

  // testing variant that modifies its argument
  pack s, c;
  c = nt2::sincos(r, s);
  std::cout << s << " " << c << std::endl;

  // testing constants
  pack r2;
  r2 = nt2::Ten<pack>();
  r = nt2::tofloat(*v+2)*nt2::Ten<pack>();
  std::cout << r2 << "\n";
  std::cout << r << "\n";

   // testing load, store, splat and make
   packi p;
   p = boost::simd::load<packi>(&array[0], 0);
   std::cout << p << "\n";
   p = p+1;
   boost::simd::store(p, &array[0], 0);
   std::cout << "{ " << array[0] << ", " << array[1] << ", " << array[2] << ", " << array[3] << " }\n";

   p = boost::simd::splat<packi>(0);
   std::cout << p << "\n";

   p = boost::simd::make<packi>(1, 2, 3, 4);
   std::cout << p << "\n";

   // testing cardinal
   std::cout << boost::simd::meta::cardinal_of<pack>::value << "\n";

   // Testing iterators for native mode
   packi p_it = boost::simd::make<packi>(1, 2, 3, 4);
   packi::iterator it = p_it.begin();
   packi::const_iterator it_ = p_it.begin();
   packi::iterator it__ = p_it.end();
   packi::const_iterator it___ = p_it.end();
   for(;it!=it__;++it) std::cout << *it << " " ;
   std::cout << "\n";
   for(;it_!=it___;++it_) std::cout << *it_ << " " ;
   std::cout << "\n";
   std::cout << "is_empty : " << p_it.empty() << "\n";
   std::cout << "size     : " << p_it.size() << "\n";
   // Testing Fusion interface
   std::cout << boost::fusion::deref(boost::fusion::begin(p_it)) << "\n";
   packi copy = p_it;
   std::cout << copy << std::endl;

   // Testing iterators for emulate mode
//   typedef boost::simd::pack<int,8> emulate;
//   emulate p_emu = boost::simd::make<emulate>(1, 2, 3, 4, 5, 6, 7, 8);
//   emulate::iterator ite = p_emu.begin();
//   emulate::const_iterator ite_ = p_emu.begin();
//   emulate::iterator ite__ = p_emu.end();
//   emulate::const_iterator ite___ = p_emu.end();
//   for(;ite!=ite__;++ite) std::cout << *ite << " " ;
//   std::cout << "\n";
//   for(;ite_!=ite___;++ite_) std::cout << *ite_ << " " ;
//   std::cout << "\n";
//   std::cout << "is_empty : " << p_emu.empty() << "\n";
//   std::cout << "size     : " << p_emu.size() << "\n";

}


//==================================================================================================
/*!
  @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_IFNOT_PLUS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IFNOT_PLUS_HPP_INCLUDED

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

  /*!
    @ingroup group-boolean
    This function object conditionally returns  @c x (respectively @c x+y)
    if @c c is @ref True (respectively  @ref False)

    @par Header <boost/simd/function/ifnot_plus.hpp>

    @par Example:

      @snippet ifnot_plus.cpp ifnot_plus

    @par Possible output:

      @snippet ifnot_plus.txt ifnot_plus

  **/
  Value1 ifnot_plus(Value0 const& c, Value1 const& x, Value1 const& y);
} }
#endif

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

#endif