// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
// SPDX-License-Identifier: BSD-3-Clause
/*
 * Originally developed for Prioritized Optimal Control (2014)
 * Refactored in 2018.
 * Design inspired by
 * - ACADO toolbox (http://acado.github.io)
 * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
 */

#include <iDynTree/Optimizers/AlglibInterface.h>

#include <Eigen/Dense>
#include <iDynTree/EigenHelpers.h>

#include <iDynTree/MatrixDynSize.h>
#include <iDynTree/Utils.h>
#include <iDynTree/VectorDynSize.h>

#include <optimization.h>
#include <stdafx.h>

#include <cassert>
#include <map>

namespace iDynTree
{

namespace optimization
{

typedef struct
{
    double boundValue;
    double constraintSign;
    unsigned int index;
} ConstraintInfo;

typedef struct
{
    std::shared_ptr<OptimizationProblem> problem;
    std::map<size_t, ConstraintInfo> expandedToOriginalInequalities, expandedToOriginalEqualities;
    VectorDynSize variablesBuffer, costJacobianBuffer, constraintsBuffer;
    MatrixDynSize constraintJacobian, zeroMatrix, jacBuffer;
} SharedData;

class AlglibInterface::AlglibInterfaceImplementation
{
public:
    SharedData* nlpData;
    VectorDynSize constraintsLowerBounds, constraintsUpperBounds, variablesLowerBounds,
        variablesUpperBounds, x0, solution;
    alglib::minnlcstate alglibState;
    alglib::minnlcreport alglibReport;
    alglib::real_1d_array alglibScaling;
    int exitCode;
    double epsx;
    alglib::ae_int_t maxits;
    alglib::ae_int_t outerits;
    double rho;

