#include <boost/algorithm/string.hpp>
#include <boost/optional/optional_io.hpp>
#include <catch2/catch.hpp>
#include <map>
#include <numeric>
#include <string>
#include <vector>
#include "common/geom.hpp"
#include "ear/bs2051.hpp"
#include "helper/compare.hpp"

using namespace ear;

TEST_CASE("layout") {
  auto layout = getLayout("4+5+0");
  REQUIRE(layout.name() == "4+5+0");
  REQUIRE(layout.channelNames()[0] == "M+030");
  REQUIRE(layout.channelNames()[1] == "M-030");
  REQUIRE(layout.channels().size() == 10);
  REQUIRE(layout.channels()[0].polarPosition() ==
          PolarPosition{30.0, 0.0, 1.0});
  REQUIRE(layout.channels()[1].polarPosition() ==
          PolarPosition{-30.0, 0.0, 1.0});
}

TEST_CASE("unkown_layout") { REQUIRE_THROWS(getLayout("wat")); }

TEST_CASE("all_positions_in_range") {
  std::vector<std::string> errors;
  for (const auto& layout : loadLayouts()) {
    SECTION(layout.name()) {
      errors.clear();
      layout.checkPositions(
          [&errors](const std::string& msg) { errors.push_back(msg); });
      for (const auto& error : errors) {
        WARN(error);
      }
      REQUIRE(errors.size() == 0);
    }
  }
}

/*
Test that most ranges are reasonably small; this detects ranges that
have been inverted. Screen speakers and LFE channels are ignored.
*/
TEST_CASE("azimuth_ranges") {
  for (const auto& layout : loadLayouts()) {
    for (const auto& channel : layout.channels()) {
      if (!channel.isLfe() && channel.name().find("SC") == std::string::npos) {
        auto azRange = channel.azimuthRange();
        auto range_size =
            relativeAngle(azRange.first, azRange.second) - azRange.first;
        REQUIRE(range_size <= 180.0);
      }
    }
  }
}

TEST_CASE("test_symmetry") {
  for (const auto& layout : loadLayouts()) {
    // find symmetric pairs of speakers
    std::map<std::string, std::vector<Channel>> symmetricPairs;
    for (auto& channel : layout.channels()) {
      std::vector<std::string> channelNameVec;
      std::string name = channel.name();
      boost::split(channelNameVec, name, boost::is_any_of("+-"));
      if (channelNameVec.size() > 1) {
        std::string key;
        key = std::accumulate(begin(channelNameVec), end(channelNameVec), key);
        symmetricPairs[key].push_back(channel);
      } else {
        REQUIRE(channel.name().find("LFE") != std::string::npos);
        REQUIRE(channel.isLfe());
      }
    }
    for (const auto& entry : symmetricPairs) {
      auto key = entry.first;
      auto pair = entry.second;
      if (pair.size() == 1) {
        // any non-paired speakers should be on the centre line
        std::vector<double> values = {0.0, -180.0, 180.0};
        REQUIRE(std::find(values.begin(), values.end(),
                          pair[0].polarPosition().azimuth) != values.end());
      } else if (pair.size() == 2) {
        // all pairs should be have symmetrical positions and ranges
        Channel a = pair[0];
        Channel b = pair[1];

        REQUIRE(a.polarPosition().elevation == b.polarPosition().elevation);
        REQUIRE(a.polarPosition().azimuth == -b.polarPosition().azimuth);
        REQUIRE(a.elevationRange() == b.elevationRange());
        REQUIRE(a.azimuthRange() == std::make_pair(-b.azimuthRange().second,
                                                   -b.azimuthRange().first));
      } else {
        REQUIRE(false);
      }
    }
  }
}


/*******************************************************************************
 * Copyright (c) 2012, Dougal J. Sutherland (dsutherl@cs.cmu.edu).             *
 * All rights reserved.                                                        *
 *                                                                             *
 * Redistribution and use in source and binary forms, with or without          *
 * modification, are permitted provided that the following conditions are met: *
 *                                                                             *
 *     * Redistributions of source code must retain the above copyright        *
 *       notice, this list of conditions and the following disclaimer.         *
 *                                                                             *
 *     * Redistributions in binary form must reproduce the above copyright     *
 *       notice, this list of conditions and the following disclaimer in the   *
 *       documentation and/or other materials provided with the distribution.  *
 *                                                                             *
 *     * Neither the name of Carnegie Mellon University nor the                *
 *       names of the contributors may be used to endorse or promote products  *
 *       derived from this software without specific prior written permission. *
 *                                                                             *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" *
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE   *
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE  *
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE   *
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR         *
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF        *
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS    *
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN     *
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)     *
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE  *
 * POSSIBILITY OF SUCH DAMAGE.                                                 *
 ******************************************************************************/
#ifndef SDM_MODEL_HPP_
#define SDM_MODEL_HPP_
#include "sdm/basics.hpp"
#include "sdm/defaults.hpp"
#include "sdm/kernels/kernel.hpp"
#include "sdm/kernel_projection.hpp"
#include "sdm/log.hpp"
#include "sdm/tune_params.hpp"
#include "sdm/utils.hpp"

#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <ios>
#include <iostream>
#include <sstream>
#include <stdexcept>
#include <utility>
#include <vector>

#include <boost/exception_ptr.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <boost/thread.hpp>
#include <boost/utility.hpp>

#include <np-divs/matrix_arrays.hpp>
#include <np-divs/np_divs.hpp>
#include <flann/util/matrix.h>
#include <svm.h>

namespace sdm {

// TODO: optionally save and reuse flann indices and/or rhos, nus
//       (requires adding these options to npdivs...)

// TODO: probability models seem to just suck (at least on one example)
//       - is this a bug or something wrong with the model?
// TODO: memory ownership with this is all screwy...make it clearer
// TODO: switch from arbitrary typenames for label_type to an enum of kinds
template <typename Scalar, typename label_type>
class SDM {
    const svm_model &svm;
    const svm_problem &svm_prob; // needs to live at least as long as the model
    const npdivs::DivFunc *div_func;
    const Kernel *kernel;
    const npdivs::DivParams div_params;
    const size_t num_classes;

    const flann::Matrix<Scalar> *train_bags;
    const size_t num_train;
    const size_t dim;

    public:
        SDM(const svm_model &svm, const svm_problem &svm_prob,
            const npdivs::DivFunc &div_func, const Kernel &kernel,
            const npdivs::DivParams &div_params,
            size_t num_classes,
            const flann::Matrix<Scalar> *train_bags, size_t num_train,
            size_t dim=0)
        :
            svm(svm), svm_prob(svm_prob),
            div_func(new_clone(div_func)), kernel(new_clone(kernel)),
            div_params(div_params), num_classes(num_classes),
            train_bags(train_bags), num_train(num_train),
            dim(dim > 0 ? dim :
                (num_train > 0 && train_bags != NULL ? train_bags[0].cols : 0))
        { }

        ~SDM() {
            delete div_func;
            delete kernel;
        }

        void destroyModelAndProb();
        void destroyTrainBags();
        void destroyTrainBagMatrices();

        // getters and so on
        const Kernel *getKernel() const { return kernel; }
        const npdivs::DivFunc *getDivFunc() const { return div_func; }
        const svm_model *getSVM() const { return &svm; }
        const npdivs::DivParams *getDivParams() const { return &div_params; }

        bool isRegression() const;
        bool doesProbability() const {
            return svm_check_probability_model(&svm);
        }

        size_t getNumTrain() const { return num_train; }
        size_t getDim() const { return dim; }
        size_t getNumClasses() const { return num_classes; }
        const flann::Matrix<Scalar> * getTrainBags() { return train_bags; }

        std::string name() const {
            return (boost::format(
                "SDM(%d classes, dim %d, %s, kernel %s, C %g, %d training%s)")
                    % num_classes % dim
                    % div_func->name() % kernel->name()
                    % svm.param.C
                    % num_train
                    % (doesProbability() ? ", with prob" : "")
                ).str();
        }

        // Prediction functions, for either one or many test bags.
        // TODO: do we want dec values for regression? does it mean anything?
        //       or is there a good equivalent?
        //
        // The overloads which take a vector reference will put either decision
        // values or probabilities into it, depending on doesProbability().
        // Any existing contents will be discarded.
        //
        // For decision values:
        //    There are num_classes * (num_classes - 1) / 2 values, for all of
        //    the pairwise class comparisons.  The order is
        //        label[0]-vs-label[1], ..., label[0]-vs-label[n-1],
        //        label[1]-vs-label[2], ..., label[n-2] vs label[n-1],
        //    where the label ordering is as in svm_get_labels(getSVM(), ...).
        //
        // For probabilities:
        //    There is one estimate per class, in the same order as in
        //    getLabels().

        label_type predict(const flann::Matrix<Scalar> &test_bag) const;
        label_type predict(const flann::Matrix<Scalar> &test_bag,
                std::vector<double> &vals) const;

        std::vector<label_type> predict(
                const flann::Matrix<Scalar> *test_bags, size_t num_test)
            const;
        std::vector<label_type> predict(
                const flann::Matrix<Scalar> *test_bags, size_t num_test,
                std::vector< std::vector<double> > &vals)
            const;

