#ifdef HOPS_GUROBI_FOUND

#include <Eigen/Core>

#include <hops/LinearProgram/LinearProgramGurobiImpl.hpp>
#include <hops/LinearProgram/GurobiEnvironmentSingleton.hpp>

namespace {
    std::vector<GRBVar> addVariablesToModel(GRBModel *model, size_t numberOfVariables) {
        std::vector<GRBVar> variables;
        for (size_t i = 0; i < numberOfVariables; ++i) {
            variables.emplace_back(model->addVar(
                    -GRB_INFINITY,
                    +GRB_INFINITY,
                    0,
                    GRB_CONTINUOUS,
                    "x_" + std::to_string(i))
            );
        }
        return variables;
    }

    void addLinearConstraints(const Eigen::MatrixXd &inequalityA,
                              const Eigen::VectorXd &inequalityB,
                              GRBModel *model,
                              const std::vector<GRBVar> &variables) {
        for (long i = 0; i < inequalityA.rows(); ++i) {
            GRBLinExpr expression;
            double coefficients[inequalityA.cols()];
            for (long j = 0; j < inequalityA.cols(); ++j) {
                coefficients[j] = inequalityA.coeff(i, j);
            }
            expression.addTerms(coefficients, &variables[0], inequalityA.cols());
            model->addConstr(expression, GRB_LESS_EQUAL, inequalityB(i), "row_" + std::to_string(i));
        }
    }

    void addObjective(const Eigen::VectorXd &objective, GRBModel *model,
                      const std::vector<GRBVar> &variables) {
        GRBLinExpr objectiveExpression = 0.0;
        objectiveExpression.addTerms(objective.data(), &variables[0], objective.rows());
        model->setObjective(objectiveExpression, GRB_MAXIMIZE);
    }

    hops::LinearProgramStatus parseGurobiStatus(int returnCode) {
        switch (returnCode) {
            case 2: {
                return hops::LinearProgramStatus::OPTIMAL;
            }
            case 3: {
                return hops::LinearProgramStatus::INFEASIBLE;
            }
            case 4: {
                return hops::LinearProgramStatus::UNDEFINED;
            }
            case 5: {
                return hops::LinearProgramStatus::UNBOUNDED;
            }
            default: {
                return hops::LinearProgramStatus::ERROR;
            }
        }
    }
}

hops::LinearProgramGurobiImpl::LinearProgramGurobiImpl(const Eigen::MatrixXd &A, const Eigen::VectorXd &b) :
        LinearProgram(A, b),
        model(std::make_unique<GRBModel>(GRBModel(GurobiEnvironmentSingleton::getInstance().getGurobiEnvironment()))) {
    variables = addVariablesToModel(model.get(), A.cols());
    addLinearConstraints(A, b, model.get(), variables);
    model->update();
}

hops::LinearProgramGurobiImpl::LinearProgramGurobiImpl(const hops::LinearProgramGurobiImpl &other) :
        LinearProgram(other.A, other.b),
        variables(other.variables) {
    model = std::make_unique<GRBModel>(*other.model);
}

hops::LinearProgramGurobiImpl &hops::LinearProgramGurobiImpl::operator=(const hops::LinearProgramGurobiImpl &other) {
    this->A = other.A;
    this->b = other.b;
    this->model = std::make_unique<GRBModel>(*other.model);
    this->variables = other.variables;
    return *this;
}

hops::LinearProgramSolution hops::LinearProgramGurobiImpl::solve(const Eigen::VectorXd &objective) const {
    addObjective(objective, model.get(), variables);
    model->update();

    try {
        model->optimize();

        int status = model->get(GRB_IntAttr_Status);

        if (status == GRB_INF_OR_UNBD) {
            model->set(GRB_IntParam_Presolve, 0);
            model->optimize();
            status = model->get(GRB_IntAttr_Status);
        }
        if (status == GRB_OPTIMAL) {
            double objectiveValue = model->get(GRB_DoubleAttr_ObjVal);
            auto numberOfColumns = model->get(GRB_IntAttr_NumVars);
            auto modelVariables = model->getVars();
            Eigen::VectorXd solution(numberOfColumns);
            for (int i = 0; i < numberOfColumns; ++i) {
                solution(i) = modelVariables[i].get(GRB_DoubleAttr_X);
            }
            return LinearProgramSolution(objectiveValue, solution, parseGurobiStatus(status));
        } else if (status == GRB_INFEASIBLE) {
            return LinearProgramSolution(std::numeric_limits<double>::quiet_NaN(),
                                         Eigen::VectorXd(),
                                         LinearProgramStatus::INFEASIBLE);
        } else if (status == GRB_UNBOUNDED) {
            return LinearProgramSolution(std::numeric_limits<double>::quiet_NaN(),
                                         Eigen::VectorXd(),
                                         LinearProgramStatus::UNBOUNDED);
        } else if (status == GRB_UNDEFINED) {
            return LinearProgramSolution(std::numeric_limits<double>::quiet_NaN(),
                                         Eigen::VectorXd(),
                                         LinearProgramStatus::UNDEFINED);
        }
    }
    catch (const GRBException &e) {
        throw std::runtime_error("Gurobi encountered an exception: " + e.getMessage());
    }
    throw std::runtime_error("Exception: Gurobi failed to provide problem status or exception.");
}

std::tuple<Eigen::MatrixXd, Eigen::VectorXd>
hops::LinearProgramGurobiImpl::removeRedundantConstraints(double tolerance) {
    std::vector<long> constraintsToRemove;
    for (int i = 0; i < static_cast<int>(A.rows()); ++i) {
        GRBConstr constraintToTestForRedundancy = model->getConstrByName("row_" + std::to_string(i));
        model->remove(constraintToTestForRedundancy);
        model->update();
        GRBLinExpr constraintLHS;
        double coefficients[A.cols()];
        for (long j = 0; j < A.cols(); ++j) {
            coefficients[j] = A.coeff(i, j);
        }
        constraintLHS.addTerms(coefficients, &variables[0], A.cols());
        try {
            auto temporaryConstraint = model->addConstr(constraintLHS, GRB_LESS_EQUAL, b(i) + 10);
            model->update();
            auto solution = solve(A.row(i));
            model->remove(temporaryConstraint);
            model->update();

            if (solution.status != LinearProgramStatus::OPTIMAL || solution.objectiveValue + tolerance > b(i)) {
                model->addConstr(constraintLHS, GRB_LESS_EQUAL, b(i), "row_" + std::to_string(i));
                model->update();
            } else {
                constraintsToRemove.emplace_back(i);
            }
        }
        catch (GRBException &e) {
            std::cerr << "error code " << e.getErrorCode() << ": " << e.getMessage() << std::endl;
        }
    }

    std::vector<long> constraintsToKeep;
    for (long i = 0; i < A.rows(); ++i) {
        if (std::find(constraintsToRemove.begin(), constraintsToRemove.end(), i) == constraintsToRemove.end()) {
            constraintsToKeep.emplace_back(i);
        }
    }

    Eigen::MatrixXd newA(constraintsToKeep.size(), A.cols());
    Eigen::VectorXd newb(constraintsToKeep.size());
    for (size_t i = 0; i < constraintsToKeep.size(); ++i) {
        newb(i) = b(constraintsToKeep.at(i));
        newA.row(i) = A.row(constraintsToKeep.at(i));
    }

    *this = LinearProgramGurobiImpl(newA, newb);
    return std::make_tuple(A, b);
}

hops::LinearProgramSolution hops::LinearProgramGurobiImpl::computeChebyshevCenter() const {
    //Extend system by dimension for radius
    const long numberOfRows = A.rows();
    const long numberOfColumns = A.cols();
    Eigen::MatrixXd A_ext = A;
    Eigen::MatrixXd l_col(numberOfRows + 1, 1);
    l_col << A.rowwise().norm(), -1;
    A_ext.conservativeResize(numberOfRows + 1, numberOfColumns + 1);
    A_ext.row(numberOfRows) = Eigen::VectorXd::Zero(numberOfColumns + 1);
    A_ext.col(numberOfColumns) = l_col;
    Eigen::VectorXd b_ext = b;
    b_ext.conservativeResize(numberOfRows + 1);
    b_ext(numberOfRows) = 0;

    //make objective
    Eigen::VectorXd obj = Eigen::VectorXd::Zero(numberOfColumns + 1);
    obj(numberOfColumns) = 1;

    LinearProgramSolution chebyshevSolution = LinearProgramGurobiImpl(A_ext, b_ext).solve(obj);
    chebyshevSolution.optimalParameters.conservativeResize(A.cols());
    return chebyshevSolution;
}

std::vector<long> hops::LinearProgramGurobiImpl::computeUnconstrainedDimensions() const {
    std::vector<long> directions;
    for (long i = 0; i < A.cols(); ++i) {
        Eigen::VectorXd objective = Eigen::VectorXd::Zero(A.cols());
        objective(i) = 1.0;
        auto forwardSolution = solve(objective);
        if (forwardSolution.status != hops::LinearProgramStatus::OPTIMAL) {
            directions.push_back(i + 1);
        }

        auto backwardSolution = solve(-objective);
        if (backwardSolution.status != hops::LinearProgramStatus::OPTIMAL) {
            directions.push_back(-i - 1);
        }
    }
    return directions;
}

std::tuple<Eigen::MatrixXd, Eigen::VectorXd>
hops::LinearProgramGurobiImpl::addBoxConstraintsToUnconstrainedDimensions(double lb, double ub) {
    std::vector<long> unconstrainedDimensions = computeUnconstrainedDimensions();

    for (const auto &unconstrainedDimension : unconstrainedDimensions) {
        A.conservativeResize(A.rows() + 1, A.cols());
        A.row(A.rows() - 1) = Eigen::VectorXd::Zero(A.cols());
        b.conservativeResize(b.rows() + 1);
        if (unconstrainedDimension > 0) {
            A(A.rows() - 1, unconstrainedDimension - 1) = 1;
            b(b.rows() - 1) = ub;
        } else {
            A(A.rows() - 1, -unconstrainedDimension - 1) = -1;
            b(b.rows() - 1) = lb;
        }
    }
    *this = LinearProgramGurobiImpl(A, b);
    return std::make_tuple(A, b);
}

#endif //HOPS_GUROBI_FOUND


#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>

#include "../include/brute_force.hpp"
#include "../include/encryption.hpp"
#include "../include/key_generator.hpp"

int main() {
  Key_pair keys = generate_keys(256, true);
  encrypt_file("original.txt", "encriptado.txt", keys.pub_key);
  decrypt_file("encriptado.txt", "decriptado.txt", keys.priv_key);
}


/*
 Copyright (C) 2021 Quaternion Risk Management Ltd
 All rights reserved.
*/

// clang-format off
#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
// clang-format on

#include <qle/cashflows/durationadjustedcmscoupon.hpp>
#include <qle/cashflows/durationadjustedcmscoupontsrpricer.hpp>
#include <qle/models/linearannuitymapping.hpp>

#include "toplevelfixture.hpp"

#include <ql/time/date.hpp>
#include <ql/cashflows/cmscoupon.hpp>
#include <ql/cashflows/lineartsrpricer.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/termstructures/volatility/swaption/swaptionconstantvol.hpp>
#include <ql/indexes/swap/euriborswap.hpp>

using namespace QuantExt;
using namespace QuantLib;

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(DurationAdjustedCouponTest)

BOOST_AUTO_TEST_CASE(testAgainstCmsCoupon) {

    BOOST_TEST_MESSAGE("Testing duration adjusted cms coupons vs. vanilla cms coupon...");

    Date today(25, January, 2021);
    Settings::instance().evaluationDate() = today;

    Handle<YieldTermStructure> discountCurve(
        boost::make_shared<FlatForward>(0, NullCalendar(), 0.01, Actual365Fixed()));
    Handle<YieldTermStructure> forwardCurve(boost::make_shared<FlatForward>(0, NullCalendar(), 0.02, Actual365Fixed()));
    Handle<SwaptionVolatilityStructure> swaptionVol(boost::make_shared<ConstantSwaptionVolatility>(
        0, NullCalendar(), Unadjusted, 0.0050, Actual365Fixed(), Normal));
    Handle<Quote> reversion(boost::make_shared<SimpleQuote>(0.01));

    Date startDate = Date(25, January, 2025);
    Date endDate = Date(25, January, 2026);
    Date payDate = Date(27, January, 2026);
    Size fixingDays = 2;
    auto index = boost::make_shared<EuriborSwapIsdaFixA>(10 * Years, forwardCurve, discountCurve);

    CmsCoupon cmsCoupon(payDate, 1.0, startDate, endDate, fixingDays, index);
    DurationAdjustedCmsCoupon durationAdjustedCmsCoupon(payDate, 1.0, startDate, endDate, fixingDays, index, 0);

    auto cmsPricer = boost::make_shared<LinearTsrPricer>(swaptionVol, reversion, discountCurve,
                                                         LinearTsrPricer::Settings().withRateBound(-2.0, 2.0));

    auto durationAdjustedCmsPricer = boost::make_shared<DurationAdjustedCmsCouponTsrPricer>(
        swaptionVol, boost::make_shared<LinearAnnuityMappingBuilder>(reversion), -2.0, 2.0);

    cmsCoupon.setPricer(cmsPricer);
    durationAdjustedCmsCoupon.setPricer(durationAdjustedCmsPricer);

    BOOST_TEST_MESSAGE("cms coupon rate                   = " << cmsCoupon.rate());
    BOOST_TEST_MESSAGE("cms coupon convexity adj          = " << cmsCoupon.convexityAdjustment());
    BOOST_TEST_MESSAGE("duration adjusted cms coupon rate = " << durationAdjustedCmsCoupon.rate());
    BOOST_TEST_MESSAGE("dur adj cms coupon convexity adj  = " << durationAdjustedCmsCoupon.convexityAdjustment());

    Real tol = 1E-6; // percentage tolerance, i.e. we have 1E-8 effectively

    BOOST_CHECK_CLOSE(cmsCoupon.rate(), durationAdjustedCmsCoupon.rate(), tol);
    BOOST_CHECK_CLOSE(cmsCoupon.convexityAdjustment(), durationAdjustedCmsCoupon.convexityAdjustment(), tol);
}

