/*
 * Copyright 2016-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#include "LMPC.h"
#include "PreviewSystem.h"
#include "QuadProgSolver.h"
#include "constraints.h"
#include "costFunctions.h"
#include "doctest.h"
#include "systems.h"
#include "tools.h"
#ifdef EIGEN_QLD_FOUND
#include "QLDSolver.h"
#endif
#ifdef EIGEN_LSSOL_FOUND
#include "LSSOLSolver.h"
#endif
#ifdef EIGEN_GUROBI_FOUND
#include "GUROBISolver.h"
#endif
#ifdef EIGEN_OSQP_FOUND
#include "OSQPSolver.h"
#endif

#include <Eigen/Core>
#include <algorithm>
#include <memory>
#include <numeric>
#include <vector>

/********************************************************************************************************
 *                               Check Bound constraint                                                 *
 ********************************************************************************************************/

TEST_CASE_FIXTURE(BoundedSystem, "MPC_TARGET_COST_WITH_BOUND_CONSTRAINTS")
{
    tools::SolverTimers sTimers;

    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TargetCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryBoundConstraint>(xLower, xUpper);
    auto contConstr = std::make_shared<copra::ControlBoundConstraint>(uLower, uUpper);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);
    controller.addConstraint(contConstr);

    auto pcCheck = [&](const std::string& solverName, copra::SolverFlag sFlag, std::unique_ptr<copra::SolverInterface>&& solver = nullptr) {
        if (solver) {
            controller.useSolver(std::move(solver));
        } else {
            controller.selectQPSolver(sFlag);
        }

        REQUIRE(controller.solve());
        sTimers.st.emplace_back(solverName, controller.solveTime() * 1e3);
        sTimers.ct.emplace_back(solverName, controller.solveAndBuildTime() * 1e3);
        sTimers.bt.emplace_back(solverName, (controller.solveAndBuildTime() - controller.solveTime()) * 1e3);

        Eigen::VectorXd fullTraj = controller.trajectory();
        auto trajLen = fullTraj.rows() / 2;
        Eigen::VectorXd posTraj(trajLen);
        Eigen::VectorXd velTraj(trajLen);
        for (auto i = 0; i < trajLen; ++i) {
            posTraj(i) = fullTraj(2 * i);
            velTraj(i) = fullTraj(2 * i + 1);
        }
        Eigen::VectorXd control = controller.control();

        // Check result
        CHECK_LE(std::abs(xd(1) - velTraj.tail(1)(0)), 0.001);

        // Check constrains
        REQUIRE_LE(posTraj.maxCoeff(), x0(0));
        REQUIRE_LE(velTraj.maxCoeff(), xUpper(1) + 1e-6);
        REQUIRE_LE(control.maxCoeff(), uUpper(0) + 1e-6);
    };

    for (auto s : tools::Solvers) {
        std::unique_ptr<copra::SolverInterface> solver(nullptr);
#ifdef EIGEN_LSSOL_FOUND
        if (s.second == copra::SolverFlag::LSSOL) {
            solver = copra::solverFactory(copra::SolverFlag::LSSOL);
            solver->SI_maxIter(200);
        }
#endif
        pcCheck(s.first, s.second, std::move(solver));
    }

    MESSAGE(tools::getSortedTimers(sTimers));
}

TEST_CASE_FIXTURE(BoundedSystem, "MPC_TRAJECTORY_COST_WITH_BOUND_CONSTRAINTS")
{
    tools::SolverTimers sTimers;

    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TrajectoryCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryBoundConstraint>(xLower, xUpper);
    auto contConstr = std::make_shared<copra::ControlBoundConstraint>(uLower, uUpper);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);
    controller.addConstraint(contConstr);

    auto pcCheck = [&](const std::string& solverName, copra::SolverFlag sFlag, std::unique_ptr<copra::SolverInterface>&& solver = nullptr) {
        std::unique_ptr<copra::SolverInterface> newSolver;
        if (solver) {
            newSolver = std::move(solver);
        } else {
            newSolver = solverFactory(sFlag);
        }
#ifdef EIGEN_OSQP_FOUND
        // Increase precision
        if (sFlag == copra::SolverFlag::OSQP) {
            auto& bs = static_cast<copra::OSQPSolver&>(*newSolver).baseSolver();
            bs.scalingIter(0);
            bs.absConvergenceTol(1e-6);
            bs.relConvergenceTol(1e-6);
            bs.primalInfeasibilityTol(1e-7);
            bs.dualInfeasibilityTol(1e-7);
        }
#endif
        controller.useSolver(std::move(newSolver));

        REQUIRE(controller.solve());
        sTimers.st.emplace_back(solverName, controller.solveTime() * 1e3);
        sTimers.ct.emplace_back(solverName, controller.solveAndBuildTime() * 1e3);
        sTimers.bt.emplace_back(solverName, (controller.solveAndBuildTime() - controller.solveTime()) * 1e3);

        Eigen::VectorXd fullTraj = controller.trajectory();
        auto trajLen = fullTraj.rows() / 2;
        Eigen::VectorXd posTraj(trajLen);
        Eigen::VectorXd velTraj(trajLen);
        for (auto i = 0; i < trajLen; ++i) {
            posTraj(i) = fullTraj(2 * i);
            velTraj(i) = fullTraj(2 * i + 1);
        }
        Eigen::VectorXd control = controller.control();

        // Check result
        CHECK_LE(std::abs(xd(1) - velTraj.tail(1)(0)), 0.001);

        // Check constrains
        REQUIRE_LE(posTraj.maxCoeff(), x0(0));
        REQUIRE_LE(velTraj.maxCoeff(), xUpper(1) + 1e-6);
        REQUIRE_LE(control.maxCoeff(), uUpper(0) + 1e-6); // QuadProg allows to exceeds the constrain of a small amount.
    };

    for (auto s : tools::Solvers) {
        std::unique_ptr<copra::SolverInterface> solver(nullptr);
#ifdef EIGEN_LSSOL_FOUND
        if (s.second == copra::SolverFlag::LSSOL) {
            solver = copra::solverFactory(copra::SolverFlag::LSSOL);
            solver->SI_maxIter(200);
        }
#endif
        pcCheck(s.first, s.second, std::move(solver));
    }

    MESSAGE(tools::getSortedTimers(sTimers));
}

TEST_CASE_FIXTURE(BoundedSystem, "MPC_MIXED_COST_WITH_BOUND_CONSTRAINTS")
{
    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::MixedCost>(M, Eigen::MatrixXd::Zero(2, 1), xd); // min(||X - Xt||^2)
    auto uCost = std::make_shared<copra::MixedCost>(Eigen::MatrixXd::Zero(1, 2), N, ud); // min(||U - Ut||^2)
    auto trajConstr = std::make_shared<copra::TrajectoryBoundConstraint>(xLower, xUpper);
    auto contConstr = std::make_shared<copra::ControlBoundConstraint>(uLower, uUpper);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);
    controller.addConstraint(contConstr);

    REQUIRE(controller.solve());

    Eigen::VectorXd fullTraj = controller.trajectory();
    auto trajLen = fullTraj.rows() / 2;
    Eigen::VectorXd posTraj(trajLen);
    Eigen::VectorXd velTraj(trajLen);
    for (auto i = 0; i < trajLen; ++i) {
        posTraj(i) = fullTraj(2 * i);
        velTraj(i) = fullTraj(2 * i + 1);
    }
    Eigen::VectorXd control = controller.control();

    // Check result
    CHECK_LE(std::abs(xd(1) - velTraj.tail(3)(0)), 0.001); // Check X_{N-1} for mixed cost because X_N is not evaluated.

    // Check constrains
    REQUIRE_LE(posTraj.maxCoeff(), x0(0));
    REQUIRE_LE(velTraj.maxCoeff(), xUpper(1) + 1e-6);
    REQUIRE_LE(control.maxCoeff(), uUpper(0) + 1e-6); // QuadProg allows to exceeds the constrain of a small amount.
}

/********************************************************************************************************
 *                            Check inequality constraint                                               *
 ********************************************************************************************************/

TEST_CASE_FIXTURE(IneqSystem, "MPC_TARGET_COST_WITH_INEQUALITY_CONSTRAINTS")
{
    tools::SolverTimers sTimers;

    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TargetCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(E, p);
    auto contConstr = std::make_shared<copra::ControlConstraint>(G, h);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);
    controller.addConstraint(contConstr);

    auto pcCheck = [&](const std::string& solverName, copra::SolverFlag sFlag, std::unique_ptr<copra::SolverInterface>&& solver = nullptr) {
        std::unique_ptr<copra::SolverInterface> newSolver;
        if (solver) {
            newSolver = std::move(solver);
        } else {
            newSolver = solverFactory(sFlag);
        }
#ifdef EIGEN_OSQP_FOUND
        // Increase precision
        if (sFlag == copra::SolverFlag::OSQP) {
            auto& bs = static_cast<copra::OSQPSolver&>(*newSolver).baseSolver();
            bs.scalingIter(0);
            bs.absConvergenceTol(1e-6);
            bs.relConvergenceTol(1e-6);
            bs.primalInfeasibilityTol(1e-7);
            bs.dualInfeasibilityTol(1e-7);
        }
#endif
        controller.useSolver(std::move(newSolver));

        REQUIRE(controller.solve());
        sTimers.st.emplace_back(solverName, controller.solveTime() * 1e3);
        sTimers.ct.emplace_back(solverName, controller.solveAndBuildTime() * 1e3);
        sTimers.bt.emplace_back(solverName, (controller.solveAndBuildTime() - controller.solveTime()) * 1e3);

        Eigen::VectorXd fullTraj = controller.trajectory();
        auto trajLen = fullTraj.rows() / 2;
        Eigen::VectorXd posTraj(trajLen);
        Eigen::VectorXd velTraj(trajLen);
        for (auto i = 0; i < trajLen; ++i) {
            posTraj(i) = fullTraj(2 * i);
            velTraj(i) = fullTraj(2 * i + 1);
        }
        Eigen::VectorXd control = controller.control();

        // Check result
        CHECK_LE(std::abs(xd(1) - velTraj.tail(1)(0)), 0.001);

        // Check constrains
        REQUIRE_LE(posTraj.maxCoeff(), x0(0));
        REQUIRE_LE(velTraj.maxCoeff(), p(0) + 1e-6);
        REQUIRE_LE(control.maxCoeff(), h(0) + 1e-6);
    };

    for (auto s : tools::Solvers) {
        std::unique_ptr<copra::SolverInterface> solver(nullptr);
#ifdef EIGEN_LSSOL_FOUND
        if (s.second == copra::SolverFlag::LSSOL) {
            solver = copra::solverFactory(copra::SolverFlag::LSSOL);
            solver->SI_maxIter(200);
        }
#endif
        pcCheck(s.first, s.second, std::move(solver));
    }

    MESSAGE(tools::getSortedTimers(sTimers));
}