        // Prediction if you already have kernel values; used in CV.
        // km should be a row-major flat matrix with num_test rows and
        // num_train columns.
        void predict_from_kerns(double* km, size_t num_test,
                std::vector<label_type> &preds,
                std::vector< std::vector<double> > &vals)
            const;
};

// Function to train a new SDM. Note that the caller is responsible for
// deleting the svm and svm_prob attributes, as well as train_bags.
// The destroyModelAndProb() and destroyTrainBags() functions might be
// helpful for this.
//
// NOTE: divs, if passed, is transformed into the kernel values.
//
// TODO: a mass-training method for more than one kernel
// TODO: option to do the projection on test data as well
// TODO: more flexible tuning CV options...tune on a subset of the data?
template <typename Scalar, typename label_type>
SDM<Scalar, label_type> * train_sdm(
    const flann::Matrix<Scalar> *train_bags, size_t num_train,
    const std::vector<label_type> &labels,
    const npdivs::DivFunc &div_func,
    const KernelGroup &kernel_group,
    const npdivs::DivParams &div_params,
    const std::vector<double> &c_vals = default_c_vals,
    const svm_parameter &svm_params = default_svm_params,
    size_t tuning_folds = 3,
    double *divs = NULL,
    bool allow_transduction = false);

// Function that gets divergences among the training and test data,
// projects them to be a valid kernel, trains an SDM on the training data,
// and returns its predicted labels for the test data.
// If passed, divs should be the divergences of
//      test-to-test   test-to-train
//      train-to-test  train-to-train
// as a row-major, (num_train + num_test) x (num_train + num_test) matrix.
template <typename Scalar, typename label_type>
std::vector<label_type> transduct_sdm(
    const flann::Matrix<Scalar> *train_bags, size_t num_train,
    const std::vector<label_type> &train_labels,
    const flann::Matrix<Scalar> *test_bags, size_t num_test,
    const npdivs::DivFunc &div_func,
    const KernelGroup &kernel_group,
    const npdivs::DivParams &div_params,
    const std::vector<double> &c_vals = default_c_vals,
    const svm_parameter &svm_params = default_svm_params,
    size_t tuning_folds = 3,
    double *divs = NULL);

// As above, but with training and test bags concatenated into the same
// array (training first).
// The above overload just copies the data and calls this one, so this is
// better if you can put it in this format initially.
template <typename Scalar, typename label_type>
std::vector<label_type> transduct_sdm(
    const flann::Matrix<Scalar> *train_test_bags,
    size_t num_train, size_t num_test,
    const std::vector<label_type> &train_labels,
    const npdivs::DivFunc &div_func,
    const KernelGroup &kernel_group,
    const npdivs::DivParams &div_params,
    const std::vector<double> &c_vals = default_c_vals,
    const svm_parameter &svm_params = default_svm_params,
    size_t tuning_folds = 3,
    double *divs = NULL);

////////////////////////////////////////////////////////////////////////////////
// Helper functions

namespace detail {

    // string representation of an SVM model
    std::string SVMtoString(const svm_model &model);

