/**
 * @file linear_svm_test.cpp
 *
 * Test the Linear SVM class.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <ensmallen.hpp>
#include <ensmallen_bits/callbacks/callbacks.hpp>
#include <mlpack/core.hpp>
#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/rnn.hpp>
#include <mlpack/methods/ann/rbm/rbm.hpp>
#include <mlpack/methods/ann/loss_functions/mean_squared_error.hpp>
#include <mlpack/methods/logistic_regression/logistic_regression.hpp>
#include <mlpack/methods/lmnn/lmnn.hpp>
#include <mlpack/methods/nca/nca.hpp>
#include <mlpack/core/metrics/lmetric.hpp>
#include <mlpack/methods/softmax_regression/softmax_regression.hpp>
#include <mlpack/methods/softmax_regression/softmax_regression_impl.hpp>
#include <mlpack/methods/ann/init_rules/gaussian_init.hpp>
#include <boost/test/unit_test.hpp>

using namespace mlpack;
using namespace mlpack::ann;
using namespace mlpack::regression;
using namespace mlpack::lmnn;
using namespace mlpack::metric;
using namespace mlpack::nca;
using namespace mlpack::distribution;

BOOST_AUTO_TEST_SUITE(CallbackTest);

/**
 * Test a FFN model with PrintLoss callback.
 */
BOOST_AUTO_TEST_CASE(FFNCallbackTest)
{
  arma::mat data;
  arma::mat labels;

  data::Load("lab1.csv", data, true);
  data::Load("lab3.csv", labels, true);

  FFN<MeanSquaredError<>, RandomInitialization> model;

  model.Add<Linear<>>(1, 2);
  model.Add<SigmoidLayer<>>();
  model.Add<Linear<>>(2, 1);
  model.Add<SigmoidLayer<>>();

  std::stringstream stream;
  model.Train(data, labels, ens::PrintLoss(stream));

  BOOST_REQUIRE_GT(stream.str().length(), 0);
}

/**
 * Test a FFN model with PrintLoss callback and optimizer parameter.
 */
BOOST_AUTO_TEST_CASE(FFNWithOptimizerCallbackTest)
{
  arma::mat data;
  arma::mat labels;

  data::Load("lab1.csv", data, true);
  data::Load("lab3.csv", labels, true);

  FFN<MeanSquaredError<>, RandomInitialization> model;

  model.Add<Linear<>>(1, 2);
  model.Add<SigmoidLayer<>>();
  model.Add<Linear<>>(2, 1);
  model.Add<SigmoidLayer<>>();

  std::stringstream stream;
  ens::StandardSGD opt(0.1, 1, 5);
  model.Train(data, labels, opt, ens::PrintLoss(stream));

  BOOST_REQUIRE_GT(stream.str().length(), 0);
}

/**
 * Test a RNN model with PrintLoss callback.
 */
BOOST_AUTO_TEST_CASE(RNNCallbackTest)
{
  const size_t rho = 5;
  arma::cube input = arma::randu(1, 1, 5);
  arma::cube target = arma::ones(1, 1, 5);
  RandomInitialization init(0.5, 0.5);

  // Create model with user defined rho parameter.
  RNN<NegativeLogLikelihood<>, RandomInitialization> model(
      rho, false, NegativeLogLikelihood<>(), init);
  model.Add<IdentityLayer<>>();
  model.Add<Linear<>>(1, 10);

  // Use LSTM layer with rho.
  model.Add<LSTM<>>(10, 3, rho);
  model.Add<LogSoftMax<>>();

  std::stringstream stream;
  model.Train(input, target, ens::PrintLoss(stream));

  BOOST_REQUIRE_GT(stream.str().length(), 0);
}

/**
 * Test a RNN model with PrintLoss callback and optimizer parameter.
 */
BOOST_AUTO_TEST_CASE(RNNWithOptimizerCallbackTest)
{
  const size_t rho = 5;
  arma::cube input = arma::randu(1, 1, 5);
  arma::cube target = arma::ones(1, 1, 5);
  RandomInitialization init(0.5, 0.5);

  // Create model with user defined rho parameter.
  RNN<NegativeLogLikelihood<>, RandomInitialization> model(
      rho, false, NegativeLogLikelihood<>(), init);
  model.Add<IdentityLayer<>>();
  model.Add<Linear<>>(1, 10);

  // Use LSTM layer with rho.
  model.Add<LSTM<>>(10, 3, rho);
  model.Add<LogSoftMax<>>();

  std::stringstream stream;
  ens::StandardSGD opt(0.1, 1, 5);
  model.Train(input, target, opt, ens::PrintLoss(stream));

  BOOST_REQUIRE_GT(stream.str().length(), 0);
}

/**
 * Test Logistic regression implementation with PrintLoss callback.
 */
BOOST_AUTO_TEST_CASE(LRWithOptimizerCallback)
{
  arma::mat data("1 2 3;"
                 "1 2 3");
  arma::Row<size_t> responses("1 1 0");

  ens::StandardSGD sgd(0.1, 1, 5);
  LogisticRegression<> logisticRegression(data, responses, sgd, 0.001);
  std::stringstream stream;
  logisticRegression.Train<ens::StandardSGD>(data, responses, sgd,
                                             ens::PrintLoss(stream));

  BOOST_REQUIRE_GT(stream.str().length(), 0);
}

/**
 * Test LMNN implementation with ProgressBar callback.
 */
BOOST_AUTO_TEST_CASE(LMNNWithOptimizerCallback)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat dataset = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                      " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  LMNN<> lmnn(dataset, labels, 1);

  arma::mat outputMatrix;
  std::stringstream stream;

  lmnn.LearnDistance(outputMatrix, ens::ProgressBar(70, stream));
  BOOST_REQUIRE_GT(stream.str().length(), 0);
}

/**
 * Test NCA implementation with ProgressBar callback.
 */
BOOST_AUTO_TEST_CASE(NCAWithOptimizerCallback)
{
  // Useful but simple dataset with six points and two classes.
  arma::mat data = "-0.1 -0.1 -0.1  0.1  0.1  0.1;"
                   " 1.0  0.0 -1.0  1.0  0.0 -1.0 ";
  arma::Row<size_t> labels = " 0    0    0    1    1    1   ";

  NCA<SquaredEuclideanDistance> nca(data, labels);

  arma::mat outputMatrix;
  std::stringstream stream;

  nca.LearnDistance(outputMatrix, ens::ProgressBar(70, stream));
  BOOST_REQUIRE_GT(stream.str().length(), 0);
}

/**
 * Test softmax_regression implementation with PrintLoss callback.
 */
BOOST_AUTO_TEST_CASE(SRWithOptimizerCallback)
{
  const size_t points = 1000;
  const size_t inputSize = 3;
  const size_t numClasses = 3;
  const double lambda = 0.5;

  // Generate two-Gaussian dataset.
  GaussianDistribution g1(arma::vec("1.0 9.0 1.0"), arma::eye<arma::mat>(3, 3));
  GaussianDistribution g2(arma::vec("4.0 3.0 4.0"), arma::eye<arma::mat>(3, 3));

  arma::mat data(inputSize, points);
  arma::Row<size_t> labels(points);

  for (size_t i = 0; i < points / 2; i++)
  {
    data.col(i) = g1.Random();
    labels(i) = 0;
  }
  for (size_t i = points / 2; i < points; i++)
  {
    data.col(i) = g2.Random();
    labels(i) = 1;
  }
  ens::StandardSGD sgd(0.1, 1, 5);
  std::stringstream stream;
  // Train softmax regression object.
  SoftmaxRegression sr(data, labels, numClasses, lambda);
  sr.Train(data, labels, numClasses, sgd, ens::ProgressBar(70, stream));

  BOOST_REQUIRE_GT(stream.str().length(), 0);
}

/*
 * Tests the RBM Implementation with PrintLoss callback.
 */
BOOST_AUTO_TEST_CASE(RBMCallbackTest)
{
  // Normalised dataset.
  int hiddenLayerSize = 10;
  size_t batchSize = 10;
  arma::mat trainData, testData, dataset;
  arma::mat trainLabelsTemp, testLabelsTemp;
  trainData.load("digits_train.arm");

  GaussianInitialization gaussian(0, 0.1);
  RBM<GaussianInitialization> model(trainData,
                                    gaussian,
                                    trainData.n_rows,
                                    hiddenLayerSize,
                                    batchSize);

  size_t numRBMIterations = 10;
  ens::StandardSGD msgd(0.03, batchSize, numRBMIterations, 0, true);
  std::stringstream stream;

  // Call the train function with printloss callback.
  double objVal = model.Train(msgd, ens::ProgressBar(70, stream));
  BOOST_REQUIRE(!std::isnan(objVal));
  BOOST_REQUIRE_GT(stream.str().length(), 0);
}

BOOST_AUTO_TEST_SUITE_END();


// Copyright 2021 Tier IV, Inc.
//
// 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 MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_
#define MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_

#include "motion_velocity_smoother/smoother/smoother_base.hpp"

#include <autoware_utils/geometry/geometry.hpp>
#include <autoware_utils/trajectory/trajectory.hpp>
#include <osqp_interface/osqp_interface.hpp>

#include <autoware_planning_msgs/msg/trajectory.hpp>

#include <boost/optional.hpp>

#include <vector>

namespace motion_velocity_smoother
{
using autoware_planning_msgs::msg::Trajectory;

class JerkFilteredSmoother : public SmootherBase
{
public:
  struct Param
  {
    double jerk_weight;
    double over_v_weight;
    double over_a_weight;
    double over_j_weight;
    double jerk_filter_ds;
  };

  explicit JerkFilteredSmoother(const Param & p);

  bool apply(
    const double initial_vel, const double initial_acc, const Trajectory & input,
    Trajectory & output, std::vector<Trajectory> & debug_trajectories) override;

  boost::optional<Trajectory> resampleTrajectory(
    const Trajectory & input, const double v_current, const int closest_id) const override;

  void setParam(const Param & param);

private:
  Param smoother_param_;
  osqp::OSQPInterface qp_solver_;
  rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("jerk_filtered_smoother")};

  Trajectory forwardJerkFilter(
    const double v0, const double a0, const double a_max, const double a_stop, const double j_max,
    const Trajectory & input) const;
  Trajectory backwardJerkFilter(
    const double v0, const double a0, const double a_min, const double a_stop, const double j_min,
    const Trajectory & input) const;
  Trajectory mergeFilteredTrajectory(
    const double v0, const double a0, const double a_min, const double j_min,
    const Trajectory & forward_filtered, const Trajectory & backward_filtered) const;
};
}  // namespace motion_velocity_smoother

#endif  // MOTION_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_


#include <fstream>

#include <boost/format.hpp>

#include "local_push_yche.h"

using boost::format;

double cal_rmax(double c, double epsilon) {
    return (1 - c) * epsilon;
}

double cal_rmax(GraphYche &g, double c, double epsilon, double delta) {
    // calculate r_max
    int m, n;
    m = g.m;
    n = g.n;
    double d = double(m) / double(n);
    double a = (1 - c) * pow(d, 2) * pow(epsilon, 2) / (c * log(2 / delta));
    return pow(a, 1.0 / 3.0);
}