TEST_CASE_FIXTURE(IneqSystem, "MPC_TRAJECTORY_COST_WITH_INEQUALITY_CONSTRAINTS")
{
    tools::SolverTimers sTimers;

    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TrajectoryCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(E, p);
    auto contConstr = std::make_shared<copra::ControlConstraint>(G, h);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);
    controller.addConstraint(contConstr);

    auto pcCheck = [&](const std::string& solverName, copra::SolverFlag sFlag, std::unique_ptr<copra::SolverInterface>&& solver = nullptr) {
        std::unique_ptr<copra::SolverInterface> newSolver;
        if (solver) {
            newSolver = std::move(solver);
        } else {
            newSolver = solverFactory(sFlag);
        }
#ifdef EIGEN_OSQP_FOUND
        // Increase precision
        if (sFlag == copra::SolverFlag::OSQP) {
            auto& bs = static_cast<copra::OSQPSolver&>(*newSolver).baseSolver();
            bs.scalingIter(0);
            bs.absConvergenceTol(1e-6);
            bs.relConvergenceTol(1e-6);
            bs.primalInfeasibilityTol(1e-7);
            bs.dualInfeasibilityTol(1e-7);
        }
#endif
        controller.useSolver(std::move(newSolver));

        REQUIRE(controller.solve());
        sTimers.st.emplace_back(solverName, controller.solveTime() * 1e3);
        sTimers.ct.emplace_back(solverName, controller.solveAndBuildTime() * 1e3);
        sTimers.bt.emplace_back(solverName, (controller.solveAndBuildTime() - controller.solveTime()) * 1e3);

        Eigen::VectorXd fullTraj = controller.trajectory();
        auto trajLen = fullTraj.rows() / 2;
        Eigen::VectorXd posTraj(trajLen);
        Eigen::VectorXd velTraj(trajLen);
        for (auto i = 0; i < trajLen; ++i) {
            posTraj(i) = fullTraj(2 * i);
            velTraj(i) = fullTraj(2 * i + 1);
        }
        Eigen::VectorXd control = controller.control();

        // Check result
        CHECK_LE(std::abs(xd(1) - velTraj.tail(1)(0)), 0.001);

        // Check constrains
        REQUIRE_LE(posTraj.maxCoeff(), x0(0));
        REQUIRE_LE(velTraj.maxCoeff(), p(0) + 1e-6);
        REQUIRE_LE(control.maxCoeff(), h(0) + 1e-6); // QuadProg allows to exceeds the constrain of a small amount.
    };

    for (auto s : tools::Solvers) {
        std::unique_ptr<copra::SolverInterface> solver(nullptr);
#ifdef EIGEN_LSSOL_FOUND
        if (s.second == copra::SolverFlag::LSSOL) {
            solver = copra::solverFactory(copra::SolverFlag::LSSOL);
            solver->SI_maxIter(200);
        }
#endif
        pcCheck(s.first, s.second, std::move(solver));
    }

    MESSAGE(tools::getSortedTimers(sTimers));
}

TEST_CASE_FIXTURE(IneqSystem, "MPC_MIXED_COST_WITH_INEQUALITY_CONSTRAINTS")
{
    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::MixedCost>(M, Eigen::MatrixXd::Zero(2, 1), xd);
    auto uCost = std::make_shared<copra::MixedCost>(Eigen::MatrixXd::Zero(1, 2), N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(E, p);
    auto contConstr = std::make_shared<copra::ControlConstraint>(G, h);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);
    controller.addConstraint(contConstr);

    REQUIRE(controller.solve());

    Eigen::VectorXd fullTraj = controller.trajectory();
    auto trajLen = fullTraj.rows() / 2;
    Eigen::VectorXd posTraj(trajLen);
    Eigen::VectorXd velTraj(trajLen);
    for (auto i = 0; i < trajLen; ++i) {
        posTraj(i) = fullTraj(2 * i);
        velTraj(i) = fullTraj(2 * i + 1);
    }
    Eigen::VectorXd control = controller.control();

    // Check result
    CHECK_LE(std::abs(xd(1) - velTraj.tail(3)(0)), 0.001); // Check X_{N-1} for mixed cost because X_N is not evaluated.

    // Check constrains
    REQUIRE_LE(posTraj.maxCoeff(), x0(0));
    REQUIRE_LE(velTraj.maxCoeff(), p(0) + 1e-6);
    REQUIRE_LE(control.maxCoeff(), h(0) + 1e-6); // QuadProg allows to exceeds the constrain of a small amount.
}

/********************************************************************************************************
 *                               Check Mixed constraint                                                 *
 ********************************************************************************************************/

TEST_CASE_FIXTURE(MixedSystem, "MPC_TARGET_COST_WITH_MIXED_CONSTRAINTS")
{
    tools::SolverTimers sTimers;

    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TargetCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto mixedConstr = std::make_shared<copra::MixedConstraint>(E, G, p);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(mixedConstr);

    auto pcCheck = [&](const std::string& solverName, copra::SolverFlag sFlag, std::unique_ptr<copra::SolverInterface>&& solver = nullptr) {
        if (solver) {
            controller.useSolver(std::move(solver));
        } else {
            controller.selectQPSolver(sFlag);
        }

        REQUIRE(controller.solve());
        sTimers.st.emplace_back(solverName, controller.solveTime() * 1e3);
        sTimers.ct.emplace_back(solverName, controller.solveAndBuildTime() * 1e3);
        sTimers.bt.emplace_back(solverName, (controller.solveAndBuildTime() - controller.solveTime()) * 1e3);

        Eigen::VectorXd fullTraj = controller.trajectory();
        auto trajLen = fullTraj.rows() / 2;
        Eigen::VectorXd posTraj(trajLen);
        Eigen::VectorXd velTraj(trajLen);
        for (auto i = 0; i < trajLen; ++i) {
            posTraj(i) = fullTraj(2 * i);
            velTraj(i) = fullTraj(2 * i + 1);
        }
        Eigen::VectorXd control = controller.control();

        // Check result
        CHECK_LE(std::abs(xd(1) - velTraj.tail(1)(0)), 0.001);

        // Check constrains
        REQUIRE_LE(posTraj.maxCoeff(), x0(0));
        for (int i = 0; i < nbStep; ++i) {
            auto res = E * fullTraj.segment(i * E.cols(), E.cols()) + G * control.segment(i * G.cols(), G.cols());
            if (!(res(0) <= p(0) + 1e-6)) {
                FAIL("Mixed constraint violated!");
            }
        }
    };

    for (auto s : tools::Solvers) {
        std::unique_ptr<copra::SolverInterface> solver(nullptr);
#ifdef EIGEN_LSSOL_FOUND
        if (s.second == copra::SolverFlag::LSSOL) {
            solver = copra::solverFactory(copra::SolverFlag::LSSOL);
            solver->SI_maxIter(200);
        }
#endif
        pcCheck(s.first, s.second, std::move(solver));
    }

    MESSAGE(tools::getSortedTimers(sTimers));
}

TEST_CASE_FIXTURE(MixedSystem, "MPC_TRAJECTORY_COST_WITH_MIXED_CONSTRAINTS")
{
    tools::SolverTimers sTimers;

    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TrajectoryCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto mixedConstr = std::make_shared<copra::MixedConstraint>(E, G, p);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(mixedConstr);

    auto pcCheck = [&](const std::string& solverName, copra::SolverFlag sFlag, std::unique_ptr<copra::SolverInterface>&& solver = nullptr) {
        if (solver) {
            controller.useSolver(std::move(solver));
        } else {
            controller.selectQPSolver(sFlag);
        }

        REQUIRE(controller.solve());
        sTimers.st.emplace_back(solverName, controller.solveTime() * 1e3);
        sTimers.ct.emplace_back(solverName, controller.solveAndBuildTime() * 1e3);
        sTimers.bt.emplace_back(solverName, (controller.solveAndBuildTime() - controller.solveTime()) * 1e3);

        Eigen::VectorXd fullTraj = controller.trajectory();
        auto trajLen = fullTraj.rows() / 2;
        Eigen::VectorXd posTraj(trajLen);
        Eigen::VectorXd velTraj(trajLen);
        for (auto i = 0; i < trajLen; ++i) {
            posTraj(i) = fullTraj(2 * i);
            velTraj(i) = fullTraj(2 * i + 1);
        }
        Eigen::VectorXd control = controller.control();

        // Check result
        CHECK_LE(std::abs(xd(1) - velTraj.tail(1)(0)), 0.001);

        // Check constrains
        REQUIRE_LE(posTraj.maxCoeff(), x0(0));
        for (int i = 0; i < nbStep; ++i) {
            auto res = E * fullTraj.segment(i * E.cols(), E.cols()) + G * control.segment(i * G.cols(), G.cols());
            if (!(res(0) <= p(0) + 1e-6)) {
                FAIL("Mixed constraint violated!");
            }
        }
    };

    for (auto s : tools::Solvers) {
        std::unique_ptr<copra::SolverInterface> solver(nullptr);
#ifdef EIGEN_LSSOL_FOUND
        if (s.second == copra::SolverFlag::LSSOL) {
            solver = copra::solverFactory(copra::SolverFlag::LSSOL);
            solver->SI_maxIter(200);
        }
#endif
        pcCheck(s.first, s.second, std::move(solver));
    }

    MESSAGE(tools::getSortedTimers(sTimers));
}

TEST_CASE_FIXTURE(MixedSystem, "MPC_MIXED_COST_WITH_MIXED_CONSTRAINTS")
{
    tools::SolverTimers sTimers;

    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::MixedCost>(M, Eigen::MatrixXd::Zero(2, 1), xd);
    auto uCost = std::make_shared<copra::MixedCost>(Eigen::MatrixXd::Zero(1, 2), N, ud);
    auto mixedConstr = std::make_shared<copra::MixedConstraint>(E, G, p);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(mixedConstr);

    REQUIRE(controller.solve());

    Eigen::VectorXd fullTraj = controller.trajectory();
    auto trajLen = fullTraj.rows() / 2;
    Eigen::VectorXd posTraj(trajLen);
    Eigen::VectorXd velTraj(trajLen);
    for (auto i = 0; i < trajLen; ++i) {
        posTraj(i) = fullTraj(2 * i);
        velTraj(i) = fullTraj(2 * i + 1);
    }
    Eigen::VectorXd control = controller.control();

    // Check result
    CHECK_LE(std::abs(xd(1) - velTraj.tail(3)(0)), 0.001); // Check X_{N-1} for mixed cost because X_N is not evaluated.

    // Check constrains
    REQUIRE_LE(posTraj.maxCoeff(), x0(0));
    for (int i = 0; i < nbStep; ++i) {
        auto res = E * fullTraj.segment(i * E.cols(), E.cols()) + G * control.segment(i * G.cols(), G.cols());
        if (!(res(0) <= p(0) + 1e-6)) {
            FAIL("Mixed constraint violated!");
        }
    }
}