BOOST_AUTO_TEST_CASE(testHistoricalValues) {

    BOOST_TEST_MESSAGE("Testing duration adjusted cms coupon historical rates...");

    Date today(25, January, 2021);
    Settings::instance().evaluationDate() = today;

    Date startDate = Date(25, June, 2020);
    Date endDate = Date(25, June, 2021);
    Date payDate = Date(27, June, 2021);
    Size fixingDays = 2;

    Handle<YieldTermStructure> discountCurve(
        boost::make_shared<FlatForward>(0, NullCalendar(), 0.01, Actual365Fixed()));
    Handle<YieldTermStructure> forwardCurve(boost::make_shared<FlatForward>(0, NullCalendar(), 0.02, Actual365Fixed()));

    auto index = boost::make_shared<EuriborSwapIsdaFixA>(10 * Years, forwardCurve, discountCurve);

    Date fixingDate = index->fixingCalendar().advance(startDate, -(fixingDays * Days), Preceding);
    Real fixingValue = 0.01;
    index->addFixing(fixingDate, fixingValue);

    // we do not need a vol surface or an annuity mapping builder, since the coupon amount is determistic
    auto pricer =
        boost::make_shared<DurationAdjustedCmsCouponTsrPricer>(Handle<SwaptionVolatilityStructure>(), nullptr);

    DurationAdjustedCmsCoupon cpn0(payDate, 1.0, startDate, endDate, fixingDays, index, 0);
    DurationAdjustedCmsCoupon cpn1(payDate, 1.0, startDate, endDate, fixingDays, index, 1);
    DurationAdjustedCmsCoupon cpn10(payDate, 1.0, startDate, endDate, fixingDays, index, 10);

    cpn0.setPricer(pricer);
    cpn1.setPricer(pricer);
    cpn10.setPricer(pricer);

    BOOST_TEST_MESSAGE("duration = 0  : rate = " << cpn0.rate());
    BOOST_TEST_MESSAGE("duration = 1  : rate = " << cpn1.rate());
    BOOST_TEST_MESSAGE("duration = 10 : rate = " << cpn10.rate());

    BOOST_TEST_MESSAGE("duration = 0  : indexFixing = " << cpn0.indexFixing());
    BOOST_TEST_MESSAGE("duration = 1  : indexFixing = " << cpn1.indexFixing());
    BOOST_TEST_MESSAGE("duration = 10 : indexFixing = " << cpn10.indexFixing());

    auto durationAdjustment = [](const Real S, const Size i) {
        if (i == 0)
            return 1.0;
        Real tmp = 0.0;
        for (Size j = 0; j < i; ++j)
            tmp += 1.0 / std::pow(1.0 + S, j + 1);
        return tmp;
    };

    Real tol = 1.0E-6;

    BOOST_CHECK_CLOSE(cpn0.rate(), fixingValue * durationAdjustment(fixingValue, 0), tol);
    BOOST_CHECK_CLOSE(cpn1.rate(), fixingValue * durationAdjustment(fixingValue, 1), tol);
    BOOST_CHECK_CLOSE(cpn10.rate(), fixingValue * durationAdjustment(fixingValue, 10), tol);

    BOOST_CHECK_CLOSE(cpn0.indexFixing(), fixingValue * durationAdjustment(fixingValue, 0), tol);
    BOOST_CHECK_CLOSE(cpn1.indexFixing(), fixingValue * durationAdjustment(fixingValue, 1), tol);
    BOOST_CHECK_CLOSE(cpn10.indexFixing(), fixingValue * durationAdjustment(fixingValue, 10), tol);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#pragma once
#include <Eigen/Core>
#include <memory>
#include "ear/helpers/output_gains.hpp"
#include "ear/layout.hpp"
#include "ear/metadata.hpp"
#include "ear/warnings.hpp"

namespace ear {

  class GainCalculatorHOAImpl {
   public:
    GainCalculatorHOAImpl(const Layout& layout);

    void calculate(const HOATypeMetadata& metadata, OutputGainMat& direct,
                   const WarningCB& warning_cb = default_warning_cb);

    template <typename T>
    void calculate(const HOATypeMetadata& metadata,
                   std::vector<std::vector<T>>& direct,
                   const WarningCB& warning_cb = default_warning_cb) {
      OutputGainMatVecT<T> direct_wrap(direct);
      calculate(metadata, direct_wrap, warning_cb);
    }

   private:
    Eigen::Matrix<double, Eigen::Dynamic, 3> points;
    Eigen::Array<bool, Eigen::Dynamic, 1> is_lfe;
    Eigen::MatrixXd G_virt;
  };

}  // namespace ear


// Copyright 2017 Emanuele Palazzolo (emanuele.palazzolo@uni-bonn.de), Cyrill Stachniss, University of Bonn
#include "fastcd/point_covariance3d.h"
#include <Eigen/Core>
#include "fastcd/mesh.h"

namespace fastcd {

PointCovariance3d::PointCovariance3d(const Eigen::Vector3d &point,
                           const Eigen::Matrix3d &covariance, double chi_square)
    : point_(point), covariance_(covariance) {
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es(covariance_);
  eigenvalues_ = es.eigenvalues();
  eigenvectors_ = es.eigenvectors();
  scaling_.setZero();
  for (int i = 0; i < 3; i++) {
    scaling_(i, i) = sqrt(chi_square) * sqrt(eigenvalues_(i));
  }
  Eigen::Vector3d vertex;
  vertex << 1, 0, 0;
  vertex = eigenvectors_ * scaling_ * vertex + point_;
  vertices_.push_back(vertex);
  vertex << -1, 0, 0;
  vertex = eigenvectors_ * scaling_ * vertex + point_;
  vertices_.push_back(vertex);
  vertex << 0, 1, 0;
  vertex = eigenvectors_ * scaling_ * vertex + point_;
  vertices_.push_back(vertex);
  vertex << 0, -1, 0;
  vertex = eigenvectors_ * scaling_ * vertex + point_;
  vertices_.push_back(vertex);
  vertex << 0, 0, 1;
  vertex = eigenvectors_ * scaling_ * vertex + point_;
  vertices_.push_back(vertex);
  vertex << 0, 0, -1;
  vertex = eigenvectors_ * scaling_ * vertex + point_;
  vertices_.push_back(vertex);
}

Mesh PointCovariance3d::ToMesh(int stacks, int slices, float r, float g,
                               float b, float a) const {
  Mesh::Material material;
  material.ambient = glow::vec3(0.1, 0.1, 0.1);
  material.diffuse = glow::vec3(r, g, b);
  material.specular = glow::vec3(0, 0, 0);
  material.emission = glow::vec3(0, 0, 0);
  material.alpha = a;
  std::vector<Mesh::Material> materials;
  materials.push_back(material);

  std::vector<Mesh::Vertex> vertices;
  float t_step = M_PI / static_cast<float>(slices);
  float s_step = M_PI / static_cast<float>(stacks);
  for (float t = -M_PI / 2; t <= (M_PI / 2) + .0001; t += t_step) {
    for (float s = -M_PI; s <= M_PI + .0001; s += s_step) {
      Eigen::Vector3d vertex;
      vertex << cos(t) * cos(s), cos(t) * sin(s), sin(t);
      vertex = eigenvectors_ * scaling_ * vertex + point_;
      Mesh::Vertex v1;
      v1.position = glow::vec4(vertex(0), vertex(1), vertex(2), 1);
      vertices.push_back(v1);

      Mesh::Vertex v2;
      vertex << cos(t + t_step) * cos(s), cos(t + t_step) * sin(s),
          sin(t + t_step);
      vertex = eigenvectors_ * scaling_ * vertex + point_;
      v2.position = glow::vec4(vertex(0), vertex(1), vertex(2), 1);
      vertices.push_back(v2);
    }
  }

  std::vector<Mesh::Triangle> triangles;
  for (size_t i = 2; i < vertices.size(); i++) {
    Mesh::Triangle t;
    t.vertices[0] = i-2;
    t.vertices[1] = i % 2 == 0 ? i - 1 : i;
    t.vertices[2] = i % 2 == 0 ? i : i - 1;

    if (t.vertices[0] != t.vertices[1] && t.vertices[1] != t.vertices[2] &&
        t.vertices[2] != t.vertices[0]) {
      triangles.push_back(t);
    }
  }

  Mesh mesh(vertices, triangles, materials);
  return mesh;
}

Eigen::Vector3d PointCovariance3d::Point() const { return point_; }

Eigen::Matrix3d PointCovariance3d::Covariance() const { return covariance_; }

Eigen::Vector3d PointCovariance3d::Eigenvalues() const { return eigenvalues_; }

Eigen::Matrix3d PointCovariance3d::Eigenvectors() const {
  return eigenvectors_;
}

Eigen::Matrix3d PointCovariance3d::Scaling() const { return scaling_; }

std::vector<Eigen::Vector3d> PointCovariance3d::Vertices() const {
  return vertices_;
}

}  // namespace fastcd


#ifndef CLOUD_CPP
#define CLOUD_CPP

#include <iostream>
#include "../include/cloud.h"
#include <Eigen/Dense>

using Eigen::MatrixXd;

Cloud::Cloud(float _Ox, float _Oy, float _Oz, size_t _total_points, float _dmin):
    Ox(_Ox), Oy(_Oy), Oz(_Oz), total_points(_total_points), dmin(_dmin)
{
    if( dmin < 0.0f ) dmin = 0.001f;
    if( Ox < 0.0f )   Ox   = 1.0f;
    if( Oy < 0.0f )   Oy   = 1.0f;
    if( Oz < 0.0f )   Oz   = 1.0f;
}

Cloud::~Cloud()
{
    into_points.clear();
}

bool
Cloud::build(QOpenGLShaderProgram* program)
{
    // BUILD CLOUD
    std::random_device rd;
    std::default_random_engine re(rd());

    // Generators
    std::uniform_real_distribution<float> xgen{-Ox, Ox};
    std::uniform_real_distribution<float> ygen{-Oy, Oy};
    std::uniform_real_distribution<float> zgen{-Oz, Oz};

    // Prepare deque to receive information
    into_points.clear();

    for(size_t i=0; i < total_points; ++i){
        float x = xgen(re);
        float y = ygen(re);
        float z = zgen(re);

        if( into(x, y, z) ){
            bool distance_ok = true;
            for(size_t j=0; j < into_points.size(); j+=3){
                float dist = distance(
                    x, y, z,
                    into_points[j+0],
                    into_points[j+1],
                    into_points[j+2]
                );

                if( dist <= dmin ){
                    distance_ok = false;
                    break;
                }
            }

            if( distance_ok ){
                into_points.push_back(x);
                into_points.push_back(y);
                into_points.push_back(z);
            }
        }
    }

    // Once we have all the correct points into our shape.
    // Build datas
    size_t npoints = into_points.size()/3;
    GLfloat* positions = new GLfloat[npoints*3];
    GLuint* indices = new GLuint[npoints];

    for(size_t i=0; i < npoints; ++i){
        size_t idx = (i*3);
        positions[idx+0] = into_points[idx+0];
        positions[idx+1] = into_points[idx+1];
        positions[idx+2] = into_points[idx+2];
        indices[i] = GLuint(i);
    }

    // BUILD BUFFERS (will be sent to GPU with `update_buffers()` method.
    set_vertices_geometry(
        program->attributeLocation("position"), positions, indices);

    set_vertices_colors(
        program->attributeLocation("color"), new GLfloat[npoints*3]);

    return initialize(npoints, npoints, 3);
}

QVector3D
Cloud::compute_gravity_center() const
{
    QVector3D gcenter(0.0f, 0.0f, 0.0f);
    size_t npoints = into_points.size()/3;

    for(size_t i=0; i < npoints; ++i){
        size_t idx = (i*3);
        gcenter += QVector3D(
            into_points[idx+0],
            into_points[idx+1],
            into_points[idx+2]
        );
    }

    return model_matrix() * (gcenter/float(npoints));
}

std::deque<float>
Cloud::compute_deviations() const
{
    std::deque<float> deviations(into_points.size());
    QVector3D gcenter = compute_gravity_center();
    size_t npoints = points_into_cloud();

    std::cerr << gcenter[0] << " " << gcenter[1] << " " << gcenter[2] << std::endl;

    for(size_t i=0; i < npoints; ++i){
        size_t idx = (i*3);

        QVector3D p(
            into_points[idx+0],
            into_points[idx+1],
            into_points[idx+2]
        );

        p = model_matrix() * p;

        deviations[idx+0] = p[0] - gcenter.x();
        deviations[idx+1] = p[1] - gcenter.y();
        deviations[idx+2] = p[2] - gcenter.z();
    }

    return deviations;
}

std::deque<float>
Cloud::compute_correlation_matrix() const
{
    std::deque<float> deviations = compute_deviations();
    size_t npoints = points_into_cloud();

    //          | x0, y0, z0 |
    // matrix   | x1, y1, z1 |
    //          | x2, y2, z2 |
    //
    //          | x0, x1, x2 |
    // t_matrix | y0, y1, y2 |
    //          | z0, z1, z2 |
    std::deque<std::deque<float>> matrix(npoints);

    std::deque<std::deque<float>> t_matrix(3);
    t_matrix[0] = std::deque<float>(npoints); // x
    t_matrix[1] = std::deque<float>(npoints); // y
    t_matrix[2] = std::deque<float>(npoints); // z

    for(size_t i=0; i < npoints; ++i){
        size_t idx = (i*3);
        matrix[i] = std::deque<float>(3);
        matrix[i][0] = t_matrix[0][i] = deviations[idx+0];
        matrix[i][1] = t_matrix[1][i] = deviations[idx+1];
        matrix[i][2] = t_matrix[2][i] = deviations[idx+2];
    }

    std::cerr << matrix.size() << " " << t_matrix[0].size() << std::endl;

    // Correlation matrix (out)[3*3] = t_matrix[3*npts] * matrix[npts*3];
    std::deque<float> out(9);
    for(size_t i=0; i < 3; ++i){
        for(size_t j=0; j < 3; ++j){
            out[(i*3)+j] = 0.0f;
            for(size_t h=0; h < npoints; ++h){
                out[(i*3)+j] += (t_matrix[i][h] * matrix[h][j]);
            }
        }
    }

    return out;
}

// Compute determinant of a matrix (3x3)
float
Cloud::compute_determinant(const std::deque<float>& mat)
{
    if(mat.size() != 9)
        return 0;

    return + mat[0]*((mat[4]*mat[8]) - (mat[5]*mat[7]))
           - mat[1]*((mat[3]*mat[8]) - (mat[5]*mat[6]))
           + mat[2]*((mat[3]*mat[7]) - (mat[4]*mat[6]));
}

//https://en.wikipedia.org/wiki/Eigenvalue_algorithm#3%C3%973_matrices
std::deque<float>
Cloud::eigenvalues(const std::deque<float>& matrix)
{
    std::deque<float> eigenvalues(3);
    if( matrix.size() == 9 ){
        float r0 = (matrix[1]*matrix[1]) +
                   (matrix[2]*matrix[2]) +
                   (matrix[5]*matrix[5]);

        if( r0 == 0.0f ){
            eigenvalues[0] = matrix[0];
            eigenvalues[1] = matrix[4];
            eigenvalues[2] = matrix[8];
        }
        else {
            float q = (matrix[0] + matrix[4] + matrix[8])/3;
            float r1 = (
                std::pow(matrix[0] - q, 2.0f) +
                std::pow(matrix[4] - q, 2.0f) +
                std::pow(matrix[8] - q, 2.0f) +
                2 * r0
            );

            float p = std::sqrt(r1/6);
            std::deque<float> A_minus_LambdaI = matrix;
            A_minus_LambdaI[0] -= q;
            A_minus_LambdaI[4] -= q;
            A_minus_LambdaI[8] -= q;

            for(size_t i = 0; i < 9; ++i)
                A_minus_LambdaI[i] *= (1/p);

            float det = Cloud::compute_determinant(A_minus_LambdaI)/2.0f;
            float phi;
            const float PI = std::atan(1.0f) * 4.0f;

            if( det <= -1 ){
                phi = PI / 3.0f;
            }
            else
            if( det >= 1 ){
                phi = 0.0f;
            }
            else {
                phi = std::acos(det) / 3.0f;
            }

            eigenvalues[0] = q + 2 * p * std::cos(phi);
            eigenvalues[2] = q + 2 * p * std::cos(phi + (2*PI/3));
            eigenvalues[1] = 3 * q - eigenvalues[0] - eigenvalues[2];
        }
    }

    return eigenvalues;
}

// Naive test
bool
Cloud::into(float x, float y, float z)
{
    return (  (x >= -Ox && x <= Ox)
           && (y >= -Oy && y <= Oy)
           && (z >= -Oz && z <= Oz) );
}

// Compute euclidean distance between two 3D points.
float
Cloud::distance(float x, float y, float z, float xx, float yy, float zz)
{
    return std::sqrt(
                std::pow(xx-x, 2.0f) +
                std::pow(yy-y, 2.0f) +
                std::pow(zz-z, 2.0f));
}

// ELLIPSOID CLOUD CLASS
EllipsoidCloud::EllipsoidCloud(float _Ox, float _Oy, float _Oz, size_t _total_points, float _dmin):
    Cloud(_Ox, _Oy, _Oz, _total_points, _dmin)
{}

// Maths from https://en.wikipedia.org/wiki/Ellipsoid
bool
EllipsoidCloud::into(float x, float y, float z)
{
    return (((x*x)/(Ox*Ox)) + ((y*y)/(Oy*Oy)) + ((z*z)/(Oz*Oz))) <= 1.0f;
}

#endif // CLOUD_CPP


//  Copyright (c) 2018-2019 Cem Bassoy
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Fraunhofer and Google in producing this work
//  which started as a Google Summer of Code project.
//

#include <boost/numeric/ublas/tensor.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include "utility.hpp"

BOOST_AUTO_TEST_SUITE(test_tensor_arithmetic_operations/*, * boost::unit_test::depends_on("test_tensor")*/)

using double_extended = boost::multiprecision::cpp_bin_float_double_extended;

using test_types = zip<int,float,double_extended>::with_t<boost::numeric::ublas::layout::first_order, boost::numeric::ublas::layout::last_order>;

struct fixture
{
  using extents_type = boost::numeric::ublas::extents<>;

  std::vector<extents_type> extents =
    {
//      extents_type{},    // 0
      extents_type{1,1}, // 1
      extents_type{1,2}, // 2
      extents_type{2,1}, // 3
      extents_type{2,3}, // 4
      extents_type{2,3,1}, // 5
      extents_type{4,1,3}, // 6
      extents_type{1,2,3}, // 7
      extents_type{4,2,3}, // 8
      extents_type{4,2,3,5} // 9
  };
};

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_binary_arithmetic_operations, value,  test_types, fixture)
{
    namespace ublas = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;
    using tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;

    auto check = [](auto const& e)
    {
        auto t  = tensor_type (e);
        auto t2 = tensor_type (e);
        auto r  = tensor_type (e);
        auto v  = value_type  {};

        std::iota(t.begin(), t.end(), v);
        std::iota(t2.begin(), t2.end(), v+2);

        r = t + t + t + t2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 3*t(i) + t2(i) );

        r = t2 / (t+3) * (t+1) - t2; // r = ( t2/ ((t+3)*(t+1)) ) - t2

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), t2(i) / (t(i)+3)*(t(i)+1) - t2(i) );

        r = 3+t2 / (t+3) * (t+1) * t - t2; // r = 3+( t2/ ((t+3)*(t+1)*t) ) - t2

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 3+t2(i) / (t(i)+3)*(t(i)+1)*t(i) - t2(i) );

        r = t2 - t + t2 - t;

        for(auto i = 0ul; i < r.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 4 );

        r = t * t * t * t2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), t(i)*t(i)*t(i)*t2(i) );

        r = (t2/t2) * (t2/t2);

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 1 );
    };

    for(auto const& e : extents)
        check(e);

    BOOST_CHECK_NO_THROW ( tensor_type t = tensor_type(extents.at(0)) + tensor_type(extents.at(0))  );
    BOOST_CHECK_THROW    ( tensor_type t = tensor_type(extents.at(0)) + tensor_type(extents.at(2)), std::runtime_error  );
    BOOST_CHECK_THROW    ( tensor_type t = tensor_type(extents.at(1)) + tensor_type(extents.at(2)), std::runtime_error  );

}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_unary_arithmetic_operations, value,  test_types, fixture)
{
    namespace ublas = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;
    using tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;

    auto check = [](auto const& e)
    {
        auto t  = tensor_type (e);
        auto t2 = tensor_type (e);
        auto v  = value_type  {};

        std::iota(t.begin(), t.end(), v);
        std::iota(t2.begin(), t2.end(), v+2);

        tensor_type r1 = t + 2 + t + 2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r1(i), 2*t(i) + 4 );

        tensor_type r2 = 2 + t + 2 + t;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r2(i), 2*t(i) + 4 );

        tensor_type r3 = (t-2) + (t-2);

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r3(i), 2*t(i) - 4 );

        tensor_type r4 = (t*2) * (3*t);

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r4(i), 2*3*t(i)*t(i) );

        tensor_type r5 = (t2*2) / (2*t2) * t2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r5(i), (t2(i)*2) / (2*t2(i)) * t2(i) );

        tensor_type r6 = (t2/2+1) / (2/t2+1) / t2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r6(i), (t2(i)/2+1) / (2/t2(i)+1) / t2(i) );

    };

    for(auto const& e : extents)
        check(e);

    BOOST_CHECK_NO_THROW ( tensor_type t = tensor_type(extents.at(0)) + 2 + tensor_type(extents.at(0))  );
    BOOST_CHECK_THROW    ( tensor_type t = tensor_type(extents.at(0)) + 2 + tensor_type(extents.at(2)), std::runtime_error  );
    BOOST_CHECK_THROW    ( tensor_type t = tensor_type(extents.at(1)) + 2 + tensor_type(extents.at(2)), std::runtime_error  );
    BOOST_CHECK_THROW    ( tensor_type t = tensor_type(extents.at(2)) + 2 + tensor_type(extents.at(2)) + tensor_type(extents.at(1)), std::runtime_error  );
    BOOST_CHECK_THROW    ( tensor_type t = tensor_type(extents.at(2)) + 2 + tensor_type(extents.at(2)) + 2 + tensor_type(extents.at(1)), std::runtime_error  );
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_assign_arithmetic_operations, value,  test_types, fixture)
{
    namespace ublas = boost::numeric::ublas;
    using value_type  = typename value::first_type;
    using layout_type = typename value::second_type;
    using tensor_type  = ublas::tensor_dynamic<value_type,layout_type>;

    auto check = [](auto const& e)
    {
        auto t  = tensor_type (e);
        auto t2 = tensor_type (e);
        auto r  = tensor_type (e);
        auto v  = value_type  {};

        std::iota(t.begin(), t.end(), v);
        std::iota(t2.begin(), t2.end(), v+2);

        r  = t + 2;
        r += t;
        r += 2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 2*t(i) + 4 );

        r  = 2 + t;
        r += t;
        r += 2;
        (void)r;
        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 2*t(i) + 4 );

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 2*t(i) + 4 );

        r = (t-2);
        r += t;
        r -= 2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 2*t(i) - 4 );

        r  = (t*2);
        r *= 3;
        r *= t;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), 2*3*t(i)*t(i) );

        r  = (t2*2);
        r /= 2;
        r /= t2;
        r *= t2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), (t2(i)*2) / (2*t2(i)) * t2(i) );

        r  = (t2/2+1);
        r /= (2/t2+1);
        r /= t2;

        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( r(i), (t2(i)/2+1) / (2/t2(i)+1) / t2(i) );

        tensor_type q = -r;
        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( q(i), -r(i) );

        tensor_type p = +r;
        for(auto i = 0ul; i < t.size(); ++i)
            BOOST_CHECK_EQUAL ( p(i), r(i) );

    };

    for(auto const& e : extents)
        check(e);

    auto r  = tensor_type (extents.at(0));

    BOOST_CHECK_NO_THROW ( r += tensor_type(extents.at(0)) + 2 + tensor_type(extents.at(0))  );
    BOOST_CHECK_THROW    ( r += tensor_type(extents.at(0)) + 2 + tensor_type(extents.at(2)), std::runtime_error  );
    BOOST_CHECK_THROW    ( r += tensor_type(extents.at(1)) + 2 + tensor_type(extents.at(2)), std::runtime_error  );
    BOOST_CHECK_THROW    ( r += tensor_type(extents.at(2)) + 2 + tensor_type(extents.at(2)) + tensor_type(extents.at(1)), std::runtime_error  );
    BOOST_CHECK_THROW    ( r += tensor_type(extents.at(2)) + 2 + tensor_type(extents.at(2)) + 2 + tensor_type(extents.at(1)), std::runtime_error  );
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright 2016, 2017 Peter Dimov.
//
// Distributed under the Boost Software License, Version 1.0.
//
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt

#include <boost/mp11/map.hpp>
#include <boost/mp11/list.hpp>
#include <boost/mp11/integral.hpp>
#include <boost/core/lightweight_test_trait.hpp>
#include <type_traits>
#include <tuple>
#include <utility>

using boost::mp11::mp_int;

struct Q_inc
{
    template<class T, class U> using fn = mp_int<U::value + 1>;
};

int main()
{
    using boost::mp11::mp_map_update_q;
    using boost::mp11::mp_list;

    using M1 = mp_list<>;

    using M2 = mp_map_update_q<M1, std::pair<char, mp_int<0>>, Q_inc>;
    BOOST_TEST_TRAIT_TRUE((std::is_same<M2, mp_list<std::pair<char, mp_int<0>>>>));

    using M3 = mp_map_update_q<M2, std::pair<char, mp_int<0>>, Q_inc>;
    BOOST_TEST_TRAIT_TRUE((std::is_same<M3, mp_list<std::pair<char, mp_int<1>>>>));

    using M4 = mp_map_update_q<M3, std::pair<int, mp_int<0>>, Q_inc>;
    BOOST_TEST_TRAIT_TRUE((std::is_same<M4, mp_list<std::pair<char, mp_int<1>>, std::pair<int, mp_int<0>>>>));

    using M5 = mp_map_update_q<M4, std::pair<long, mp_int<0>>, Q_inc>;
    BOOST_TEST_TRAIT_TRUE((std::is_same<M5, mp_list<std::pair<char, mp_int<1>>, std::pair<int, mp_int<0>>, std::pair<long, mp_int<0>>>>));

    using M6 = mp_map_update_q<M5, std::pair<long, mp_int<0>>, Q_inc>;
    BOOST_TEST_TRAIT_TRUE((std::is_same<M6, mp_list<std::pair<char, mp_int<1>>, std::pair<int, mp_int<0>>, std::pair<long, mp_int<1>>>>));

    using M7 = mp_map_update_q<M6, std::pair<char, mp_int<0>>, Q_inc>;
    BOOST_TEST_TRAIT_TRUE((std::is_same<M7, mp_list<std::pair<char, mp_int<2>>, std::pair<int, mp_int<0>>, std::pair<long, mp_int<1>>>>));

    return boost::report_errors();
}