void LocalPush::push(NodePair &pab, double inc) {
    // the actually action of push
    n_push++;
    R[pab] += inc;
    if (fabs(R[pab]) > r_max) {
        // Q.insert(pab);
        if (marker[pab] == false) {
            Q.push(pab);
            marker[pab] = true;
        }
    }
}

LocalPush::LocalPush(GraphYche &g, string gName_, double c_, double epsilon_, size_t n_) {
    // init data members
    g_name = gName_;
    c = c_;
    // r_max = r_max_;
    epsilon = epsilon_;
    r_max = cal_rmax(c, epsilon);
    n = n_;
    string path = get_edge_list_path(gName_);
    n_push = 0;
    cpu_time = -1; // set the init value
}

Full_LocalPush::Full_LocalPush(GraphYche &g, string name, double c_, double r_max_, size_t n_) :
        LocalPush(g, name, c_, r_max_, n_) {
    string data_path = get_file_path_base() + ".P";
    cout << "data path " << data_path << endl;
//    if (file_exists(data_path)) { // local push from start
//        cout << "file exisis.." << endl;
//        load();
//    } else { // file exists
    if (!file_exists(data_path)) {
        /* init the P and R */
        P.add(n);
        R.add(n);
        marker.add(n);

        cout << "file not exists, compute from scratch" << endl;
        for (int i = 0; i < n; i++) {
            NodePair np(i, i);
            // R.insert({np,1});
            R[np] = 1;
            Q.push(np);
            marker[np] = true;
        }
    }
}

Reduced_LocalPush::Reduced_LocalPush(GraphYche &g, string name, double c_, double r_max_, size_t n_) :
        LocalPush(g, name, c_, r_max_, n_) {
    string data_path = get_file_path_base() + ".P";
//    if (file_exists(data_path)) { // local push from start
//        load();
//    } else { // file exists
    if (!file_exists(data_path)) {
        /* init the P and R */
        P.add(n);
        R.add(n);
        marker.add(n);

        cout << "file not exists, compute from scratch!!!" << endl;
        for (int i = 0; i < n; i++) {
            NodePair np(i, i);
            // R.insert({np,1});
            R[np] = 1;
            Q.push(np);
            marker[np] = true;
        }
    }
}

void Reduced_LocalPush::push_to_neighbors(GraphYche &g, NodePair &np, double current_residual) {
    // the push method using reduced linear system
    // out-neighbors of a,b
    auto a = np.first;
    auto b = np.second;
    bool is_singleton = (a == b);

    // /* only push to partial pairs*/
    auto out_degree_i = static_cast<size_t>(g.out_degree(a));
    auto out_degree_j = static_cast<size_t>(g.out_degree(b));

    /*the indicator whether the position is common neighbor */
    vector<bool> outs_i_common(out_degree_i, false);
    vector<bool> outs_j_common(out_degree_j, false);

    if (is_singleton) {
        /* starting push for singleton nodes*/
        for (auto off_out_a = g.off_out[a]; off_out_a < g.off_out[a + 1]; off_out_a++) {
            auto out_nei_a = g.neighbors_out[off_out_a];
            for (auto off_out_b = g.off_out[b]; off_out_b < g.off_out[b + 1]; off_out_b++) {
                auto out_nei_b = g.neighbors_out[off_out_b];
                auto in_degree_a = g.in_degree(out_nei_a);
                auto in_degree_b = g.in_degree(out_nei_b);
                auto total_in = in_degree_a * in_degree_b;
                if (out_nei_a < out_nei_b) { // only push to partial pairs for a < b
                    NodePair pab(out_nei_a, out_nei_b); // the node-pair to be pushed to
                    double inc = c * current_residual / total_in;
                    push(pab, inc); // do the push action
                }
            }
        }
    } else {
        /* mark the common neighbors */
        const auto off_beg_a = g.off_out[a];
        const auto off_beg_b = g.off_out[b];
        for (auto off_a = off_beg_a; off_a < g.off_out[a + 1]; off_a++) {
            auto out_nei_a = g.neighbors_out[off_a];
            for (auto off_b = off_beg_b; off_b < g.off_out[b + 1]; off_b++) {
                auto out_nei_b = g.neighbors_out[off_b];
                if (out_nei_a == out_nei_b) {
                    outs_i_common[off_a - off_beg_a] = true;
                    outs_j_common[off_b - off_beg_b] = true;
                    break;
                }
            }
        }

        /* starting push for non-singleton nodes*/
        for (auto off_a = off_beg_a; off_a < g.off_out[a + 1]; off_a++) {
            auto is_i_common = outs_i_common[off_a - off_beg_a];

            for (auto off_b = off_beg_b; off_b < g.off_out[b + 1]; off_b++) {
                auto is_j_common = outs_j_common[off_b - off_beg_b];
                // put out_nei_a here, since swap happens later
                auto out_nei_a = g.neighbors_out[off_a];
                auto out_nei_b = g.neighbors_out[off_b];

                auto in_degree_a = g.in_degree(out_nei_a);
                auto in_degree_b = g.in_degree(out_nei_b);
                auto total_in = in_degree_a * in_degree_b;
                double inc = c * current_residual / total_in;
                if (out_nei_a == out_nei_b) { //don't push to singleton nodes
                    continue;
                }
                bool oa_less_ob = (out_nei_a < out_nei_b);
                if (!oa_less_ob) {
                    swap(out_nei_a, out_nei_b);
                }

                NodePair pab(out_nei_a, out_nei_b);
                if (!is_i_common) {
                    // i is not common neighbor
                    push(pab, inc);
                } else {
                    // i is a common neighbor
                    if (is_j_common) {
                        if (oa_less_ob) {
                            push(pab, 2 * inc); // push twice for two commons
                        }
                    } else {// normal case
                        push(pab, inc);
                    }
                }
            }
        }
    }
}

void Full_LocalPush::push_to_neighbors(GraphYche &g, NodePair &np, double current_residual) {
    auto a = np.first;
    auto b = np.second;
    for (auto off_a = g.off_out[a]; off_a < g.off_out[a + 1]; off_a++) {
        auto out_nei_a = g.neighbors_out[off_a];
        for (auto off_b = g.off_out[b]; off_b < g.off_out[b + 1]; off_b++) {
            auto out_nei_b = g.neighbors_out[off_b];
            if (out_nei_a == out_nei_b) {
                continue;
            }
            auto in_degree_a = g.in_degree(out_nei_a);
            auto in_degree_b = g.in_degree(out_nei_b);
            auto total_in = in_degree_a * in_degree_b;
            NodePair pab(out_nei_a, out_nei_b);
            double inc = c * current_residual / total_in;
            push(pab, inc);
        }
    }
}

double Full_LocalPush::how_much_residual_to_push(GraphYche &g, NodePair &np) {
    return R[np];
}

double Reduced_LocalPush::how_much_residual_to_push(GraphYche &g, NodePair &np) {
    // determine the residual value for current pair to push
    double r = R[np];
    if (r > 1.01) {
        cout << "err" << endl;
        exit(0);
    }
    if (np.first == np.second) { //singleton node
        return r - r_max / (1 - c); // singleton nodes do not need to push all residual as 1
        // return r;
        // return 1;
    }
    /* check whether np forms a self-loop */
    if (g.exists_edge(np.first, np.second) &&
        g.exists_edge(np.second, np.first)) { // check whether exists reverse edge
        auto in_deg_a = g.in_degree(np.first);
        auto in_deg_b = g.in_degree(np.second);
        double alpha = c / (in_deg_a * in_deg_b);
        int k = ceil(log(r_max / fabs(r)) / log(alpha));
        double residual_to_push = (1 - pow(alpha, k)) * r / (1 - alpha);
        return residual_to_push;
    } else {
        auto push_residual = r;
        return push_residual;
    }
}

void LocalPush::local_push(GraphYche &g) { // local push given current P and R
    // cout << r_max << endl;
    auto start = std::chrono::high_resolution_clock::now();
    double sum_of_est = 0;

    while (!Q.empty()) {
        NodePair np = Q.front();
        Q.pop();
        marker[np] = false;
        double residual_to_push = how_much_residual_to_push(g, np);
        sum_of_est += residual_to_push;
//        cout << residual_to_push << endl;

        // R.erase(np); //  remove from residual
        R[np] -= residual_to_push;
        P[np] += residual_to_push;
        push_to_neighbors(g, np, residual_to_push); // push residuals to neighbros of np

    }
    auto finish = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed = finish - start;
    if (cpu_time == -1) {
        cpu_time = elapsed.count();
        mem_size = getValue();
    }
}

void LocalPush::save() {
    // save data to disk
    // save P
    string p_path = get_file_path_base() + ".P";
    P.save(p_path);

    //save R
    string r_path = get_file_path_base() + ".R";
    R.save(r_path);

    ofstream out;
    // // save meta info
    string meta_path = get_file_path_base() + ".meta";
    out.open(meta_path);
    out << g_name << endl;
    out << n << endl;
    out << epsilon << endl;
    out << c << endl;
    out << cpu_time << endl;
    out << mem_size << endl;
    auto p_size = P.size();
    auto r_size = R.size();
    out << p_size << endl;
    out << r_size << endl;
    out << double(p_size) / (n * n) << endl;
    out << double(r_size) / (n * n) << endl;
    out.close();

    // save exp data
    cout << "save complete" << endl;

}

void LocalPush::load() {
    // load existing local push data
    // load P

    string p_path = get_file_path_base() + ".P";
    P.load(p_path);

    //load R
    string r_path = get_file_path_base() + ".R";
    R.load(r_path);

    string meta_path = get_file_path_base() + ".meta";
    ifstream in;
    in.open(meta_path);
    in >> g_name >> n >> epsilon >> c >> cpu_time >> mem_size;
    r_max = cal_rmax(c, epsilon);
    marker.add(n); // initialize marker
    in.close();
}

string Reduced_LocalPush::get_file_path_base() {
    // return the file path, exluding the suffix
    return LOCAL_PUSH_DIR + str(format("RLP_%s-%.3f-%.6f") % g_name % c % epsilon);
}

double Reduced_LocalPush::query_P(int a, int b) {
    if (a > b) {
        return P.query(b, a);
    } else {
        return P.query(a, b);
    }
}

double Reduced_LocalPush::query_R(int a, int b) {
    if (a > b) {
        return R.query(b, a);
    } else {
        return R.query(a, b);
    }
}

string Full_LocalPush::get_file_path_base() {
    // return the file path, exluding the suffix
    return LOCAL_PUSH_DIR + str(format("FLP_%s-%.3f-%.6f") % g_name % c % epsilon);
}

double Full_LocalPush::query_P(int a, int b) {
    return P.query(a, b);
}

double Full_LocalPush::query_R(int a, int b) {
    return R.query(a, b);
}


// Boost.Function library examples

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

// For more information, see http://www.boost.org

#include <iostream>
#include <boost/function.hpp>

struct int_div {
  float operator()(int x, int y) const { return ((float)x)/y; };
};