/********************************************************************************************************
 *                              Check Equality constraint                                               *
 ********************************************************************************************************/

TEST_CASE_FIXTURE(EqSystem, "MPC_TARGET_COST_WITH_EQUALITY_CONSTRAINTS")
{
    tools::SolverTimers sTimers;

    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TargetCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(E, p, false);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);

    auto pcCheck = [&](const std::string& solverName, copra::SolverFlag sFlag, std::unique_ptr<copra::SolverInterface>&& solver = nullptr) {
        std::unique_ptr<copra::SolverInterface> newSolver;
        if (solver) {
            newSolver = std::move(solver);
        } else {
            newSolver = solverFactory(sFlag);
        }
#ifdef EIGEN_OSQP_FOUND
        // Increase precision
        if (sFlag == copra::SolverFlag::OSQP) {
            auto& bs = static_cast<copra::OSQPSolver&>(*newSolver).baseSolver();
            bs.scalingIter(0);
            bs.absConvergenceTol(1e-6);
            bs.relConvergenceTol(1e-6);
            bs.primalInfeasibilityTol(1e-7);
            bs.dualInfeasibilityTol(1e-7);
        }
#endif
        controller.useSolver(std::move(newSolver));

        REQUIRE(controller.solve());
        sTimers.st.emplace_back(solverName, controller.solveTime() * 1e3);
        sTimers.ct.emplace_back(solverName, controller.solveAndBuildTime() * 1e3);
        sTimers.bt.emplace_back(solverName, (controller.solveAndBuildTime() - controller.solveTime()) * 1e3);

        Eigen::VectorXd fullTraj = controller.trajectory();
        auto trajLen = fullTraj.rows() / 2;
        Eigen::VectorXd posTraj(trajLen);
        Eigen::VectorXd velTraj(trajLen);
        for (auto i = 0; i < trajLen; ++i) {
            posTraj(i) = fullTraj(2 * i);
            velTraj(i) = fullTraj(2 * i + 1);
        }
        Eigen::VectorXd control = controller.control();

        // Check result
        CHECK_LE(std::abs(xd(1) - velTraj.tail(1)(0)), 0.001);

        // Check constrains
        REQUIRE_LE(posTraj.maxCoeff(), x0(0) + 1e-6);
#ifdef EIGEN_OSQP_FOUND
        if (sFlag == copra::SolverFlag::OSQP)
            REQUIRE_LE(velTraj.maxCoeff(), p(0) + 1e-4); // I could not get a better precision...
        else
#endif
            REQUIRE_LE(velTraj.maxCoeff(), p(0) + 1e-6);
    };

    for (auto s : tools::Solvers) {
        std::unique_ptr<copra::SolverInterface> solver(nullptr);
#ifdef EIGEN_LSSOL_FOUND
        if (s.second == copra::SolverFlag::LSSOL) {
            solver = copra::solverFactory(copra::SolverFlag::LSSOL);
            solver->SI_maxIter(200);
        }
#endif
        pcCheck(s.first, s.second, std::move(solver));
    }

    MESSAGE(tools::getSortedTimers(sTimers));
}

TEST_CASE_FIXTURE(EqSystem, "MPC_TRAJECTORY_COST_WITH_EQUALITY_CONSTRAINTS")
{
    tools::SolverTimers sTimers;

    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TrajectoryCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(E, p, false);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);

    auto pcCheck = [&](const std::string& solverName, copra::SolverFlag sFlag, std::unique_ptr<copra::SolverInterface>&& solver = nullptr) {
        if (solver) {
            controller.useSolver(std::move(solver));
        } else {
            controller.selectQPSolver(sFlag);
        }

        REQUIRE(controller.solve());
        sTimers.st.emplace_back(solverName, controller.solveTime() * 1e3);
        sTimers.ct.emplace_back(solverName, controller.solveAndBuildTime() * 1e3);
        sTimers.bt.emplace_back(solverName, (controller.solveAndBuildTime() - controller.solveTime()) * 1e3);

        Eigen::VectorXd fullTraj = controller.trajectory();
        auto trajLen = fullTraj.rows() / 2;
        Eigen::VectorXd posTraj(trajLen);
        Eigen::VectorXd velTraj(trajLen);
        for (auto i = 0; i < trajLen; ++i) {
            posTraj(i) = fullTraj(2 * i);
            velTraj(i) = fullTraj(2 * i + 1);
        }
        Eigen::VectorXd control = controller.control();

        // Check result
        CHECK_LE(std::abs(xd(1) - velTraj.tail(1)(0)), 0.001);

        // Check constrains
        REQUIRE_LE(posTraj.maxCoeff(), x0(0) + 1e-6);
#ifdef EIGEN_OSQP_FOUND
        if (sFlag == copra::SolverFlag::OSQP)
            REQUIRE_LE(velTraj.maxCoeff(), p(0) + 1e-4); // I could not get a better precision...
        else
#endif
            REQUIRE_LE(velTraj.maxCoeff(), p(0) + 1e-6);
    };

    for (auto s : tools::Solvers) {
        std::unique_ptr<copra::SolverInterface> solver(nullptr);
#ifdef EIGEN_LSSOL_FOUND
        if (s.second == copra::SolverFlag::LSSOL) {
            solver = copra::solverFactory(copra::SolverFlag::LSSOL);
            solver->SI_maxIter(200);
        }
#endif
        pcCheck(s.first, s.second, std::move(solver));
    }

    MESSAGE(tools::getSortedTimers(sTimers));
}

TEST_CASE_FIXTURE(EqSystem, "MPC_MIXED_COST_WITH_EQUALITY_CONSTRAINTS")
{
    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::MixedCost>(M, Eigen::MatrixXd::Zero(2, 1), xd);
    auto uCost = std::make_shared<copra::MixedCost>(Eigen::MatrixXd::Zero(1, 2), N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(E, p, false);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);

    REQUIRE(controller.solve());

    Eigen::VectorXd fullTraj = controller.trajectory();
    auto trajLen = fullTraj.rows() / 2;
    Eigen::VectorXd posTraj(trajLen);
    Eigen::VectorXd velTraj(trajLen);
    for (auto i = 0; i < trajLen; ++i) {
        posTraj(i) = fullTraj(2 * i);
        velTraj(i) = fullTraj(2 * i + 1);
    }
    Eigen::VectorXd control = controller.control();

    // Check result
    CHECK_LE(std::abs(xd(1) - velTraj.tail(3)(0)), 0.001); // Check X_{N-1} for mixed cost because X_N is not evaluated.

    // Check constrains
    REQUIRE_LE(posTraj.maxCoeff(), x0(0) + 1e-6);
    REQUIRE_LE(velTraj.maxCoeff(), p(0) + 1e-6);
}

/********************************************************************************************************
 *                                   Check Autospan                                                     *
 ********************************************************************************************************/

TEST_CASE_FIXTURE(BoundedSystem, "CHECK_AUTOSPAN_AND_WHOLE_MATRIX_ON_BOUND_CONSTRAINT")
{
    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    int nbXStep = nbStep + 1;

    auto checkSpan = [&](const Eigen::VectorXd& xLower, const Eigen::VectorXd& xUpper, const Eigen::VectorXd& uLower, const Eigen::VectorXd& uUpper) {
        auto trajConstr = std::make_shared<copra::TrajectoryBoundConstraint>(xLower, xUpper);
        trajConstr->autoSpan();

        auto contConstr = std::make_shared<copra::ControlBoundConstraint>(uLower, uUpper);
        contConstr->autoSpan();

        REQUIRE_NOTHROW(controller.addConstraint(trajConstr));
        REQUIRE_NOTHROW(controller.addConstraint(contConstr));
    };

    auto fullxLower = tools::spanVector(xLower, nbXStep);
    auto fullxUpper = tools::spanVector(xUpper, nbXStep);
    auto fulluLower = tools::spanVector(uLower, nbStep);
    auto fulluUpper = tools::spanVector(uUpper, nbStep);

    checkSpan(xLower, xUpper, uLower, uUpper);
    checkSpan(fullxLower, xUpper, fulluLower, uUpper);
    checkSpan(xLower, fullxUpper, uLower, fulluUpper);
    checkSpan(fullxLower, fullxUpper, fulluLower, fulluUpper);
}

TEST_CASE_FIXTURE(IneqSystem, "CHECK_AUTOSPAN_AND_WHOLE_MATRIX_ON_INEQUALITY_CONSTRAINT")
{
    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    int nbXStep = nbStep + 1;

    auto checkSpan = [&](const Eigen::MatrixXd& E, const Eigen::VectorXd& p, const Eigen::MatrixXd& G, const Eigen::VectorXd& h) {
        auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(E, p);
        trajConstr->autoSpan();

        auto contConstr = std::make_shared<copra::ControlConstraint>(G, h);
        contConstr->autoSpan();

        REQUIRE_NOTHROW(controller.addConstraint(trajConstr));
        REQUIRE_NOTHROW(controller.addConstraint(contConstr));
    };

    auto fullE = tools::spanMatrix(E, nbXStep);
    auto fullf = tools::spanVector(p, nbXStep);
    auto fullG = tools::spanMatrix(G, nbStep);
    auto fullh = tools::spanVector(h, nbStep);

    checkSpan(E, p, G, h);
    checkSpan(fullE, p, fullG, h);
    checkSpan(E, fullf, G, fullh);
    checkSpan(fullE, fullf, fullG, fullh);
}

TEST_CASE_FIXTURE(MixedSystem, "CHECK_AUTOSPAN_AND_WHOLE_MATRIX_ON_MIXED_CONSTRAINT")
{
    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto checkSpan = [&](const Eigen::MatrixXd& E, const Eigen::MatrixXd& G, const Eigen::VectorXd& p) {
        auto mixedConstr = std::make_shared<copra::MixedConstraint>(E, G, p);
        mixedConstr->autoSpan();

        REQUIRE_NOTHROW(controller.addConstraint(mixedConstr));
    };

    auto fullE = tools::spanMatrix(E, nbStep, 1);
    auto fullG = tools::spanMatrix(G, nbStep);
    auto fullf = tools::spanVector(p, nbStep);

    checkSpan(E, G, p);
    checkSpan(fullE, G, p);
    checkSpan(fullE, fullG, p);
    checkSpan(fullE, G, fullf);
    checkSpan(E, fullG, p);
    checkSpan(fullE, fullG, p);
    checkSpan(E, fullG, fullf);
    checkSpan(E, G, fullf);
    checkSpan(fullE, G, fullf);
    checkSpan(E, fullG, fullf);
    checkSpan(fullE, fullG, fullf);
}