#include <big_types.h>
#include <big_absolute_convergence.h>
#include <big_relative_convergence.h>
#include <big_stagnation.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(big_stop_criteria);

BOOST_AUTO_TEST_CASE(absolute_testing)
{
  BOOST_CHECK(  big::absolute_convergence(  1.0,   0.01) == false  );
  BOOST_CHECK(  big::absolute_convergence(  0.01,  0.01) == true   );
  BOOST_CHECK(  big::absolute_convergence(  0.005, 0.01) == true   );
  BOOST_CHECK(  big::absolute_convergence(  0.0,   0.01) == true   );
  BOOST_CHECK(  big::absolute_convergence(  0.0,   0.0 ) == true   );

//  BOOST_CHECK_THROW(  big::absolute_convergence( -1.0,   0.01), std::invalid_argument  ); // 2012-09-30 Kenny: Should we throw an exception?
//  BOOST_CHECK_THROW(  big::absolute_convergence(  1.0,  -0.01), std::invalid_argument  ); // 2012-09-30 Kenny: Should we throw an exception?

}

BOOST_AUTO_TEST_CASE(relative_testing)
{

  // Test is:  fabs(old - new) / fabs(old) <= tol

  BOOST_CHECK(  big::relative_convergence(  1.0, 1.0, 0.01 ) == true   );
  BOOST_CHECK(  big::relative_convergence(  1.0, 1.0, 0.0  ) == true   );

  BOOST_CHECK(  big::relative_convergence(  1.0, 1.1, 0.11 ) == true   );
  BOOST_CHECK(  big::relative_convergence(  1.0, 1.1, 0.09 ) == false  );
  BOOST_CHECK(  big::relative_convergence(  1.0, 0.9, 0.11 ) == true   );
  BOOST_CHECK(  big::relative_convergence(  1.0, 0.9, 0.09 ) == false  );

  BOOST_CHECK(  big::relative_convergence(  0.0, 0.0, 0.11 ) == true   );
  BOOST_CHECK(  big::relative_convergence(  0.0, .1, 0.11  ) == false  );

  //  BOOST_CHECK_THROW(  big::relative_convergence( 1.0, 1.0, -0.01), std::invalid_argument  ); // 2012-09-30 Kenny: Should we throw an exception?
}

BOOST_AUTO_TEST_CASE(stagnation_testing)
{
  typedef ublas::vector<double>            vector_type;
  vector_type x;
  vector_type y;

  x.resize(2);
  y.resize(2);

  x(0) = 0.01;
  x(1) = 0.01;

  y(0) = 0.01;
  y(1) = 0.01;

  BOOST_CHECK(  big::stagnation(  x, y, 0.01 ) == true   );

  x(0) = 0.01;
  x(1) = 0.01;

  y(0) = 0.01 + 2*0.01;
  y(1) = 0.01;

  BOOST_CHECK(  big::stagnation(  x, y, 0.01 ) == false   );

  BOOST_CHECK(  big::stagnation(  x, y, 0.0  ) == false  );
  BOOST_CHECK(  big::stagnation(  x, x, 0.0  ) == true   );

  //  BOOST_CHECK_THROW(  big::stagnation( x, y, -0.01), std::invalid_argument  ); // 2012-09-30 Kenny: Should we throw an exception?

}

BOOST_AUTO_TEST_SUITE_END();


//  (C) Copyright Eric Niebler, Olivier Gygi 2006.
//  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)

// Test case for weighted_p_square_cumul_dist.hpp

#include <cmath>
#include <boost/random.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/numeric/functional/vector.hpp>
#include <boost/accumulators/numeric/functional/complex.hpp>
#include <boost/accumulators/numeric/functional/valarray.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/weighted_p_square_cumul_dist.hpp>

using namespace boost;
using namespace unit_test;
using namespace boost::accumulators;