    // silly helper XXX - change to boost::is_same
    template <typename T> struct SVMType { static const int val; };
    template <> struct SVMType<int> { static const int val = C_SVC; };
    template <> struct SVMType<double> { static const int val = EPSILON_SVR; };
}

template <typename Scalar, typename label_type>
void SDM<Scalar, label_type>::destroyModelAndProb() {
    // FIXME: rework SDM memory model to avoid gross const_casts
    // TODO: rename
    // TODO: just have SDM copy everything it needs?

    svm_free_model_content(const_cast<svm_model*> (&svm));
    delete const_cast<svm_model*>(&svm);

    for (size_t i = 0; i < svm_prob.l; i++)
        delete[] svm_prob.x[i];
    delete[] svm_prob.x;
    delete[] svm_prob.y;
    delete const_cast<svm_problem*>(&svm_prob);
}

template <typename Scalar, typename label_type>
void SDM<Scalar, label_type>::destroyTrainBags() {
    npdivs::free_matrix_array(
            const_cast<flann::Matrix<Scalar> *>(train_bags),
            num_train);
}

template <typename Scalar, typename label_type>
void SDM<Scalar, label_type>::destroyTrainBagMatrices() {
    delete[] train_bags;
}

////////////////////////////////////////////////////////////////////////////////
// Training

template <typename Scalar, typename label_type>
SDM<Scalar, label_type> * train_sdm(
        const flann::Matrix<Scalar> *train_bags, size_t num_bags,
        const std::vector<label_type> &labels,
        const npdivs::DivFunc &div_func,
        const KernelGroup &kernel_group,
        const npdivs::DivParams &div_params,
        const std::vector<double> &c_vals,
        const svm_parameter &svm_params,
        size_t tuning_folds,
        double* divs,
        bool allow_transduction)
{
    if (c_vals.size() == 0) {
        BOOST_THROW_EXCEPTION(std::length_error("c_vals is empty"));
    } else if (labels.size() != num_bags) {
        if (!allow_transduction || labels.size() > num_bags)
            BOOST_THROW_EXCEPTION(std::length_error(
                        "labels.size() disagrees with num_bags"));
    }

    // copy the svm params so we can change them
    svm_parameter svm_p = svm_params;
    svm_p.svm_type = detail::SVMType<label_type>::val;
    svm_p.kernel_type = PRECOMPUTED;

    // make libSVM log properly
    svm_set_print_string_function(&log_string<logDEBUG4>);

    // first compute divergences, if necessary
    bool free_divs = false;
    if (divs == NULL) {
        FILE_LOG(logDEBUG) << "train: computing divergences";

        flann::Matrix<double>* divs_f =
            npdivs::alloc_matrix_array<double>(1, num_bags, num_bags);
        np_divs(train_bags, num_bags, div_func, divs_f, div_params, false);

        divs = divs_f[0].ptr();
        free_divs = true;
        delete[] divs_f; // just deletes the Matrix objects, not the data
    }

    // symmetrize divergence estimates
    symmetrize(divs, num_bags);

    // ask the kernel group for the kernels we'll pick from for tuning
    const boost::ptr_vector<Kernel>* kernels =
        kernel_group.getTuningVector(divs, num_bags);

    // tune_params will need this
    std::srand(unsigned(std::time(NULL)));

    // tuning: cross-validate over possible svm/kernel parameters
    FILE_LOG(logDEBUG) << "train: tuning parameters";
    const std::pair<size_t, size_t> &best_config =
        detail::tune_params<label_type>(divs, num_bags, labels, *kernels,
                c_vals, svm_p, tuning_folds, div_params.num_threads);

    // copy the kernel object so we can free the rest
    const Kernel *kernel = new_clone((*kernels)[best_config.first]);
    svm_p.C = c_vals[best_config.second];
    delete kernels; // FIXME: potential leaks in here if something crashes

    FILE_LOG(logINFO) << "train: using " << kernel->name() <<
        "; C = " << c_vals[best_config.second];

    // compute the final kernel matrix
    kernel->transformDivergences(divs, num_bags);
    project_to_symmetric_psd(divs, num_bags);

    FILE_LOG(logDEBUG3) << "train: final kernel matrix is:\n" <<
        detail::matrixToString(divs, num_bags, num_bags);

    // set up the svm_problem
    size_t num_train = labels.size();
    svm_problem *prob = new svm_problem;
    prob->l = num_train;
    prob->y = new double[num_train];
    for (size_t i = 0; i < num_train; i++)
        prob->y[i] = labels[i];

    if (num_train < num_bags) {
        double * train_divs = new double[num_train * num_train];
        detail::copy_upper(divs, train_divs, num_bags, num_train);
        detail::store_kernel_matrix(*prob, train_divs, true);
        delete[] train_divs;
    } else {
        detail::store_kernel_matrix(*prob, divs, true);
    }

    // don't need the kernel values anymore
    if (free_divs)
        delete[] divs;

    // check svm parameters
    const char* error = svm_check_parameter(prob, &svm_p);
    if (error != NULL) {
        FILE_LOG(logERROR) << "LibSVM parameter error: " << error;
        BOOST_THROW_EXCEPTION(std::domain_error(error));
    }

    // train away!
    FILE_LOG(logDEBUG) << "train: training SVM";
    svm_model *svm = svm_train(prob, &svm_p);
    FILE_LOG(logDEBUG3) << "train: final SVM:\n" << detail::SVMtoString(*svm);

    SDM<Scalar, label_type>* sdm = new SDM<Scalar, label_type>(
            *svm, *prob, div_func, *kernel, div_params, svm_get_nr_class(svm),
            train_bags, num_train);

    delete kernel; // copied in the constructor
    return sdm;
}

////////////////////////////////////////////////////////////////////////////////
// Transduction

template <typename Scalar, typename label_type>
std::vector<label_type> transduct_sdm(
    const flann::Matrix<Scalar> *train_bags, size_t num_train,
    const std::vector<label_type> &train_labels,
    const flann::Matrix<Scalar> *test_bags, size_t num_test,
    const npdivs::DivFunc &div_func,
    const KernelGroup &kernel_group,
    const npdivs::DivParams &div_params,
    const std::vector<double> &c_vals,
    const svm_parameter &svm_params,
    size_t tuning_folds,
    double *divs)
{
    typedef flann::Matrix<Scalar> Matrix;

    Matrix * combo_bags = new Matrix[num_train + num_test];
    for (size_t i = 0; i < num_train; i++)
        combo_bags[i] = train_bags[i];
    for (size_t i = 0; i < num_test; i++)
        combo_bags[num_train + i] = test_bags[i];

    return transduct_sdm(
            combo_bags, num_train, num_test, train_labels,
            div_func, kernel_group, div_params,
            c_vals, svm_params, tuning_folds, divs);
}

template <typename Scalar, typename label_type>
std::vector<label_type> transduct_sdm(
    const flann::Matrix<Scalar> *train_test_bags,
    size_t num_train, size_t num_test,
    const std::vector<label_type> &train_labels,
    const npdivs::DivFunc &div_func,
    const KernelGroup &kernel_group,
    const npdivs::DivParams &div_params,
    const std::vector<double> &c_vals,
    const svm_parameter &svm_params,
    size_t tuning_folds,
    double *divs)
{
    size_t num_bags = num_train + num_test;
    if (train_labels.size() != num_train) {
        throw std::invalid_argument(
                "tranduct: train_labels.size() disagrees with num_train");
    }

    // we want to make sure we're getting divs transformed into kernels by
    // train_sdm, so we don't have to recalculate them for prediction
    bool free_divs = false;
    if (divs == NULL) {
        FILE_LOG(logDEBUG) << "transduct: computing divergences";

        flann::Matrix<double>* divs_f =
            npdivs::alloc_matrix_array<double>(1, num_bags, num_bags);
        np_divs(train_test_bags, num_bags, div_func, divs_f, div_params, false);

        divs = divs_f[0].ptr();
        free_divs = true;
        delete[] divs_f; // just deletes the Matrix objects, not the data
    }

    // train the model
    FILE_LOG(logDEBUG) << "transduct: training model";
    SDM<Scalar, label_type> * model = train_sdm(
            train_test_bags, num_train + num_test, train_labels,
            div_func, kernel_group, div_params, c_vals, svm_params,
            tuning_folds, divs, true);
    FILE_LOG(logDEBUG) << "transduct: trained " << model->name();

    // get out the bottom-left block of divs, which is now kernel vals :)
    std::vector<double> test_to_train_kerns(num_test * num_train);
    for (size_t i = 0; i < num_test; i++) {
        double * row_start = divs + (num_train + i) * num_bags;

        std::copy(row_start, row_start + num_train,
                  test_to_train_kerns.begin() + i * num_train);
    }

    // do predictions
    FILE_LOG(logDEBUG) << "transduct: making predictions";
    std::vector<label_type> preds;
    std::vector< std::vector<double> > vals; // TODO: support returning these
    model->predict_from_kerns(&test_to_train_kerns[0], num_test, preds, vals);

    // clean up
    model->destroyModelAndProb();
    delete model;

    return preds;
}

////////////////////////////////////////////////////////////////////////////////
// Prediction

// one distribution
template <typename Scalar, typename label_type>
label_type SDM<Scalar, label_type>::predict(
        const flann::Matrix<Scalar> &test_bag)
const {
    std::vector< std::vector<double> > vals(1);
    return this->predict(&test_bag, 1, vals)[0];
}

// one distribution, with decision values
template <typename Scalar, typename label_type>
label_type SDM<Scalar, label_type>::predict(
        const flann::Matrix<Scalar> &test_bag,
        std::vector<double> &val)
const {
    std::vector< std::vector<double> > vals(1);
    const std::vector<label_type> &pred_labels =
        this->predict(&test_bag, 1, vals);
    val = vals[0];
    return pred_labels[0];
}

// many distributions
template <typename Scalar, typename label_type>
std::vector<label_type> SDM<Scalar, label_type>::predict(
        const flann::Matrix<Scalar> *test_bags, size_t num_test)
const {
    std::vector< std::vector<double> > vals(num_test);
    return this->predict(test_bags, num_test, vals);
}

// many distributions, with decision values
template <typename Scalar, typename label_type>
std::vector<label_type> SDM<Scalar, label_type>::predict(
        const flann::Matrix<Scalar> *test_bags, size_t num_test,
        std::vector< std::vector<double> > &vals)
const {
    // TODO: np_divs option to compute things both ways and/or save trees
    // TODO: only compute divergences from support vectors

    if (train_bags == NULL) {
        BOOST_THROW_EXCEPTION(std::domain_error("this SDM doesn't have "
                    "its training bags saved; can only predict_from_kerns"));
    }

    double *div_data = new double[num_test * num_train];
    flann::Matrix<double> divs(div_data, num_test, num_train);

    double *div_data_oth = new double[num_train * num_test];
    flann::Matrix<double> divs_oth(div_data_oth, num_train, num_test);

    // compute divergences
    npdivs::np_divs(test_bags, num_test, train_bags, num_train,
            *div_func, &divs, div_params);
    npdivs::np_divs(train_bags, num_train, test_bags, num_test,
            *div_func, &divs_oth, div_params);

    // symmetrize divergence estimates
    for (size_t i = 0; i < num_test; i++)
        for (size_t j = 0; j < num_train; j++)
            divs[i][j] = (divs_oth[j][i] + divs[i][j]) / 2.0;

    delete[] div_data_oth;

    // pass through the kernel
    kernel->transformDivergences(div_data, num_test, num_train);

    // can't project because it's not square...

    std::vector<label_type> preds;
    predict_from_kerns(div_data, num_test, preds, vals);
    delete[] div_data;
    return preds;
}

// many distributions, with decision values, from kernels
template <typename Scalar, typename label_type>
void SDM<Scalar, label_type>::predict_from_kerns(
        double* km, size_t num_test,
        std::vector<label_type> &preds,
        std::vector< std::vector<double> > &vals)
const {
    // figure out which prediction function we want to use
    bool do_prob = doesProbability();

    double (*pred_fn)(const svm_model*, const svm_node*, double*) =
        do_prob ? &svm_predict_probability : &svm_predict_values;

    size_t num_vals = do_prob
        ? num_classes : (num_classes * (num_classes - 1)) / 2;

    // we'll reuse this svm_node array for testing
    svm_node *kernel_row = new svm_node[num_train+2];
    for (size_t i = 0; i <= num_train; i++)
        kernel_row[i].index = i;
    kernel_row[num_train+1].index = -1;

    // predict!
    preds.resize(num_test);
    vals.resize(num_test);

    for (size_t i = 0; i < num_test; i++) {
        // fill in our kernel evaluations
        kernel_row[0].value = -i - 1;
        for (size_t j = 0; j < num_train; j++)
            kernel_row[j+1].value = km[i * num_train + j];

        // get space to store our decision/probability values
        vals[i].resize(num_vals);

        // ask the SVM for a prediction
        double res = pred_fn(&svm, kernel_row, &vals[i][0]);
        preds[i] = (label_type) res;
    }

    delete[] kernel_row;
}

} // end namespace

#endif


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * 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 "extend_pla.hpp"

#include "../io/read_pla.hpp"

#include <iostream>

#include <boost/assign/std/vector.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/iterator_range.hpp>

#include <cuddObj.hh>

using namespace boost::assign;