TEST_CASE_FIXTURE(IneqSystem, "CHECK_AUTOSPAN_AND_WHOLE_MATRIX_ON_TRAJECTORY_COST")
{
    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    int nbXStep = nbStep + 1;

    auto checkSpan = [&](const Eigen::MatrixXd& M, const Eigen::VectorXd& p, const Eigen::VectorXd& weights) {
        auto cost = std::make_shared<copra::TrajectoryCost>(M, p);
        cost->weights(weights);
        cost->autoSpan();

        REQUIRE_NOTHROW(controller.addCost(cost));
    };

    auto fullM = tools::spanMatrix(M, nbXStep);
    auto fullxd = tools::spanVector(xd, nbXStep);

    checkSpan(M, xd, wx);
    checkSpan(M, fullxd, wx);
    checkSpan(fullM, xd, wx);
    checkSpan(fullM, fullxd, wx);
}

TEST_CASE_FIXTURE(IneqSystem, "CHECK_AUTOSPAN_AND_WHOLE_MATRIX_ON_CONTROL_COST")
{
    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto checkSpan = [&](const Eigen::MatrixXd& M, const Eigen::VectorXd& p, const Eigen::VectorXd& weights) {
        auto cost = std::make_shared<copra::ControlCost>(M, p);
        cost->weights(weights);
        cost->autoSpan();

        REQUIRE_NOTHROW(controller.addCost(cost));
    };

    auto fullN = tools::spanMatrix(N, nbStep);
    auto fullud = tools::spanVector(ud, nbStep);

    checkSpan(N, ud, wu);
    checkSpan(N, fullud, wu);
    checkSpan(fullN, ud, wu);
    checkSpan(fullN, fullud, wu);
}

TEST_CASE_FIXTURE(IneqSystem, "CHECK_AUTOSPAN_AND_WHOLE_MATRIX_ON_MIXED_COST")
{
    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto checkSpan = [&](const Eigen::MatrixXd& M, const Eigen::MatrixXd& N, const Eigen::VectorXd& p, const Eigen::VectorXd& weights) {
        auto cost = std::make_shared<copra::MixedCost>(M, N, p);
        cost->weights(weights);
        cost->autoSpan();

        REQUIRE_NOTHROW(controller.addCost(cost));
    };

    auto MVec = std::vector<Eigen::MatrixXd>();
    MVec.push_back(M);
    MVec.push_back(tools::spanMatrix(M, nbStep, 1));
    auto nnVec = std::vector<Eigen::MatrixXd>();
    nnVec.push_back(Eigen::MatrixXd::Ones(2, 1));
    nnVec.push_back(tools::spanMatrix(Eigen::MatrixXd::Ones(2, 1), nbStep));
    auto xdVec = std::vector<Eigen::VectorXd>();
    xdVec.push_back(xd);
    xdVec.push_back(tools::spanVector(xd, nbStep));

    for (auto& i : MVec) {
        for (auto& j : nnVec) {
            for (auto& k : xdVec) {
                checkSpan(i, j, k, wx);
            }
        }
    }
}

/********************************************************************************************************
 *                                Check Error Messages                                                  *
 ********************************************************************************************************/

TEST_CASE_FIXTURE(IneqSystem, "ERROR_HANDLER_FOR_PREVIEW_SYSTEM")
{
    auto ps = std::make_shared<copra::PreviewSystem>();
    REQUIRE_THROWS_AS(ps->system(Eigen::MatrixXd::Ones(5, 2), B, c, x0, nbStep), std::domain_error);
    REQUIRE_THROWS_AS(ps->system(Eigen::MatrixXd::Ones(2, 5), B, c, x0, nbStep), std::domain_error);
    REQUIRE_THROWS_AS(ps->system(A, Eigen::MatrixXd::Ones(5, 1), c, x0, nbStep), std::domain_error);
    REQUIRE_THROWS_AS(ps->system(A, B, Eigen::VectorXd::Ones(5), x0, nbStep), std::domain_error);
    REQUIRE_THROWS_AS(ps->system(A, B, c, x0, -1), std::domain_error);
}

TEST_CASE_FIXTURE(IneqSystem, "ERROR_HANDLER_FOR_WEIGTHS")
{
    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto cost = std::make_shared<copra::TrajectoryCost>(M, xd);

    REQUIRE_NOTHROW(cost->weight(2));
    REQUIRE_THROWS_AS(cost->weights(Eigen::VectorXd::Ones(5)), std::domain_error);
    REQUIRE_NOTHROW(cost->weights(wx));
    REQUIRE_NOTHROW(controller.addCost(cost));
    REQUIRE_NOTHROW(cost->weights(Eigen::VectorXd::Ones(2)));
}

TEST_CASE_FIXTURE(IneqSystem, "ERROR_HANDLER_FOR_TRAJECTORY_COST")
{
    auto ps = std::make_shared<copra::PreviewSystem>(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badCost1 = std::make_shared<copra::TrajectoryCost>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(2));
    REQUIRE_THROWS_AS(controller.addCost(badCost1), std::domain_error);
    auto badCost2 = std::make_shared<copra::TrajectoryCost>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(5));
    REQUIRE_THROWS_AS(controller.addCost(badCost2), std::domain_error);
}

TEST_CASE_FIXTURE(IneqSystem, "ERROR_HANDLER_FOR_TARGET_COST")
{
    auto ps = std::make_shared<copra::PreviewSystem>(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badCost1 = std::make_shared<copra::TargetCost>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(2));
    REQUIRE_THROWS_AS(controller.addCost(badCost1), std::domain_error);
    auto badCost2 = std::make_shared<copra::TargetCost>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(5));
    REQUIRE_THROWS_AS(controller.addCost(badCost2), std::domain_error);
}

TEST_CASE_FIXTURE(IneqSystem, "ERROR_HANDLER_FOR_CONTROL_COST")
{
    auto ps = std::make_shared<copra::PreviewSystem>(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badCost1 = std::make_shared<copra::ControlCost>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(2));
    REQUIRE_THROWS_AS(controller.addCost(badCost1), std::domain_error);
    auto badCost2 = std::make_shared<copra::ControlCost>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(5));
    REQUIRE_THROWS_AS(controller.addCost(badCost2), std::domain_error);
}

TEST_CASE_FIXTURE(IneqSystem, "ERROR_HANDLER_FOR_MIXED_COST")
{
    auto ps = std::make_shared<copra::PreviewSystem>(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badCost1 = std::make_shared<copra::MixedCost>(Eigen::MatrixXd::Identity(5, 5), Eigen::MatrixXd::Identity(2, 1), Eigen::VectorXd::Ones(2));
    REQUIRE_THROWS_AS(controller.addCost(badCost1), std::domain_error);
    auto badCost2 = std::make_shared<copra::MixedCost>(Eigen::MatrixXd::Identity(2, 1), Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(2));
    REQUIRE_THROWS_AS(controller.addCost(badCost2), std::domain_error);
    auto badCost3 = std::make_shared<copra::MixedCost>(Eigen::MatrixXd::Identity(5, 5), Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(5));
    REQUIRE_THROWS_AS(controller.addCost(badCost3), std::domain_error);
}

TEST_CASE_FIXTURE(IneqSystem, "ERROR_HANDLER_FOR_TRAJECTORY_CONSTRAINT")
{
    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badConstr = std::make_shared<copra::TrajectoryConstraint>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(2));
    REQUIRE_THROWS_AS(controller.addConstraint(badConstr), std::domain_error);
    auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(5));
    REQUIRE_THROWS_AS(controller.addConstraint(trajConstr), std::domain_error);
}

TEST_CASE_FIXTURE(IneqSystem, "ERROR_HANDLER_FOR_CONTROL_CONSTRAINT")
{
    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badConstr1 = std::make_shared<copra::ControlConstraint>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(2));
    REQUIRE_THROWS_AS(controller.addConstraint(badConstr1), std::domain_error);
    auto badConstr2 = std::make_shared<copra::ControlConstraint>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(5));
    REQUIRE_THROWS_AS(controller.addConstraint(badConstr2), std::domain_error);

    auto goodConstr = std::make_shared<copra::ControlConstraint>(G, h);
    controller.addConstraint(goodConstr);
    REQUIRE_THROWS_AS(controller.addConstraint(goodConstr), std::runtime_error);
}

TEST_CASE_FIXTURE(IneqSystem, "ERROR_HANDLER_FOR_MIXED_CONSTRAINT")
{
    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badConstr1 = std::make_shared<copra::MixedConstraint>(Eigen::MatrixXd::Identity(5, 5), Eigen::MatrixXd::Identity(2, 1), Eigen::VectorXd::Ones(2));
    REQUIRE_THROWS_AS(controller.addConstraint(badConstr1), std::domain_error);
    auto badConstr2 = std::make_shared<copra::MixedConstraint>(Eigen::MatrixXd::Identity(2, 1), Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(2));
    REQUIRE_THROWS_AS(controller.addConstraint(badConstr2), std::domain_error);
    auto badConstr3 = std::make_shared<copra::MixedConstraint>(Eigen::MatrixXd::Identity(5, 5), Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(5));
    REQUIRE_THROWS_AS(controller.addConstraint(badConstr3), std::domain_error);
}

TEST_CASE_FIXTURE(BoundedSystem, "ERROR_HANDLER_FOR_TRAJECTORY_BOUND_CONSTRAINT")
{
    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badConstr = std::make_shared<copra::TrajectoryBoundConstraint>(Eigen::VectorXd::Ones(3), Eigen::VectorXd::Ones(2));
    REQUIRE_THROWS_AS(controller.addConstraint(badConstr), std::domain_error);
    auto tbConstr = std::make_shared<copra::TrajectoryBoundConstraint>(Eigen::VectorXd::Ones(3), Eigen::VectorXd::Ones(3));
    REQUIRE_THROWS_AS(controller.addConstraint(tbConstr), std::domain_error);
}