int
main()
{
  boost::function<float (int, int)> f;
  f = int_div();

  std::cout << f(5, 3) << std::endl; // 1.66667

  return 0;
}


//
// Created by a.kiryanenko on 2020-02-04.
//

#include <libspu.hpp>
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(testDataContainerOperators)

    using namespace SPU;
    using namespace boost;

    BOOST_AUTO_TEST_CASE(test_compare_data_containers)
    {
        data_t a(3);
        data_t b(4);
        BOOST_CHECK(a < b);
        BOOST_CHECK(b > a);

        a = 5;
        b = 5;
        BOOST_CHECK(a == b);

        a[0] = 4;
        a[1] = 1;
        b[0] = 10;
        b[1] = 0;
        BOOST_CHECK(a > b);
    }

    BOOST_AUTO_TEST_CASE(test_addition_data_containers)
    {
        data_t a(3);
        data_t b(4);
        data_t c = a + b;
        BOOST_CHECK_EQUAL((int) c, 7);
    }

    BOOST_AUTO_TEST_CASE(test_overflow_addition_data_containers)
    {
        data_t a(0xFFFFFFFEu);
        data_t b(4);
        data_t c = a + b;
        BOOST_CHECK_EQUAL((int) c[0], 2);
        BOOST_CHECK_EQUAL((int) c[1], 1);
    }

    BOOST_AUTO_TEST_CASE(test_subtract_data_containers)
    {
        data_t a(7);
        data_t b(4);
        data_t c = a - b;
        BOOST_CHECK_EQUAL((int) c, 3);
    }

    BOOST_AUTO_TEST_CASE(test_overflow_subtract_data_containers)
    {
        data_t a(3);
        data_t b(5);
        data_t c = a - b;
        BOOST_CHECK_EQUAL((int) c[0], 0xFFFFFFFEu);
        BOOST_CHECK_EQUAL((int) c[1], 0xFFFFFFFFu);
    }

    BOOST_AUTO_TEST_CASE(test_increment_data_containers)
    {
        data_t a(3);
        a++;
        BOOST_CHECK_EQUAL((int) a, 4);
    }

    BOOST_AUTO_TEST_CASE(test_overflow_increment_data_containers)
    {
        data_t a(0xFFFFFFFFu);
        a++;
        BOOST_CHECK_EQUAL((int) a[0], 0);
        BOOST_CHECK_EQUAL((int) a[1], 1);
    }

    BOOST_AUTO_TEST_CASE(test_decrement_data_containers)
    {
        data_t a(3);
        a--;
        BOOST_CHECK_EQUAL((int) a, 2);
    }

    BOOST_AUTO_TEST_CASE(test_overflow_decrement_data_containers)
    {
        data_t a(0);
        a--;
        BOOST_CHECK_EQUAL((int) a[0], 0xFFFFFFFFu);
        BOOST_CHECK_EQUAL((int) a[1], 0xFFFFFFFFu);
    }

    BOOST_AUTO_TEST_CASE(test_shift_right)
    {
        data_t cont = 0x10;
        auto res = cont << (u8) 4;
        BOOST_CHECK_EQUAL(res[0], 0x100);
        BOOST_CHECK_EQUAL(res[1], 0);

        cont = 0x10;
        res = cont << (u8) 0;
        BOOST_CHECK_EQUAL(res[0], 0x10);
        BOOST_CHECK_EQUAL(res[1], 0);

        cont = 0x10000001;
        res = cont << (u8) 12;
        BOOST_CHECK_EQUAL(res[0], 0x1000);
        BOOST_CHECK_EQUAL(res[1], 0x100);

        cont = 0x1;
        res = cont << (u8) 36;
        BOOST_CHECK_EQUAL(res[0], 0);
        BOOST_CHECK_EQUAL(res[1], 0x10);

        cont = 0x1;
        res = cont << (u8) 32;
        BOOST_CHECK_EQUAL(res[0], 0);
        BOOST_CHECK_EQUAL(res[1], 0x1);

        cont = 0x1;
        res = cont << (u8) 100;
        BOOST_CHECK_EQUAL(res[0], 0);
        BOOST_CHECK_EQUAL(res[1], 0);
    }

    BOOST_AUTO_TEST_CASE(test_shift_left)
    {
        data_t cont = 0x10;
        auto res = cont >> (u8) 4;
        BOOST_CHECK_EQUAL(res[0], 0x1);
        BOOST_CHECK_EQUAL(res[1], 0);

        cont = 0x10;
        res = cont >> (u8) 0;
        BOOST_CHECK_EQUAL(res[0], 0x10);
        BOOST_CHECK_EQUAL(res[1], 0);

        cont[0] = 0x1000;
        cont[1] = 0x10001;
        res = cont >> (u8) 8;
        BOOST_CHECK_EQUAL(res[0], 0x1000010);
        BOOST_CHECK_EQUAL(res[1], 0x100);

        cont[0] = 0;
        cont[1] = 0x100;
        res = cont >> (u8) 36;
        BOOST_CHECK_EQUAL(res[0], 0x10);
        BOOST_CHECK_EQUAL(res[1], 0);

        cont[0] = 0;
        cont[1] = 0x1;
        res = cont >> (u8) 32;
        BOOST_CHECK_EQUAL(res[0], 0x1);
        BOOST_CHECK_EQUAL(res[1], 0);

        cont = 0x1234;
        res = cont >> (u8) 100;
        BOOST_CHECK_EQUAL(res[0], 0);
        BOOST_CHECK_EQUAL(res[1], 0);
    }

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright © 2020 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "ParserFlatbuffersFixture.hpp"
#include "../TfLiteParser.hpp"

#include <string>

BOOST_AUTO_TEST_SUITE(TensorflowLiteParser)

struct ExpFixture : public ParserFlatbuffersFixture
{
    explicit ExpFixture(const std::string & inputShape,
                                   const std::string & outputShape)
    {
        m_JsonString = R"(
            {
                "version": 3,
                "operator_codes": [ { "builtin_code": "EXP" } ],
                "subgraphs": [ {
                    "tensors": [
                        {
                            "shape": )" + inputShape + R"(,
                            "type": "FLOAT32",
                            "buffer": 0,
                            "name": "inputTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.0 ],
                                "zero_point": [ 0 ],
                            }
                        },
                        {
                            "shape": )" + outputShape + R"( ,
                            "type": "FLOAT32",
                            "buffer": 1,
                            "name": "outputTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.0 ],
                                "zero_point": [ 0 ],
                            }
                        }
                    ],
                    "inputs": [ 0 ],
                    "outputs": [ 1 ],
                    "operators": [
                        {
                            "opcode_index": 0,
                            "inputs": [ 0 ],
                            "outputs": [ 1 ],
                            "custom_options_format": "FLEXBUFFERS"
                        }
                    ],
                } ],
                "buffers" : [
                    { },
                    { }
                ]
            }
        )";
        Setup();
    }
};

struct SimpleExpFixture : public ExpFixture
{
    SimpleExpFixture() : ExpFixture("[ 1, 2, 3, 1 ]", "[ 1, 2, 3, 1 ]") {}
};

BOOST_FIXTURE_TEST_CASE(ParseExp, SimpleExpFixture)
{
    using armnn::DataType;
    RunTest<4, DataType::Float32>(0, {{ "inputTensor", { 0.0f,  1.0f,  2.0f,
                                                            3.0f,  4.0f,  5.0f} }},
                                  {{ "outputTensor", { 1.0f,  2.718281f,  7.3890515f,
                                                         20.0855185f, 54.5980834f, 148.4129329f} } });
}

BOOST_AUTO_TEST_SUITE_END()


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   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 <gtest/gtest.h>
#include <gmock/gmock.h>
#include <boost/assign.hpp>
//#include <Core/Datatypes/Mesh/MeshFactory.h>
#include <Core/Datatypes/Legacy/Field/FieldInformation.h>
#include <Core/Datatypes/Legacy/Field/LatVolMesh.h>

using namespace SCIRun;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Geometry;
using ::testing::_;
using ::testing::NiceMock;
using ::testing::DefaultValue;
using ::testing::Return;
using namespace boost::assign;

class LatticeVolumeMeshTests : public ::testing::Test
{
protected:
  void SetUp() override
  {
    int basisOrder = 1;
    FieldInformation lfi("LatVolMesh", basisOrder, "double");
    int sizex,sizey,sizez;
    sizex = sizey = sizez = 2;
    Point minb(0,0,0);
    Point maxb(1,1,1);
    mesh_ = CreateMesh(lfi, sizex, sizey, sizez, minb, maxb);
  }

  MeshHandle mesh_;
};

namespace
{
  template <typename T>
  std::string join(const T& list)
  {
    std::ostringstream oss;
    const int SIZE = list.size();
    for (int i = 0; i < SIZE; ++i)
    {
      oss << list[i];
      if (i < SIZE - 1)
        oss << ", ";
    }
    return oss.str();
  }
}

TEST_F(LatticeVolumeMeshTests, BasicCubeTest)
{
  ASSERT_TRUE(mesh_.get() != nullptr);

  auto latVolVMesh = mesh_->vmesh();
  ASSERT_TRUE(latVolVMesh != nullptr);

  VMesh::dimension_type dims;
  latVolVMesh->get_dimensions(dims);

  VMesh::dimension_type expectedDims;
  expectedDims += 2,2,2;
  EXPECT_EQ(expectedDims, dims);

  EXPECT_EQ(8, latVolVMesh->num_nodes());
  EXPECT_EQ(12, latVolVMesh->num_edges());
  EXPECT_EQ(6, latVolVMesh->num_faces());
  EXPECT_EQ(1, latVolVMesh->num_elems());

#ifdef SCIRUN4_CODE_TO_BE_ENABLED_LATER
  ASSERT_TRUE(latVolVMesh->is_linearmesh());

  ASSERT_FALSE(latVolVMesh->has_normals());
#endif
}