namespace cirkit
{

void extend_pla( binary_truth_table& base, binary_truth_table& extended, const extend_pla_settings& settings )
{
  // copy metadata
  extended.set_inputs( base.inputs() );
  extended.set_outputs( base.outputs() );

  // CUDD stuff
  Cudd mgr( 0, 0 );
  std::vector<BDD> vars( base.num_inputs() );
  boost::generate( vars, [&mgr]() { return mgr.bddVar(); } );

  // A function to create a BDD from a cube
  auto bddFromCube = [&mgr, &vars]( const std::pair<binary_truth_table::cube_type::const_iterator, binary_truth_table::cube_type::const_iterator>& cube ) {
    BDD c = mgr.bddOne();
    unsigned pos = 0u;
    for ( const auto& literal : boost::make_iterator_range( cube.first, cube.second ) ) {
      if ( literal ) c &= ( *literal ? vars.at( pos ) : !vars.at( pos ) );
      ++pos;
    }
    return c;
  };

  // A function to get truth table cubes from a BDD
  auto cubesFromBdd = [&mgr, &vars]( BDD& from, std::vector<binary_truth_table::cube_type>& cubes ) {
    char * cube = new char[vars.size()];
    unsigned pos;
    while ( from.CountMinterm( vars.size() ) > 0.0 ) {
      from.PickOneCube( cube );
      binary_truth_table::cube_type tcube;
      BDD bcube = mgr.bddOne();
      for ( pos = 0u; pos < vars.size(); ++pos ) {
        switch ( cube[pos] ) {
        case 0:
          bcube &= !vars.at( pos );
          tcube += false;
          break;
        case 1:
          bcube &= vars.at( pos );
          tcube += true;
          break;
        case 2:
          tcube += boost::optional<bool>();
          break;
        }
      }
      cubes += tcube;
      from &= !bcube;
    }

    delete[] cube;
  };

  while ( std::distance( base.begin(), base.end() ) > 0 )
  {
    // Pick one cube from base
    binary_truth_table::iterator base_cube = base.begin();
    binary_truth_table::cube_type base_in( base_cube->first.first, base_cube->first.second );
    binary_truth_table::cube_type base_out( base_cube->second.first, base_cube->second.second );

    if ( settings.verbose )
    {
      std::cout << "[I] Processing:" << std::endl;
      std::cout << "[I] ";
      std::for_each( base_cube->first.first, base_cube->first.second, []( const boost::optional<bool>& b ) { std::cout << (b ? (*b ? "1" : "0") : "-"); } );
      std::cout << " ";
      std::for_each( base_cube->second.first, base_cube->second.second, []( const boost::optional<bool>& b ) { std::cout << (b ? (*b ? "1" : "0") : "-"); } );
      std::cout << std::endl;
    }

    BDD bicube = bddFromCube( base_cube->first );
    base.remove_entry( base_cube );

    // Go through all cubes of extended
    bool found_match = false;
    for ( binary_truth_table::iterator extended_cube = extended.begin(); extended_cube != extended.end(); ++extended_cube )
    {
      BDD bocube = bddFromCube( extended_cube->first );

      if ( ( bicube & bocube ).CountMinterm( base.num_inputs() ) > 0.0 )
      {
        if ( settings.verbose )
        {
          std::cout << "[I] Intersection detected with" << std::endl;
          std::cout << "[I] ";
          std::for_each( extended_cube->first.first, extended_cube->first.second, []( const boost::optional<bool>& b ) { std::cout << (b ? (*b ? "1" : "0") : "-"); } );
          std::cout << " ";
          std::for_each( extended_cube->second.first, extended_cube->second.second, []( const boost::optional<bool>& b ) { std::cout << (b ? (*b ? "1" : "0") : "-"); } );
          std::cout << std::endl;
        }
        binary_truth_table::cube_type extended_in( extended_cube->first.first, extended_cube->first.second );
        binary_truth_table::cube_type extended_out( extended_cube->second.first, extended_cube->second.second );

        extended.remove_entry( extended_cube );

        BDD keep_in_base = bicube & !bocube;
        BDD intersection = bicube & bocube;
        BDD keep_in_extended = !bicube & bocube;

        std::vector<binary_truth_table::cube_type> cubes;

        cubesFromBdd( keep_in_base, cubes );
        for ( const auto& cube : cubes )
        {
          bool match = false;
          for ( binary_truth_table::iterator base_inner_cube = base.begin(); base_inner_cube != base.end(); ++base_inner_cube )
          {
            binary_truth_table::cube_type base_inner_in( base_inner_cube->first.first, base_inner_cube->first.second );
            binary_truth_table::cube_type base_inner_out( base_inner_cube->second.first, base_inner_cube->second.second );

            if ( cube == base_inner_in )
            {
              base.remove_entry( base_inner_cube );
              base.add_entry( cube, combine_pla_cube( base_out, base_inner_out ) );
              match = true;
              break;
            }
          }

          if ( !match )
          {
            base.add_entry( cube, base_out );
          }
        }
        cubes.clear();

        cubesFromBdd( intersection, cubes );
        for ( const auto& cube : cubes ) extended.add_entry( cube, combine_pla_cube( base_out, extended_out ) );
        cubes.clear();

        cubesFromBdd( keep_in_extended, cubes );
        for ( const auto& cube : cubes ) extended.add_entry( cube, extended_out );

        found_match = true;
        break;
      }
    }

    // Copy the base_cube if no match has been found
    if ( !found_match )
    {
      if ( settings.verbose )
      {
        std::cout << "[I] Add directly!" << std::endl;
      }
      extended.add_entry( base_in, base_out );
    }

    if ( settings.verbose )
    {
      std::cout << "[I] base:" << std::endl;
      std::cout << base << std::endl;
      std::cout << "[I] extended:" << std::endl;
      std::cout << extended << std::endl << std::endl;
    }
  }

  /* Compact */
  if ( settings.post_compact ) {
    // Compute compacted nouns
    std::map<binary_truth_table::cube_type, BDD> compacted_monoms;
    for ( const auto& row : extended ) {
      BDD in_cube = bddFromCube( row.first );
      binary_truth_table::cube_type out_pattern( row.second.first, row.second.second );

      auto it = compacted_monoms.find( out_pattern );
      if ( it == compacted_monoms.end() ) {
        compacted_monoms[out_pattern] = in_cube;
      } else {
        it->second |= in_cube;
      }
    }

    // Clear extended PLA representation
    extended.clear();

    // Add compacted monoms back to PLA representation
    for ( const auto& p : compacted_monoms ) {
      std::vector<binary_truth_table::cube_type> cubes;

      BDD bdd = p.second;
      cubesFromBdd( bdd, cubes );
      for ( const auto& cube : cubes ) extended.add_entry( cube, p.first );
    }
  }
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SDK_MEMORY_IS_ALIGNED_HPP_INCLUDED
#define BOOST_SIMD_SDK_MEMORY_IS_ALIGNED_HPP_INCLUDED

#include <boost/simd/sdk/memory/parameters.hpp>
#include <boost/simd/sdk/memory/is_power_of_2.hpp>
#include <boost/dispatch/attributes.hpp>
#include <boost/assert.hpp>
#include <cstddef>

namespace boost { namespace simd
{
  /*!
    @brief Check a value or address is aligned on an arbitrary alignment boundary

    @param value Value to check
    @param align Alignment boundary to check for.
  **/
  BOOST_FORCEINLINE bool is_aligned(std::size_t value, std::size_t align)
  {
    BOOST_ASSERT_MSG
    ( boost::simd::is_power_of_2(align)
    , "Invalid alignment boundary. You tried to check if "
      "an address or a value is aligned on a non-power of 2 boundary."
    );

    return !(value & (align-1) );
  }

  /*! @overload **/
  template<class T> BOOST_FORCEINLINE bool is_aligned(T* ptr, std::size_t align)
  {
    return boost::simd::is_aligned(reinterpret_cast<std::size_t>(ptr),align);
  }

  /*!
    @brief Check a value or address is aligned on default alignment boundary

    @param value Value to check
  **/
  template<class T> BOOST_FORCEINLINE bool is_aligned(T value)
  {
    return boost::simd::is_aligned(value,BOOST_SIMD_CONFIG_ALIGNMENT);
  }
} }

#endif


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

  Defines the as_simd metafunction for AVX like extensions

  @copyright 2012 - 2015 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_X86_AVX_AS_SIMD_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX_AS_SIMD_HPP_INCLUDED

#include <boost/simd/arch/x86/sse2/as_simd.hpp>

namespace boost { namespace simd
{
  template<typename T> struct logical;

  namespace ext
  {
    template<typename T>
    struct as_simd<logical<T>, boost::simd::avx_> : as_simd<T, boost::simd::avx_> {};

    template<> struct as_simd<float, boost::simd::avx_>
    {
      using type = __m256;
    };

    template<> struct as_simd<double, boost::simd::avx_>
    {
      using type = __m256d;
    };
  }
} }

#endif


#ifndef STATIC_LINK_VW
#define BOOST_TEST_DYN_LINK
#endif

#include <boost/test/unit_test.hpp>
#include <boost/mpl/vector.hpp>

#include "array_parameters.h"
#include "array_parameters_dense.h"

#include "test_common.h"

constexpr auto LENGTH = 16;
constexpr auto STRIDE_SHIFT = 2;

using weight_types = boost::mpl::vector<sparse_parameters, dense_parameters>;

BOOST_AUTO_TEST_CASE_TEMPLATE(test_default_function_weight_initialization_strided_index, T, weight_types)
{
  T w(LENGTH, STRIDE_SHIFT);
  auto weight_initializer = [](weight* weights, uint64_t index) { weights[0] = 1.f * index; };
  w.set_default(weight_initializer);
  for(size_t i = 0; i < LENGTH; i++)
  {
    BOOST_CHECK_CLOSE(w.strided_index(i), 1.f * (i * w.stride()) , FLOAT_TOL);
  }
}


#include <splx/curve/PiecewiseCurve.hpp>
#include <boost/filesystem/operations.hpp>
#include <ios>
#include <rlss/CollisionShapes/AlignedBoxCollisionShape.hpp>
#include <rlss/OccupancyGrid.hpp>
#include <rlss/RLSS.hpp>
#include <iostream>
#include "../third_party/cxxopts.hpp"
#include "../third_party/json.hpp"
#include <fstream>
#include <boost/filesystem.hpp>
#include <splx/opt/PiecewiseCurveQPGenerator.hpp>
#include <splx/curve/Bezier.hpp>
#include <splx/curve/PiecewiseCurve.hpp>
#include <memory>
//#include <rlss/internal/LegacyJSONBuilder.hpp>
#include <rlss/internal/JSONBuilder.hpp>
#include <rlss/TrajectoryOptimizers/RLSSHardOptimizer.hpp>
#include <rlss/TrajectoryOptimizers/RLSSSoftOptimizer.hpp>
#include <rlss/TrajectoryOptimizers/RLSSHardSoftOptimizer.hpp>
#include <rlss/DiscretePathSearchers/RLSSDiscretePathSearcher.hpp>
#include <rlss/ValidityCheckers/RLSSValidityChecker.hpp>
#include <rlss/GoalSelectors/RLSSGoalSelector.hpp>
#include <random>

constexpr unsigned int DIM = SIMULATION_DIMENSION; // must be provided as define

namespace fs = boost::filesystem;
using RLSS = rlss::RLSS<double, DIM>;
using OccupancyGrid = rlss::OccupancyGrid<double, DIM>;
using AlignedBoxCollisionShape = rlss::AlignedBoxCollisionShape<double, DIM>;
using CollisionShape = rlss::CollisionShape<double, DIM>;
using StdVectorVectorDIM = OccupancyGrid::StdVectorVectorDIM;
using VectorDIM = OccupancyGrid::VectorDIM;
using MatrixDIMDIM = rlss::internal::MatrixRC<double, DIM, DIM>;
using PiecewiseCurveQPGenerator
= splx::PiecewiseCurveQPGenerator<double, DIM>;
using PiecewiseCurve = splx::PiecewiseCurve<double, DIM>;
using Bezier = splx::Bezier<double, DIM>;
using AlignedBox = OccupancyGrid::AlignedBox;
using Ellipsoid = rlss::Ellipsoid<double, DIM>;
using RLSSHardOptimizer = rlss::RLSSHardOptimizer<double, DIM>;
using RLSSSoftOptimizer = rlss::RLSSSoftOptimizer<double, DIM>;
using RLSSHardSoftOptimizer = rlss::RLSSHardSoftOptimizer<double, DIM>;
using RLSSDiscretePathSearcher = rlss::RLSSDiscretePathSearcher<double, DIM>;
using RLSSValidityChecker = rlss::RLSSValidityChecker<double, DIM>;
using RLSSGoalSelector = rlss::RLSSGoalSelector<double, DIM>;
using TrajectoryOptimizer = rlss::TrajectoryOptimizer<double, DIM>;
using DiscretePathSearcher = rlss::DiscretePathSearcher<double, DIM>;
using ValidityChecker = rlss::ValidityChecker<double, DIM>;
using GoalSelector = rlss::GoalSelector<double, DIM>;
using JSONBuilder = rlss::internal::JSONBuilder<double, DIM>;

bool allRobotsReachedFinalStates(
        const std::vector<RLSS>& planners,
        const std::vector<PiecewiseCurve>& original_trajectories,
        const std::vector<StdVectorVectorDIM>& states,
        double reach_distance
) {
    for(std::size_t i = 0; i < planners.size(); i++) {
        VectorDIM goal_position
                = original_trajectories[i].eval(
                        original_trajectories[i].maxParameter(), 0);

        double dist = (goal_position - states[i][0]).norm();

        if(dist > reach_distance) {
            std::cout << "robot " << i << " distance: " << dist << std::endl;
            return false;
        }
    }
    return true;
}

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

    std::random_device rd;
    std::mt19937 gen{rd()};

    cxxopts::Options options("RLSS", "");
    options.add_options()
            (
                    "c,config",
                    "Config file path",
                    cxxopts::value<std::string>()
                            ->default_value(
                                    absl::StrCat(
                                            "../examples/",
                                            DIM,
                                            "d_config.json"
                                    )
                            )
            )
            (
                    "h, help",
                    "help"
            )
            ;

    auto parsed_options = options.parse(argc, argv);

    if(parsed_options.count("help")) {
        std::cout << options.help() << std::endl;
        return 0;
    }

    std::fstream json_fs(
            parsed_options["config"].as<std::string>(), std::ios_base::in);
    nlohmann::json config_json = nlohmann::json::parse(json_fs);
    json_fs.close();

    std::string base_path = config_json["base_path"];
    std::string obstacle_directory = config_json["obstacle_directory"];
    std::string robot_parameters_directory
            = config_json["robot_parameters_directory"];
    double replanning_period = config_json["replanning_period"];
    double reach_distance = config_json["reach_distance"];
    std::vector<double> occupancy_grid_step_size
            = config_json["occupancy_grid_step_size"];

    OccupancyGrid::Coordinate step_size;
    for(unsigned int i = 0; i < DIM; i++) {
        step_size(i) = occupancy_grid_step_size[i];
    }
    OccupancyGrid occupancy_grid(step_size);

    for(auto& p: fs::directory_iterator(base_path + obstacle_directory)) {
        std::fstream obstacle_file(p.path().string(), std::ios_base::in);
        std::string type;
        obstacle_file >> type;
        if(type == "cvxhull") {
            StdVectorVectorDIM obstacle_pts;
            VectorDIM vec;
            while (obstacle_file >> vec(0)) {
                for(unsigned int d = 1; d < DIM; d++)
                    obstacle_file >> vec(d);
                obstacle_pts.push_back(vec);
            }
            occupancy_grid.addObstacle(obstacle_pts);
        } else if(type == "ellipsoid") {
            VectorDIM center;
            MatrixDIMDIM mtr;

            for(unsigned int i = 0; i < DIM; i++) {
                obstacle_file >> center(i);
            }

            for(unsigned int r = 0; r < DIM; r++) {
                for(unsigned int c = 0; c < DIM; c++) {
                    obstacle_file >> mtr(r, c);
                }
            }
            Ellipsoid ell(center, mtr);
            occupancy_grid.addObstacle(ell);

        }
    }

    std::cout << "num obstacles: " << occupancy_grid.size() << std::endl;

    std::vector<RLSS> planners;
    std::vector<std::shared_ptr<CollisionShape>> collision_shapes;
    std::vector<PiecewiseCurve> original_trajectories;
    std::vector<unsigned int> contUpto;

    // for each robot there is a vector of pairs where each pair
    // is (degree, distribution)
    std::vector<
            std::vector<
                    std::pair<unsigned int, std::normal_distribution<>
                    >
            >
    > noise;

    for(auto& p:
            fs::directory_iterator(base_path + robot_parameters_directory)
            ) {
        std::cout << p.path().string() << std::endl;
        std::fstream robot_description(p.path().string(), std::ios_base::in);
        nlohmann::json robot_json = nlohmann::json::parse(robot_description);

        PiecewiseCurve original_trajectory;
        for(nlohmann::json piece_json
                : robot_json["original_trajectory"]["pieces"]) {
            if(piece_json["type"] == "BEZIER") {
                Bezier piece(piece_json["duration"]);
                for(const std::vector<double>& cpt
                        : piece_json["control_points"]) {
                    assert(cpt.size() == DIM);
                    VectorDIM cpt_vec;
                    for(unsigned int d = 0; d < DIM; d++)
                        cpt_vec(d) = cpt[d];

                    piece.appendControlPoint(cpt_vec);
                }
                original_trajectory.addPiece(piece);
            }
        }
        original_trajectories.push_back(original_trajectory);

        PiecewiseCurveQPGenerator qp_generator;
        nlohmann::json pieces_json;
        if(config_json.contains("plan_for_trajectory")) {
            // override robot json
            pieces_json = config_json["plan_for_trajectory"]["pieces"];
        } else {
            pieces_json = robot_json["plan_for_trajectory"]["pieces"];
        }

        for(const nlohmann::json& plan_for_piece: pieces_json) {
            if(plan_for_piece["type"] == "BEZIER") {
                qp_generator.addBezier(plan_for_piece["num_control_points"], 0);
            } else {
                throw std::domain_error (
                        absl::StrCat(
                                "curve type not handled"
                        )
                );
            }
        }

        double search_step = robot_json["search_step"];
        double rescaling_multiplier = robot_json["rescaling_multiplier"];
        unsigned int max_rescaling_count
                = config_json.contains("max_rescaling_count")
                  ? config_json["max_rescaling_count"]
                  : robot_json["max_rescaling_count"];
        double desired_time_horizon = robot_json["desired_time_horizon"];
        unsigned int continuity_upto_degree
                = config_json.contains("continuity_upto_degree")
                  ? config_json["continuity_upto_degree"]
                  : robot_json["continuity_upto_degree"];
        contUpto.push_back(continuity_upto_degree);
        std::vector<std::pair<unsigned int, double>>
                maximum_derivative_magnitudes;
        for(const std::pair<unsigned int, double>& der_mag
                : (config_json.contains("maximum_derivative_magnitudes") ?
                   config_json["maximum_derivative_magnitudes"]
                 : robot_json["maximum_derivative_magnitudes"])
                ) {
            maximum_derivative_magnitudes.push_back(der_mag);
        }
        std::vector<std::pair<unsigned int, double>>
                integrated_squared_derivative_weights;
        for(const std::pair<unsigned int, double>& deg_weight
                : robot_json["integrated_squared_derivative_weights"]) {
            integrated_squared_derivative_weights.push_back(deg_weight);
        }
        std::vector<double> piece_endpoint_cost_weights;
        for(const double weight: robot_json["piece_endpoint_cost_weights"]) {
            piece_endpoint_cost_weights.push_back(weight);
        }

        nlohmann::json colshapeatzero;
        if(config_json.contains("collision_shape_at_zero")) {
            colshapeatzero = config_json["collision_shape_at_zero"];
        } else {
            colshapeatzero = robot_json["collision_shape_at_zero"];
        }

        VectorDIM colshapemin, colshapemax;
        for(unsigned int d = 0; d < DIM; d++) {
            colshapemin(d) = colshapeatzero[0][d];
            colshapemax(d) = colshapeatzero[1][d];
        }

        auto albox_col_shape = std::make_shared<AlignedBoxCollisionShape>(
                AlignedBox(colshapemin, colshapemax));
        auto collision_shape = std::static_pointer_cast<CollisionShape>(
                albox_col_shape);

        collision_shapes.push_back(collision_shape);

        nlohmann::json workspace_json;
        if(config_json.contains("workspace")) {
            workspace_json = config_json["workspace"];
        } else {
            workspace_json = robot_json["workspace"];
        }

        VectorDIM workspacemin, workspacemax;

        for(unsigned int d = 0; d < DIM; d++) {
            workspacemin(d) = workspace_json[0][d];
            workspacemax(d) = workspace_json[1][d];
        }

        AlignedBox workspace(workspacemin, workspacemax);

        std::string optimizer = config_json.contains("optimizer")
                                ? config_json["optimizer"] : robot_json["optimizer"];

        // parameter -> [enabled, weight]
        std::unordered_map<std::string, std::pair<bool, double>>
                soft_optimization_parameters;
        nlohmann::json sop_json
                = config_json.contains("soft_optimization_parameters") ?
                  config_json["soft_optimization_parameters"] :
                  (robot_json.contains("soft_optimization_parameters") ?
                   robot_json["soft_optimization_parameters"]
                   : nlohmann::json());

        for(auto it = sop_json.begin(); it != sop_json.end(); it++) {
            soft_optimization_parameters[it.key()]
                    = std::make_pair(it.value()["enable"], it.value()["weight"]);
        }

        double optimization_obstacle_check_distance
                = config_json.contains("optimization_obstacle_check_distance") ?
                  config_json["optimization_obstacle_check_distance"] :
                  robot_json["optimization_obstacle_check_distance"];

        nlohmann::json noise_json =
                config_json.contains("noise") ?
                config_json["noise"] :
                (robot_json.contains("noise") ?
                 robot_json["noise"] :
                 nlohmann::json()
                );

        noise.push_back(
                std::vector<
                        std::pair<unsigned int, std::normal_distribution<>>
                >()
        );
        for(const auto& n : noise_json) {
            noise.back().emplace_back(
                    n[0],
                    std::normal_distribution<>(n[1], n[2])
            );
        }

        auto rlss_goal_selector = std::make_shared<RLSSGoalSelector>
                (
                        desired_time_horizon,
                        original_trajectory,
                        workspace,
                        collision_shape,
                        search_step
                );
        auto goal_selector
                = std::static_pointer_cast<GoalSelector>(rlss_goal_selector);

        double maximum_velocity = std::numeric_limits<double>::max();
        for(const auto& [d, v]: maximum_derivative_magnitudes) {
            if(d == 1) {
                maximum_velocity = v;
                break;
            }
        }
        auto rlss_discrete_path_searcher
                = std::make_shared<RLSSDiscretePathSearcher>
                        (
                                replanning_period,
                                workspace,
                                collision_shape,
                                maximum_velocity,
                                qp_generator.numPieces()
                        );
        auto discrete_path_searcher
                = std::static_pointer_cast<DiscretePathSearcher>(
                        rlss_discrete_path_searcher);

        std::shared_ptr<TrajectoryOptimizer> trajectory_optimizer;
        if(optimizer == "rlss-hard-soft") {
            auto rlss_hard_soft_optimizer = std::make_shared<RLSSHardSoftOptimizer>
                    (
                            collision_shape,
                            qp_generator,
                            workspace,
                            continuity_upto_degree,
                            integrated_squared_derivative_weights,
                            piece_endpoint_cost_weights,
                            soft_optimization_parameters,
                            optimization_obstacle_check_distance
                    );
            trajectory_optimizer
                    = std::static_pointer_cast<TrajectoryOptimizer>(
                    rlss_hard_soft_optimizer);
        } else if (optimizer == "rlss-soft") {
            auto rlss_soft_optimizer = std::make_shared<RLSSSoftOptimizer>
                    (
                            collision_shape,
                            qp_generator,
                            workspace,
                            continuity_upto_degree,
                            integrated_squared_derivative_weights,
                            piece_endpoint_cost_weights,
                            soft_optimization_parameters,
                            optimization_obstacle_check_distance
                    );
            trajectory_optimizer
                    = std::static_pointer_cast<TrajectoryOptimizer>(
                    rlss_soft_optimizer);
        } else if (optimizer == "rlss-hard") {
            auto rlss_hard_optimizer = std::make_shared<RLSSHardOptimizer>
                    (
                            collision_shape,
                            qp_generator,
                            workspace,
                            continuity_upto_degree,
                            integrated_squared_derivative_weights,
                            piece_endpoint_cost_weights,
                            optimization_obstacle_check_distance
                    );
            trajectory_optimizer
                    = std::static_pointer_cast<TrajectoryOptimizer>(
                    rlss_hard_optimizer);
        } else {
            throw std::domain_error(
                    absl::StrCat(
                            "optimizer ",
                            optimizer,
                            " not recognized."
                    )
            );
        }

        auto rlss_validity_checker = std::make_shared<RLSSValidityChecker>
                (
                        maximum_derivative_magnitudes,
                        search_step
                );
        auto validity_checker
                = std::static_pointer_cast<ValidityChecker>(
                        rlss_validity_checker);

        planners.emplace_back(
                goal_selector,
                trajectory_optimizer,
                discrete_path_searcher,
                validity_checker,
                max_rescaling_count,
                rescaling_multiplier
        );

    }

    unsigned int num_robots = planners.size();

    std::cout << "num robots: " << num_robots << std::endl;

    std::vector<StdVectorVectorDIM> states(num_robots);
    for(unsigned int i = 0; i < num_robots; i++) {
        for(unsigned int j = 0; j <= contUpto[i]; j++) {
            states[i].push_back(original_trajectories[i].eval(0, j));
        }
    }

    JSONBuilder json_builder;
    json_builder.setRobotCount(num_robots);
    for(std::size_t i = 0; i < num_robots; i++) {
        json_builder.setRobotShape(i, collision_shapes[i]->boundingBox(VectorDIM::Zero()));
    }

    for(unsigned int r_id = 0; r_id < num_robots; r_id++) {
        json_builder.setOriginalTrajectory(
                r_id,
                original_trajectories[r_id]
        );
    }
    json_builder.setFrameDt(0.01);
    json_builder.addOccupancyGridToCurrentFrame(occupancy_grid);

    double current_time = 0;
    std::vector<PiecewiseCurve> trajectories(num_robots);
    std::vector<double> trajectory_current_times(num_robots, 0);

    while(!allRobotsReachedFinalStates(
            planners, original_trajectories, states, reach_distance)) {
        rlss::debug_message("starting the loop for time ", current_time, "...");

        rlss::debug_message("collecting robot shapes...");
        std::vector<AlignedBox> robot_collision_boxes(num_robots);
        for(std::size_t i = 0; i < num_robots; i++) {
            robot_collision_boxes[i]
                    = collision_shapes[i]->boundingBox(states[i][0]);
        }

        for(std::size_t i = 0; i < planners.size(); i++) {
            rlss::debug_message("planning for robot ", i, "...");
            std::vector<AlignedBox> other_robot_collision_boxes
                    = robot_collision_boxes;
            other_robot_collision_boxes.erase(
                    other_robot_collision_boxes.begin() + i);

//            double cutoff_duration = 100000; // us
//            auto plan_start_time = std::chrono::steady_clock::now();
            std::optional<PiecewiseCurve> curve
                    = planners[i].plan(
                            current_time,
                            states[i],
                            other_robot_collision_boxes,
                            occupancy_grid
                    );
//            auto plan_end_time = std::chrono::steady_clock::now();
//            auto planning_duration = std::chrono::duration_cast<std::chrono::microseconds>(
//                        plan_end_time - plan_start_time
//                ).count();

            if(curve /*&& planning_duration < cutoff_duration*/) {
                rlss::debug_message(
                        rlss::internal::debug::colors::GREEN,
                        "planning successful.",
                        rlss::internal::debug::colors::RESET
                );
                trajectories[i] = *curve;
                trajectory_current_times[i] = 0;
                json_builder.addTrajectoryToCurrentFrame(i, trajectories[i]);
            } else {
                rlss::debug_message(
                        rlss::internal::debug::colors::RED,
                        "planning failed.",
                        rlss::internal::debug::colors::RESET
                );
                trajectory_current_times[i] += replanning_period;
            }

            for(std::size_t j = 0; j < states[i].size(); j++) {
                if(j == 0) {
                    json_builder.setRobotPositionInCurrentFrame(
                            i, states[i][j]);
                }
//                std::cout << "robot " << i << " eval" << std::endl;
                states[i][j] = trajectories[i].eval(
                        std::min(
                                trajectory_current_times[i] + replanning_period,
                                trajectories[i].maxParameter()
                        ),
                        j
                );
                for(auto& n: noise[i]) {
                    if(n.first == j) {
                        VectorDIM noise_addition;
                        for(unsigned int d = 0; d < DIM; d++) {
                            noise_addition(d) = n.second(gen);
                        }
                        states[i][j] += noise_addition;
                    }
                }
            }
        }

        json_builder.nextFrame();
        for(double t = 0.01; t < replanning_period - 0.005; t += 0.01) {
            for(std::size_t i = 0; i < num_robots; i++) {
                json_builder.setRobotPositionInCurrentFrame(
                        i,
                        trajectories[i].eval(
                                std::min(
                                        trajectory_current_times[i] + t,
                                        trajectories[i].maxParameter()
                                ),
                                0
                        )
                );
            }
            json_builder.nextFrame();
        }

        json_builder.save("vis.json");
        rlss::internal::StatisticsStorage<double> all_stats;
        for(const auto& planner: planners) {
//        planner.statisticsStorage().save();
            all_stats += planner.statisticsStorage();
        }

        all_stats.save("all_stats.json");
        current_time += replanning_period;
    }

    rlss::internal::StatisticsStorage<double> all_stats;
    for(const auto& planner: planners) {
//        planner.statisticsStorage().save();
        all_stats += planner.statisticsStorage();
    }

    all_stats.save("all_stats.json");

    json_builder.save("vis.json");

    return 0;
}


/**************************************************************************\
|
|    Copyright (C) 2009 Marc Stevens
|
|    This program is free software: you can redistribute it and/or modify
|    it under the terms of the GNU General Public License as published by
|    the Free Software Foundation, either version 3 of the License, or
|    (at your option) any later version.
|
|    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
|    GNU General Public License for more details.
|
|    You should have received a copy of the GNU General Public License
|    along with this program.  If not, see <http://www.gnu.org/licenses/>.
|
\**************************************************************************/

#ifndef MAIN_HPP
#define MAIN_HPP

#include <iostream>
#include <vector>
#include <string>

#include <boost/filesystem/operations.hpp>

#include <hashclash/sdr.hpp>
#include <hashclash/saveload_bz2.hpp>
#include <hashclash/sha1differentialpath.hpp>
#include <hashclash/sha1messagespace.hpp>

using namespace hashclash;
using namespace std;

struct parameters_type {
	parameters_type()
	{
		usetunnelbitconditions = false;
		tunneldeepanalysis = false;
		changebitstat = false;
		mecarry = 0;
		threebittunnels = true;
		simpletunnels = false;
		mintunnelpov = -1;
	}