TEST_CASE_FIXTURE(BoundedSystem, "ERROR_HANDLER_FOR_CONTROL_BOUND_CONSTRAINT")
{
    auto ps = std::make_shared<copra::PreviewSystem>();
    ps->system(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badConstr1 = std::make_shared<copra::ControlBoundConstraint>(Eigen::VectorXd::Ones(3), Eigen::VectorXd::Ones(2));
    REQUIRE_THROWS_AS(controller.addConstraint(badConstr1), std::domain_error);
    auto badConstr2 = std::make_shared<copra::ControlBoundConstraint>(Eigen::VectorXd::Ones(3), Eigen::VectorXd::Ones(3));
    REQUIRE_THROWS_AS(controller.addConstraint(badConstr2), std::domain_error);

    auto goodConstr = std::make_shared<copra::ControlBoundConstraint>(uLower, uUpper);
    controller.addConstraint(goodConstr);
    REQUIRE_THROWS_AS(controller.addConstraint(goodConstr), std::runtime_error);
}

/********************************************************************************************************
 *                               Check remove functions                                                 *
 ********************************************************************************************************/

TEST_CASE_FIXTURE(IneqSystem, "REMOVE_COST_AND_CONSTRAINT")
{
    auto ps = std::make_shared<copra::PreviewSystem>(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    {
        auto xCost = std::make_shared<copra::TargetCost>(M, xd);
        auto uCost = std::make_shared<copra::ControlCost>(N, ud);
        auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(E, p);
        auto contConstr = std::make_shared<copra::ControlConstraint>(G, h);

        controller.addCost(xCost);
        controller.addCost(uCost);
        controller.addConstraint(trajConstr);
        controller.addConstraint(contConstr);

        controller.removeCost(xCost);
        controller.removeCost(uCost);
        controller.removeConstraint(trajConstr);
        controller.removeConstraint(contConstr);
    }

    MESSAGE("\nIn DEBUG mode, if a message appears between this line\n*******");
    controller.solve();
    MESSAGE("*******\nand this line, the remove methods have failed!");
}


#include <cmath>
#include <iostream>
#include <iomanip>
#include <string>
#include <vector>
#include <algorithm>
#include <boost/range/algorithm.hpp>
#include <boost/integer/common_factor_rt.hpp>

using namespace std;

int N, M;

int main() {

    while (cin >> N >> M) {
        int k[M];
        vector<vector<int>> s_v;
        for (int i = 0; i < M; i++) {
            cin >> k[i];
            int s[k[i]];
            for (int j = 0; j < k[i]; j++) {
                cin >> s[j];
            }
            s_v.push_back(vector<int> (s, s + k[i]));
        }
        int p[M];
        for (int i = 0; i < M; i++) {
            cin >> p[i];
        }

        int ret = 0;
        for (int i = 0; i < (1 << N); i++) {
            bool n[N];
            for (int j = 0; j < N; j++) {
                n[j] = i & (1 << j);
            }
            int j;
            for (j = 0; j < M; j++) {
                int tmp = 0;
                for (int l = 0; l < k[j]; l++) {
                    if (n[s_v.at(j).at(l) - 1]) {
                        tmp++;
                    }
                }
                if (p[j] != tmp % 2) {
                    break;
                }
            }
            if (j == M) {
                ret++;
            }
        }
        cout << ret << endl;
    }

    return 0;
}


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

#include <iostream>
#include <nt2/include/functions/rosenbrock.hpp>

#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/bind.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/norm.hpp>
#include <nt2/include/functions/globalsum.hpp>
#include <nt2/include/functions/globalasum2.hpp>
#include <nt2/include/functions/sqrt.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/globalsum.hpp>
#include <nt2/include/functions/globalmax.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/sqrteps.hpp>
#include <nt2/include/constants/four.hpp>
#include <nt2/table.hpp>

template < class Tabout >
struct fpp
{
  template < class Tabin> inline
  Tabout operator()(const Tabin & x ) const
  {
    typedef typename Tabin::value_type value_type;
    Tabout r = globalsum(nt2::sqr((x-nt2::_(value_type(1), value_type(numel(x))))));
    return r;
  }
};
template < class Tabout >
struct gpp
{
  template < class Tabin> inline
  Tabout operator()(const Tabin & x ) const
  {
    typedef typename Tabin::value_type value_type;
    Tabout r=Tabout(100)*nt2::sqr(x(2)-nt2::sqr(x(1)))+nt2::sqr(1-x(1));
    return r;
  }
};

// template<class Tabout, class Tabin >  Tabout f1(const Tabin & x )
// {
//     typedef typename Tabin::value_type value_type;
//     Tabout r =  globalasum2(nt2::sqr(x)-value_type(3));
//     return r;
// }

// NT2_TEST_CASE_TPL( rosenbrock_function_ptr, NT2_REAL_TYPES )
// {
//   using nt2::rosenbrock;
//   using nt2::optimization::output;
//   typedef nt2::table<T> tab_t;
//   typedef typename nt2::meta::as_logical<T>::type lT;
//   typedef nt2::table<T> ltab_t;
//   tab_t x0 = nt2::ones(nt2::of_size(1, 3), nt2::meta::as_<T>());
//   NT2_DISPLAY(x0);
//   tab_t r = nt2::sqrt(T(3))*nt2::ones (nt2::of_size(1, 3), nt2::meta::as_<T>());
//   NT2_DISPLAY(r);
//   output<tab_t,T> res = rosenbrock(&f1<T, tab_t>, x0);

//   std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
//             << " after " << res.iterations_count <<  " iterations\n";

//   NT2_TEST(res.successful);
//   NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::abs(res.minimum()-r)), nt2::Sqrteps<T>());
// }

// NT2_TEST_CASE_TPL( rosenbrock_functor, NT2_REAL_TYPES )
// {
//   using nt2::rosenbrock;
//   using nt2::options;
//   using nt2::optimization::output;
//   typedef nt2::table<T> tab_t;
//   typedef typename nt2::meta::as_logical<T>::type lT;
//   typedef nt2::table<T> ltab_t;
//   tab_t x0 = nt2::zeros(nt2::of_size(1, 3), nt2::meta::as_<T>());
//   tab_t r = nt2::_(T(1), T(3));
//   output<tab_t,T> res = rosenbrock(fpp<tab_t>(), x0,
//                                   options [ nt2::iterations_ = 100,
//                                             nt2::tolerance::absolute_ = nt2::Eps<T>()
//                                     ]);

//   std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
//             << " after " << res.iterations_count <<  " iterations\n";

//   NT2_TEST(res.successful);
//   NT2_TEST_LESSER_EQUAL(nt2::globalmax(nt2::abs(res.minimum()-r)), nt2::Four<T>()*nt2::Sqrteps<T>());
// }

NT2_TEST_CASE_TPL( rosenbrock_functor2, NT2_REAL_TYPES )
{
  using nt2::rosenbrock;
  using nt2::options;
  using nt2::optimization::output;
  typedef nt2::table<T> tab_t;
  typedef typename nt2::meta::as_logical<T>::type lT;
  typedef nt2::table<T> ltab_t;
  tab_t x0 = T(5)*nt2::ones(nt2::of_size(1, 2), nt2::meta::as_<T>());
  tab_t r = nt2::cons(nt2::of_size(1, 2), T(1),T(1));
  output<tab_t,T> res = rosenbrock(gpp<tab_t>(), x0,
                                  options [ nt2::iterations_ = 10000,
                                            nt2::tolerance::absolute_ = T(100)*nt2::Eps<T>()
                                    ]);

  std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
            << " after " << res.iterations_count <<  " iterations\n";

  NT2_TEST(res.successful);
}

NT2_TEST_CASE_TPL( rosenbrock_functor3, (float) )
{
  using nt2::rosenbrock;
  using nt2::options;
  using nt2::optimization::output;
  typedef nt2::table<T> tab_t;
  tab_t x0 = T(5)*nt2::ones(nt2::of_size(1, 2), nt2::meta::as_<T>());
  tab_t r = nt2::cons(nt2::of_size(1, 2), T(1),T(1));
  output<tab_t,T> res = rosenbrock(gpp<tab_t>(), x0,
                                  options [ nt2::iterations_ = 10000,
                                            nt2::tolerance::absolute_ = 1.0e-5
                                    ]);

  std::cout << "Minimum : f(" << res.minimum << ") = " << res.value
            << " after " << res.iterations_count <<  " iterations\n";

  NT2_TEST(res.successful);
  typedef nt2::table<double> tab_d;
  tab_d x0d = nt2::cast<double>(res.minimum);
  output<tab_d,double> resd = rosenbrock(gpp<tab_d>(), x0d,
                                  options [ nt2::iterations_ = 10000,
                                            nt2::tolerance::absolute_ = 1.0e-8
                                    ]);
   std::cout << "Minimum : f(" << resd.minimum << ") = " << resd.value
            << " after " << resd.iterations_count <<  " iterations\n";

}


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

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file fxvolsmile.cpp
    \brief fx vol smile
*/

#include "toplevelfixture.hpp"
#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <ql/math/matrix.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/volatility/equityfx/blackvariancesurface.hpp>
#include <ql/termstructures/yield/discountcurve.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/daycounters/actualactual.hpp>
#include <ql/utilities/dataparsers.hpp>
#include <qle/termstructures/blackinvertedvoltermstructure.hpp>
#include <qle/termstructures/fxblackvolsurface.hpp>
#include <qle/termstructures/fxvannavolgasmilesection.hpp>

using namespace QuantLib;
using namespace QuantExt;
using namespace boost::unit_test_framework;
using namespace std;

namespace {

struct CommonVars {

    /* ------ GLOBAL VARIABLES ------ */
    Date today;
    DayCounter dc;
    vector<Date> dates;
    vector<Real> strikes;
    Matrix vols;
    vector<Real> atmVols;

    vector<Volatility> rrs;
    vector<Volatility> bfs;

    Handle<Quote> baseSpot;
    Handle<YieldTermStructure> baseDomesticYield;
    Handle<YieldTermStructure> baseForeignYield;

    CommonVars() {

        today = Date(1, Jan, 2014);
        dc = ActualActual();

        Settings::instance().evaluationDate() = today;

        dates.push_back(Date(1, Feb, 2014));
        dates.push_back(Date(1, Mar, 2014));
        dates.push_back(Date(1, Apr, 2014));
        dates.push_back(Date(1, Jan, 2015));

        strikes.push_back(90);
        strikes.push_back(100);
        strikes.push_back(110);

        vols = Matrix(3, 4);
        vols[0][0] = 0.12;
        vols[1][0] = 0.10;
        vols[2][0] = 0.13;
        vols[0][1] = 0.22;
        vols[1][1] = 0.20;
        vols[2][1] = 0.23;
        vols[0][2] = 0.32;
        vols[1][2] = 0.30;
        vols[2][2] = 0.33;
        vols[0][3] = 0.42;
        vols[1][3] = 0.40;
        vols[2][3] = 0.43;

        atmVols.push_back(0.1);
        atmVols.push_back(0.2);
        atmVols.push_back(0.3);
        atmVols.push_back(0.4);

        rrs = vector<Volatility>(atmVols.size(), 0.01);
        bfs = vector<Volatility>(atmVols.size(), 0.001);

        baseSpot = Handle<Quote>(boost::shared_ptr<Quote>(new SimpleQuote(100)));

        baseDomesticYield = Handle<YieldTermStructure>(
            boost::make_shared<FlatForward>(today, Handle<Quote>(boost::make_shared<SimpleQuote>(0.03)), dc));
        baseForeignYield = Handle<YieldTermStructure>(
            boost::make_shared<FlatForward>(today, Handle<Quote>(boost::make_shared<SimpleQuote>(0.01)), dc));
    }
};

struct VolData {
    const char* tenor;
    Volatility atm;
    Volatility rr;
    Volatility bf;
    Time time;
    Real df_d;
    Real df_f;
};

} // namespace

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

BOOST_AUTO_TEST_SUITE(FxVolSmileTest)

BOOST_AUTO_TEST_CASE(testVannaVolgaFxSmileSection) {

    BOOST_TEST_MESSAGE("Testing fx vanna volga smile");

    SavedSettings backup;

    // test numbers from Castagna & Mercurio (2006)
    // http://papers.ssrn.com/sol3/papers.cfm?abstract_id=873788
    // page 5
    Settings::instance().evaluationDate() = Date(1, July, 2005);
    Time t = 94 / (double)365;
    Real S0 = 1.205;
    Volatility sig_atm = 0.0905;
    Volatility sig_rr = -0.005;
    Volatility sig_bf = 0.0013;
    // page 11
    DiscountFactor df_usd = 0.9902752;
    DiscountFactor df_eur = 0.9945049;

    // Rates
    Real rd = -::log(df_usd) / t;
    Real rf = -::log(df_eur) / t;

    VannaVolgaSmileSection vvss(S0, rd, rf, t, sig_atm, sig_rr, sig_bf);

    // Check the Strike and Vol values from the paper
    Real tolerance = 0.0001; // 4 decimal places
    if (fabs(vvss.k_atm() - 1.2114) > tolerance)
        BOOST_FAIL("VannaVolgaSmileSection failed to calculte ATM strike, got " << vvss.k_atm());
    if (fabs(vvss.k_25p() - 1.1733) > tolerance)
        BOOST_FAIL("VannaVolgaSmileSection failed to calculate 25P strike, got " << vvss.k_25p());
    if (fabs(vvss.k_25c() - 1.2487) > tolerance)
        BOOST_FAIL("VannaVolgaSmileSection failed to calculate 25C strike, got " << vvss.k_25c());
    if (fabs(vvss.vol_atm() - 0.0905) > tolerance)
        BOOST_FAIL("VannaVolgaSmileSection failed to calculate ATM vol, got " << vvss.vol_atm());
    if (fabs(vvss.vol_25p() - 0.0943) > tolerance)
        BOOST_FAIL("VannaVolgaSmileSection failed to calculate 25P vol, got " << vvss.vol_25p());
    if (fabs(vvss.vol_25c() - 0.0893) > tolerance)
        BOOST_FAIL("VannaVolgaSmileSection failed to calculate 25C vol, got " << vvss.vol_25c());

    // Now check that our smile returns these
    if (fabs(vvss.volatility(vvss.k_atm()) - vvss.vol_atm()) > tolerance)
        BOOST_FAIL("VannaVolgaSmileSection failed to recover ATM vol, got " << vvss.volatility(vvss.k_atm()));
    if (fabs(vvss.volatility(vvss.k_25p()) - vvss.vol_25p()) > tolerance)
        BOOST_FAIL("VannaVolgaSmileSection failed to recover 25P vol, got " << vvss.volatility(vvss.k_25p()));
    if (fabs(vvss.volatility(vvss.k_25c()) - vvss.vol_25c()) > tolerance)
        BOOST_FAIL("VannaVolgaSmileSection failed to recover 25C vol, got " << vvss.volatility(vvss.k_25c()));

    // To graph the smile, uncomment this code
    /*
    cout << "strike,vol" << endl;
    //for (Real k = 1.1; k < 1.35; k += 0.002) // normal (as per paper)
    //for (Real k = 0.9; k < 1.5; k += 0.01) // large
    for (Real k = 0.1; k < 3; k += 0.05) // extreme
        cout << k << "," << vvss.volatility(k) << endl;
    */
}

BOOST_AUTO_TEST_CASE(testVannaVolgaFxVolSurface) {

    BOOST_TEST_MESSAGE("Testing fx vanna volga surface");

    SavedSettings backup;

    // Data from
    // "Consistent pricing and hedging of an FX options book" (2005)
    // L. Bisesti, A. Castagna and F. Mercurio
    // http://www.fabiomercurio.it/fxbook.pdf
    Date asof(12, Feb, 2004);
    Settings::instance().evaluationDate() = asof;

    Handle<Quote> fxSpot(boost::shared_ptr<Quote>(new SimpleQuote(1.2832)));

    // vols are % here
    // tenor, atm, rr, bf, T, p_d, p_f
    VolData volData[] = { { "1W", 11.75, 0.50, 0.190, 0.0192, 0.999804, 0.999606 },
                          { "2W", 11.60, 0.50, 0.190, 0.0384, 0.999595, 0.999208 },
                          { "1M", 11.50, 0.60, 0.190, 0.0877, 0.999044, 0.998179 },
                          { "2M", 11.25, 0.60, 0.210, 0.1726, 0.998083, 0.996404 },
                          { "3M", 11.00, 0.60, 0.220, 0.2493, 0.997187, 0.994803 },
                          { "6M", 10.87, 0.65, 0.235, 0.5014, 0.993959, 0.989548 },
                          { "9M", 10.83, 0.69, 0.235, 0.7589, 0.990101, 0.984040 },
                          { "1Y", 10.80, 0.70, 0.240, 1.0110, 0.985469, 0.978479 },
                          { "2Y", 10.70, 0.65, 0.255, 2.0110, 0.960102, 0.951092 } };

    // Assume act/act
    DayCounter dc = ActualActual();
    Calendar cal = TARGET();

    // set up vectors
    Size len = sizeof(volData) / sizeof(volData[0]);
    vector<Date> dates(len);
    vector<Volatility> atm(len);
    vector<Volatility> rr(len);
    vector<Volatility> bf(len);
    // For DiscountCurve we need the T=0 points.
    vector<Date> discountDates(len + 1);
    vector<DiscountFactor> dfDom(len + 1);
    vector<DiscountFactor> dfFor(len + 1);
    discountDates[0] = asof;
    dfDom[0] = 1.0;
    dfFor[0] = 1.0;

    for (Size i = 0; i < sizeof(volData) / sizeof(volData[0]); i++) {
        dates[i] = asof + PeriodParser::parse(volData[i].tenor);
        // check time == volData[i].time
        /*
        if (fabs(dc.yearFraction(asof, dates[i]) - volData[i].time) > 0.001)
            BOOST_FAIL("Did not match vol data time (" << volData[i].time <<
                       ") with aosf " << asof << " and maturity " << dates[i] <<
                       " got year fraction of " << dc.yearFraction(asof, dates[i]));
         */

        atm[i] = volData[i].atm / 100;
        rr[i] = volData[i].rr / 100;
        bf[i] = volData[i].bf / 100;

        discountDates[i + 1] = dates[i];
        dfDom[i + 1] = volData[i].df_d;
        dfFor[i + 1] = volData[i].df_f;
    }

    // Now build discount curves
    Handle<YieldTermStructure> domYTS(
        boost::shared_ptr<YieldTermStructure>(new DiscountCurve(discountDates, dfDom, dc)));
    Handle<YieldTermStructure> forYTS(
        boost::shared_ptr<YieldTermStructure>(new DiscountCurve(discountDates, dfFor, dc)));

    // build surface
    FxBlackVannaVolgaVolatilitySurface volSurface(asof, dates, atm, rr, bf, dc, cal, fxSpot, domYTS, forYTS);

    // 1.55,1.75,0.121507
    Real vol = volSurface.blackVol(1.75, 1.55);
    Real expected = 0.121507;
    if (fabs(vol - expected) > 0.00001)
        BOOST_FAIL("Failed to get expected vol from surface");
    /*
    cout << "strike,time,vol" << endl;
    for (Real k = 1.0; k < 1.6; k += 0.01) // extreme
        for (Time tt = 0.1; tt < 2; tt+= 0.05)
            cout << k << "," << tt << "," << volSurface.blackVol(tt, k) << endl;
     */
}

BOOST_AUTO_TEST_CASE(testInvertedVolTermStructure) {

    BOOST_TEST_MESSAGE("Testing inverted vol term structure");

    SavedSettings backup;

    CommonVars vars;

    Handle<BlackVolTermStructure> surface(boost::shared_ptr<BlackVolTermStructure>(
        new BlackVarianceSurface(vars.today, TARGET(), vars.dates, vars.strikes, vars.vols, vars.dc)));

    BlackInvertedVolTermStructure bivt(surface);

    if (surface->maxDate() != bivt.maxDate())
        BOOST_FAIL("inverted maxDate() vol surface does not match base");

    if (surface->referenceDate() != bivt.referenceDate())
        BOOST_FAIL("inverted referenceDate() vol surface does not match base");

    // base spot is 100
    // test cases <Time, Strike>
    double testCases[][2] = { { 0.1, 104 }, { 0.5, 90 },  { 0.6, 110 }, { 0.9, 90 },
                              { 0.9, 95 },  { 0.9, 100 }, { 0.9, 105 }, { 0.9, 110 } };

    for (Size i = 0; i < sizeof(testCases) / sizeof(testCases[0]); i++) {
        Time t = testCases[i][0];
        Real k = testCases[i][1];

        Real vol1 = surface->blackVol(t, k);

        Real invertedStrike = 1.0 / k;
        Real vol2 = bivt.blackVol(t, invertedStrike);
        if (fabs(vol1 - vol2) > 0.00001)
            BOOST_FAIL("Failed to get expected vol (" << vol1 << ") from inverted vol surface, got (" << vol2 << ")");
    }
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


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

#include <manipulability_metrics/metrics/damm.h>
#include <manipulability_metrics/util/ellipsoid.h>
#include <manipulability_metrics/util/similarity.h>

#include <Eigen/SVD>

#include <numeric>

namespace manipulability_metrics
{
double damm(const DualChain& dual_chain, const DualTcp& tcp, const KDL::JntArray& left_joint_positions,
            const KDL::JntArray& right_joint_positions)
{
  auto left_jac = KDL::Jacobian{ static_cast<unsigned int>(dual_chain.leftChain().n_joints) };
  dual_chain.leftChain().jacobian(left_joint_positions, tcp.leftTcp().p, left_jac);
  auto right_jac = KDL::Jacobian{ static_cast<unsigned int>(dual_chain.rightChain().n_joints) };
  dual_chain.rightChain().jacobian(right_joint_positions, tcp.rightTcp().p, right_jac);

  auto left_ellipsoid = ellipsoidFromJacobian(left_jac.data);
  auto right_ellipsoid = ellipsoidFromJacobian(right_jac.data);

  return std::max(volumeIntersection(left_ellipsoid, right_jac.data),
                  volumeIntersection(right_ellipsoid, left_jac.data));
}
}  // namespace manipulability_metrics


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/function/raw.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/traits.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD_IF( rsqrt_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::pack_< bd::floating_<A0>, X >
                            )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      return bs::rec(bs::sqrt(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF( rsqrt_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , boost::simd::raw_tag
                            , bs::pack_< bd::floating_<A0>, X >
                            )
  {
    BOOST_FORCEINLINE A0 operator() (const raw_tag &,  A0 const& a0) const BOOST_NOEXCEPT
    {
      return bs::rec(bs::raw_(bs::sqrt)(a0));
    }
  };
} } }

#endif


/** lexical_cast_nonfinite_facets.cpp
*
* Copyright (c) 2011 Paul A. Bristow
*
* 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)
*
* This very simple program illustrates how to use the
* `boost/math/nonfinite_num_facets.hpp' with lexical cast
* to obtain C99 representation of infinity and NaN.
* This example is from the original Floating Point  Utilities contribution by Johan Rade.
* Floating Point Utility library has been accepted into Boost,
* but the utilities are incorporated into Boost.Math library.
*
\file

\brief A very simple example of using lexical cast with
non_finite_num facet for C99 standard output of infinity and NaN.

\detail This example shows how to create a C99 non-finite locale,
and imbue input and output streams with the non_finite_num put and get facets.
This allows lexical_cast output and input of infinity and NaN in a Standard portable way,
This permits 'loop-back' of output back into input (and portably across different system too).

See also lexical_cast_native.cpp which is expected to fail on many systems,
but might succeed if the default locale num_put and num_get facets
comply with C99 nonfinite input and output specification.

*/

#include <boost/math/special_functions/nonfinite_num_facets.hpp>
using boost::math::nonfinite_num_get;
using boost::math::nonfinite_num_put;

#include <boost/lexical_cast.hpp>
using boost::lexical_cast;

#include <iostream>
using std::cout;
using std::endl;
using std::cerr;

#include <iomanip>
using std::setw;
using std::left;
using std::right;
using std::internal;

#include <string>
using std::string;

#include <sstream>
using std::istringstream;

#include <limits>
using std::numeric_limits;

#include <locale>
using std::locale;

#include <boost/assert.hpp>

int main ()
{
  std::cout << "finite_num_facet with lexical_cast example." << std::endl;

  // Example of using non_finite num_put and num_get facets with lexical_cast.
  //locale old_locale;
  //locale tmp_locale(old_locale, new nonfinite_num_put<char>);
  //// Create a new temporary output locale, and add the output nonfinite_num_put facet.

  //locale new_locale(tmp_locale, new nonfinite_num_get<char>);
  // Create a new output locale (from the tmp locale), and add the input nonfinite_num_get facet.

  // Note that you can only add facets one at a time,
  // unless you chain thus:

  std::locale new_locale(std::locale(std::locale(),
    new boost::math::nonfinite_num_put<char>),
    new boost::math::nonfinite_num_get<char>);

  locale::global(new_locale); // Newly constructed streams
  // (including those streams inside lexical_cast)
  // now use new_locale with nonfinite facets.

  // Output using the new locale.
  cout << "Using C99_out_locale " << endl;
  cout.imbue(new_locale);
  // Necessary because cout already constructed using default C locale,
  // and default facets for nonfinites.

    // Create plus and minus infinity.
  double plus_infinity = +std::numeric_limits<double>::infinity();
  double minus_infinity = -std::numeric_limits<double>::infinity();

  // and create a NaN (NotANumber)
  double NaN = +std::numeric_limits<double>::quiet_NaN ();
  cout << "+std::numeric_limits<double>::infinity() = " << plus_infinity << endl;
  cout << "-std::numeric_limits<double>::infinity() = " << minus_infinity << endl;
  cout << "+std::numeric_limits<double>::quiet_NaN () = " << NaN << endl;

  // Now try some 'round-tripping', 'reading' "inf".
  double x = boost::lexical_cast<double>("inf");
  // and check we get a floating-point infinity.
  BOOST_ASSERT(x == std::numeric_limits<double>::infinity());
  cout << "boost::lexical_cast<double>(\"inf\") = " << x << endl;

  // Check we can convert the other way from floating-point infinity,
  string s = boost::lexical_cast<string>(numeric_limits<double>::infinity());
  // to a C99 string representation as "inf".
  BOOST_ASSERT(s == "inf");

  // Finally try full 'round-tripping' (in both directions):
  BOOST_ASSERT(lexical_cast<double>(lexical_cast<string>(numeric_limits<double>::infinity()))
    == numeric_limits<double>::infinity());
  BOOST_ASSERT(lexical_cast<string>(lexical_cast<double>("inf")) == "inf");

    return 0;
} // int main()

/*

Output:
  finite_num_facet with lexical_cast example.
  Using C99_out_locale
  +std::numeric_limits<double>::infinity() = inf
  -std::numeric_limits<double>::infinity() = -inf
  +std::numeric_limits<double>::quiet_NaN () = nan
  boost::lexical_cast<double>("inf") = inf

*/


/**
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 * @date Sat Jun 4 21:38:59 2011 +0200
 *
 * @brief Implements a multi-class Fisher/LDA linear machine Training using
 * Singular Value Decomposition (SVD). For more information on Linear Machines
 * and associated methods, please consult Bishop, Machine Learning and Pattern
 * Recognition chapter 4.
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <boost/format.hpp>
#include <bob.math/pinv.h>
#include <bob.math/eig.h>
#include <bob.math/linear.h>
#include <bob.math/stats.h>

#include <bob.learn.linear/lda.h>

namespace bob { namespace learn { namespace linear {

  FisherLDATrainer::FisherLDATrainer
    (bool use_pinv, bool strip_to_rank)
    : m_use_pinv(use_pinv),
      m_strip_to_rank(strip_to_rank)
  {
  }

  FisherLDATrainer::FisherLDATrainer
    (const FisherLDATrainer& other)
    : m_use_pinv(other.m_use_pinv),
      m_strip_to_rank(other.m_strip_to_rank)
  {
  }

  FisherLDATrainer::~FisherLDATrainer()
  {
  }

  FisherLDATrainer& FisherLDATrainer::operator=
    (const FisherLDATrainer& other)
    {
      if (this != &other) // avoid auto assignment
      {
        m_use_pinv = other.m_use_pinv;
        m_strip_to_rank = other.m_strip_to_rank;
      }
      return *this;
    }

  bool FisherLDATrainer::operator==
    (const FisherLDATrainer& other) const
    {
      return m_use_pinv == other.m_use_pinv && \
                         m_strip_to_rank == other.m_strip_to_rank;
    }

  bool FisherLDATrainer::operator!=
    (const FisherLDATrainer& other) const
    {
      return !(this->operator==(other));
    }

  /**
   * Returns the indexes for sorting a given blitz::Array<double,1>
   */
  struct compare_1d_blitz {
    const blitz::Array<double,1>& v_;
    compare_1d_blitz(const blitz::Array<double,1>& v): v_(v) { }
    bool operator() (size_t i, size_t j) { return v_(i) < v_(j); }
  };

  static std::vector<size_t> sort_indexes(const blitz::Array<double,1>& v) {

    // initialize original index locations
    std::vector<size_t> idx(v.size());
    for (size_t i = 0; i != idx.size(); ++i) idx[i] = i;

    // sort indexes based on comparing values in v
    std::sort(idx.begin(), idx.end(), compare_1d_blitz(v));

    return idx;
  }

  void FisherLDATrainer::train
    (Machine& machine, blitz::Array<double,1>& eigen_values,
     const std::vector<blitz::Array<double, 2> >& data) const
    {
      // if #classes < 2, then throw
      if (data.size() < 2) {
        boost::format m("The number of arrays in the input data == %d whereas for LDA you should provide at least 2");
        m % data.size();
        throw std::runtime_error(m.str());
      }

      // checks for arrayset data type and shape once
      int n_features = data[0].extent(1);

      for (size_t cl=0; cl<data.size(); ++cl) {
        if (data[cl].extent(1) != n_features) {
          boost::format m("The number of features/columns (%d) in array at position %d of your input differs from that of array at position 0 (%d)");
          m % data[cl].extent(1) % n_features;
          throw std::runtime_error(m.str());
        }
      }

      int osize = output_size(data);

      // Checks that the dimensions are matching
      if (machine.inputSize() != (size_t)data[0].extent(1)) {
        boost::format m("Number of features at input data set (%d columns) does not match machine input size (%d)");
        m % data[0].extent(1) % machine.inputSize();
        throw std::runtime_error(m.str());
      }
      if (machine.outputSize() != (size_t)osize) {
        boost::format m("Number of outputs of the given machine (%d) does not match the expected number of outputs calculated by this trainer = %d");
        m % machine.outputSize() % osize;
        throw std::runtime_error(m.str());
      }
      if (eigen_values.extent(0) != osize) {
        boost::format m("Number of eigenvalues on the given 1D array (%d) does not match the expected number of outputs calculated by this trainer = %d");
        m % eigen_values.extent(0) % osize;
        throw std::runtime_error(m.str());
      }

      blitz::Array<double,1> preMean(n_features);
      blitz::Array<double,2> Sw(n_features, n_features);
      blitz::Array<double,2> Sb(n_features, n_features);
      bob::math::scatters_(data, Sw, Sb, preMean);

      // computes the generalized eigenvalue decomposition
      // so to find the eigen vectors/values of Sw^(-1) * Sb
      blitz::Array<double,2> V(Sw.shape());
      blitz::Array<double,1> eigen_values_(n_features);

      if (m_use_pinv) {

        //note: misuse V and Sw as temporary place holders for data
        bob::math::pinv_(Sw, V); //V now contains Sw^-1
        bob::math::prod_(V, Sb, Sw); //Sw now contains Sw^-1*Sb
        blitz::Array<std::complex<double>,1> Dtemp(eigen_values_.shape());
        blitz::Array<std::complex<double>,2> Vtemp(V.shape());
        bob::math::eig_(Sw, Vtemp, Dtemp); //V now contains eigen-vectors

        //sorting: we know this problem on has real eigen-values
        blitz::Range a = blitz::Range::all();
        blitz::Array<double,1> Dunordered(blitz::real(Dtemp));
        std::vector<size_t> order = sort_indexes(Dunordered);
        for (int i=0; i<n_features; ++i) {
          eigen_values_(i) = Dunordered(order[i]);
          V(a,i) = blitz::real(Vtemp(a,order[i]));
        }
      }
      else {
        bob::math::eigSym_(Sb, Sw, V, eigen_values_);
      }

      // Convert ascending order to descending order
      eigen_values_.reverseSelf(0);
      V.reverseSelf(1);

      // limit the dimensions of the resulting projection matrix and eigen values
      eigen_values = eigen_values_(blitz::Range(0,osize-1));
      V.resizeAndPreserve(V.extent(0), osize);

      // normalizes the eigen vectors so they have unit length
      blitz::Range a = blitz::Range::all();
      for (int column=0; column<V.extent(1); ++column) {
        math::normalizeSelf(V(a,column));
      }

      // updates the machine
      machine.setWeights(V);
      machine.setInputSubtraction(preMean);

      // also set input_div and biases to neutral values...
      machine.setInputDivision(1.0);
      machine.setBiases(0.0);
    }

  void FisherLDATrainer::train(Machine& machine,
      const std::vector<blitz::Array<double,2> >& data) const {
    blitz::Array<double,1> throw_away(output_size(data));
    train(machine, throw_away, data);
  }

  size_t FisherLDATrainer::output_size(const std::vector<blitz::Array<double,2> >& data) const {
    return m_strip_to_rank ? std::min(data.size()-1, (size_t)data[0].extent(1)) : data[0].extent(1);
  }

}}}