TEST_F(LatticeVolumeMeshTests, CubeIterationTest)
{
  auto latVolVMesh = mesh_->vmesh();

  {
    VMesh::Edge::iterator meshEdgeIter;
    VMesh::Edge::iterator meshEdgeEnd;

    VMesh::Node::array_type nodesFromEdge(2);

    latVolVMesh->end(meshEdgeEnd);

    std::ostringstream ostr;
    for (latVolVMesh->begin(meshEdgeIter); meshEdgeIter != meshEdgeEnd; ++meshEdgeIter)
    {
      // get nodes from edge

      VMesh::Edge::index_type edgeID = *meshEdgeIter;
      latVolVMesh->get_nodes(nodesFromEdge, edgeID);
      Point p0, p1;
      latVolVMesh->get_point(p0, nodesFromEdge[0]);
      latVolVMesh->get_point(p1, nodesFromEdge[1]);
      ostr << "Edge " << edgeID << " nodes=[" << nodesFromEdge[0] << " point=" << p0.get_string()
        << ", " << nodesFromEdge[1] << " point=" << p1.get_string() << "]" << std::endl;
    }

    EXPECT_EQ(
      "Edge 0 nodes=[0 point=[0, 0, 0], 1 point=[1, 0, 0]]\n"
      "Edge 1 nodes=[2 point=[0, 1, 0], 3 point=[1, 1, 0]]\n"
      "Edge 2 nodes=[4 point=[0, 0, 1], 5 point=[1, 0, 1]]\n"
      "Edge 3 nodes=[6 point=[0, 1, 1], 7 point=[1, 1, 1]]\n"
      "Edge 4 nodes=[0 point=[0, 0, 0], 2 point=[0, 1, 0]]\n"
      "Edge 5 nodes=[4 point=[0, 0, 1], 6 point=[0, 1, 1]]\n"
      "Edge 6 nodes=[1 point=[1, 0, 0], 3 point=[1, 1, 0]]\n"
      "Edge 7 nodes=[5 point=[1, 0, 1], 7 point=[1, 1, 1]]\n"
      "Edge 8 nodes=[0 point=[0, 0, 0], 4 point=[0, 0, 1]]\n"
      "Edge 9 nodes=[1 point=[1, 0, 0], 5 point=[1, 0, 1]]\n"
      "Edge 10 nodes=[2 point=[0, 1, 0], 6 point=[0, 1, 1]]\n"
      "Edge 11 nodes=[3 point=[1, 1, 0], 7 point=[1, 1, 1]]\n"
      , ostr.str());
  }

  {
    VMesh::Face::iterator meshFaceIter;
    VMesh::Face::iterator meshFaceEnd;

    VMesh::Edge::array_type edgesFromFace(4);
    VMesh::Node::array_type nodesFromFace(4);

    latVolVMesh->end(meshFaceEnd);

    std::ostringstream ostr;
    for (latVolVMesh->begin(meshFaceIter); meshFaceIter != meshFaceEnd; ++meshFaceIter)
    {
      // get edges and nodes from face

      VMesh::Face::index_type faceID = *meshFaceIter;
      latVolVMesh->get_edges(edgesFromFace, faceID);
      ostr << "Face " << faceID << " edges=[" << join(edgesFromFace) << "]" << std::endl;

      latVolVMesh->get_nodes(nodesFromFace, faceID);
      ostr << "Face " << faceID << " nodes=[" << join(nodesFromFace) << "]" << std::endl;
    }

    EXPECT_EQ("Face 0 edges=[0, 1, 4, 6]\n"
      "Face 0 nodes=[0, 1, 3, 2]\n"
      "Face 1 edges=[2, 3, 5, 7]\n"
      "Face 1 nodes=[4, 6, 7, 5]\n"
      "Face 2 edges=[4, 5, 8, 10]\n"
      "Face 2 nodes=[0, 2, 6, 4]\n"
      "Face 3 edges=[6, 7, 9, 11]\n"
      "Face 3 nodes=[1, 5, 7, 3]\n"
      "Face 4 edges=[0, 2, 8, 9]\n"
      "Face 4 nodes=[0, 4, 5, 1]\n"
      "Face 5 edges=[1, 3, 10, 11]\n"
      "Face 5 nodes=[2, 3, 7, 6]\n"
      ,ostr.str());
  }

  {
    VMesh::Cell::iterator meshCellIter;
    VMesh::Cell::iterator meshCellEnd;

    VMesh::Edge::array_type edgesFromCell(12);
    VMesh::Node::array_type nodesFromCell(8);

    latVolVMesh->end(meshCellEnd);
    std::ostringstream ostr;

    for (latVolVMesh->begin(meshCellIter); meshCellIter != meshCellEnd; ++meshCellIter)
    {
      // get edges and nodes from mesh element

      VMesh::Cell::index_type elemID = *meshCellIter;
      latVolVMesh->get_edges(edgesFromCell, elemID);
      ostr << "Cell " << elemID << " edges=[" << join(edgesFromCell) << "]" << std::endl;

      latVolVMesh->get_nodes(nodesFromCell, elemID);
      ostr << "Cell " << elemID << " nodes=["<< join(nodesFromCell) << "]" << std::endl;
    }

    EXPECT_EQ(
      "Cell 0 edges=[0, 1, 2, 3, 4, 6, 5, 7, 8, 9, 10, 11]\n"
      "Cell 0 nodes=[0, 1, 3, 2, 4, 5, 7, 6]\n",
      ostr.str());
  }

  {
    VMesh::Node::iterator meshNodeIter;
    VMesh::Node::iterator meshNodeEnd;

    VMesh::Edge::array_type edgesFromNode(3);

    latVolVMesh->end(meshNodeEnd);
    std::ostringstream ostr;

    for (latVolVMesh->begin(meshNodeIter); meshNodeIter != meshNodeEnd; ++meshNodeIter)
    {
      // get edges and point from mesh node

      VMesh::Node::index_type nodeID = *meshNodeIter;
      Point p;
      latVolVMesh->get_point(p, nodeID);
      ostr << "Node " << nodeID << " point=" << p.get_string();
      latVolVMesh->get_edges(edgesFromNode, nodeID);
      ostr << " edges=[" << join(edgesFromNode) << "]" << std::endl;
    }

    EXPECT_EQ("Node 0 point=[0, 0, 0] edges=[0, 4, 8]\n"
      "Node 1 point=[1, 0, 0] edges=[0, 6, 9]\n"
      "Node 2 point=[0, 1, 0] edges=[1, 6, 10]\n"
      "Node 3 point=[1, 1, 0] edges=[1, 8, 11]\n"
      "Node 4 point=[0, 0, 1] edges=[2, 5, 9]\n"
      "Node 5 point=[1, 0, 1] edges=[2, 7, 10]\n"
      "Node 6 point=[0, 1, 1] edges=[3, 7, 11]\n"
      "Node 7 point=[1, 1, 1] edges=[3, 9, 12]\n",
      ostr.str());
  }
}


////////////////////////////////////////////////////////////////////////////////
/// Copyright of this program is the property of AMADEUS, without
/// whose written permission reproduction in whole or in part is prohibited.
////////////////////////////////////////////////////////////////////////////////

#include <mdw/formula/Expression.hpp>
#include <mdw/formula/Operator.hpp>
#include <mdw/formula/IContext.hpp>
#include <mdw/formula/Grammar.hpp>
#include <mdw/formula/Constant.hpp>
#include <mdw/formula/StandardTypes.hpp>
#include <mdw/formula/Facts.hpp>

#include <mdw/Tracer.hpp>
#include <boost/mem_fn.hpp>
#include <iostream>

namespace mdw { namespace formula {
  class Pouet;
}}

#define ASSERT_TRUE(x) if (!(x)) {std::cerr << "Failed to check: " #x << std::endl;}

namespace mdw { namespace formula {

  class Pouet {
    int64_t _foo;
  public:

    friend std::ostream& operator<<(std::ostream& oStream, const mdw::formula::Pouet& iFact)
    {
      oStream << "Pouet with foo: " << iFact.getFoo();
      return oStream;
    }

    Pouet(): _foo(456)
    {}

    int64_t getFoo() const
    {
      return _foo;
    }
  } gPouet;

  std::string kStringValue("PouetValue");

  ConstExpression<std::string> gString(kStringValue);
  ConstExpression<int> gInt(524);
  ConstExpression<double> gDouble(35.);
  ConstExpression<bool> gBool(true);

  int TestPlus()
  {
    ConstExpression<int> aInt(87);
    Grammar aGrammar;
    static const std::string kPlus("+");
    SymmetricTypedOperator<int, int, std::plus<int64_t> > aPlusInt(gInt, aInt, aGrammar, kPlus);
    IContext aContext;
    FORMULA_DEBUG("Formula is: " << aPlusInt.toString() << " with value " << aPlusInt.evaluate(aContext));
    ASSERT_TRUE(aPlusInt.evaluate(aContext) == (524+87));

    return 0;
  }

  int TestChoice()
  {
    Grammar aGrammar;
    ConstExpression<int> aInt(93);
    ChoiceOperator<int> aChoice(gBool, aGrammar, aInt, gInt);
    IContext aContext;
    FORMULA_DEBUG("Formula is: " << aChoice.toString() << " with value " << aChoice.evaluate(aContext));
    ASSERT_TRUE(aChoice.evaluate(aContext) == 93);

    ChoiceOperator<int> aChoice2(gBool, aGrammar, gInt, aInt);

    ASSERT_TRUE(aChoice2.evaluate(aContext) == 524);
    return 0;
  }

  int TestString()
  {
    std::string *aStr = new std::string("PouetValue");
    Grammar aGrammar;
    ConstExpression<std::string> aString(*aStr);
    SymmetricTypedOperator<std::string, bool, std::equal_to<std::string> > aEqualString(gString, aString, aGrammar, "==");
    IContext aContext;
    ASSERT_TRUE(gString.toString() == "'PouetValue'");

    ASSERT_TRUE(gString.evaluate(aContext) == "PouetValue");
    ASSERT_TRUE(aEqualString.evaluate(aContext) == true);
    delete aStr;
    return 0;
  }

  int AttributeTest()
  {
    IContext aContext;
    Grammar aGrammar;
    ArenaAllocator aAllocator;
    aGrammar.registerType<Pouet>();
    ConstExpression<Pouet> aFactExpr(gPouet, aGrammar);
    TypedExpression<int>& aResolver =
      formula::make_resolver(aAllocator, aGrammar, aFactExpr, boost::mem_fn(&Pouet::getFoo), "foo");

    ASSERT_TRUE(aResolver.evaluate(aContext) == 456);
    return 0;
  }

  int AllFormulaTests() {
    int aResult = 0;
    aResult += TestPlus();
    aResult += TestChoice();
    aResult += TestString();
    aResult += AttributeTest();
    return aResult;
  }

}}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "test_cubic"

#include <boost/test/unit_test.hpp>
#include "nanocv/math/abs.hpp"
#include "nanocv/math/cubic.hpp"
#include "nanocv/math/random.hpp"
#include "nanocv/math/epsilon.hpp"

BOOST_AUTO_TEST_CASE(test_cubic)
{
        using namespace ncv;

        const size_t tests = 1327;

        for (size_t t = 0; t < tests; t ++)
        {
                random_t<double> rnd(-1.0, +1.0);

                // build random cubic
                const double a = rnd();
                const double b = rnd();
                const double c = rnd();
                const double d = rnd();
                const cubic_t<double> q(a, b, c, d);
                BOOST_CHECK(q);

                const double x0 = rnd();
                const double f0 = q.value(x0);
                const double g0 = q.gradient(x0);

                const double x1 = x0 + rnd() + 1.0;
                const double f1 = q.value(x1);
                const double g1 = q.gradient(x1);

                // check interpolation
                const cubic_t<double> iq(x0, f0, g0, x1, f1, g1);
                if (!iq)
                {
                        continue;
                }

                BOOST_CHECK_LE(math::abs(f0 - iq.value(x0)), math::epsilon1<double>());
                BOOST_CHECK_LE(math::abs(g0 - iq.gradient(x0)), math::epsilon1<double>());

                BOOST_CHECK_LE(math::abs(f1 - iq.value(x1)), math::epsilon1<double>());
                BOOST_CHECK_LE(math::abs(g1 - iq.gradient(x1)), math::epsilon1<double>());

//                BOOST_CHECK_LE(math::abs(q.a() - iq.a()), math::epsilon1<double>());
//                BOOST_CHECK_LE(math::abs(q.b() - iq.b()), math::epsilon1<double>());
//                BOOST_CHECK_LE(math::abs(q.c() - iq.c()), math::epsilon1<double>());
//                BOOST_CHECK_LE(math::abs(q.d() - iq.d()), math::epsilon1<double>());

                // check extremum
                double extremum1, extremum2;
                iq.extremum(extremum1, extremum2);

                if (!std::isfinite(extremum1) || !std::isfinite(extremum2))
                {
                        continue;
                }

                BOOST_CHECK_LE(math::abs(iq.gradient(extremum1)), math::epsilon1<double>());
                BOOST_CHECK_LE(math::abs(iq.gradient(extremum2)), math::epsilon1<double>());

                const size_t etests = 1843;
                for (size_t e = 0; e < etests; e ++)
                {
                        BOOST_CHECK_GE(math::abs(iq.gradient(rnd())),
                                       math::abs(iq.gradient(extremum1)));

                        BOOST_CHECK_GE(math::abs(iq.gradient(rnd())),
                                       math::abs(iq.gradient(extremum2)));
                }
        }
}