	vector<string> rnd234_m_bitrelationfiles;
	string rnd234_pathfile; // single path
	string rnd1_pathsfile; // possibly multiple paths allowing choice without m bitrelation contradictions
	unsigned tend_rnd1_me;

	unsigned mod,index;
	uint32 m_mask[80];
	bool usetunnelbitconditions;
	std::string tunnelfile;
	unsigned mecarry;
	bool tunneldeepanalysis;
	bool changebitstat;
	bool tunnelfilterbitcondition;
	bool threebittunnels;
	bool simpletunnels;
	int mintunnelpov;
	void show_mdiffs()
	{
	}
};
extern std::string workdir;
extern sha1differentialpath maindiffpath;
extern sha1messagespace mainmespace;
extern parameters_type mainparameters;
const int offset = 4;

unsigned load_block(istream& i, uint32 block[]);
void save_block(ostream& o, uint32 block[]);

int collisionfinding(parameters_type& parameters);

// filters.cpp
void mespace_to_pathbitrelationsmatrix(const sha1messagespace& mespace, vector< vector< vector<uint32> > >& pathbitrelationsmatrix);
void random_me(uint32 me[80], const vector< vector< vector<uint32> > >& pathbitrelationsmatrix);

void filter_tunnels_bitconditions(vector< sha1differentialpath >& tunnels, const sha1differentialpath& path);
bool filter_tunnel_bitrelations(const sha1differentialpath& tunnel, const vector< vector<uint32> >& bitrels);
void filter_tunnels_bitrelations(vector< sha1differentialpath >& tunnels, const vector< vector<uint32> >& bitrels);

// program_generator.cpp
string mask2bit_to_string(const string& variable, const uint32 mask, const unsigned bit);
void generate_program();

// tunnel_analysis.cpp
unsigned analyze_bc_tunnel(const sha1differentialpath& tunnel);
unsigned analyze_tunnel(const sha1differentialpath& tunnel, bool verbose = false, bool deepanalyze = false);
void analyze_tunnels_diffpath(const sha1differentialpath& mypath, const vector< vector<uint32> >& bitrels, vector<sha1differentialpath> tunnels = vector<sha1differentialpath>());

// checkoppaths.cpp
void checkokpaths();

#endif // MAIN_HPP


#include <boost/geometry/strategies/spherical/side_by_cross_track.hpp>


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY 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 GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Sat 27 Aug 2016 12:43:04

#include "SingletDM_two_scale_initial_guesser.hpp"
#include "SingletDM_two_scale_model.hpp"
#include "lowe.h"
#include "error.hpp"
#include "ew_input.hpp"
#include "wrappers.hpp"

#include <Eigen/Core>
#include <cassert>

namespace flexiblesusy {

#define DERIVEDPARAMETER(p) model->p()
#define INPUTPARAMETER(p) model->get_input().p
#define MODELPARAMETER(p) model->get_##p()
#define PHASE(p) model->get_##p()
#define LowEnergyConstant(p) Electroweak_constants::p
#define MODEL model

SingletDM_initial_guesser<Two_scale>::SingletDM_initial_guesser(
   SingletDM<Two_scale>* model_,
   const softsusy::QedQcd& qedqcd_,
   const SingletDM_low_scale_constraint<Two_scale>& low_constraint_,
   const SingletDM_susy_scale_constraint<Two_scale>& susy_constraint_,
   const SingletDM_high_scale_constraint<Two_scale>& high_constraint_
)
   : Initial_guesser<Two_scale>()
   , model(model_)
   , qedqcd(qedqcd_)
   , mu_guess(0.)
   , mc_guess(0.)
   , mt_guess(0.)
   , md_guess(0.)
   , ms_guess(0.)
   , mb_guess(0.)
   , me_guess(0.)
   , mm_guess(0.)
   , mtau_guess(0.)
   , running_precision(1.0e-3)
   , low_constraint(low_constraint_)
   , susy_constraint(susy_constraint_)
   , high_constraint(high_constraint_)
{
   assert(model && "SingletDM_initial_guesser: Error: pointer to model"
          " SingletDM<Two_scale> must not be zero");
}

SingletDM_initial_guesser<Two_scale>::~SingletDM_initial_guesser()
{
}

/**
 * Guesses the DR-bar model parameters by calling
 * guess_susy_parameters() and guess_soft_parameters() .
 */
void SingletDM_initial_guesser<Two_scale>::guess()
{
   guess_susy_parameters();
   guess_soft_parameters();
}

/**
 * Guesses the SUSY parameters (gauge, Yukawa couplings) at
 * \f$m_\text{top}^\text{pole}\f$ from the Standard Model gauge
 * couplings and fermion masses.  Threshold corrections are ignored.
 * The user-defined initial guess at the low-scale
 * (InitialGuessAtLowScale) is applied here:
 *
 * \code{.cpp}
   const auto LamSHInput = INPUTPARAMETER(LamSHInput);
   const auto LamSInput = INPUTPARAMETER(LamSInput);
   const auto muSInput = INPUTPARAMETER(muSInput);
   const auto HiggsIN = INPUTPARAMETER(HiggsIN);

   MODEL->set_v(Re(LowEnergyConstant(vev)));
   calculate_Yu_DRbar();
   calculate_Yd_DRbar();
   calculate_Ye_DRbar();
   MODEL->set_LamSH(Re(LamSHInput));
   MODEL->set_LamS(Re(LamSInput));
   MODEL->set_muS(Re(muSInput));
   MODEL->set_muH(Re(HiggsIN));

 * \endcode
 */
void SingletDM_initial_guesser<Two_scale>::guess_susy_parameters()
{
   using namespace softsusy;

   softsusy::QedQcd leAtMt(qedqcd);
   const double MZ = Electroweak_constants::MZ;
   const double MW = Electroweak_constants::MW;
   const double sinThetaW2 = 1.0 - Sqr(MW / MZ);
   const double mtpole = leAtMt.displayPoleMt();

   mu_guess = leAtMt.displayMass(mUp);
   mc_guess = leAtMt.displayMass(mCharm);
   mt_guess = model->get_thresholds() > 0 ?
      leAtMt.displayMass(mTop) - 30.0 :
      leAtMt.displayPoleMt();
   md_guess = leAtMt.displayMass(mDown);
   ms_guess = leAtMt.displayMass(mStrange);
   mb_guess = leAtMt.displayMass(mBottom);
   me_guess = model->get_thresholds() > 0 ?
      leAtMt.displayMass(mElectron) :
      leAtMt.displayPoleMel();
   mm_guess = model->get_thresholds() > 0 ?
      leAtMt.displayMass(mMuon) :
      leAtMt.displayPoleMmuon();
   mtau_guess = leAtMt.displayMass(mTau);

   // guess gauge couplings at mt
   const DoubleVector alpha_sm(leAtMt.getGaugeMu(mtpole, sinThetaW2));

   MODEL->set_g1(Sqrt(4. * Pi * alpha_sm(1)));
   MODEL->set_g2(Sqrt(4. * Pi * alpha_sm(2)));
   MODEL->set_g3(Sqrt(4. * Pi * alpha_sm(3)));

   model->set_scale(mtpole);

   // apply user-defined initial guess at the low scale
   const auto LamSHInput = INPUTPARAMETER(LamSHInput);
   const auto LamSInput = INPUTPARAMETER(LamSInput);
   const auto muSInput = INPUTPARAMETER(muSInput);
   const auto HiggsIN = INPUTPARAMETER(HiggsIN);

   MODEL->set_v(Re(LowEnergyConstant(vev)));
   calculate_Yu_DRbar();
   calculate_Yd_DRbar();
   calculate_Ye_DRbar();
   MODEL->set_LamSH(Re(LamSHInput));
   MODEL->set_LamS(Re(LamSInput));
   MODEL->set_muS(Re(muSInput));
   MODEL->set_muH(Re(HiggsIN));

}

void SingletDM_initial_guesser<Two_scale>::calculate_DRbar_yukawa_couplings()
{
   calculate_Yu_DRbar();
   calculate_Yd_DRbar();
   calculate_Ye_DRbar();
}

/**
 * Calculates the Yukawa couplings Yu of the up-type quarks
 * from the Standard Model up-type quark masses (ignoring threshold
 * corrections).
 */
void SingletDM_initial_guesser<Two_scale>::calculate_Yu_DRbar()
{
   Eigen::Matrix<std::complex<double>,3,3> upQuarksDRbar(ZEROMATRIXCOMPLEX(3,3));
   upQuarksDRbar(0,0) = mu_guess;
   upQuarksDRbar(1,1) = mc_guess;
   upQuarksDRbar(2,2) = mt_guess;

   const auto v = MODELPARAMETER(v);
   MODEL->set_Yu((-((1.4142135623730951*upQuarksDRbar)/v).transpose()).real());

}

/**
 * Calculates the Yukawa couplings Yd of the down-type
 * quarks from the Standard Model down-type quark masses (ignoring
 * threshold corrections).
 */
void SingletDM_initial_guesser<Two_scale>::calculate_Yd_DRbar()
{
   Eigen::Matrix<std::complex<double>,3,3> downQuarksDRbar(ZEROMATRIXCOMPLEX(3,3));
   downQuarksDRbar(0,0) = md_guess;
   downQuarksDRbar(1,1) = ms_guess;
   downQuarksDRbar(2,2) = mb_guess;

   const auto v = MODELPARAMETER(v);
   MODEL->set_Yd((((1.4142135623730951*downQuarksDRbar)/v).transpose()).real())
      ;

}

/**
 * Calculates the Yukawa couplings Ye of the leptons
 * from the Standard Model down-type lepton masses (ignoring threshold
 * corrections).
 */
void SingletDM_initial_guesser<Two_scale>::calculate_Ye_DRbar()
{
   Eigen::Matrix<std::complex<double>,3,3> downLeptonsDRbar(ZEROMATRIXCOMPLEX(3,3));
   downLeptonsDRbar(0,0) = me_guess;
   downLeptonsDRbar(1,1) = mm_guess;
   downLeptonsDRbar(2,2) = mtau_guess;

   const auto v = MODELPARAMETER(v);
   MODEL->set_Ye((((1.4142135623730951*downLeptonsDRbar)/v).transpose()).real()
      );

}

/**
 * Guesses the soft-breaking parameters.  At first it runs to the
 * guess of the high-scale (HighScaleFirstGuess) and imposes the
 * high-scale constraint (HighScaleInput):
 *
 * \code{.cpp}

 * \endcode
 *
 * Afterwards, it runs to the low-scale guess (LowScaleFirstGuess) and
 * solves the EWSB conditions at the tree-level.  Finally the DR-bar
 * mass spectrum is calculated.
 */
void SingletDM_initial_guesser<Two_scale>::guess_soft_parameters()
{
   const double low_scale_guess = low_constraint.get_initial_scale_guess();
   const double high_scale_guess = high_constraint.get_initial_scale_guess();

   model->run_to(high_scale_guess, running_precision);

   // apply high-scale constraint
   high_constraint.set_model(model);
   high_constraint.apply();

   // apply user-defined initial guess at the high scale

   model->run_to(low_scale_guess, running_precision);

   // apply EWSB constraint
   model->solve_ewsb_tree_level();

   // calculate tree-level spectrum
   model->calculate_DRbar_masses();
}

} // namespace flexiblesusy


/*
 * Stanford Whole-Body Control Framework http://stanford-wbc.sourceforge.net/
 *
 * Copyright (C) 2010 The Board of Trustees of The Leland Stanford Junior University. All rights reserved.
 *
 * This program is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 *
 * 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 GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this program.  If not, see
 * <http://www.gnu.org/licenses/>
 */

/**
   \file sejong/wrap_eigen.hpp
   \author Roland Philippsen
*/

#ifndef SEJONG_WRAP_EIGEN_HPP
#define SEJONG_WRAP_EIGEN_HPP

#define SJ_SAFE_DELETE(p)			if(p) { delete (p); (p) = NULL; }
#define SJ_SAFE_DESTROY_WINDOW(p) if(p) { p->DestroyWindow(); delete (p); (p) = NULL; }
#define SJ_SAFE_DELETE_AR(p)		if(p) { delete [] p; (p) = NULL; }
#define SJ_SAFE_RELEASE(p)		if(p) { (p)->Release(); (p) = NULL; }

#define SJ_ISZERO(x)	(fabs(x) < 1.e-6)			// zero test for floating point numbers
#define SJ_ISEQUAL(x,y)	(fabs((x) - (y)) < 1.e-6) // test for equality of float numbers
#define SJ_ROUND(x)		(floor((x) + 0.5))			// floating point number rounding
#define SJ_RAND(l,u)	((double)rand() / RAND_MAX * ((u) - (l)) + (l))	// float random number from interval < l ; u >
#define SJ_RAND_INT(l,u)  (rand() % (u - l) + l)  // int random number in interval [l,u) including l, excluding u

#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <vector>
#include <list>
#include <string>
#include <cmath>
#include <ctime>

namespace sejong {
  typedef Eigen::Transform<double, 3, Eigen::Affine> Transform;
  typedef Eigen::Translation3d Translation;
  typedef Eigen::Quaternion<double> Quaternion;
  typedef Eigen::Matrix<double,2,1> Vect2;
  typedef Eigen::Matrix<double,3,1> Vect3;
  typedef Eigen::Matrix<double,4,1> Vect4;
  typedef Eigen::VectorXd Vector;
  typedef Eigen::MatrixXd Matrix;
  // Euler ange (Yaw, Pitch, Roll) to Quaternion
  void convert(double yaw, double pitch, double roll, sejong::Quaternion& to);
  // Quaternion to Euler ZYX
  void convert(const sejong::Quaternion& from, double & yaw, double & pitch, double & roll);
  // Quaternion to so(3)
  void convert(sejong::Quaternion const & from, sejong::Vector & to);

  // so(3) to Quaternion
  void convert(sejong::Vector const & from, sejong::Quaternion & to);
  // sejong::Vector to std::vector
  void convert(sejong::Vector const & from, std::vector<double> & to);
  // std::vector to sejong::Vector
  void convert(std::vector<double> const & from, sejong::Vector & to);
  // double array to sejong::Vector
  void convert(double const * from, size_t length, sejong::Vector & to);

  Quaternion QuatMultiply(const Quaternion & q1, const Quaternion & q2);

  bool compare(sejong::Matrix const & lhs, sejong::Matrix const & rhs, double precision);
  bool compare(sejong::Quaternion const & lhs, sejong::Quaternion const & rhs, double precision);

  double _bind_half_pi(double);

  void Copy(const sejong::Vector & sub, double* obj);
  void Copy(const double* sub, double* obj, int dim);
  void SetArrayZero(double* array, int dim);
  double Dot(const double* a, const double * b, int dim);
  // Signum returns (-1, 0, or 1) depending on the sign of the argument
  template <typename T> int sgn(T val) {
    return (T(0) < val) - (val < T(0)) ;
  }
}

#endif