#pragma once

#include <array>
#include <base/polyval.hpp>
#include <boost/math/constants/constants.hpp>

class TransitionFunction
{
 public:
  template <typename NUMERIC>
  NUMERIC operator()(const NUMERIC &x) const;
};

template <typename NUMERIC>
inline NUMERIC
TransitionFunction::operator()(const NUMERIC &x) const
{
  constexpr static std::array<int, 8> coeffs_ = {0, 0, 0, 0, 35, -84, 70, -20};

  if (x >= 1)
    return 1;
  else if (x <= 0)
    return 0;
  else {
    double v = polyval(coeffs_.begin(), x, coeffs_.size());
    assert(v >= 0 && v <= 1);
    return v;
  }
}

// --------------------------------------------------------------------------------
template <typename TF = TransitionFunction>
class PsiRadial1
{
 public:
  template <typename NUMERIC>
  NUMERIC operator()(const NUMERIC &x) const;

 private:
  TF t_;
};

template <typename TF>
template <typename NUMERIC>
inline NUMERIC
PsiRadial1<TF>::operator()(const NUMERIC &x) const
{
  using namespace boost::math::constants;

  if (std::abs(x) >= 1 && std::abs(x) <= 2) {
    return std::sin(pi<NUMERIC>() / 2 * t_(std::abs(x) - 1));
  } else if (2 < std::abs(x) && std::abs(x) < 4) {
    return std::cos(pi<NUMERIC>() / 2 * t_(0.5 * std::abs(x) - 1));
  } else {
    return 0;
  }
}