#ifndef LBV_EIGEN_ROS_CONVERSIONS_HPP
#define LBV_EIGEN_ROS_CONVERSIONS_HPP

#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Transform.h>

#include <Eigen/Dense>
#include <Eigen/StdVector>

////////////////////////////////////////////////////////////////////////////////

template <typename Output>
Output ConvertTo(geometry_msgs::Point const& geo);

template <typename Output>
Output ConvertTo(geometry_msgs::Vector3 const& eig);

template <typename Output>
Output ConvertTo(Eigen::Vector3d const& eig);

template <typename Output>
Output ConvertTo(Eigen::Vector3f const& eig);

template <>
inline Eigen::Vector3f ConvertTo<Eigen::Vector3f>(geometry_msgs::Point const& geo) {
  return Eigen::Vector3f(geo.x, geo.y, geo.z);
}

template <>
inline Eigen::Vector3f ConvertTo<Eigen::Vector3f>(geometry_msgs::Vector3 const& eig) {
  return Eigen::Vector3f(eig.x, eig.y, eig.z);
}

template <>
inline geometry_msgs::Point ConvertTo<geometry_msgs::Point>(Eigen::Vector3f const& eig) {
  geometry_msgs::Point geo;
  geo.x = eig.x();
  geo.y = eig.y();
  geo.z = eig.z();
  return geo;
}

template <>
inline geometry_msgs::Vector3 ConvertTo<geometry_msgs::Vector3>(Eigen::Vector3f const& eig) {
  geometry_msgs::Vector3 geo;
  geo.x = eig.x();
  geo.y = eig.y();
  geo.z = eig.z();
  return geo;
}

template <>
inline Eigen::Vector3d ConvertTo<Eigen::Vector3d>(geometry_msgs::Point const& geo) {
  return Eigen::Vector3d(geo.x, geo.y, geo.z);
}

template <>
inline Eigen::Vector3d ConvertTo<Eigen::Vector3d>(geometry_msgs::Vector3 const& eig) {
  return Eigen::Vector3d(eig.x, eig.y, eig.z);
}

template <>
inline geometry_msgs::Point ConvertTo<geometry_msgs::Point>(Eigen::Vector3d const& eig) {
  geometry_msgs::Point geo;
  geo.x = eig.x();
  geo.y = eig.y();
  geo.z = eig.z();
  return geo;
}

template <>
inline geometry_msgs::Vector3 ConvertTo<geometry_msgs::Vector3>(Eigen::Vector3d const& eig) {
  geometry_msgs::Vector3 geo;
  geo.x = eig.x();
  geo.y = eig.y();
  geo.z = eig.z();
  return geo;
}

////////////////////////////////////////////////////////////////////////////////

template <typename Output>
Output ConvertTo(geometry_msgs::Quaternion const& quat);

template <typename Output>
Output ConvertTo(Eigen::Quaterniond const& quat);

template <typename Output>
Output ConvertTo(Eigen::Matrix3d const& rot);

template <typename Output>
Output ConvertTo(Eigen::Quaternionf const& quat);

template <typename Output>
Output ConvertTo(Eigen::Matrix3f const& rot);

template <>
inline Eigen::Quaternionf ConvertTo<Eigen::Quaternionf>(geometry_msgs::Quaternion const& quat) {
  return Eigen::Quaternionf(quat.w, quat.x, quat.y, quat.z);
}

template <>
inline geometry_msgs::Quaternion ConvertTo<geometry_msgs::Quaternion>(Eigen::Quaternionf const& quat) {
  geometry_msgs::Quaternion geo_quat;
  geo_quat.x = quat.x();
  geo_quat.y = quat.y();
  geo_quat.z = quat.z();
  geo_quat.w = quat.w();
  return geo_quat;
}

template <>
inline geometry_msgs::Quaternion ConvertTo<geometry_msgs::Quaternion>(Eigen::Matrix3f const& rot) {
  return ConvertTo<geometry_msgs::Quaternion>(Eigen::Quaternionf(rot));
}

template <>
inline Eigen::Quaterniond ConvertTo<Eigen::Quaterniond>(geometry_msgs::Quaternion const& quat) {
  return Eigen::Quaterniond(quat.w, quat.x, quat.y, quat.z);
}

template <>
inline geometry_msgs::Quaternion ConvertTo<geometry_msgs::Quaternion>(Eigen::Quaterniond const& quat) {
  geometry_msgs::Quaternion geo_quat;
  geo_quat.x = quat.x();
  geo_quat.y = quat.y();
  geo_quat.z = quat.z();
  geo_quat.w = quat.w();
  return geo_quat;
}

template <>
inline geometry_msgs::Quaternion ConvertTo<geometry_msgs::Quaternion>(Eigen::Matrix3d const& rot) {
  return ConvertTo<geometry_msgs::Quaternion>(Eigen::Quaterniond(rot));
}

////////////////////////////////////////////////////////////////////////////////

template <typename Output>
Output ConvertTo(geometry_msgs::Pose const& pose);

template <typename Output>
Output ConvertTo(Eigen::Affine3d const& transform);

template <typename Output>
Output ConvertTo(Eigen::Isometry3d const& transform);

template <typename Output>
Output ConvertTo(Eigen::Translation3d const& translation);

