// 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/LinearCost.h>
#include <iDynTree/Utils.h>

namespace iDynTree
{
namespace optimalcontrol
{

LinearCost::LinearCost(const std::string& costName)
    : QuadraticLikeCost(costName)
{
    m_hasSecondPartialDerivativeWRTStateSparsity = true;
    m_hasSecondPartialDerivativeWRTControlSparsity = true;
    m_hasSecondPartialDerivativeWRTStateControlSparsity = true;
}

LinearCost::~LinearCost()
{
}

bool LinearCost::setStateCost(const VectorDynSize& stateGradient)
{
    m_timeVaryingStateGradient.reset(new TimeInvariantVector(stateGradient));

    return true;
}

bool LinearCost::setStateCost(std::shared_ptr<TimeVaryingVector> timeVaryingStateGradient)
{
    if (!timeVaryingStateGradient)
    {
        reportError("LinearCost", "setStateCost", "Empty gradient pointer.");
        return false;
    }

    m_timeVaryingStateGradient = timeVaryingStateGradient;

    return true;
}

bool LinearCost::setControlCost(const VectorDynSize& controlGradient)
{

    m_timeVaryingControlGradient.reset(new TimeInvariantVector(controlGradient));

    return true;
}

bool LinearCost::setControlCost(std::shared_ptr<TimeVaryingVector> timeVaryingControlGradient)
{

    if (!timeVaryingControlGradient)
    {
        reportError("QuadraticCost", "setControlCost", "Empty gradient pointer.");
        return false;
    }

    m_timeVaryingControlGradient = timeVaryingControlGradient;

    return true;
}

bool LinearCost::setCostBias(double stateCostBias, double controlCostBias)
{
    m_timeVaryingStateCostBias.reset(new TimeInvariantDouble(stateCostBias));
    m_timeVaryingControlCostBias.reset(new TimeInvariantDouble(controlCostBias));
    return true;
}

bool LinearCost::setCostBias(std::shared_ptr<TimeVaryingDouble> timeVaryingStateCostBias,
                             std::shared_ptr<TimeVaryingDouble> timeVaryingControlCostBias)
{
    if (!timeVaryingStateCostBias)
    {
        reportError("QuadraticCost",
                    "addCostBias",
                    "The timeVaryingStateCostBias pointer is empty.");
        return false;
    }

    if (!timeVaryingControlCostBias)
    {
        reportError("QuadraticCost",
                    "addCostBias",
                    "The timeVaryingControlCostBias pointer is empty.");
        return false;
    }

    m_timeVaryingStateCostBias = timeVaryingStateCostBias;
    m_timeVaryingControlCostBias = timeVaryingControlCostBias;
    return true;
}

} // namespace optimalcontrol
} // namespace iDynTree