// --------------------------------------------------------------------------------
template <typename TF = TransitionFunction>
class PsiSpherical1
{
 public:
  template <typename NUMERIC>
  NUMERIC operator()(const NUMERIC &x) const;

 private:
  TF t_;
};

template <typename TF>
template <typename NUMERIC>
inline NUMERIC
PsiSpherical1<TF>::operator()(const NUMERIC &x) const
{
  if (x <= 0)
    return std::sqrt(t_(1 + x));
  else
    return std::sqrt(t_(1 - x));
}

// --------------------------------------------------------------------------------
/**
 * @brief \f$ \Psi_{0,s,0} \f$
 *
 */
template <typename TF = TransitionFunction>
class PsiScaling1
{
 public:
  template <typename NUMERIC>
  NUMERIC operator()(const NUMERIC &x, const NUMERIC &y) const;

  template <typename NUMERIC>
  NUMERIC operator()(const NUMERIC &z) const;

 private:
  TF t_;
};

// --------------------------------------------------------------------------------
template <typename TF>
template <typename NUMERIC>
inline NUMERIC
PsiScaling1<TF>::operator()(const NUMERIC &x, const NUMERIC &y) const
{
  using namespace boost::math::constants;
  NUMERIC z = std::max(std::abs(x), std::abs(y));
  if (z < 1)
    return 1;
  else if (z >= 1 && z <= 2)
    return std::cos(pi<NUMERIC>() / 2 * t(z - 1));
  else
    return 0;
}