template <>
inline Eigen::Isometry3d ConvertTo<Eigen::Isometry3d>(geometry_msgs::Pose const& pose) {
  Eigen::Translation3d const trans(pose.position.x, pose.position.y, pose.position.z);
  Eigen::Quaterniond const quat(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
  return trans * quat;
}

template <>
inline Eigen::Affine3d ConvertTo<Eigen::Affine3d>(geometry_msgs::Pose const& pose) {
  Eigen::Translation3d const trans(pose.position.x, pose.position.y, pose.position.z);
  Eigen::Quaterniond const quat(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
  return trans * quat;
}

template <>
inline geometry_msgs::Transform ConvertTo<geometry_msgs::Transform>(Eigen::Affine3d const& transform) {
  geometry_msgs::Transform geo_transform;
  geo_transform.translation.x = transform.translation().x();
  geo_transform.translation.y = transform.translation().y();
  geo_transform.translation.z = transform.translation().z();
  const Eigen::Quaterniond quat(transform.rotation());
  geo_transform.rotation.x = quat.x();
  geo_transform.rotation.y = quat.y();
  geo_transform.rotation.z = quat.z();
  geo_transform.rotation.w = quat.w();
  return geo_transform;
}

template <>
inline geometry_msgs::Transform ConvertTo<geometry_msgs::Transform>(Eigen::Isometry3d const& transform) {
  geometry_msgs::Transform geo_transform;
  geo_transform.translation.x = transform.translation().x();
  geo_transform.translation.y = transform.translation().y();
  geo_transform.translation.z = transform.translation().z();
  const Eigen::Quaterniond quat(transform.rotation());
  geo_transform.rotation.x = quat.x();
  geo_transform.rotation.y = quat.y();
  geo_transform.rotation.z = quat.z();
  geo_transform.rotation.w = quat.w();
  return geo_transform;
}

template <>
inline geometry_msgs::Transform ConvertTo<geometry_msgs::Transform>(Eigen::Translation3d const& translation) {
  geometry_msgs::Transform geo_transform;
  geo_transform.translation.x = translation.translation().x();
  geo_transform.translation.y = translation.translation().y();
  geo_transform.translation.z = translation.translation().z();
  geo_transform.rotation.x = 0;
  geo_transform.rotation.y = 0;
  geo_transform.rotation.z = 0;
  geo_transform.rotation.w = 1;
  return geo_transform;
}

////////////////////////////////////////////////////////////////////////////////

template <typename Output>
Output ConvertTo(geometry_msgs::Transform const& transform);

template <>
inline Eigen::Isometry3d ConvertTo<Eigen::Isometry3d>(geometry_msgs::Transform const& transform) {
  Eigen::Translation3d const trans(transform.translation.x, transform.translation.y, transform.translation.z);
  Eigen::Quaterniond const quat(transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z);
  return trans * quat;
}

template <>
inline Eigen::Affine3d ConvertTo<Eigen::Affine3d>(geometry_msgs::Transform const& transform) {
  Eigen::Translation3d const trans(transform.translation.x, transform.translation.y, transform.translation.z);
  Eigen::Quaterniond const quat(transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z);
  return trans * quat;
}

////////////////////////////////////////////////////////////////////////////////

template <typename Input, typename Output>
std::vector<Output, Eigen::aligned_allocator<Output>> ConvertTo(std::vector<Input> const& geom) {
  std::vector<Output, Eigen::aligned_allocator<Output>> eig(geom.size());
  for (size_t idx = 0; idx < geom.size(); ++idx) {
    eig[idx] = ConvertTo<Output>(geom[idx]);
  }
  return eig;
}

#endif  // LBV_EIGEN_ROS_CONVERSIONS_HPP


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2006, 2011 Ferdinando Ametrano
 Copyright (C) 2006 Giorgio Facchinetti

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#ifndef qla_analysis_hpp
#define qla_analysis_hpp

#include <boost/shared_ptr.hpp>
#include <vector>
#include <oh/property.hpp>

namespace QuantLib {
    class CashFlow;
    class Date;
    typedef std::vector<boost::shared_ptr<CashFlow> > Leg;
}

namespace QuantLibAddin {

    std::vector<std::vector<ObjectHandler::property_t> >
    flowAnalysis(const QuantLib::Leg& leg,
                 const QuantLib::Date& d);

}

#endif


#include "attack.h"

#include "../Log.h"
#include "../Config.h"

#include <cmath>

#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

bool SortByGrowthRate(PlanetPtr a, PlanetPtr b) {
    return a->GrowthRate() < b->GrowthRate();
}

void Flee(GameState &state, Player player) {
    static bool flee = Config::Value<bool>("flee");
    if ( ! flee  ) return;

    LOG("Flee phase");

    std::vector<PlanetPtr> my_planets = state.PlanetsOwnedBy(player);
    sort(my_planets.begin(), my_planets.end(), SortByGrowthRate);

    std::vector<PlanetPtr> planets = state.Planets();

    foreach ( PlanetPtr &p, my_planets ) {
        if ( p->FutureOwner() == player ) continue;

        int p_id = p->id;
        int ships = p->FutureState(0).ships;

        int closest_distance = INF;
        int destination = -1;

        // if the future owner is not me then see if we can use our ships elsewhere
        foreach ( PlanetPtr &dest, planets ) {
            int dest_id = dest->id;
            if ( dest_id == p_id ) continue;
            if ( p->GrowthRate() >= dest->GrowthRate() ) break;

            if ( dest->Owner() == NEUTRAL ) continue;
            if ( dest->FutureOwner() != -player ) continue;

            int distance = Map::Distance(p_id, dest_id);
            int cost = dest->Cost(distance, player);

            LOG( "   Cost from " << p_id << " to " << dest_id << ": " << cost );

            if ( cost > ships ) continue;

            if ( distance < closest_distance ) {
                closest_distance = distance;
                destination = dest_id;
            }
        }

        if ( destination >= 0 ) {
            state.IssueOrder(Fleet(player, p_id, destination, ships));
            LOG( " Fleeing from " << p_id << " to " << destination );
        }
    }
}


// Copyright 2018 Hans Dembinski
//
// Distributed under the Boost Software License, version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_HISTOGRAM_ACCUMULATORS_SUM_HPP
#define BOOST_HISTOGRAM_ACCUMULATORS_SUM_HPP

#include <boost/core/nvp.hpp>
#include <boost/histogram/fwd.hpp> // for sum<>
#include <cmath>                   // std::abs
#include <type_traits>             // std::is_floating_point, std::common_type

namespace boost {
namespace histogram {
namespace accumulators {

/**
  Uses Neumaier algorithm to compute accurate sums of floats.

  The algorithm is an improved Kahan algorithm
  (https://en.wikipedia.org/wiki/Kahan_summation_algorithm). The algorithm uses memory for
  two numbers and is three to five times slower compared to using a single number to
  accumulate a sum, but the relative error of the sum is at the level of the machine
  precision, independent of the number of samples.

  A. Neumaier, Zeitschrift fuer Angewandte Mathematik und Mechanik 54 (1974) 39-51.
*/
template <class ValueType>
class sum {
  static_assert(std::is_floating_point<ValueType>::value,
                "ValueType must be a floating point type");

public:
  using value_type = ValueType;
  using const_reference = const value_type&;

  sum() = default;

  /// Initialize sum to value and allow implicit conversion
  sum(const_reference value) noexcept : sum(value, 0) {}

  /// Allow implicit conversion from sum<T>
  template <class T>
  sum(const sum<T>& s) noexcept : sum(s.large(), s.small()) {}

  /// Initialize sum explicitly with large and small parts
  sum(const_reference large, const_reference small) noexcept
      : large_(large), small_(small) {}

  /// Increment sum by one
  sum& operator++() noexcept { return operator+=(1); }

  /// Increment sum by value
  sum& operator+=(const_reference value) noexcept {
    // prevent compiler optimization from destroying the algorithm
    // when -ffast-math is enabled
    volatile value_type l;
    value_type s;
    if (std::abs(large_) >= std::abs(value)) {
      l = large_;
      s = value;
    } else {
      l = value;
      s = large_;
    }
    large_ += value;
    l = l - large_;
    l = l + s;
    small_ += l;
    return *this;
  }

  /// Add another sum
  sum& operator+=(const sum& s) noexcept {
    operator+=(s.large_);
    small_ += s.small_;
    return *this;
  }

  /// Scale by value
  sum& operator*=(const_reference value) noexcept {
    large_ *= value;
    small_ *= value;
    return *this;
  }

  bool operator==(const sum& rhs) const noexcept {
    return large_ + small_ == rhs.large_ + rhs.small_;
  }

  bool operator!=(const sum& rhs) const noexcept { return !operator==(rhs); }

  /// Return value of the sum.
  value_type value() const noexcept { return large_ + small_; }

  /// Return large part of the sum.
  const_reference large() const noexcept { return large_; }

  /// Return small part of the sum.
  const_reference small() const noexcept { return small_; }

  // lossy conversion to value type must be explicit
  explicit operator value_type() const noexcept { return value(); }

  template <class Archive>
  void serialize(Archive& ar, unsigned /* version */) {
    ar& make_nvp("large", large_);
    ar& make_nvp("small", small_);
  }

  // begin: extra operators to make sum behave like a regular number

  sum& operator*=(const sum& rhs) noexcept {
    const auto scale = static_cast<value_type>(rhs);
    large_ *= scale;
    small_ *= scale;
    return *this;
  }

  sum operator*(const sum& rhs) const noexcept {
    sum s = *this;
    s *= rhs;
    return s;
  }

  sum& operator/=(const sum& rhs) noexcept {
    const auto scale = 1.0 / static_cast<value_type>(rhs);
    large_ *= scale;
    small_ *= scale;
    return *this;
  }

  sum operator/(const sum& rhs) const noexcept {
    sum s = *this;
    s /= rhs;
    return s;
  }

  bool operator<(const sum& rhs) const noexcept {
    return operator value_type() < rhs.operator value_type();
  }

  bool operator>(const sum& rhs) const noexcept {
    return operator value_type() > rhs.operator value_type();
  }

  bool operator<=(const sum& rhs) const noexcept {
    return operator value_type() <= rhs.operator value_type();
  }

  bool operator>=(const sum& rhs) const noexcept {
    return operator value_type() >= rhs.operator value_type();
  }

  // end: extra operators

private:
  value_type large_{};
  value_type small_{};
};

} // namespace accumulators
} // namespace histogram
} // namespace boost

#ifndef BOOST_HISTOGRAM_DOXYGEN_INVOKED
namespace std {
template <class T, class U>
struct common_type<boost::histogram::accumulators::sum<T>,
                   boost::histogram::accumulators::sum<U>> {
  using type = boost::histogram::accumulators::sum<common_type_t<T, U>>;
};
} // namespace std
#endif

#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_FUNCTIONAL_GEOMETRY_HPP
#define BOOST_COMPUTE_FUNCTIONAL_GEOMETRY_HPP

#include <boost/compute/type_traits.hpp>
#include <boost/compute/functional/detail/macros.hpp>

namespace boost {
namespace compute {

BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(cross, T (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(dot, typename scalar_type<T>::type (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(distance, typename scalar_type<T>::type (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fast_distance, typename scalar_type<T>::type (T, T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(length, typename scalar_type<T>::type (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fast_length, typename scalar_type<T>::type (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(normalize, T (T), class T)
BOOST_COMPUTE_DECLARE_BUILTIN_FUNCTION(fast_normalize, T (T), class T)

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_FUNCTIONAL_GEOMETRY_HPP


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * 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 "FirstOrderLinearDS.hpp"
#include "TimeStepping.hpp"

#include "LinearSMCimproved.hpp"
#include "SiconosVector.hpp"
#include "ControlSensor.hpp"
#include "ZeroOrderHoldOSI.hpp"
#include "TimeDiscretisation.hpp"
#include "ActuatorFactory.hpp"

#include <boost/circular_buffer.hpp>

LinearSMCimproved::LinearSMCimproved(SP::ControlSensor sensor):
  LinearSMC(sensor, LINEAR_SMC_IMPROVED), _predictionPerturbation(false), _inDisceteTimeSlidingPhase(false), _ubPerturbation(0.0)
{
}

LinearSMCimproved::LinearSMCimproved(SP::ControlSensor sensor, SP::SimpleMatrix B, SP::SimpleMatrix D):
  LinearSMC(sensor, B, D, LINEAR_SMC_IMPROVED), _predictionPerturbation(false), _inDisceteTimeSlidingPhase(false),_ubPerturbation(0.0)
{
}

LinearSMCimproved::~LinearSMCimproved()
{
}

void LinearSMCimproved::initialize(const Model& m)
{
  LinearSMC::initialize(m);
  _up.reset(new SiconosVector(_us->size()));
  _measuredPert.reset(new boost::circular_buffer<SP::SiconosVector>(0));
  _predictedPert.reset(new boost::circular_buffer<SP::SiconosVector>(0));
}

void LinearSMCimproved::predictionPerturbation(const SiconosVector& xTk, SimpleMatrix& CBstar)
{
  if (_us->normInf() < _alpha)
  {
    if (_inDisceteTimeSlidingPhase)
    {
      SiconosVector& up = *_up;
      if (_measuredPert->full())
      {
        if (_measuredPert->size() > 1)
        {
          _measuredPert->rotate(_measuredPert->end()-1);
          _predictedPert->rotate(_predictedPert->end()-1);
        }
      }
      else
      {
        // inject new vector in the case where the measurement vector is not full.
        SP::SiconosVector sp1(new SiconosVector(_us->size(), 0));
        SP::SiconosVector sp2(new SiconosVector(_us->size(), 0));
        _measuredPert->push_front(sp1);
        _predictedPert->push_front(sp2);
      }

      // inject new measured value and also perturbation prediction
      SiconosVector& predictedPertC = *(*_predictedPert)[0];
      SiconosVector& measuredPertC = *(*_measuredPert)[0];

      // Cp_k = s_k + Cp_k-tilde
      prod(*_Csurface, xTk, measuredPertC);
      measuredPertC += *(*_predictedPert)[std::min((unsigned int)1, (unsigned int)_predictedPert->size()-1)];

      // compute prediction
      switch(_measuredPert->size()-1)
      {
        case 0:
          predictedPertC = measuredPertC;
          break;
        case 1:
          predictedPertC = 2*measuredPertC - *(*_measuredPert)[1];
          break;
        case 2:
          predictedPertC = 3*measuredPertC - 3*(*(*_measuredPert)[1]) + *(*_measuredPert)[2];
          break;
        default:
          RuntimeException::selfThrow("LinearSMCimproved::predictionPerturbation: unknown order " + _measuredPert->size());
      }

      // Compute the control to counteract the perturbation
      up = predictedPertC;
      up *= -1;
      CBstar.PLUForwardBackwardInPlace(up);

      // project onto feasible set
      double norm = up.norm2();
      if (norm > _ubPerturbation)
      {
        up *= _ubPerturbation/norm;
        predictedPertC *= _ubPerturbation/norm;
      }
   }
    else
      _inDisceteTimeSlidingPhase = true;
  }
  else if (_inDisceteTimeSlidingPhase)
  {
    _inDisceteTimeSlidingPhase = false;
    _up->zero();
  }
}

void LinearSMCimproved::actuate()
{
  unsigned int sDim = _u->size();
  SP::SimpleMatrix tmpM1(new SimpleMatrix(*_Csurface));
  SP::SimpleMatrix CBstar(new SimpleMatrix(sDim, sDim, 0));
  SP::SiconosVector xTk(new SiconosVector(_sensor->y()));

  ZeroOrderHoldOSI& zoh = *std11::static_pointer_cast<ZeroOrderHoldOSI>(_integratorSMC);

  // equivalent part
  zoh.updateMatrices(_DS_SMC);
  prod(*_Csurface, zoh.Ad(_DS_SMC), *tmpM1);
  *tmpM1 *= -1.0;
  *tmpM1 += *_Csurface;
  prod(*_Csurface, zoh.Bd(_DS_SMC), *CBstar);
  // compute C(I-e^{Ah})x_k
  prod(*tmpM1, *xTk, *_ueq);
  // compute the solution u^eq of the system CB^{*}u^eq = C(I-e^{Ah})x_k
  CBstar->PLUForwardBackwardInPlace(*_ueq);

  *(_DS_SMC->x()) = *xTk;
  prod(*_B, *_ueq, *(std11::static_pointer_cast<FirstOrderLinearDS>(_DS_SMC)->b()));
  _simulationSMC->computeOneStep();
  _simulationSMC->nextStep();

  *_us = *_lambda;

  // inject those in the system
  *_u = *_us;
  *_u += *_ueq;

  // prediction of the perturbation
  if (_predictionPerturbation)
  {
    predictionPerturbation(*xTk, *CBstar);
    *_u += *_up;
  }

  _indx++;

}

void LinearSMCimproved::setPredictionOrder(unsigned int order)
{
  _measuredPert->set_capacity(order+1);
  _predictedPert->set_capacity(order+1);
}

AUTO_REGISTER_ACTUATOR(LINEAR_SMC_IMPROVED, LinearSMCimproved)


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/arch/common/scalar/function/idivnearbyint.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/inearbyint.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD_IF ( div_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::tag::inearbyint_
                          , bs::pack_< bd::arithmetic_<A0>, X>
                          , bs::pack_< bd::arithmetic_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( bd::functor<bs::tag::inearbyint_> const&,
                                      A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      return div(nearbyint, a0, a1);
    }
  };

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

  BOOST_DISPATCH_OVERLOAD_IF ( div_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::tag::inearbyint_
                          , bs::pack_< bd::floating_<A0>, X>
                          , bs::pack_< bd::floating_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<A0> operator() ( bd::functor<bs::tag::inearbyint_> const&
                                                      , A0 const& a0
                                                      , A0 const& a1) const BOOST_NOEXCEPT
    {
      return inearbyint(a0/a1);
    }
  };

#ifdef BOOST_MSVC
  #pragma warning(pop)
#endif
} } }

#endif


#include "../coin.h"
#include "../params.h"

#include "streams.h"

#include <boost/test/unit_test.hpp>

namespace lelantus {

BOOST_AUTO_TEST_SUITE(lelantus_coin_tests)

BOOST_AUTO_TEST_CASE(privatecoin)
{
    auto params = Params::get_default();

    uint64_t v = 1;

    // default constructor
    PrivateCoin priv(params, v);
    auto &serial = priv.getSerialNumber();
    auto &randomness = priv.getRandomness();
    std::vector<unsigned char> seckey(32, 0);

    // calculate commitment
    auto commitment = LelantusPrimitives::double_commit(
        params->get_g(), serial,
        params->get_h1(), priv.getV(),
        params->get_h0(), randomness);

    // verify
    BOOST_CHECK(serial.isMember());
    BOOST_CHECK(randomness.isMember());
    BOOST_CHECK_EQUAL(0, priv.getVersion());
    BOOST_CHECK_EQUAL(v, priv.getV());
    BOOST_CHECK_EQUAL(commitment, priv.getPublicCoin().getValue());

    // Construct the identical coin by another constructor
    PrivateCoin anotherPriv(params, serial, v, randomness, seckey, 0);

    // verify
    BOOST_CHECK_EQUAL(serial, anotherPriv.getSerialNumber());
    BOOST_CHECK_EQUAL(randomness, anotherPriv.getRandomness());
    BOOST_CHECK_EQUAL(0, anotherPriv.getVersion());
    BOOST_CHECK_EQUAL(v, anotherPriv.getV());
    BOOST_CHECK_EQUAL(commitment, anotherPriv.getPublicCoin().getValue());
}

BOOST_AUTO_TEST_CASE(privcoin_getset)
{
    auto const params = Params::get_default();

    PrivateCoin coin1(params, 1);
    PrivateCoin coin2(params, 2);

    BOOST_CHECK(coin1.getPublicCoin() != coin2.getPublicCoin());
    BOOST_CHECK(coin1.getRandomness() != coin2.getRandomness());
    BOOST_CHECK(coin1.getSerialNumber() != coin2.getSerialNumber());
    BOOST_CHECK(coin1.getV() != coin2.getV());

    // set
    coin2.setPublicCoin(coin1.getPublicCoin());
    coin2.setRandomness(coin1.getRandomness());
    coin2.setSerialNumber(coin1.getSerialNumber());
    coin2.setV(coin1.getV());
    coin2.setVersion(10);

    // verify
    BOOST_CHECK(coin1.getPublicCoin() == coin2.getPublicCoin());
    BOOST_CHECK(coin1.getRandomness() == coin2.getRandomness());
    BOOST_CHECK(coin1.getSerialNumber() == coin2.getSerialNumber());
    BOOST_CHECK(coin1.getV() == coin2.getV());
    BOOST_CHECK(10 == coin2.getVersion());
}

BOOST_AUTO_TEST_CASE(publiccoin_constructor)
{
    // default
    PublicCoin defaultPub;

    BOOST_CHECK(defaultPub.getValue().isInfinity());

    // specify commitment
    GroupElement coin;
    coin.randomize();
    PublicCoin pub(coin);

    BOOST_CHECK_EQUAL(coin, pub.getValue());
}

BOOST_AUTO_TEST_CASE(publiccoin_hash)
{
    // seed
    std::array<uint8_t, 32> seed; // 32 bytes of 0s
    std::fill(seed.begin(), seed.end(), 0);

    GroupElement r;
    r.generate(seed.data());

    PublicCoin coin(r);

    BOOST_CHECK(coin.validate());

    // double hash of 7ed851c84a73fce904dc38fd709836dba446d875846522d4f016bba815a21fc10000
    // the result of generated group element using 32 bytes of 0 as seed
    BOOST_CHECK_EQUAL(
        "c3ca472773e3334f057875b8268dabd16dcda3d2b4cad3e924ff8c31affacf67",
        coin.getValueHash().GetHex());
}

BOOST_AUTO_TEST_CASE(validate_publiccoin)
{
    // default pubcoin
    BOOST_CHECK(!PublicCoin().validate());

    // inf coin
    GroupElement infCoin;
    BOOST_CHECK(infCoin.isMember() && infCoin.isInfinity());
    BOOST_CHECK(!PublicCoin(infCoin).validate());

    // valid coin
    GroupElement coin;
    coin.randomize();
    BOOST_CHECK(coin.isMember() && !coin.isInfinity());
    BOOST_CHECK(PublicCoin(coin).validate());
}

BOOST_AUTO_TEST_CASE(publiccoin_equality)
{
    GroupElement a, b;
    a.randomize();
    b.randomize();

    PublicCoin pubA(a);
    PublicCoin pubB(b);
    PublicCoin pubC(a);

    BOOST_CHECK(pubA == pubA);
    BOOST_CHECK(pubA == pubC);
    BOOST_CHECK(pubA != pubB);
}

BOOST_AUTO_TEST_CASE(publiccoin_serialization)
{
    GroupElement coin;
    coin.randomize();

    PublicCoin pub(coin);

    CDataStream serialize(SER_NETWORK, PROTOCOL_VERSION);
    serialize << pub;

    CDataStream deserialize(
        std::vector<unsigned char>(serialize.begin(), serialize.end()),
        SER_NETWORK, PROTOCOL_VERSION);

    PublicCoin deserializedCoin;
    deserialize >> deserializedCoin;

    BOOST_CHECK(pub == deserializedCoin);
}

BOOST_AUTO_TEST_SUITE_END()

} // namespace lelantus


#include <boost/numeric/odeint/stepper/velocity_verlet.hpp>


/*
@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/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/maybe.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto f = _ + _;
    BOOST_HANA_CONSTEXPR_ASSERT(
        ap(lift<List>(f), list(1, 2), list(3, 4, 5))
            ==
        list(
            f(1, 3), f(1, 4), f(1, 5),
            f(2, 3), f(2, 4), f(2, 5)
        )
    );

    BOOST_HANA_CONSTEXPR_LAMBDA auto g = [](auto a, auto b, auto c) {
        return a * b * c;
    };
    BOOST_HANA_CONSTEXPR_ASSERT(
        ap(lift<Maybe>(g), just(1), just(2), just(3)) == just(1 * 2 * 3)
    );
    BOOST_HANA_CONSTANT_ASSERT(
        ap(lift<Maybe>(g), just(1), nothing, just(3)) == nothing
    );
    //! [main]
}


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

#include "math_unit_test.hpp"
#include <cmath>
#include <boost/math/special_functions/powm1.hpp>

template <typename T>
void test()
{
    CHECK_EQUAL(std::pow(T(0), T(2)) - 1, boost::math::powm1(T(0), T(2)));
    CHECK_EQUAL(std::pow(T(0), T(-2)) - 1, boost::math::powm1(T(0), T(-2)));
    CHECK_EQUAL(std::pow(T(0), T(0.1)) - 1, boost::math::powm1(T(0), T(0.1)));
    CHECK_EQUAL(std::pow(T(0), T(-0.1)) - 1, boost::math::powm1(T(0), T(-0.1)));
}

int main()
{
    test<float>();
    test<double>();

    #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test<long double>();
    #endif

    return boost::math::test::report_errors();
}


#include <iostream>
#include <stdexcept>
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include "learntopicmat.h"
#include "mexutils.h"

using namespace Eigen;

/***** MATLAB mex function *****/
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
    /* Verify and load arguments */
    Eigen::SparseMatrix<double> Zt;
    Eigen::MatrixXd logPMat;
    Eigen::MatrixXd modLMat;
    std::vector< Eigen::VectorXd > thetaNodeArray;
    std::vector< Eigen::SparseMatrix<double> > thetaEdgeArray;
    std::vector< Eigen::VectorXd > logPArray;
    std::vector< Eigen::VectorXd > modLArray;
    try {
        if( nrhs < 4 ) { throw std::invalid_argument( "Incorrect number of arguments." ); }
        Zt = getSparseMatrix( prhs[0] );

        /* Extract thetaNode and thetaEdge */
        mwSize k = mxGetNumberOfElements( prhs[1] );
        for(mwIndex j = 0; j < k; ++j) {
            mxArray* model = mxGetCell(prhs[1], j);
            Eigen::VectorXd thetaNode = getVector(mxGetField( model, 0, "thetaNode" ));
            thetaNodeArray.push_back( thetaNode );

            Eigen::SparseMatrix<double> thetaEdge = getSparseMatrix(mxGetField( model, 0, "thetaEdge" ));
            thetaEdgeArray.push_back( thetaEdge );
        }

        logPMat = getMatrix( prhs[2] );
        modLMat = getMatrix( prhs[3] );
        for(size_t j = 0; j < thetaNodeArray.size(); ++j) {
            Eigen::VectorXd logPj = logPMat.row(j).transpose();
            Eigen::VectorXd modLj = modLMat.row(j).transpose();
            logPArray.push_back(logPj);
            modLArray.push_back(modLj);
        }

    } catch ( const std::invalid_argument& e ) {
        std::cerr << "Error loading arguments: " << e.what() << std::endl;
        fake_answer(plhs);
        return;
    }
    /* Load arguments into LPMRF models */
    typedef LPMRF< SparseMatrix<double> > LPMRFSparse;
    std::vector< LPMRFSparse > modelArray;
    for(size_t j = 0; j < thetaNodeArray.size(); ++j) {
        modelArray.push_back( LPMRFSparse(thetaNodeArray[j], thetaEdgeArray[j], logPArray[j], modLArray[j] ) );
    }

    /* Simply call C++ function */
    //std::cout << "Zt Before:" << std::endl << Zt << std::endl;
    learntopicmat( modelArray, 0, Zt );
    //std::cout << "Zt After:" << std::endl << Zt << std::endl;

    /* Save final answer into MATLAB output */
    if( nlhs >= 1 ) {
        plhs[0] = eigenSparse2matlabSparse( Zt );
    }
}


/**
 * DataInterface.cpp
 *
 * @author Krzysztof Okupski
 * @version 1.0
 */

#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>
#include "Main.cpp"

#include "Utilities.h"
#include "DataInterface.h"

using namespace DataInterface;

BOOST_AUTO_TEST_SUITE(DataInterfaceTests)

BOOST_AUTO_TEST_CASE(DataEncoding_Pubkey)
{
	for (unsigned int i = 0; i < 1000; i++)
	{
		DataBits originalData = Utilities::GenerateRandomBits(1+(i % 250));
		DataBits recoveredData;

		CPubKey pk = EncodeDataInPubkey(originalData, 255 - (1 + i % 250));
		recoveredData = DecodeDataInPubkey(pk, 255 - (1 + i % 250));

		BOOST_REQUIRE(originalData == recoveredData);
	}
}

BOOST_AUTO_TEST_CASE(DataEncoding_SeqNr)
{
	for (unsigned int i = 0; i < 1000; i++)
	{
		DataBits originalData = Utilities::GenerateRandomBits(32);
		DataBits recoveredData;

		unsigned int seqnr = EncodeDataInSequenceNr(originalData);
		recoveredData = DecodeDataInSequenceNr(seqnr);

		BOOST_REQUIRE(originalData == recoveredData);
	}
}

BOOST_AUTO_TEST_CASE(EmbeddableBits_TxAmounts)
{
	uint32_t nBits1 = EmbeddableBitsInValues(pow(10,8), 20);
	uint32_t nBits2 = EmbeddableBitsInValues(pow(10,15), 39);

	BOOST_REQUIRE(nBits1 == 448);
	BOOST_REQUIRE(nBits2 == 1744);
}

BOOST_AUTO_TEST_CASE(DataEncoding_TxAmounts)
{
	for (unsigned int i = 0; i < 1000; i++)
	{
		uint32_t MaxBits = EmbeddableBitsInValues(pow(10,3+(i%13)), 2+(i%29));

		DataBits originalData = Utilities::GenerateRandomBits(MaxBits);
		DataBits recoveredData;

		std::vector<uint64_t> vals = EncodeDataInValues(originalData, pow(10,3+(i%13)), 2+(i%29));
		recoveredData = DecodeDataInValues(vals);

		BOOST_REQUIRE(originalData == recoveredData);
	}
}

BOOST_AUTO_TEST_CASE(EmbeddableBits_TxPermutation)
{
	uint32_t nBits1 = EmbeddableBitsInPermutation(15);
	uint32_t nBits2 = EmbeddableBitsInPermutation(90);

	BOOST_REQUIRE(nBits1 == 40);
	BOOST_REQUIRE(nBits2 == 458);
}

BOOST_AUTO_TEST_CASE(DataEncoding_TxPermutations)
{
	for (unsigned int i = 0; i < 1000; i++)
	{
		uint32_t MaxBits = EmbeddableBitsInPermutation(2+(i % 39));

		DataBits originalData = Utilities::GenerateRandomBits(MaxBits);
		DataBits recoveredData;

		std::vector<uint16_t> perm = EncodeDataInPermutation(originalData, 2+(i % 39));
		recoveredData = DecodeDataInPermutation(perm);

		BOOST_REQUIRE(originalData == recoveredData);
	}
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE "root_name"

#include "SIMDMacros.h"

#include <boost/test/unit_test.hpp>

#include <iostream>

#if 0
BOOST_AUTO_TEST_CASE(BoostTestWorks)
{
    BOOST_CHECK_CLOSE(100.0,101.0,0.1);
    BOOST_CHECK(false);
    BOOST_CHECK_EQUAL(0,1);
}
#endif

BOOST_AUTO_TEST_CASE(OpLoad4i)
{
    uint32_t  __attribute__ ((aligned (SIMD_MEM_ALIGN))) data[4] = {1,2,3,4};
    uint32_t  __attribute__ ((aligned (SIMD_MEM_ALIGN))) result[4] = {};
    SIMD4i x;
    x = Load4i(data);

    Store4i(result, x);
    for(int i=0;i<4;++i)
    {
        BOOST_CHECK_EQUAL(result[i], i+1);
    }
}

BOOST_AUTO_TEST_CASE(OpLoad8i)
{
    uint32_t  __attribute__ ((aligned (SIMD_MEM_ALIGN))) data[8];
    uint32_t  __attribute__ ((aligned (SIMD_MEM_ALIGN))) result[8] = {};

    for(int i=0;i<8;++i)
    {
        data[i] = i;
    }
    SIMD8i x;
    x = Load8i(data);

    Store8i(result, x);
    for(int i=0;i<8;++i)
    {
        BOOST_CHECK_EQUAL(result[i], i);
    }
}

BOOST_AUTO_TEST_CASE(OpExtendto4i)
{
    uint16_t data[8];
    for(int i=0;i<8;++i)
        data[i] = i;
    data[0] = 0xffff;

    uint32_t result[4] = {};
    SIMD4i x;

    x = Load4i(data);
    x = ExtendLoto4i(x);
    Store4i(result, x);

    for(int i=0;i<4;++i)
    {
        BOOST_CHECK_EQUAL(result[i], data[i]);
    }

    x = Load4i(data);
    x = ExtendHito4i(x);
    Store4i(result, x);

    for(int i=0;i<4;++i)
    {
        BOOST_CHECK_EQUAL(result[i], data[i+4]);
    }
}

BOOST_AUTO_TEST_CASE(OpSet8i)
{
    uint32_t  __attribute__ ((aligned (SIMD_MEM_ALIGN))) data[8];
    for(int i=0;i<8;++i)
        data[i] = i;

    uint32_t  __attribute__ ((aligned (SIMD_MEM_ALIGN))) result[8] = {};
    SIMD4i x,y;
    SIMD8i z;

    x = Load4i(data);
    y = Load4i(data+4);
    z = Set8i(x,y);

    Store8i(result, z);

    for(int i=0;i<8;++i)
    {
        BOOST_CHECK_EQUAL(result[i], data[i]);
    }
}

BOOST_AUTO_TEST_CASE(SetAll8i16)
{
    uint16_t  __attribute__ ((aligned (SIMD_MEM_ALIGN))) result[8] = {};

    SIMD8i16 x;
    const uint16_t val = 5;

    x = SetAll8i16(val);

    Store4i(result, x);

    for(int i=0;i<8;++i)
    {
        BOOST_CHECK_EQUAL(result[i], val);
    }
}

BOOST_AUTO_TEST_CASE(OpCast8ito8f)
{
    uint32_t  __attribute__ ((aligned (SIMD_MEM_ALIGN))) data[8];
    for(int i=0;i<8;++i)
        data[i] = i;

    float  __attribute__ ((aligned (SIMD_MEM_ALIGN))) result[8];
    SIMD8i a;
    SIMD8f x;
    a = Load8i(data);
    x = Convert8ito8f(a);
    Store8f(result, x);

    for(int i=0;i<8;++i)
    {
        BOOST_CHECK_EQUAL(result[i],(float)i);
    }
}

BOOST_AUTO_TEST_CASE(OpMultiply8f)
{
    float  __attribute__ ((aligned (SIMD_MEM_ALIGN))) data[8];
    float  __attribute__ ((aligned (SIMD_MEM_ALIGN))) result[8]={};
    for(int i=0;i<8;++i)
        data[i] = i;

    SIMD8f x,y;
    x = Load8f(data);

    y = Multiply8f(x,x);
    Store8f(result, y);
    for(int i=0;i<8;++i)
    {
        BOOST_CHECK_EQUAL(result[i],(float)i*i);
    }
}

BOOST_AUTO_TEST_CASE(OpAdd8f)
{
    float  __attribute__ ((aligned (SIMD_MEM_ALIGN))) data[8];
    float  __attribute__ ((aligned (SIMD_MEM_ALIGN))) result[8]={};
    for(int i=0;i<8;++i)
        data[i] = i;

    SIMD8f x,y;
    x = Load8f(data);

    y = Add8f(x,x);
    Store8f(result, y);
    for(int i=0;i<8;++i)
    {
        BOOST_CHECK_EQUAL(result[i],(float)i+i);
    }
}

BOOST_AUTO_TEST_CASE(OpEqual8i)
{
    uint32_t  __attribute__ ((aligned (SIMD_MEM_ALIGN))) data[8];
    uint32_t  __attribute__ ((aligned (SIMD_MEM_ALIGN))) data2[8];
    uint32_t  __attribute__ ((aligned (SIMD_MEM_ALIGN))) result[8]={};

    for(int i=0;i<8;++i)
    {
      data[i] = i;
      data2[i] = i+1;
    }
    data2[0] = 0;

    SIMD8i a,b,c;
    a = Load8i(data);
    b = Load8i(data2);
    c = Equal8i(a,b);

    Store8i(result, c);

    for(int i=0;i<8;++i)
    {
        if(i==0)
        {
           BOOST_CHECK_EQUAL(result[i], 0xffffffffu);
        }
        else
        {
           BOOST_CHECK_EQUAL(result[i], 0);
        }
    }
}

BOOST_AUTO_TEST_CASE(OpSelectXorY8f)
{
    uint32_t  __attribute__ ((aligned (SIMD_MEM_ALIGN))) mask[8];
    float  __attribute__ ((aligned (SIMD_MEM_ALIGN))) result[8]={};

    for(int i=0;i<8;++i)
    {
        mask[i] = i%2 ? 0xffffffffu:0;
    }
    BOOST_CHECK(mask[1]);
    BOOST_CHECK(false == mask[0]);

    SIMD8f x,y,z;
    SIMD8i m;
    const float yval(5.0), xval(-4.0);
    x = SetAll8f(xval);
    y = SetAll8f(yval);
    m = Load8i(mask);

    z = SelectXorY8f(x,y,m);

    Store8f(result, z);
    for(int i=0;i<8;++i)
    {
        BOOST_CHECK_EQUAL(result[i], mask[i]?yval:xval);
    }
}