///////////////////////////////////////////////////////////////////////////////
// erf() not known by VC++ compiler!
// my_erf() computes error function by numerically integrating with trapezoidal rule
//
double my_erf(double const& x, int const& n = 1000)
{
    double sum = 0.;
    double delta = x/n;
    for (int i = 1; i < n; ++i)
        sum += std::exp(-i*i*delta*delta) * delta;
    sum += 0.5 * delta * (1. + std::exp(-x*x));
    return sum * 2. / std::sqrt(3.141592653);
}

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    // tolerance in %
    double epsilon = 4;

    typedef accumulator_set<double, stats<tag::weighted_p_square_cumulative_distribution>, double > accumulator_t;

    accumulator_t acc_upper(p_square_cumulative_distribution_num_cells = 100);
    accumulator_t acc_lower(p_square_cumulative_distribution_num_cells = 100);

    // two random number generators
    double mu_upper = 1.0;
    double mu_lower = -1.0;
    boost::lagged_fibonacci607 rng;
    boost::normal_distribution<> mean_sigma_upper(mu_upper,1);
    boost::normal_distribution<> mean_sigma_lower(mu_lower,1);
    boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> > normal_upper(rng, mean_sigma_upper);
    boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> > normal_lower(rng, mean_sigma_lower);

    for (std::size_t i=0; i<100000; ++i)
    {
        double sample = normal_upper();
        acc_upper(sample, weight = std::exp(-mu_upper * (sample - 0.5 * mu_upper)));
    }

    for (std::size_t i=0; i<100000; ++i)
    {
        double sample = normal_lower();
        acc_lower(sample, weight = std::exp(-mu_lower * (sample - 0.5 * mu_lower)));
    }

    typedef iterator_range<std::vector<std::pair<double, double> >::iterator > histogram_type;
    histogram_type histogram_upper = weighted_p_square_cumulative_distribution(acc_upper);
    histogram_type histogram_lower = weighted_p_square_cumulative_distribution(acc_lower);

    // Note that applying importance sampling results in a region of the distribution
    // to be estimated more accurately and another region to be estimated less accurately
    // than without importance sampling, i.e., with unweighted samples

    for (std::size_t i = 0; i < histogram_upper.size(); ++i)
    {
        // problem with small results: epsilon is relative (in percent), not absolute!

        // check upper region of distribution
        if ( histogram_upper[i].second > 0.1 )
            BOOST_CHECK_CLOSE( 0.5 * (1.0 + my_erf( histogram_upper[i].first / std::sqrt(2.0) )), histogram_upper[i].second, epsilon );
        // check lower region of distribution
        if ( histogram_lower[i].second < -0.1 )
            BOOST_CHECK_CLOSE( 0.5 * (1.0 + my_erf( histogram_lower[i].first / std::sqrt(2.0) )), histogram_lower[i].second, epsilon );
    }
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("weighted_p_square_cumulative_distribution test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


/*    Copyright (c) 2010-2014, Delft University of Technology
 *    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 the Delft University of Technology nor the names of its 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      150411    D. Dirkx          Migrated and updated from personal code.
 *
 *    References
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <fstream>
#include <limits>
#include <map>

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

#include <Eigen/Core>

#include <Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h>
#include <Tudat/Basics/testMacros.h>
#include <Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h>
#include <Tudat/Mathematics/BasicMathematics/mathematicalConstants.h>

#include "Tudat/Astrodynamics/BasicAstrodynamics/UnitTests/keplerPropagatorTestData.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/keplerPropagator.h"
#include "Tudat/Astrodynamics/Ephemerides/keplerEphemeris.h"
#include "Tudat/InputOutput/basicInputOutput.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_keplerEphemeris )

//! Test 1: Comparison of KeplerEphemeris output with benchmark data from (Melman, 2010).
//! (see testPropagateKeplerOrbit_Eccentric_Melman).
BOOST_AUTO_TEST_CASE( testKeplerEphemerisElliptical )
{
    // Load the expected propagation history.
    // Create expected propagation history.
    PropagationHistory expectedPropagationHistory = getODTBXBenchmarkData( );

    // Set Earth gravitational parameter [m^3 s^-2].
    const double earthGravitationalParameter = 398600.4415e9;

    // Compute propagation history.
    PropagationHistory computedPropagationHistory;
    computedPropagationHistory[ 0.0 ] = expectedPropagationHistory[ 0.0 ];

    ephemerides::KeplerEphemeris keplerEphemeris(
                expectedPropagationHistory[ 0.0 ],
                0.0, earthGravitationalParameter );

    for( PropagationHistory::iterator stateIterator = expectedPropagationHistory.begin( );
         stateIterator != expectedPropagationHistory.end( ); stateIterator++ )
    {
        // Compute next entry.
        computedPropagationHistory[ stateIterator->first ] =
                orbital_element_conversions::convertCartesianToKeplerianElements(
                    keplerEphemeris.getCartesianStateFromEphemeris( stateIterator->first ),
                    earthGravitationalParameter );

        // Check that computed results match expected results.
        BOOST_CHECK_CLOSE_FRACTION(
                    computedPropagationHistory[ stateIterator->first ]( 5 ),
                    expectedPropagationHistory[ stateIterator->first ]( 5 ),
                    2.0e-14 );
    }
}

//! Test 2: Comparison of KeplerEphemeris with that of GTOP (hyperbolic).
//! (see testPropagateKeplerOrbit_hyperbolic_GTOP).
BOOST_AUTO_TEST_CASE( testKeplerEphemerisHyperbolic )
{
    // Load the expected propagation history.
    PropagationHistory expectedPropagationHistory = getGTOPBenchmarkData( );

    // Compute propagation history.
    PropagationHistory computedPropagationHistory;
    computedPropagationHistory[ 0.0 ] = expectedPropagationHistory[ 0.0 ];

    ephemerides::KeplerEphemeris keplerEphemeris(
                expectedPropagationHistory[ 0.0 ],
                0.0, getGTOPGravitationalParameter( ) );

    for( PropagationHistory::iterator stateIterator = expectedPropagationHistory.begin( );
         stateIterator != expectedPropagationHistory.end( ); stateIterator++ )
    {
        // Compute next entry.
        computedPropagationHistory[ stateIterator->first ] =
                orbital_element_conversions::convertCartesianToKeplerianElements(
                    keplerEphemeris.getCartesianStateFromEphemeris( stateIterator->first ),
                    getGTOPGravitationalParameter( ) );

        // Check that computed results match expected results.
        BOOST_CHECK_CLOSE_FRACTION(
                    computedPropagationHistory[ stateIterator->first ]( 5 ),
                    expectedPropagationHistory[ stateIterator->first ]( 5 ),
                    1.0e-15 );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


#include <boost/test/unit_test.hpp>
#include "a_star.h"

BOOST_AUTO_TEST_SUITE(a_star)

BOOST_AUTO_TEST_CASE(simple_path_1)
{
	typedef math::a_star::point point;

	struct dummy {
		static math::scalar score(point const &p) {
			return (p - point(1, 0)).length_sq() + (p.x == 0 && (p.y == 0 || p.y == -1) ? 1000.0f : 0.0f);
		}
	};

	math::a_star a_star(point(-1, 0), point(1, 0), &dummy::score);
	a_star.calculate_path();
	BOOST_REQUIRE(!a_star.have_failed());

	std::vector<point> path = a_star.build_path();
	BOOST_REQUIRE(path.size() == 3);
	BOOST_REQUIRE(path[0] == point(-1, 0));
	BOOST_REQUIRE(path[1] == point( 0, 1));
	BOOST_REQUIRE(path[2] == point( 1, 0));
}

BOOST_AUTO_TEST_CASE(simple_path_2)
{
	typedef math::a_star::point point;

	struct dummy {
		static math::scalar score(point const &p) {
			return (p - point(1, 0)).length_sq() + (p.x == 0 && (p.y == 0 || p.y == -1) ? 1000.0f : 0.0f);
		}
	};

	math::a_star a_star(point(-2, 0), point(1, 0), &dummy::score);
	a_star.calculate_path();
	BOOST_REQUIRE(!a_star.have_failed());

	std::vector<point> path = a_star.build_path();
	BOOST_REQUIRE(path.size() == 4);
	BOOST_REQUIRE(path[0] == point(-2, 0));
	BOOST_REQUIRE(path[1] == point(-1, 0));
	BOOST_REQUIRE(path[2] == point( 0, 1));
	BOOST_REQUIRE(path[3] == point( 1, 0));
}

BOOST_AUTO_TEST_CASE(simple_path_3)
{
	typedef math::a_star::point point;

	struct dummy {
		static math::scalar score(point const &p) {
			return (p - point(1, 0)).length_sq() + (p.x == 0 && (p.y == 0 || p.y == -1) ? 1000.0f : 0.0f);
		}
	};

	math::a_star a_star(point(-1, 0), point(2, 0), &dummy::score);
	a_star.calculate_path();
	BOOST_REQUIRE(!a_star.have_failed());

	std::vector<point> path = a_star.build_path();
	BOOST_REQUIRE(path.size() == 4);
	BOOST_REQUIRE(path[0] == point(-1, 0));
	BOOST_REQUIRE(path[1] == point( 0, 1));
	BOOST_REQUIRE(path[2] == point( 1, 0));
	BOOST_REQUIRE(path[3] == point( 2, 0));
}

BOOST_AUTO_TEST_CASE(simple_path_without_obstacles)
{
	typedef math::a_star::point point;

	struct dummy {
		static math::scalar score(point const &p) {
			return math::scalar((p - point(10, 0)).length_sq());
		}
	};

	math::a_star a_star(point(0, 0), point(10, 0), &dummy::score);
	a_star.calculate_path();
	BOOST_REQUIRE(!a_star.have_failed());

	std::vector<point> path = a_star.build_path();

	for (int i = 0; i <= 10; ++i)
	{
		BOOST_REQUIRE (path[i] == point(i, 0));
	}
}

BOOST_AUTO_TEST_CASE(simple_path_4)
{
	typedef math::a_star::point point;

	struct dummy {
		static math::scalar score(point const &p) {
			return (p - point(10, 0)).length_sq() + (p.x == 5 && (p.y > -5 && p.y < 3) ? 1000.0f : 0.0f);
		}
	};

	math::a_star a_star(point(0, 0), point(10, 0), &dummy::score);
	a_star.calculate_path();
	BOOST_REQUIRE(!a_star.have_failed());

	std::vector<point> path = a_star.build_path();

	BOOST_REQUIRE(path.size() == 12);
	BOOST_REQUIRE(path[0] == point(0, 0));
	BOOST_REQUIRE(path[1] == point(1, 0));
	BOOST_REQUIRE(path[2] == point(2, 0));
	BOOST_REQUIRE(path[3] == point(3, 0));
	BOOST_REQUIRE(path[4] == point(4, 1));
	BOOST_REQUIRE(path[5] == point(4, 2));
	BOOST_REQUIRE(path[6] == point(5, 3));
	BOOST_REQUIRE(path[7] == point(6, 2));
	BOOST_REQUIRE(path[8] == point(7, 1));
	BOOST_REQUIRE(path[9] == point(8, 0));
	BOOST_REQUIRE(path[10] == point(9, 0));
	BOOST_REQUIRE(path[11] == point(10, 0));
}

BOOST_AUTO_TEST_SUITE_END()


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

#include <memory>

#include "refill/filters/extended_kalman_filter.h"
#include "refill/system_models/linear_system_model.h"
#include "refill/measurement_models/linear_measurement_model.h"

namespace refill {

TEST(ExtendedKalmanFilterTest, ConstructorTest) {
  ExtendedKalmanFilter filter_1;

  EXPECT_EQ(GaussianDistribution().mean(), filter_1.state().mean());
  EXPECT_EQ(GaussianDistribution().cov(), filter_1.state().cov());

  GaussianDistribution initial_state(Eigen::Vector2d::Zero(),
                                     Eigen::Matrix2d::Identity());

  ExtendedKalmanFilter filter_2(initial_state);

  EXPECT_EQ(Eigen::Vector2d::Zero(), filter_2.state().mean());
  EXPECT_EQ(Eigen::Matrix2d::Identity(), filter_2.state().cov());

  GaussianDistribution system_noise(Eigen::Vector2d::Zero(),
                                    Eigen::Matrix2d::Identity());
  GaussianDistribution measurement_noise(Eigen::Vector2d::Zero(),
                                         Eigen::Matrix2d::Identity() * 2.0);

  LinearSystemModel system_model(Eigen::Matrix2d::Identity(), system_noise);
  LinearMeasurementModel measurement_model(Eigen::Matrix2d::Identity(),
                                           measurement_noise);
  ExtendedKalmanFilter filter_3(
      initial_state,
      std::unique_ptr<LinearSystemModel>(new LinearSystemModel(system_model)),
      std::unique_ptr<LinearMeasurementModel>(
          new LinearMeasurementModel(measurement_model)));

  EXPECT_EQ(Eigen::Vector2d::Zero(), filter_3.state().mean());
  EXPECT_EQ(Eigen::Matrix2d::Identity(), filter_3.state().cov());

  filter_3.predict();

  EXPECT_EQ(Eigen::Vector2d::Zero(), filter_3.state().mean());
  EXPECT_EQ(Eigen::Matrix2d::Identity() * 2.0, filter_3.state().cov());

  filter_3.update(Eigen::Vector2d::Constant(3.0));

  EXPECT_EQ(Eigen::Vector2d::Constant(1.5), filter_3.state().mean());
  EXPECT_EQ(Eigen::Matrix2d::Identity(), filter_3.state().cov());
}

TEST(ExtendedKalmanFilterTest, SetterTest) {
  GaussianDistribution initial_state(Eigen::Vector2d::Zero(),
                                     Eigen::Matrix2d::Identity());

  ExtendedKalmanFilter filter_1(initial_state);

  initial_state.setDistributionParameters(Eigen::Vector2d::Ones(),
                             Eigen::Matrix2d::Identity() * 2.0);
  filter_1.setState(initial_state);

  EXPECT_EQ(Eigen::Vector2d::Ones(), filter_1.state().mean());
  EXPECT_EQ(Eigen::Matrix2d::Identity() * 2.0, filter_1.state().cov());
}

TEST(ExtendedKalmanFilterTest, PredictionTest) {
  GaussianDistribution initial_state(Eigen::Vector2d::Zero(),
                                     Eigen::Matrix2d::Identity());
  GaussianDistribution system_noise(Eigen::Vector2d::Zero(),
                                    Eigen::Matrix2d::Identity());
  GaussianDistribution measurement_noise(Eigen::Vector2d::Zero(),
                                         Eigen::Matrix2d::Identity());

  LinearSystemModel system_model(Eigen::Matrix2d::Identity(), system_noise,
                                 Eigen::Matrix2d::Identity());
  LinearMeasurementModel measurement_model(Eigen::Matrix2d::Identity(),
                                           measurement_noise);

  // object for testing ekf with standard models
  ExtendedKalmanFilter filter_1(
      initial_state,
      std::unique_ptr<LinearSystemModel>(new LinearSystemModel(system_model)),
      std::unique_ptr<LinearMeasurementModel>(
          new LinearMeasurementModel(measurement_model)));

  // test prediction with standard models
  filter_1.predict();

  EXPECT_EQ(Eigen::Vector2d::Zero(), filter_1.state().mean());
  EXPECT_EQ(Eigen::Matrix2d::Identity() * 2.0, filter_1.state().cov());

  filter_1.setState(initial_state);

  // test prediction with standard models and input
  filter_1.predict(Eigen::Vector2d::Ones());

  EXPECT_EQ(Eigen::Vector2d::Ones(), filter_1.state().mean());
  EXPECT_EQ(Eigen::Matrix2d::Identity() * 2.0, filter_1.state().cov());

  // object for testing ekf with external model
  ExtendedKalmanFilter filter_2(initial_state);

  // test prediction with external model and no input
  filter_2.predict(system_model);

  EXPECT_EQ(Eigen::Vector2d::Zero(), filter_2.state().mean());
  EXPECT_EQ(Eigen::Matrix2d::Identity() * 2.0, filter_2.state().cov());

  filter_2.setState(initial_state);

  // test prediction with external model and input
  filter_2.predict(system_model, Eigen::Vector2d::Ones());

  EXPECT_EQ(Eigen::Vector2d::Ones(), filter_2.state().mean());
  EXPECT_EQ(Eigen::Matrix2d::Identity() * 2.0, filter_2.state().cov());
}

TEST(ExtendedKalmanFilterTest, UpdateTest) {
  GaussianDistribution initial_state(Eigen::Vector2d::Zero(),
                                     Eigen::Matrix2d::Identity());
  GaussianDistribution system_noise(Eigen::Vector2d::Zero(),
                                    Eigen::Matrix2d::Identity());
  GaussianDistribution measurement_noise(Eigen::Vector2d::Zero(),
                                         Eigen::Matrix2d::Identity());

  LinearSystemModel system_model(Eigen::Matrix2d::Identity(), system_noise,
                                 Eigen::Matrix2d::Identity());
  LinearMeasurementModel measurement_model(Eigen::Matrix2d::Identity(),
                                           measurement_noise);

  // object for testing ekf with standard models
  ExtendedKalmanFilter filter_1(
      initial_state,
      std::unique_ptr<LinearSystemModel>(new LinearSystemModel(system_model)),
      std::unique_ptr<LinearMeasurementModel>(
          new LinearMeasurementModel(measurement_model)));

  // test update with standard models
  filter_1.update(Eigen::Vector2d::Ones());

  EXPECT_EQ(Eigen::Vector2d::Ones() / 2.0, filter_1.state().mean());
  EXPECT_EQ(Eigen::Matrix2d::Identity() / 2.0, filter_1.state().cov());

  // object for testing ekf with external model
  ExtendedKalmanFilter filter_2(initial_state);

  // test update with external model
  filter_2.update(measurement_model, Eigen::Vector2d::Ones());

  EXPECT_EQ(Eigen::Vector2d::Ones() / 2.0, filter_2.state().mean());
  EXPECT_EQ(Eigen::Matrix2d::Identity() / 2.0, filter_2.state().cov());
}

}  // namespace refill


/* boost random/exponential_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: exponential_distribution.hpp 11696 2001-11-14 21:53:38Z jmaurer $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_EXPONENTIAL_DISTRIBUTION_HPP
#define BOOST_RANDOM_EXPONENTIAL_DISTRIBUTION_HPP

#include <cmath>
#include <cassert>
#include <boost/random/uniform_01.hpp>

namespace boost {

// exponential distribution: p(x) = lambda * exp(-lambda * x)
template<class UniformRandomNumberGenerator, class RealType = double>
class exponential_distribution
{
public:
  typedef UniformRandomNumberGenerator base_type;
  typedef RealType result_type;

  exponential_distribution(base_type& rng, result_type lambda)
    : _rng(rng), _lambda(lambda) { assert(lambda > 0); }
  // compiler-generated copy ctor is fine
  // uniform_01 cannot be assigned, neither can this class
  result_type operator()()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::log;
#endif
    return -1.0 / _lambda * log(1-_rng());
  }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend bool operator==(const exponential_distribution& x,
                         const exponential_distribution& y)
  { return x._lambda == y._lambda && x._rng == y._rng; }
#else
  // Use a member function
  bool operator==(const exponential_distribution& rhs) const
  { return _lambda == rhs._lambda && _rng == rhs._rng;  }
#endif
private:
  uniform_01<base_type, RealType> _rng;
  const result_type _lambda;
};

} // namespace boost

#endif // BOOST_RANDOM_EXPONENTIAL_DISTRIBUTION_HPP


#include <bsplines/BSplinePose.hpp>
#include <sm/assert_macros.hpp>
// boost::tie
#include <boost/tuple/tuple.hpp>
#include <sm/kinematics/transformations.hpp>

namespace bsplines {
  using namespace sm::kinematics;

    // given T, T'\in\mathrm{SE}(3), define Exp_{T}(h), h\in\mathbb{R}^6 , Log_{T}(T')
    // [p,\theta]\in\mathbb{R}^6 , \mathrm{SE}(3) \ni T = Exp[p, \theta] = [exp(-\theta) p]
    //                                                                     [      0      1]
    // T = [R t] \in\mathrm{SE}(3), Log T = [t  -log(R)]
    //     [0 1]
    // T \oplus [a, b] = Exp_{T}([a, b]) = Exp[a, b] * T
    // T1 \ominus T2 = Log_{T2}(T1) = Log(T1 * T2^{-1})
    BSplinePose::BSplinePose(int splineOrder, const RotationalKinematics::Ptr & rotationalKinematics)
      : BSpline(splineOrder), rotation_(rotationalKinematics)
    {

    }

    BSplinePose::~BSplinePose()
    {

    }

    Eigen::Matrix4d BSplinePose::transformation(double tk) const
    {
      return curveValueToTransformation(eval(tk));
    }

    // T: transformation  p: axis angle  C: spline control points
    // dT/dC = dT/dp * dp/dC
    Eigen::Matrix4d BSplinePose::transformationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::MatrixXd JS;  // dp/dC
      Eigen::VectorXd p;
      p = evalDAndJacobian(tk,0,&JS, coefficientIndices);

      Eigen::MatrixXd JT;  // dT/dp
      Eigen::Matrix4d T = curveValueToTransformationAndJacobian( p, &JT );

      if(J)
      {
        *J = JT * JS;
      }

      return T;
    }

    // Log_{O(\theta)}O(\theta + \delta) = J\delta
    // => J\delta = Log(O(\theta + \delta) * O(\theta)^{-1})
    //            = Log(exp(-(\theta + \delta)) * exp(\theta))
    //            = Log(exp(-Jr(\theta)\delta))
    //            = Jr(\theta)\delta
    Eigen::Matrix3d BSplinePose::orientationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::Matrix3d C;
      Eigen::MatrixXd JS;
      Eigen::VectorXd p;
      p = evalDAndJacobian(tk,0,&JS, coefficientIndices);

      Eigen::Matrix3d S;
      C = rotation_->parametersToRotationMatrix(p.tail<3>(), &S);

      Eigen::MatrixXd JO = Eigen::MatrixXd::Zero(3,6);
      JO.block(0,3,3,3) = S;
      if(J)
      {
        *J = JO * JS;
      }

      return C;

    }

    // Log_{IO(\theta)}IO(\theta + \delta) = J\delta
    // => J\delta = Log(IO(\theta + \delta) * IO(\theta)^{-1})
    //            = Log(exp(\theta + \delta) * exp(-\theta))
    //            = Log(Jl(\theta)\delta)
    //            = -Jl(\theta)\delta
    //            = -exp(\theta)Jr(\theta)\delta
    Eigen::Matrix3d BSplinePose::inverseOrientationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::Matrix3d C;
      Eigen::MatrixXd JS;
      Eigen::VectorXd p;
      p = evalDAndJacobian(tk,0,&JS, coefficientIndices);

      Eigen::Matrix3d S;
      C = rotation_->parametersToRotationMatrix(p.tail<3>(), &S).transpose();

      Eigen::MatrixXd JO = Eigen::MatrixXd::Zero(3,6);
      JO.block(0,3,3,3) = S;
      if(J)
      {
        *J = -C * JO * JS;
      }

      return C;

    }

    Eigen::Matrix4d BSplinePose::inverseTransformationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      //std::cout << __FUNCTION__ << "()\n";
      //ASRL_THROW(std::runtime_error,"Not Implemented");
      Eigen::MatrixXd JS;
      Eigen::VectorXd p;
      p = evalDAndJacobian(tk,0,&JS, coefficientIndices);

      Eigen::MatrixXd JT;
      Eigen::Matrix4d T = curveValueToTransformationAndJacobian( p, &JT );
      // Invert the transformation.
      T.topLeftCorner<3,3>().transposeInPlace();
      T.topRightCorner<3,1>() = (-T.topLeftCorner<3,3>() * T.topRightCorner<3,1>()).eval();

      if(J)
      {
        // The "box times" is the linearized transformation way of inverting the jacobian.
        *J = -sm::kinematics::boxTimes(T) * JT * JS;
      }

      if(coefficientIndices)
      {
        *coefficientIndices = localCoefficientVectorIndices(tk);
      }

      return T;
    }

    Eigen::Matrix4d BSplinePose::inverseTransformation(double tk) const
    {
      Eigen::Matrix4d T = curveValueToTransformation(eval(tk));
      T.topLeftCorner<3,3>().transposeInPlace();
      T.topRightCorner<3,1>() = (-T.topLeftCorner<3,3>() * T.topRightCorner<3,1>()).eval();
      return T;
    }

    // d(Tv)/dC = d(Tv)/dT * dT/dp * dp/dC
    // d(Tv)/dT s.t. Exp_{T}(h)v - Tv = d(Tv)/dT * h
    Eigen::Vector4d BSplinePose::transformVectorAndJacobian(double tk, const Eigen::Vector4d & v_tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::MatrixXd JT;
      Eigen::Matrix4d T_n_vk = transformationAndJacobian(tk, &JT, coefficientIndices);
      Eigen::Vector4d v_n = T_n_vk * v_tk;

      if(J)
      {
        *J = sm::kinematics::boxMinus(v_n) * JT;
      }

      return v_n;
    }

    Eigen::Vector4d BSplinePose::inverseTransformVectorAndJacobian(double tk, const Eigen::Vector4d & v_tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::MatrixXd JT;
      Eigen::Matrix4d T_n_vk = inverseTransformationAndJacobian(tk, &JT, coefficientIndices);
      Eigen::Vector4d v_n = T_n_vk * v_tk;

      if(J)
      {
        *J = sm::kinematics::boxMinus(v_n) * JT;
      }

      return v_n;
    }

    Eigen::Vector3d BSplinePose::position(double tk) const
    {
      return eval(tk).head<3>();
    }

    Eigen::Matrix3d BSplinePose::orientation(double tk) const
    {
      return rotation_->parametersToRotationMatrix(eval(tk).tail<3>());
    }

    Eigen::Matrix3d BSplinePose::inverseOrientation(double tk) const
    {
      return rotation_->parametersToRotationMatrix(eval(tk).tail<3>()).transpose();
    }

    Eigen::Vector3d BSplinePose::linearVelocity(double tk) const
    {
      return evalD(tk,1).head<3>();
    }

    Eigen::Vector3d BSplinePose::linearVelocityBodyFrame(double tk) const
    {
      Eigen::VectorXd r = evalD(tk, 0);
      Eigen::Matrix3d C_wb = rotation_->parametersToRotationMatrix(r.tail<3>());
      return C_wb.transpose() * evalD(tk, 1).head<3>();
    }

    Eigen::Vector3d BSplinePose::linearAcceleration(double tk) const
    {
      return evalD(tk,2).head<3>();
    }

    Eigen::Vector3d BSplinePose::linearAccelerationBodyFrame(double tk) const
    {
      Eigen::VectorXd r = evalD(tk, 0);
      Eigen::Matrix3d C_wb = rotation_->parametersToRotationMatrix(r.tail<3>());
      return C_wb.transpose() * evalD(tk, 2).head<3>();
    }

    Eigen::Vector3d BSplinePose::linearAccelerationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {

      Eigen::Vector3d a = evalDAndJacobian(tk,2,J,coefficientIndices).head<3>();
      if(J)
      {
        J->conservativeResize(3,J->cols());
      }
      return a;
    }

    // {}_w\omege_{wb}
    // \omega_w_{b,w} (angular velocity of the body frame as seen from the world frame, expressed in the world frame)
    Eigen::Vector3d BSplinePose::angularVelocity(double tk) const
    {
      Eigen::Vector3d omega;
      Eigen::VectorXd r = evalD(tk,0);
      Eigen::VectorXd v = evalD(tk,1);

      omega = -rotation_->parametersToSMatrix(r.tail<3>()) * v.tail<3>();
      return omega;
    }

    // {}_b\omege_{wb}
    // \omega_w_{b,b} (angular velocity of the body frame as seen from the world frame, expressed in the body frame)
    Eigen::Vector3d BSplinePose::angularVelocityBodyFrame(double tk) const
    {
      Eigen::Vector3d omega;
      Eigen::VectorXd r = evalD(tk,0);
      Eigen::VectorXd v = evalD(tk,1);
      Eigen::Matrix3d S;
      Eigen::Matrix3d C_w_b = rotation_->parametersToRotationMatrix(r.tail<3>(), &S);

      omega = -C_w_b.transpose() * S * v.tail<3>();
      return omega;

    }

    // \omega_w_{b,b} (angular velocity of the body frame as seen from the world frame, expressed in the body frame)
    Eigen::Vector3d BSplinePose::angularVelocityBodyFrameAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::MatrixXd Jr;
      Eigen::Matrix3d C_b_w = inverseOrientationAndJacobian(tk,&Jr,NULL);

      Eigen::Vector3d omega = angularVelocityAndJacobian(tk, J, coefficientIndices);
      omega = C_b_w * omega;

      if(J)
      {
        *J = C_b_w * (*J) + sm::kinematics::crossMx(omega) * Jr;
      }

      return omega;
    }

    // \omega_w_{b,w} (angular velocity of the body frame as seen from the world frame, expressed in the world frame)
    Eigen::Vector3d BSplinePose::angularVelocityAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {

      Eigen::Vector3d omega;
      Eigen::Vector3d p;
      Eigen::Vector3d pdot;
      Eigen::MatrixXd Jp;
      Eigen::MatrixXd Jpdot;
      p = evalDAndJacobian(tk,0,&Jp,NULL).tail<3>();
      pdot = evalDAndJacobian(tk,1,&Jpdot,coefficientIndices).tail<3>();

      // Rearrange the spline jacobian matrices. Now Jpdot is the
      // jacobian of p wrt the spline coefficients stacked on top
      // of the jacobian of pdot wrt the spline coefficients.
      Jpdot.block(0,0,3,Jpdot.cols()) = Jp.block(3,0,3,Jp.cols());

      //std::cout << "Jpdot\n" << Jpdot << std::endl;
      // \omega = -Jr(\theta) * \dot{\theta}
      // \theta, \dot{\theta} are independent, then
      //    d\omega/dC = d\omega/d\theta * d\theta/dC + d\omega/d\dot{\theta} * d\dot{\theta}/dC
      //  d\omega/d\theta * d\theta/dC + d\omega/d\dot{\theta} * d\dot{\theta}/dC
      // = -R * [d(Jr(\theta)\dot{\theta})/d\theta   Jr(\theta)] * [    d\theta/dC    ]
      //                                                           [ d\dot{\theta}/dC ]
      Eigen::Matrix<double,3,6> Jo;
      omega = -rotation_->angularVelocityAndJacobian(p,pdot,&Jo);

      //std::cout << "Jo:\n" << Jo << std::endl;
      if(J)
      {
        *J = -Jo * Jpdot;
      }

      return omega;
    }

    // {}_b\dot\omega_{wb}
    // \omega_dot_w_{b,b} (angular acceleration of the body frame as seen from the world frame, expressed in the body frame)
    Eigen::Vector3d BSplinePose::angularAccelerationBodyFrame(double tk) const
    {
    	Eigen::Vector3d p = evalD(tk,0).tail<3>();
      Eigen::Vector3d v = evalD(tk,1).tail<3>();
    	Eigen::Vector3d a = evalD(tk,2).tail<3>();
    	Eigen::Matrix3d C_w_b = rotation_->parametersToRotationMatrix(p);

      Eigen::Matrix<double,3,6> Jo;
      rotation_->angularVelocityAndJacobian(p,v,&Jo);

      Eigen::Matrix<double,6,1> va;
      va << v, a;

      return -C_w_b.transpose() * (Jo * va);
    }

    // \omega_dot_w_{b,b} (angular acceleration of the body frame as seen from the world frame, expressed in the body frame)
    Eigen::Vector3d BSplinePose::angularAccelerationBodyFrameAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::MatrixXd Jr;
    	Eigen::Matrix3d C_b_w = inverseOrientationAndJacobian(tk,&Jr,NULL);

      Eigen::Vector3d acce = angularAccelerationAndJacobian(tk, J, coefficientIndices);
      acce = C_b_w * acce;

      if(J)
    	{
    		*J = C_b_w * (*J) + sm::kinematics::crossMx(acce) * Jr;
    	}

      return acce;
    }

    // {}_w\dot\omega_{wb}
    // \omega_dot_w_{b,w} (angular acceleration of the body frame as seen from the world frame, expressed in the world frame)
    Eigen::Vector3d BSplinePose::angularAcceleration(double tk) const
    {
    	Eigen::Vector3d p = evalD(tk,0).tail<3>();
      Eigen::Vector3d v = evalD(tk,1).tail<3>();
    	Eigen::Vector3d a = evalD(tk,2).tail<3>();

      Eigen::Matrix<double,3,6> Jo;
      rotation_->angularVelocityAndJacobian(p,v,&Jo);

      Eigen::Matrix<double,6,1> va;
      va << v, a;

      return -(Jo * va);
    }

    // \todo Only support RotationVector by now
    // \omega_dot_w_{b,w} (angular acceleration of the body frame as seen from the world frame, expressed in the world frame)
    Eigen::Vector3d BSplinePose::angularAccelerationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::Vector3d p;
      Eigen::Vector3d v;
    	Eigen::Vector3d a;
      Eigen::MatrixXd Jp;
      Eigen::MatrixXd Jv;
      Eigen::MatrixXd Ja;
      p = evalDAndJacobian(tk,0,&Jp,NULL).tail<3>();
      v = evalDAndJacobian(tk,1,&Jv,NULL).tail<3>();
      a = evalDAndJacobian(tk,2,&Ja,coefficientIndices).tail<3>();

      Eigen::Matrix<double,3,6> Jo;
      rotation_->angularVelocityAndJacobian(p,v,&Jo);

      Eigen::Matrix<double,6,1> va;
      va << v, a;

      Eigen::Vector3d acce = -(Jo * va);

      // Rearrange the spline jacobian matrices. Now Ja is the
    	// jacobian of p wrt the spline coefficients stacked on top
    	// of the jacobian of a wrt the spline coefficients.
    	Ja.block(0,0,3,Ja.cols()) = Jp.block(3,0,3,Jp.cols());

      // Rearrange the spline jacobian matrices. Now Jv is the
    	// jacobian of p wrt the spline coefficients stacked on top
    	// of the jacobian of v wrt the spline coefficients.
    	Jv.block(0,0,3,Jv.cols()) = Jp.block(3,0,3,Jp.cols());

      rotation_->angularVelocityAndJacobian(p,a,&Jo);

      // f(\theta, \dot\theta) = \frac{\partial Jr(\theta)\dot\theta}{\partial\theta}\dot\theta
      // [ \frac{\partial f}{\partial\theta}  \frac{\partial f}{\partial\dot\theta} ]
      Eigen::Matrix<double,3,6> Jpp;

      double factor[5];
      const double pv = p.dot(v);
      const double v2 = v.squaredNorm();
      const double p2 = p.squaredNorm();
      if(p2 < 1e-14)
      {
        const double p4 = p2 * p2;
        // Series[(x Sin[x] + 2 Cos[x] - 2) / x^4, {x, 0, 4}]
        factor[0] = -1.0 / 12 + p2 / 180 - p4 / 6720;
        // Series[(x - Sin[x]) / x^3, {x, 0, 4}]
        factor[1] = 1.0 / 6 - p2 / 120 + p4 / 5040;
        // Series[(3 Sin[x] - x Cos[x] - 2 x) / (x^5), {x, 0, 4}]
        factor[2] = -1.0 / 60 + p2 / 1260 - p4 / 60480;
        // Series[((x^2 - 8) Cos[x] - 5 x Sin[x] + 8) / x^6, {x, 0, 4}]
        factor[3] = 1.0 / 90 - p2 / 1680 + p4 / 75600;
        // Series[((x^2 - 15) Sin[x] + 7 x Cos[x] + 8 x) / x^7, {x, 0, 4}]
        factor[4] = 1.0 / 630 - p2 / 15120 + p4 / 831600;
      }
      else
      {
        const double p1 = std::sqrt(p2);
        const double p3 = p1 * p2;
        const double p4 = p1 * p3;
        const double p5 = p1 * p4;
        const double p6 = p1 * p5;
        const double p7 = p1 * p6;
        const double cosp = std::cos(p1);
        const double sinp = std::sin(p1);

        factor[0] = (p1 * sinp + 2 * cosp - 2) / p4;
        factor[1] = (p1 - sinp) / p3;
        factor[2] = (3 * sinp - p1 * cosp - 2 * p1) / p5;
        factor[3] = ((p2 - 8) * cosp - 5 * p1 * sinp + 8) / p6;
        factor[4] = ((p2 - 15) * sinp + 7 * p1 * cosp + 8 * p1) / p7;
      }

      Jpp.leftCols<3>() = factor[0] * sm::kinematics::crossMx(v) * (pv * Eigen::Matrix3d::Identity() + p * v.transpose()) +
                          factor[1] * (v2 * Eigen::Matrix3d::Identity() - v * v.transpose()) +
                          factor[2] * (pv * pv * Eigen::Matrix3d::Identity() + 2 * pv * p * v.transpose() - 2 * pv * v * p.transpose() - p2 * v * v.transpose()) +
                          factor[3] * pv * sm::kinematics::crossMx(v) * p * p.transpose() +
                          factor[2] * (v2 * p * p.transpose() - pv * v * p.transpose()) +
                          factor[4] * (pv * pv * p * p.transpose() - pv * p2 * v * p.transpose());
      Jpp.rightCols<3>() = factor[0] * (sm::kinematics::crossMx(v) * p * p.transpose() - pv * sm::kinematics::crossMx(p)) +
                           factor[1] * (2 * p * v.transpose() - v * p.transpose() - pv * Eigen::Matrix3d::Identity()) +
                           factor[2] * (2 * pv * p * p.transpose() - pv * p2 * Eigen::Matrix3d::Identity() - p2 * v * p.transpose());

      if(J)
    	{
    		*J = -Jpp * Jv - Jo * Ja;
    	}

      return acce;
    }

    void BSplinePose::initPoseSpline(double t0, double t1, const Eigen::Matrix4d & T_n_t0, const Eigen::Matrix4d & T_n_t1)
    {
      Eigen::VectorXd v0 = transformationToCurveValue(T_n_t0);
      Eigen::VectorXd v1 = transformationToCurveValue(T_n_t1);

      initSpline(t0,t1,v0,v1);
    }

    void BSplinePose::addPoseSegment(double tk, const Eigen::Matrix4d & T_n_tk)
    {
      Eigen::VectorXd vk = transformationToCurveValue(T_n_tk);

      addCurveSegment(tk, vk);
    }

    void BSplinePose::addPoseSegment2(double tk, const Eigen::Matrix4d & T_n_tk, double lambda)
    {
      Eigen::VectorXd vk = transformationToCurveValue(T_n_tk);

      addCurveSegment2(tk, vk, lambda);
    }

    Eigen::Matrix4d BSplinePose::curveValueToTransformation( const Eigen::VectorXd & c ) const
    {
      SM_ASSERT_EQ_DBG(Exception, c.size(), 6, "The curve value is an unexpected size!");
      Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
      T.topLeftCorner<3,3>() = rotation_->parametersToRotationMatrix(c.tail<3>());
      T.topRightCorner<3,1>() = c.head<3>();

      return T;
    }

    // dT/dp s.t. T(p + \delta) = Exp(dT/dp \delta)T(p)
    Eigen::Matrix4d BSplinePose::curveValueToTransformationAndJacobian( const Eigen::VectorXd & p, Eigen::MatrixXd * J ) const
    {
      SM_ASSERT_EQ_DBG(Exception, p.size(), 6, "The curve value is an unexpected size!");
      Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
      Eigen::Matrix3d S;
      T.topLeftCorner<3,3>() = rotation_->parametersToRotationMatrix(p.tail<3>(), &S);
      T.topRightCorner<3,1>() = p.head<3>();

      if(J)
      {
        *J = Eigen::MatrixXd::Identity(6,6);
        J->topRightCorner<3,3>() = -crossMx(p.head<3>()) * S;
        J->bottomRightCorner<3,3>() = S;
      }

      return T;

    }

    Eigen::VectorXd BSplinePose::transformationToCurveValue( const Eigen::Matrix4d & T ) const
    {
      Eigen::VectorXd c(6);
      c.head<3>() = T.topRightCorner<3,1>();
      c.tail<3>() = rotation_->rotationMatrixToParameters(T.topLeftCorner<3,3>());

      return c;
    }

    void BSplinePose::initPoseSpline2(const Eigen::VectorXd & times, const Eigen::Matrix<double,6,Eigen::Dynamic> & poses, int numSegments, double lambda)
    {
      initSpline2(times, poses, numSegments, lambda);
    }

    void BSplinePose::initPoseSpline3(const Eigen::VectorXd & times, const Eigen::Matrix<double,6,Eigen::Dynamic> & poses, int numSegments, double lambda)
    {
      initSpline3(times, poses, numSegments, lambda);
    }

    void BSplinePose::initPoseSplineSparse(const Eigen::VectorXd & times, const Eigen::Matrix<double,6,Eigen::Dynamic> & poses, int numSegments, double lambda)
    {
      initSplineSparse(times, poses, numSegments, lambda);
    }

    void BSplinePose::initPoseSplineSparseKnots(const Eigen::VectorXd &times, const Eigen::MatrixXd &interpolationPoints, const Eigen::VectorXd knots, double lambda)
    {
    	initSplineSparseKnots(times, interpolationPoints, knots, lambda);
    }

    RotationalKinematics::Ptr BSplinePose::rotation() const
    {
      return rotation_;
    }
  } // namespace bsplines


#ifndef particle_hpp
#define particle_hpp

#include <Eigen/Core>

namespace elasty
{
    struct Particle
    {
        Particle(const Eigen::Vector3d& x, const Eigen::Vector3d& v, const double m) :
        x(x),
        v(v),
        p(x),
        f(Eigen::Vector3d::Zero()),
        m(m),
        w(1.0 / m)
        {
        }

        Eigen::Vector3d x;
        Eigen::Vector3d v;
        Eigen::Vector3d p;
        Eigen::Vector3d f;
        double m;
        double w;
    };
}

#endif /* particle_hpp */


/**
 * \file libs/numeric/ublasx/dot.cpp
 *
 * \brief Test suite for the \c dot operation.
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#include <boost/numeric/ublas/fwd.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublasx/operation/dot.hpp>
#include <boost/numeric/ublasx/tags.hpp>
#include <functional>
#include <iostream>
#include "libs/numeric/ublasx/test/utils.hpp"

static const double tol = 1.0e-5;

namespace ublas = ::boost::numeric::ublas;
namespace ublasx = ::boost::numeric::ublasx;

BOOST_UBLASX_TEST_DEF( test_vector_container )
{
	BOOST_UBLASX_DEBUG_TRACE( "TEST Vector Container" );

	typedef double value_type;
	typedef ublas::vector<value_type> vector_type;
	typedef ublas::vector_traits<vector_type>::size_type size_type;

	size_type n(3);

	vector_type v1(n);
	v1(0) = 1;
	v1(1) = 2;
	v1(2) = 3;

	vector_type v2(n);
	v2(0) = 4;
	v2(1) = 5;
	v2(2) = 6;

	value_type expect(0);
	value_type res(0);

	// dot(v1,v2)
	expect = ublas::inner_prod(v1, v2);
	res = ublasx::dot(v1, v2);
	BOOST_UBLASX_DEBUG_TRACE( "dot(" << v1 << "," << v2 << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );
}

BOOST_UBLASX_TEST_DEF( test_vector_expression )
{
	BOOST_UBLASX_DEBUG_TRACE( "TEST Vector Expression" );

	typedef double value_type;
	typedef ublas::vector<value_type> vector_type;
	typedef ublas::vector_traits<vector_type>::size_type size_type;

	size_type n(3);

	vector_type v1(n);
	v1(0) = 1;
	v1(1) = 2;
	v1(2) = 3;

	vector_type v2(n);
	v2(0) = 4;
	v2(1) = 5;
	v2(2) = 6;

	value_type expect(0);
	value_type res(0);

	// dot(-v1,-v2)
	expect = ublas::inner_prod(v1, v2);
	res = ublasx::dot(-v1, -v2);
	BOOST_UBLASX_DEBUG_TRACE( "dot(" << v1 << "," << v2 << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );
}

BOOST_UBLASX_TEST_DEF( test_vector_reference )
{
	BOOST_UBLASX_DEBUG_TRACE( "TEST Vector Reference" );

	typedef double value_type;
	typedef ublas::vector<value_type> vector_type;
	typedef ublas::vector_reference<vector_type> vector_reference_type;
	typedef ublas::vector_traits<vector_type>::size_type size_type;

	size_type n(3);

	vector_type v1(n);
	v1(0) = 1;
	v1(1) = 2;
	v1(2) = 3;

	vector_type v2(n);
	v2(0) = 4;
	v2(1) = 5;
	v2(2) = 6;

	value_type expect(0);
	value_type res(0);

	// dot(ref(v1),ref(v2))
	expect = ublas::inner_prod(vector_reference_type(v1), vector_reference_type(v2));
	res = ublasx::dot(v1, v2);
	BOOST_UBLASX_DEBUG_TRACE( "dot(" << v1 << "," << v2 << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );
}

BOOST_UBLASX_TEST_DEF( test_col_major_matrix_container )
{
	BOOST_UBLASX_DEBUG_TRACE( "TEST Column-major Matrix Container" );

	typedef double value_type;
	typedef ublas::matrix<value_type, ublas::column_major> matrix_type;
	typedef ublas::vector<value_type> vector_type;
	typedef ublas::matrix_traits<matrix_type>::size_type size_type;

	size_type nr = 3;
	size_type nc = 4;

	matrix_type A(nr, nc);
	A(0,0) =  1; A(0,1) =  2; A(0,2) =  3; A(0,3) =  4;
	A(1,0) =  5; A(1,1) =  6; A(1,2) =  7; A(1,3) =  8;
	A(2,0) =  9; A(2,1) = 10; A(2,2) = 11; A(2,3) = 12;

	matrix_type B(nr, nc);
	B(0,0) = 13; B(0,1) = 14; B(0,2) = 15; B(0,3) = 16;
	B(1,0) = 17; B(1,1) = 18; B(1,2) = 19; B(1,3) = 20;
	B(2,0) = 21; B(2,1) = 22; B(2,2) = 23; B(2,3) = 24;

	vector_type dot_1(nc);
	for (size_type i = 0; i < nc; ++i)
	{
		dot_1(i) = ublas::inner_prod(ublas::column(A,i), ublas::column(B,i));
	}

	vector_type dot_2(nr);
	for (size_type i = 0; i < nr; ++i)
	{
		dot_2(i) = ublas::inner_prod(ublas::row(A,i), ublas::row(B,i));
	}

	vector_type expect;;
	vector_type res;;

	// dot<1>(A,B)
	expect = dot_1;
	res = ublasx::dot<1>(A, B);
	BOOST_UBLASX_DEBUG_TRACE( "dot<1>(" << A << "," << B << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect, expect.size(), tol );

	// dot<2>(A,B)
	expect = dot_2;
	res = ublasx::dot<2>(A, B);
	BOOST_UBLASX_DEBUG_TRACE( "dot<2>(" << A << "," << B << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect, expect.size(), tol );
}

BOOST_UBLASX_TEST_DEF( test_row_major_matrix_container )
{
	BOOST_UBLASX_DEBUG_TRACE( "TEST Row-major Matrix Container" );

	typedef double value_type;
	typedef ublas::matrix<value_type, ublas::row_major> matrix_type;
	typedef ublas::vector<value_type> vector_type;
	typedef ublas::matrix_traits<matrix_type>::size_type size_type;

	size_type nr = 3;
	size_type nc = 4;

	matrix_type A(nr, nc);
	A(0,0) =  1; A(0,1) =  2; A(0,2) =  3; A(0,3) =  4;
	A(1,0) =  5; A(1,1) =  6; A(1,2) =  7; A(1,3) =  8;
	A(2,0) =  9; A(2,1) = 10; A(2,2) = 11; A(2,3) = 12;

	matrix_type B(nr, nc);
	B(0,0) = 13; B(0,1) = 14; B(0,2) = 15; B(0,3) = 16;
	B(1,0) = 17; B(1,1) = 18; B(1,2) = 19; B(1,3) = 20;
	B(2,0) = 21; B(2,1) = 22; B(2,2) = 23; B(2,3) = 24;

	vector_type dot_1(nc);
	for (size_type i = 0; i < nc; ++i)
	{
		dot_1(i) = ublas::inner_prod(ublas::column(A,i), ublas::column(B,i));
	}

	vector_type dot_2(nr);
	for (size_type i = 0; i < nr; ++i)
	{
		dot_2(i) = ublas::inner_prod(ublas::row(A,i), ublas::row(B,i));
	}

	vector_type expect;;
	vector_type res;;

	// dot<1>(A,B)
	expect = dot_1;
	res = ublasx::dot<1>(A, B);
	BOOST_UBLASX_DEBUG_TRACE( "dot<1>(" << A << "," << B << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect, expect.size(), tol );

	// dot<2>(A,B)
	expect = dot_2;
	res = ublasx::dot<2>(A, B);
	BOOST_UBLASX_DEBUG_TRACE( "dot<2>(" << A << "," << B << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect, expect.size(), tol );
}

BOOST_UBLASX_TEST_DEF( test_matrix_expression )
{
	BOOST_UBLASX_DEBUG_TRACE( "TEST Matrix Expression" );

	typedef double value_type;
	typedef ublas::matrix<value_type> matrix_type;
	typedef ublas::vector<value_type> vector_type;
	typedef ublas::matrix_traits<matrix_type>::size_type size_type;

	size_type nr = 3;
	size_type nc = 4;

	matrix_type A(nr, nc);
	A(0,0) =  1; A(0,1) =  2; A(0,2) =  3; A(0,3) =  4;
	A(1,0) =  5; A(1,1) =  6; A(1,2) =  7; A(1,3) =  8;
	A(2,0) =  9; A(2,1) = 10; A(2,2) = 11; A(2,3) = 12;

	matrix_type B(nr, nc);
	B(0,0) = 13; B(0,1) = 14; B(0,2) = 15; B(0,3) = 16;
	B(1,0) = 17; B(1,1) = 18; B(1,2) = 19; B(1,3) = 20;
	B(2,0) = 21; B(2,1) = 22; B(2,2) = 23; B(2,3) = 24;

	vector_type dot_1(nr);
	for (size_type i = 0; i < nr; ++i)
	{
		dot_1(i) = ublas::inner_prod(ublas::row(A,i), ublas::row(B,i));
	}

	vector_type dot_2(nc);
	for (size_type i = 0; i < nc; ++i)
	{
		dot_2(i) = ublas::inner_prod(ublas::column(A,i), ublas::column(B,i));
	}

	vector_type expect;;
	vector_type res;;

	// dot<1>(A',B')
	expect = dot_1;
	res = ublasx::dot<1>(ublas::trans(A), ublas::trans(B));
	BOOST_UBLASX_DEBUG_TRACE( "dot<1>(" << ublas::trans(A) << "," << ublas::trans(B) << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect, expect.size(), tol );

	// dot<2>(A',B')
	expect = dot_2;
	res = ublasx::dot<2>(ublas::trans(A), ublas::trans(B));
	BOOST_UBLASX_DEBUG_TRACE( "dot<2>(" << ublas::trans(A) << "," << ublas::trans(B) << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect, expect.size(), tol );
}

BOOST_UBLASX_TEST_DEF( test_matrix_reference )
{
	BOOST_UBLASX_DEBUG_TRACE( "TEST Matrix Reference" );

	typedef double value_type;
	typedef ublas::matrix<value_type> matrix_type;
	typedef ublas::matrix_reference<matrix_type> matrix_reference_type;
	typedef ublas::vector<value_type> vector_type;
	typedef ublas::matrix_traits<matrix_type>::size_type size_type;

	size_type nr = 3;
	size_type nc = 4;

	matrix_type A(nr, nc);
	A(0,0) =  1; A(0,1) =  2; A(0,2) =  3; A(0,3) =  4;
	A(1,0) =  5; A(1,1) =  6; A(1,2) =  7; A(1,3) =  8;
	A(2,0) =  9; A(2,1) = 10; A(2,2) = 11; A(2,3) = 12;

	matrix_type B(nr, nc);
	B(0,0) = 13; B(0,1) = 14; B(0,2) = 15; B(0,3) = 16;
	B(1,0) = 17; B(1,1) = 18; B(1,2) = 19; B(1,3) = 20;
	B(2,0) = 21; B(2,1) = 22; B(2,2) = 23; B(2,3) = 24;

	vector_type dot_1(nc);
	for (size_type i = 0; i < nc; ++i)
	{
		dot_1(i) = ublas::inner_prod(ublas::column(A,i), ublas::column(B,i));
	}

	vector_type dot_2(nr);
	for (size_type i = 0; i < nr; ++i)
	{
		dot_2(i) = ublas::inner_prod(ublas::row(A,i), ublas::row(B,i));
	}

	vector_type expect;;
	vector_type res;;

	// dot<1>(ref(A),ref(B))
	expect = dot_1;
	res = ublasx::dot<1>(matrix_reference_type(A), matrix_reference_type(B));
	BOOST_UBLASX_DEBUG_TRACE( "dot<1>(" << A << "," << B << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect, expect.size(), tol );

	// dot<2>(ref(A),ref(B))
	expect = dot_2;
	res = ublasx::dot<2>(matrix_reference_type(A), matrix_reference_type(B));
	BOOST_UBLASX_DEBUG_TRACE( "dot<2>(" << A << "," << B << ") = " << res << " ==> " << expect );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect, expect.size(), tol );
}

int main()
{
	BOOST_UBLASX_DEBUG_TRACE("Test Suite: 'dot' operation");

	BOOST_UBLASX_TEST_BEGIN();

	BOOST_UBLASX_TEST_DO( test_vector_container );
	BOOST_UBLASX_TEST_DO( test_vector_expression );
	BOOST_UBLASX_TEST_DO( test_vector_reference );
	BOOST_UBLASX_TEST_DO( test_col_major_matrix_container );
	BOOST_UBLASX_TEST_DO( test_row_major_matrix_container );
	BOOST_UBLASX_TEST_DO( test_matrix_expression );
	BOOST_UBLASX_TEST_DO( test_matrix_reference );

	BOOST_UBLASX_TEST_END();
}


/**
 * laminate_main reads the input file `laminate_input` and `material_data`,
 * construct a laminate model containing material properties and response to
 * the load, and save the stress and strain profile into
 * `laminate_profile_data.txt`, and save the A,B, and D submatrices into
 * `stifness_submatrices ABD.txt`.
 *
 * `laminate_profile_data.txt` contains the following columns (in order):
 * Coordinates, stress_x, stress_y, stress_xy, strain_x, strain_y, strain_xy.
 *
 * `stiffness_submatrices_ABD.txt` contains the A, B, and D submatrices of the
 * composite laminate, in the given order separate by new lines.
 *
 */

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <Eigen/Dense>
#include "../include/input_parser.h"
#include "../include/ply.h"
#include "../include/laminate.h"

void save_laminate_profile(laminate& lam);

int main() {
    std::vector<std::string> input_strings =
        read_composite_input("input_files/laminate_input.lmc");
    std::vector<ply> ply_vector =
        get_ply_vector(input_strings, "input_files/material_data.lmc");
    Eigen::Matrix<double, 6, 1> load_vector = get_load_vector(input_strings[3]);
    double min_thickness = get_minimum_ply_thickness(input_strings[2]);
    double pt_spacing = min_thickness/20.;
    laminate lam(ply_vector, load_vector, pt_spacing);
    save_laminate_profile(lam);
    std::cout << "Laminate_main -- Data saved." << std::endl;
    return 0;
}

void save_laminate_profile(laminate& lam) {
    std::ofstream profile_file;
    profile_file.open("output_files/laminate_profile_data.txt");
    auto i_sigma = lam.stresses_.begin();
    auto i_eps = lam.strains_.begin();
    auto i_pt = lam.profile_pt_.begin();
    for (; i_sigma != lam.stresses_.end(); i_sigma++, i_eps++, i_pt++) {
        Eigen::Matrix<double, 7, 1> combined_data;
        combined_data << *i_pt, *i_sigma, *i_eps;

        Eigen::Map<Eigen::Matrix<double, 1, 7>>
            row(combined_data.data(), combined_data.size());
        profile_file << row << std::endl;
    }
    std::ofstream stiffness_file;
    stiffness_file.open("output_files/stiffness_submatrices ABD.txt");
    stiffness_file << lam.A_;
    stiffness_file << std::endl<< std::endl;
    stiffness_file << lam.B_;
    stiffness_file << std::endl << std::endl;
    stiffness_file << lam.D_;
    stiffness_file << std::endl << std::endl;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2014 Benoit Steiner <benoit.steiner.goog@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

#include <Eigen/CXX11/Tensor>

using Eigen::MatrixXf;
using Eigen::Tensor;

static void test_simple()
{
  MatrixXf m1(3,3);
  MatrixXf m2(3,3);
  m1.setRandom();
  m2.setRandom();

  TensorMap<Tensor<float, 2> > mat1(m1.data(), 3,3);
  TensorMap<Tensor<float, 2> > mat2(m2.data(), 3,3);

  Tensor<float, 2> mat3(3,3);
  mat3 = mat1;

  typedef Tensor<float, 1>::DimensionPair DimPair;
  Eigen::array<DimPair, 1> dims;
  dims[0] = DimPair(1, 0);

  mat3 = mat3.contract(mat2, dims).eval();

  VERIFY_IS_APPROX(mat3(0, 0), (m1*m2).eval()(0,0));
  VERIFY_IS_APPROX(mat3(0, 1), (m1*m2).eval()(0,1));
  VERIFY_IS_APPROX(mat3(0, 2), (m1*m2).eval()(0,2));
  VERIFY_IS_APPROX(mat3(1, 0), (m1*m2).eval()(1,0));
  VERIFY_IS_APPROX(mat3(1, 1), (m1*m2).eval()(1,1));
  VERIFY_IS_APPROX(mat3(1, 2), (m1*m2).eval()(1,2));
  VERIFY_IS_APPROX(mat3(2, 0), (m1*m2).eval()(2,0));
  VERIFY_IS_APPROX(mat3(2, 1), (m1*m2).eval()(2,1));
  VERIFY_IS_APPROX(mat3(2, 2), (m1*m2).eval()(2,2));
}

static void test_const()
{
  MatrixXf input(3,3);
  input.setRandom();
  MatrixXf output = input;
  output.rowwise() -= input.colwise().maxCoeff();

  Eigen::array<int, 1> depth_dim;
  depth_dim[0] = 0;
  Tensor<float, 2>::Dimensions dims2d;
  dims2d[0] = 1;
  dims2d[1] = 3;
  Eigen::array<int, 2> bcast;
  bcast[0] = 3;
  bcast[1] = 1;
  const TensorMap<Tensor<const float, 2> > input_tensor(input.data(), 3, 3);
  Tensor<float, 2> output_tensor= (input_tensor - input_tensor.maximum(depth_dim).eval().reshape(dims2d).broadcast(bcast));

  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      VERIFY_IS_APPROX(output(i, j), output_tensor(i, j));
    }
  }
}

void test_cxx11_tensor_forced_eval()
{
  CALL_SUBTEST(test_simple());
  CALL_SUBTEST(test_const());
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   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
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_SQRT_2_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_SQRT_2_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/sdk/constant/register.hpp>
#include <boost/simd/sdk/constant/constant.hpp>

/*!
 * \ingroup boost_simd_constant
 * \defgroup boost_simd_constant_sqrt_2 Sqrt_2
 *
 * \par Description
 * Constant Sqrt_2 = \f$\sqrt2\f$
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/sqrt_2.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::sqrt_2_(A0)>::type
 *     Sqrt_2();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Sqrt_2
 *
 * \return type T value
 *
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
     * \brief Define the tag Sqrt_2 of functor Sqrt_2
     *        in namespace boost::simd::tag for toolbox boost.simd.constant
    **/
    BOOST_SIMD_CONSTANT_REGISTER( Sqrt_2, double, 1
                                , 0x3FB504F3, 0x3ff6A09E667F3BCCULL
                                );
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Sqrt_2, Sqrt_2)
} }

#include <boost/simd/sdk/constant/common.hpp>

#endif