// --------------------------------------------------------------------------------
template <typename TF>
template <typename NUMERIC>
inline NUMERIC
PsiScaling1<TF>::operator()(const NUMERIC &z) const
{
  using namespace boost::math::constants;
  if (z < 1)
    return 1;
  else if (z >= 1 && z <= 2)
    return std::cos(pi<NUMERIC>() / 2 * t_(z - 1));
  else
    return 0;
}


#pragma once

#include <Eigen/Core>
#include <ros/ros.h>

namespace nav{

template<typename T = float>
class CameraModel{
public:

    ///////////////////////////////////////////////////////////////////////////////////////////////
    // Constructors and Destructors ///////////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////

    CameraModel()
    {
    }

    ~CameraModel()
    {
    }

private:

    ///////////////////////////////////////////////////////////////////////////////////////////////
    // Private Methods ////////////////////////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////
    Eigen::Matrix<T, 3, 4> get_projection_matrix();

    ///////////////////////////////////////////////////////////////////////////////////////////////
    // Private Members ////////////////////////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////

    int ROWS = 0;
    int COLS = 0;

    // Distortion model (only the plum-bob model is currently supported)
    T D[5] = {0, 0, 0, 0, 0};

    // Camera Geometry
    Eigen::Matrix<T, 3, 3> K;  // Camera intrinsics
    Eigen::Matrix<T, 3, 3> C;  // Center of projection in world frame
    Eigen::Matrix<T, 3, 3> R;  // Orientation of camera frame relative to world frame

};

template<typename T>
Eigen::Matrix<T, 3, 4> CameraModel<T>::get_projection_matrix()
{
    Eigen::Matrix<T, 3, 1> v;
    Eigen::DiagonalMatrix<T, 3> I = v.asDiagonal();
    Eigen::Matrix<T, 3, 4> aug;
    aug.block(0, 0, 2, 2) = I;
    aug. col(2) = C;
    return K * R * aug;
}

} // namespace nav


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing acscd capabilities

    inverse cosecant in degree.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = acscd(x);
    @endcode

    is similar to:

    @code
    T r =  asind(rec(x));;
    @endcode

    @see acsc

  **/
  const boost::dispatch::functor<tag::acscd_> acscd = {};
} }
#endif

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

#endif


#ifndef CANNON_PHYSICS_SYSTEMS_INVERTED_PENDULUM_H
#define CANNON_PHYSICS_SYSTEMS_INVERTED_PENDULUM_H

#include <Eigen/Dense>

#include <ompl/control/ODESolver.h>
#include <ompl/control/spaces/RealVectorControlSpace.h>

#include <cannon/physics/systems/system.hpp>
#include <cannon/physics/rk4_integrator.hpp>

namespace oc = ompl::control;
namespace ob = ompl::base;

using namespace Eigen;

namespace cannon {
  namespace physics {
    namespace systems {

      struct Hol2DSystem : System {
        Hol2DSystem() {}

        virtual void operator()(const VectorXd& s, VectorXd& dsdt, const double /*t*/) override {
          double ux = s[2];
          double uy = s[3];

          dsdt.resize(4);
          dsdt[0] = ux;
          dsdt[1] = uy;
          dsdt[2] = 0.0;
          dsdt[3] = 0.0;
        }

        virtual void ompl_ode_adaptor(const oc::ODESolver::StateType& q,
            const oc::Control* control, oc::ODESolver::StateType& qdot) override {

          const double ux = control->as<oc::RealVectorControlSpace::ControlType>()->values[0];
          const double uy = control->as<oc::RealVectorControlSpace::ControlType>()->values[1];

          VectorXd s(4);
          s[0] = q[0];
          s[1] = q[1];
          s[2] = ux;
          s[3] = uy;
          VectorXd dsdt(4);

          (*this)(s, dsdt, 0.0);

          qdot.resize(q.size(), 0);
          for (unsigned int i = 0; i < q.size(); i++) {
            qdot[i] = dsdt[i];
          }
        }

        virtual std::tuple<MatrixXd, MatrixXd, VectorXd> get_linearization(const VectorXd& x) override {
          MatrixXd A = MatrixXd::Zero(2, 2);
          MatrixXd B = MatrixXd::Zero(2, 2);
          VectorXd c = VectorXd::Zero(2);

          // TODO
          throw std::runtime_error("Not implemented yet");

          return std::make_tuple(A, B, c);
        }

        virtual void
        get_continuous_time_linearization(const oc::ODESolver::StateType &q,
                                          Ref<MatrixXd> A,
                                          Ref<MatrixXd> B) override {
          // TODO
          throw std::runtime_error("Not implemented yet");
        }

        static void ompl_post_integration(const ob::State* /*state*/, const
            oc::Control* /*control*/, const double /*duration*/, ob::State *result) {
          // Nothing necessary
        }

        // Parameters?

      };

      class Holonomic2D {
        public:
          Holonomic2D() = delete;

          Holonomic2D(Vector2d s, Vector2d g) : e_(s_, 4, 0.01), start_(s), goal_(g) {
            state_ = Vector4d::Zero();
            reset();
          }

          std::pair<VectorXd, double> step(double ux, double uy) {
            double clipped_ux = std::max(-1.0, std::min(ux, 1.0));
            double clipped_uy = std::max(-1.0, std::min(uy, 1.0));

            state_[2] = clipped_ux;
            state_[3] = clipped_uy;

            double goal_r = -(state_.head(2) - goal_).norm();
            double control_r = -(std::abs(clipped_ux), + std::abs(clipped_uy));

            double reward = goal_r + 0.1*control_r;

            e_.set_state(state_);
            state_ = e_.step();

            return std::make_pair(state_.head(2), reward);
          }

          VectorXd reset() {
            state_.head(2) = start_ + Vector2d::Random() * 0.1;
            state_[2] = 0.0;
            state_[3] = 0.0;

            return state_.head(2);
          }

          Hol2DSystem s_;

        private:
          RK4Integrator e_;

          Vector4d state_;

          Vector2d start_;
          Vector2d goal_;

      };

    } // namespace systems
  } // namespace physics
} // namespace cannon

#endif /* ifndef CANNON_PHYSICS_SYSTEMS_INVERTED_PENDULUM_H */