    AlglibInterfaceImplementation()
    {
        nlpData = new (SharedData);
        assert(nlpData);
        nlpData->problem = nullptr;
        exitCode = -10;
        epsx = 0.000001;
        maxits = 0;
        outerits = 5;
        rho = 1000;
    }
};

void ALGLIB_NLP(const alglib::real_1d_array& x,
                alglib::real_1d_array& fi,
                alglib::real_2d_array& jac,
                void* ptr)
{
    // See http://www.alglib.net/translator/man/manual.cpp.html#example_minnlc_d_inequality
    SharedData* pimpl = reinterpret_cast<SharedData*>(ptr);
    assert(pimpl->problem);
    Eigen::Map<const Eigen::VectorXd> x_map(x.getcontent(), x.length());
    Eigen::Map<Eigen::VectorXd> f_map(fi.getcontent(), fi.length());

    assert(x_map.size() == pimpl->problem->numberOfVariables());
    assert(f_map.size()
           == pimpl->expandedToOriginalInequalities.size()
                  + pimpl->expandedToOriginalEqualities.size() + 1); // cost function + constraints
    assert((jac.rows() == f_map.size()) && (jac.cols() == x_map.size()));
    assert((jac.rows() == pimpl->jacBuffer.rows()) && (jac.cols() == pimpl->jacBuffer.cols()));

    toEigen(pimpl->variablesBuffer) = x_map;

    if (!(pimpl->problem->setVariables(pimpl->variablesBuffer)))
    {
        reportError("ALGLIBInterface",
                    "solve",
                    "Error while setting the optimizatin variables to the problem.");
        assert(false);
    }

    double cost = 0;
    if (!(pimpl->problem->evaluateCostFunction(cost)))
    {
        reportError("ALGLIBInterface", "solve", "Error while evaluating the cost function.");
        assert(false);
    }

    if (!(pimpl->problem->evaluateCostGradient(pimpl->costJacobianBuffer)))
    {
        reportError("ALGLIBInterface", "solve", "Error while retrieving the cost gradient.");
        assert(false);
    }

    if (!(pimpl->problem->evaluateConstraints(pimpl->constraintsBuffer)))
    {
        reportError("ALGLIBInterface", "solve", "Error while evaluating the constraints.");
        assert(false);
    }

    pimpl->constraintJacobian = pimpl->zeroMatrix; // initialization for nonzero elements

    if (!(pimpl->problem->evaluateConstraintsJacobian(pimpl->constraintJacobian)))
    {
        reportError("ALGLIBInterface", "solve", "Error while evaluating the constraints jacobian.");
        assert(false);
    }

    f_map(0) = cost;
    toEigen(pimpl->jacBuffer).row(0) = toEigen(pimpl->costJacobianBuffer);
    long index = 1;
    ConstraintInfo constraint;
    for (size_t i = 0; i < pimpl->expandedToOriginalEqualities.size(); ++i)
    {
        constraint = pimpl->expandedToOriginalEqualities[i];
        f_map(index) = constraint.boundValue
                       + constraint.constraintSign * pimpl->constraintsBuffer(constraint.index);
        toEigen(pimpl->jacBuffer).row(index)
            = constraint.constraintSign * toEigen(pimpl->constraintJacobian).row(constraint.index);
        index++;
    }

    for (size_t i = 0; i < pimpl->expandedToOriginalInequalities.size(); ++i)
    {
        constraint = pimpl->expandedToOriginalInequalities[i];
        f_map(index) = constraint.boundValue
                       + constraint.constraintSign * pimpl->constraintsBuffer(constraint.index);
        toEigen(pimpl->jacBuffer).row(index)
            = constraint.constraintSign * toEigen(pimpl->constraintJacobian).row(constraint.index);
        index++;
    }

    alglib::real_2d_array tempAlglibMatrix;
    tempAlglibMatrix.attach_to_ptr(pimpl->jacBuffer.rows(),
                                   pimpl->jacBuffer.cols(),
                                   pimpl->jacBuffer.data());
    jac = tempAlglibMatrix;
}

AlglibInterface::AlglibInterface()
    : m_pimpl(new AlglibInterfaceImplementation())
{
    assert(m_pimpl);
}

AlglibInterface::~AlglibInterface()
{
    if (m_pimpl)
    {
        if (m_pimpl->nlpData)
        {
            delete m_pimpl->nlpData;
            m_pimpl->nlpData = nullptr;
        }
        delete m_pimpl;
        m_pimpl = nullptr;
    }
}

bool AlglibInterface::isAvailable() const
{
    return true;
}

bool AlglibInterface::setProblem(std::shared_ptr<OptimizationProblem> problem)
{
    if (!problem)
    {
        reportError("ALGLIBInterface", "setProblem", "Empty problem pointer.");
        return false;
    }
    m_pimpl->nlpData->problem = problem;
    return true;
}

bool AlglibInterface::solve()
{
    if (!(m_pimpl->nlpData->problem))
    {
        reportError("ALGLIBInterface", "solve", "First you have to set the problem.");
        return false;
    }

    if (!(m_pimpl->nlpData->problem->prepare()))
    {
        reportError("ALGLIBInterface", "solve", "Failed while preparing the optimization problem.");
        return false;
    }

    unsigned int nx = m_pimpl->nlpData->problem->numberOfVariables();
    unsigned int nc = m_pimpl->nlpData->problem->numberOfConstraints();

    if (m_pimpl->nlpData->variablesBuffer.size() != nx)
        m_pimpl->nlpData->variablesBuffer.resize(nx);

    if (m_pimpl->solution.size() != nx)
        m_pimpl->solution.resize(nx);

    if (m_pimpl->nlpData->constraintsBuffer.size() != nc)
        m_pimpl->nlpData->constraintsBuffer.resize(nc);

    if (m_pimpl->nlpData->costJacobianBuffer.size() != nx)
        m_pimpl->nlpData->costJacobianBuffer.resize(nx);

    if ((m_pimpl->nlpData->constraintJacobian.rows() != nc)
        || (m_pimpl->nlpData->constraintJacobian.cols() != nx))
        m_pimpl->nlpData->constraintJacobian.resize(nc, nx);

    if ((m_pimpl->nlpData->zeroMatrix.rows() != nc) || (m_pimpl->nlpData->zeroMatrix.cols() != nx))
    {
        m_pimpl->nlpData->zeroMatrix.resize(nc, nx);
        m_pimpl->nlpData->zeroMatrix.zero();
    }

    if (m_pimpl->variablesLowerBounds.size() != nx)
        m_pimpl->variablesLowerBounds.resize(nx);

    if (!(m_pimpl->nlpData->problem->getVariablesLowerBound(m_pimpl->variablesLowerBounds)))
        toEigen(m_pimpl->variablesLowerBounds).setConstant(minusInfinity());

    if (m_pimpl->variablesUpperBounds.size() != nx)
        m_pimpl->variablesUpperBounds.resize(nx);

    if (!(m_pimpl->nlpData->problem->getVariablesUpperBound(m_pimpl->variablesUpperBounds)))
        toEigen(m_pimpl->variablesUpperBounds).setConstant(plusInfinity());

    if (m_pimpl->constraintsUpperBounds.size() != nc)
        m_pimpl->constraintsUpperBounds.resize(nc);

    if (m_pimpl->constraintsLowerBounds.size() != nc)
        m_pimpl->constraintsLowerBounds.resize(nc);

    if (!(m_pimpl->nlpData->problem->getConstraintsBounds(m_pimpl->constraintsLowerBounds,
                                                          m_pimpl->constraintsUpperBounds)))
    {
        reportError("ALGLIBInterface", "solve", "Error while retrieving the constraints bounds.");
        return false;
    }

    unsigned int inequalities = 0, equalities = 0;
    ConstraintInfo newConstraint;
    for (unsigned int i = 0; i < nc; ++i)
    {
        if (checkDoublesAreEqual(m_pimpl->constraintsLowerBounds(i),
                                 m_pimpl->constraintsUpperBounds(i)))
        {
            newConstraint.boundValue = -1.0 * m_pimpl->constraintsUpperBounds(i);
            newConstraint.constraintSign = +1.0;
            newConstraint.index = i;
            m_pimpl->nlpData->expandedToOriginalEqualities[equalities] = newConstraint;
            equalities++;
        } else
        {

            if (m_pimpl->constraintsLowerBounds(i) > minusInfinity())
            {
                newConstraint.boundValue = m_pimpl->constraintsLowerBounds(i);
                newConstraint.constraintSign = -1.0;
                newConstraint.index = i;
                m_pimpl->nlpData->expandedToOriginalInequalities[inequalities] = newConstraint;
                inequalities++;
            }

            if (m_pimpl->constraintsUpperBounds(i) < plusInfinity())
            {
                newConstraint.boundValue = -1.0 * m_pimpl->constraintsUpperBounds(i);
                newConstraint.constraintSign = +1.0;
                newConstraint.index = i;
                m_pimpl->nlpData->expandedToOriginalInequalities[inequalities] = newConstraint;
                inequalities++;
            }
        }
    }
    m_pimpl->nlpData->expandedToOriginalEqualities
        .erase(m_pimpl->nlpData->expandedToOriginalEqualities.find(equalities),
               m_pimpl->nlpData->expandedToOriginalEqualities.end());
    m_pimpl->nlpData->expandedToOriginalInequalities
        .erase(m_pimpl->nlpData->expandedToOriginalInequalities.find(inequalities),
               m_pimpl->nlpData->expandedToOriginalInequalities.end());

    if ((m_pimpl->nlpData->jacBuffer.rows() != (equalities + inequalities + 1))
        || (m_pimpl->nlpData->jacBuffer.cols() != nx))
        m_pimpl->nlpData->jacBuffer.resize((equalities + inequalities + 1), nx);

    // See http://www.alglib.net/translator/man/manual.cpp.html#sub_minnlccreate

    if (m_pimpl->x0.size() != nx)
    {
        m_pimpl->x0.resize(nx);
        m_pimpl->x0.zero();
    }

    if (m_pimpl->nlpData->problem->getGuess(m_pimpl->x0))
    {
        if (m_pimpl->x0.size() != nx)
        {
            reportError("ALGLIBInterface",
                        "solve",
                        "The initial guess dimension does not match the problem dimension.");
            return false;
        }
    }

    alglib::real_1d_array alglibX0, alglibStart, alglibSolution, alglibScaling, alglibLowerBound,
        alglibUpperBound;
    alglibX0.attach_to_ptr(m_pimpl->x0.size(), m_pimpl->x0.data());
    if (m_pimpl->alglibScaling.length() != nx)
        m_pimpl->alglibScaling.setlength(nx);
    Eigen::Map<Eigen::VectorXd> scaling_Map(m_pimpl->alglibScaling.getcontent(),
                                            m_pimpl->alglibScaling.length());
    scaling_Map.setConstant(1.0); // no scaling

    // Create optimizer object, choose AUL algorithm and tune its settings:
    // * rho=1000       penalty coefficient
    // * outerits=5     number of outer iterations to tune Lagrange coefficients
    // * epsx=0.000001  stopping condition for inner iterations
    // * s=[1,1]        all variables have unit scale
    // * exact low-rank preconditioner is used, updated after each 10 iterations
    //
    if ((m_pimpl->exitCode >= 0) && (m_pimpl->solution.size() == nx))
    {
        alglibStart.attach_to_ptr(m_pimpl->solution.size(), m_pimpl->solution.data());
        alglib::minnlcrestartfrom(m_pimpl->alglibState, alglibStart);
    } else
    {
        alglib::minnlccreate(nx, alglibX0, m_pimpl->alglibState);
    }

    alglib::minnlcsetalgoaul(m_pimpl->alglibState, m_pimpl->rho, m_pimpl->outerits);
    alglib::minnlcsetcond(m_pimpl->alglibState, m_pimpl->epsx, m_pimpl->maxits);
    alglib::minnlcsetscale(m_pimpl->alglibState, m_pimpl->alglibScaling);
    alglib::minnlcsetprecinexact(m_pimpl->alglibState);

    alglibLowerBound.attach_to_ptr(m_pimpl->variablesLowerBounds.size(),
                                   m_pimpl->variablesLowerBounds.data());
    alglibUpperBound.attach_to_ptr(m_pimpl->variablesUpperBounds.size(),
                                   m_pimpl->variablesUpperBounds.data());

    alglib::minnlcsetbc(m_pimpl->alglibState, alglibLowerBound, alglibUpperBound);
    alglib::minnlcsetnlc(m_pimpl->alglibState,
                         m_pimpl->nlpData->expandedToOriginalEqualities.size(),
                         m_pimpl->nlpData->expandedToOriginalInequalities.size());

    alglib::minnlcoptimize(m_pimpl->alglibState, ALGLIB_NLP, nullptr, m_pimpl->nlpData);

    alglibSolution.attach_to_ptr(m_pimpl->solution.size(), m_pimpl->solution.data());
    alglib::minnlcresultsbuf(m_pimpl->alglibState, alglibSolution, m_pimpl->alglibReport);
    m_pimpl->exitCode = m_pimpl->alglibReport.terminationtype;

    if (m_pimpl->exitCode < 0)
    {
        std::ostringstream errorMsg;
        errorMsg << "Optimization problem failed because ";
        switch (m_pimpl->exitCode)
        {
        case -3:
            errorMsg << "constraints are inconsistent.";
            break;

        case -5:
            errorMsg << "inappropriate solver was used.";
            break;

        case -7:
            errorMsg << "derivative correctness check failed.";
            break;

        case -8:
            errorMsg << "optimizer detected NAN/INF values either in the function itself, or in "
                        "its Jacobian.";
            break;

        default:
            errorMsg << "of an unknown error.";
        }
        reportError("ALGLIBInterface", "solve", errorMsg.str().c_str());
        return false;
    }

    return true;
}

bool AlglibInterface::getPrimalVariables(VectorDynSize& primalVariables)
{
    if (m_pimpl->exitCode < 0)
    {
        reportError("ALGLIBInterface",
                    "getPrimalVariables",
                    "Previous run was not successfull. Cannot retrieve primal variables.");
        return false;
    }
    primalVariables = m_pimpl->solution;
    return true;
}

bool AlglibInterface::getDualVariables(VectorDynSize& constraintsMultipliers,
                                       VectorDynSize& lowerBoundsMultipliers,
                                       VectorDynSize& upperBoundsMultipliers)
{
    reportError("ALGLIBInterface",
                "getDualVariables",
                "ALGLIB does not give information about the dual variables.");
    return false;
}

bool AlglibInterface::getOptimalCost(double& optimalCost)
{
    if (m_pimpl->exitCode < 0)
    {
        reportError("ALGLIBInterface",
                    "getOptimalCost",
                    "Previous run was not successfull. Cannot retrieve optimal cost.");
        return false;
    }

    if (!(m_pimpl->nlpData->problem->setVariables(m_pimpl->solution)))
    {
        reportError("ALGLIBInterface",
                    "getOptimalCost",
                    "Error while setting the optimal variables to the problem.");
        return false;
    }

    double cost = 0;
    if (!(m_pimpl->nlpData->problem->evaluateCostFunction(cost)))
    {
        reportError("ALGLIBInterface",
                    "getOptimalCost",
                    "Error while evaluating the cost function.");
        return false;
    }
    optimalCost = cost;
    return true;
}

bool AlglibInterface::getOptimalConstraintsValues(VectorDynSize& constraintsValues)
{
    if (m_pimpl->exitCode < 0)
    {
        reportError("ALGLIBInterface",
                    "getOptimalConstraintsValues",
                    "Previous run was not successfull. Cannot retrieve optimal constraints value.");
        return false;
    }

    if (!(m_pimpl->nlpData->problem->setVariables(m_pimpl->solution)))
    {
        reportError("ALGLIBInterface",
                    "getOptimalConstraintsValues",
                    "Error while setting the optimal variables to the problem.");
        return false;
    }

    if (!(m_pimpl->nlpData->problem->evaluateConstraints(m_pimpl->nlpData->variablesBuffer)))
    {
        reportError("ALGLIBInterface",
                    "getOptimalConstraintsValues",
                    "Error while evaluating the constraints function.");
        return false;
    }
    constraintsValues = m_pimpl->nlpData->variablesBuffer;
    return true;
}

double AlglibInterface::minusInfinity()
{
    return alglib::fp_neginf;
}

double AlglibInterface::plusInfinity()
{
    return alglib::fp_posinf;
}

bool AlglibInterface::setRHO(double rho)
{
    if (rho <= 0)
    {
        reportError("ALGLIBInterface", "setRHO", "The rho parameter is supossed to be positive;");
    }
    m_pimpl->rho = rho;
    return true;
}

bool AlglibInterface::setOuterIterations(unsigned int outerIterations)
{
    m_pimpl->outerits = outerIterations;
    return true;
}

bool AlglibInterface::setStoppingCondition(double epsX)
{
    if (epsX < 0)
    {
        reportError("ALGLIBInterface",
                    "setStoppingCondition",
                    "The epsX parameter is supposed to be non negative");
        return false;
    }

    m_pimpl->epsx = epsX;

    return true;
}

bool AlglibInterface::setMaximumIterations(unsigned int maxIter)
{
    m_pimpl->maxits = maxIter;
    return true;
}

} // namespace optimization

} // namespace iDynTree
