bmi_implementation.hpp

/***********************************************************************************
Copyright (c) 2017, Diego Pardo. All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright notice,
      this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice,
      this list of conditions and the following disclaimer in the documentation
      and/or other materials provided with the distribution.
    * Neither the name of ETH ZURICH nor the names of its contributors may be used
      to endorse or promote products derived from this software without specific
      prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL ETH ZURICH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***************************************************************************************/
/*
 * bmi_implementation.hpp
 *
 *  Created on: Aug 29, 2016
 *      Author: depardo
 */

namespace DirectTrajectoryOptimization {

template<typename DIMENSIONS>
DTOMethodInterface<DIMENSIONS>::DTOMethodInterface(
        std::vector<std::shared_ptr<DynamicsBase<DIMENSIONS> > > dynamics,
        std::vector<std::shared_ptr<DerivativesBaseDS<DIMENSIONS> > > derivatives,
        std::shared_ptr<BaseClass::CostFunctionBase<DIMENSIONS> > costFunction,
        std::vector<std::shared_ptr<BaseClass::ConstraintsBase<DIMENSIONS> > > constraints,
        Eigen::Vector2d duration, int number_of_nodes, bool methodVerbose) :
            _dynamics(dynamics),
            _derivatives(derivatives),
            _costFunction(costFunction),
            _constraints(constraints),
            _trajTimeRange(duration),
            _numberOfNodes(number_of_nodes),
            _numControls(DIMENSIONS::CONTROL_SIZE),
            _numStates(DIMENSIONS::STATE_SIZE),
            _verbose(methodVerbose) {

    if (dynamics.empty()) {
        throw std::invalid_argument("Empty dynamics vector recieved.");
    } else if (derivatives.empty()) {
        throw std::invalid_argument("Empty derivatives vector recieved.");
    } else if (costFunction == nullptr) {
        throw std::invalid_argument("Cost object is nullptr.");
    } else if (constraints .empty()) {
        throw std::invalid_argument("Constraints object is nullptr");
    }

    num_phases = _dynamics.size();

    if(num_phases > 1)
        _multiphaseproblem = true;

    // not required?
    _guardVector.clear(); // ToDo: Not use this anymore
    _interphase_constraint.clear();
    _guardVectorDerivative.clear(); // ToDo: Not required completely remove this
    _guards.clear();

    // Default values for initial and final states/control flags
    _flagInitialState = Eigen::VectorXi::Zero(_numStates);
    _flagFinalState = Eigen::VectorXi::Zero(_numStates);
    _flagInitialControl = Eigen::VectorXi::Zero(_numControls);
    _flagFinalControl = Eigen::VectorXi::Zero(_numControls);

    // Default values of initial and final states
    y_0.setConstant(0);
    y_f.setConstant(0);

    if(_verbose){
        std::cout << "[VERBOSE] num_phases : " << num_phases << std::endl;
        std::cout << "[VERBOSE] multiphaseproblem : " << _multiphaseproblem << std::endl;
        getchar();
    }

}

/*user interfaces */

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::SetInitialState(const state_vector_t & y0) {
    y_0 = y0;
    _flagInitialState = Eigen::VectorXi::Constant(y0.size(),1);
    return;
}

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::SetInitialControl(const control_vector_t & u0) {
    u_0 = u0;
    _flagInitialControl = Eigen::VectorXi::Constant(u0.size(),1);
    return;
}

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::SetFinalControl(const control_vector_t & uf) {
    u_f = uf;
    _flagFinalControl = Eigen::VectorXi::Constant(uf.size(),1);
    return;
}

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::SetFinalState(const state_vector_t & yF) {
    y_f = yF;
    _flagFinalState = Eigen::VectorXi::Constant(yF.size(),1);

    return;
}

template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::getTrajectoryIndexes(Eigen::VectorXi &hs,
        Eigen::MatrixXi &ys, Eigen::MatrixXi &us) {
    hs = h_inds;
    ys = y_inds;
    us = u_inds;
}

template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::SetStateBounds(const state_vector_t & ylb,
        const state_vector_t & yub) {
    for (int i = 0; i < num_phases; i++) {
        y_lb.push_back(ylb);
        y_ub.push_back(yub);
    }
}

template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::SetStateBounds(
        const state_vector_array_t & ylb, const state_vector_array_t & yub) {

    assert(ylb.size() == num_phases);
    assert(yub.size() == num_phases);

    y_lb = ylb;
    y_ub = yub;
}

template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::SetControlBounds(const control_vector_t & ulb,
        const control_vector_t & uub) {

    for (int i = 0; i < num_phases; i++) {
        u_lb.push_back(ulb);
        u_ub.push_back(uub);
    }

}

template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::SetControlBounds(
        const control_vector_array_t & ulb, const control_vector_array_t & uub) {

    assert(ulb.size() == num_phases);
    assert(uub.size() == num_phases);

    u_lb = ulb;
    u_ub = uub;
}

template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::setTimeIncrementsBounds(
        const double & h_min, const double & h_max) {

    if (h_max < h_min || h_min < 0) {
        std::cout << "Time increments bounds are not consistent!" << std::endl;
        std::exit (EXIT_FAILURE);
    }
    minimum_increment_size = h_min;
    maximum_increment_size = h_max;
}

template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::SetFreeFinalState(int state_number) {
    _flagFinalState(state_number) = 0;
}

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::Initialize() {

    if (ValidateDuration() == false) {
        exit(EXIT_FAILURE);
    }

    if (ValidateStateControlBounds() == false) {
        exit(EXIT_FAILURE);
    }

    if(_guards.size() != num_phases - 1) {
        throw std::invalid_argument("Vector of guard functions does not match number of dynamics passed.");
    }

    SetSizeOfVariables();
    SetSizeOfConstraints();
    SetTOConstraintsBounds();
    SetTOVariablesBounds();
    SetupJacobianVariables();
    Method_specific_initialization();

    return;
}

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::SetPercentageNodesPerDynamics(Eigen::VectorXd nodeSplit) {
        // make sure we have same number of dynamics as percentages
        if (nodeSplit.size() != num_phases) {
            throw std::invalid_argument("Mismatch between number of splits in percentage vector and number of dynamics.");
        }

        // ensure that sum of percentages == 100
        int total = 0;
        for (int i = 0; i < nodeSplit.size(); i++) {
            total += nodeSplit[i];
        }

        if (total != 100) {
            throw std::invalid_argument("Percentage split vector does not sum to 100%.");
            return;
        }

        // ToDo ensure that no single phase has less than _minNodesPerPhase amount of nodes

        _nodeSplitPercentage = nodeSplit;
        _nodeSplitSet = true;
}

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::SetNodesPerPhase(const Eigen::VectorXi & nodes) {

    //check if values are valid
    if(nodes.rows() < num_phases) {
        std::cout << "nodes distribution doesn't fit into number of phases" << std::endl;
        std::exit(EXIT_FAILURE);
    }

    for(int i = 0 ; i < num_phases ; i++){
        number_nodes_phase.push_back(nodes(i));
        std::cout << "nnp : "  << number_nodes_phase[i] << std::endl;
    }

    //for(auto n:number_nodes_phase) {
    for(int k = 0 ; k < num_phases ; k++) {
        for(int i = 0 ; i < number_nodes_phase[k] ; i++){
            node_to_phase_map.push_back(k);
            //std::cout << node_to_phase_map[i] << std::endl;
        }
//      inc++;
    }

    _nodeSplitSet = true;
    _nodeDistribution = true;
}


/* Constructing the problem */

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::SetSizeOfVariables() {

    // setup size of the problem
    n_inc = _numberOfNodes - num_phases; /* no increments in between phases */
                                         /* n_inc and numberOfNodes are not interchangeable anymore*/
    int num_vars = _numberOfNodes * (_numStates + _numControls) + n_inc;

    myNLP.SetVariablesSize(num_vars);

    //setup dto trajectories size (added for efficiency)
    _y_trajectory.resize(_numberOfNodes);
    _ydot_trajectory.resize(_numberOfNodes);
    _u_trajectory.resize(_numberOfNodes);
    _h_trajectory.resize(n_inc);

    // setup decision variables indexes
    h_inds.resize(n_inc);
    y_inds.resize(_numStates, _numberOfNodes);
    u_inds.resize(_numControls, _numberOfNodes);

    if(this->_multiphaseproblem) {
        leftphase_nodes.resize(num_phases-1);
        rightphase_nodes.resize(num_phases-1);
        size_interphase_constraint.resize(num_phases - 1);
        size_guard.resize(num_phases-1);
    }

    //ToDo : Remove the interface to determine the No of nodes per phase.
    //       User should request enough number of nodes such that
    //       all phases use the same number of nodes.
    // if user didn't set the splitting, use even allotments
    if (!_nodeSplitSet) {
        int sumAmt = 0;
        _nodeSplitPercentage.resize(num_phases);

        for (int j = 0; j < num_phases - 1; j++) {
            _nodeSplitPercentage[j] = 100.00/static_cast<double>(num_phases);
            sumAmt += _nodeSplitPercentage[j];
        }
        _nodeSplitPercentage[num_phases - 1] = 100 - sumAmt;
    }

    // now split up the phases
    if (!_nodeDistribution) {
        int startingNode = 0;
        number_nodes_phase.clear();

        for (int i = 0; i < num_phases - 1; i++) {
            std::cout << "_nodeSplitPercentage[i] : " << _nodeSplitPercentage[i] << std::endl;
            //int numberNodesThisPhase =
                //  static_cast<int>(_nodeSplitPercentage[i])/100.0 * static_cast<double>(_numberOfNodes);
            int numberNodesThisPhase = _numberOfNodes/num_phases;

            number_nodes_phase.push_back(numberNodesThisPhase);
            for (int j = startingNode; j < numberNodesThisPhase + startingNode; j++) {
                node_to_phase_map.push_back(i);
            }
            startingNode += numberNodesThisPhase;
        }
        // Last phase includes the remaining nodes
        number_nodes_phase.push_back(_numberOfNodes-startingNode);
        // ...and the corresponding node to phase
        for (int j = startingNode; j < _numberOfNodes; j++) {
            node_to_phase_map.push_back(num_phases - 1);
        }
    }

    assert(node_to_phase_map.size() == _numberOfNodes);

    // assigning the indexes
    for (int i = 0; i < n_inc; i++)
        h_inds(i) = i;

    int initial_index_y = n_inc;
    int initial_index_u = n_inc + (_numberOfNodes * _numStates);

    for (int node = 0; node < _numberOfNodes; node++) {
        for (int state = 0; state < _numStates; state++) {
            y_inds(state, node) = initial_index_y + node * _numStates + state;
        }
        for (int control = 0; control < _numControls; control++) {
            u_inds(control, node) = initial_index_u + node * _numControls + control;
        }
    }

    if(_multiphaseproblem) {
        int first_node_this_phase = 0;
        // nodes indexes at the boundary of the phases
        for(int i = 0 ; i < num_phases - 1 ; i++) {
            leftphase_nodes(i) = first_node_this_phase + number_nodes_phase[i]-1;
            rightphase_nodes(i)= first_node_this_phase + number_nodes_phase[i];
            first_node_this_phase += number_nodes_phase[i];
        }
    }

    increments_solution.resize(n_inc);
    states_solution.resize(_numStates, _numberOfNodes);
    controls_solution.resize(_numControls, _numberOfNodes);

    // default to -inf
    increments_solution.setConstant(-infy);
    states_solution.setConstant(-infy);
    controls_solution.setConstant(-infy);

    if (_verbose) {
        std::cout   << "[VERBOSE] h_inds : " << h_inds.transpose() << std::endl
                    << "[VERBOSE] y_inds : " << std::endl << y_inds << std::endl
                    << "[VERBOSE] u_inds : " << std::endl << u_inds << std::endl
                    << "[VERBOSE] Number of nodes : " << _numberOfNodes << std::endl
                    << "[VERBOSE] n_inc : " << n_inc << std::endl
                    << "[VERBOSE] _numStates : " << _numStates << std::endl
                    << "[VERBOSE] _numControls : " << _numControls << std::endl
                    << "[VERBOSE] number of phases : " << num_phases << std::endl;

        for (int i = 0 ; i < _numberOfNodes ; i++) {
            std::cout << "Node ["<<i<<"] = " << node_to_phase_map[i] << std::endl;
            std::cout << "Testing Dynamics[" << i << "] : "
                    << this->_dynamics[node_to_phase_map[i]]->systemDynamics(state_vector_t::Zero(),control_vector_t::Zero()).transpose() << std::endl;
        }
        for (int k = 0 ; k < this->num_phases ; k++) {
            std::cout << "[VERBOSE] Number_of_nodes_phase[" << k << "] : " << number_nodes_phase[k] << std::endl;
        }
        std::getchar();

        std::cout << "[VERBOSE] leftphase_nodes : " << leftphase_nodes.transpose() << std::endl;
        std::cout << "[VERBOSE] rightphase_nodes: " << rightphase_nodes.transpose() << std::endl;
        std::getchar();
    }

    return;
}

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::SetSizeOfConstraints() {

    // defects bounds
    num_defects = n_inc * _numStates;
    defects_lb.resize(num_defects);
    defects_ub.resize(num_defects);
  defect_out_.resize(num_defects);
  defect_out_.setZero();

    // total time bounds
    num_parametric_constraints = _evenTimeIncr ? n_inc : 1;
    parametricConstraints_lb.resize(num_parametric_constraints);
    parametricConstraints_ub.resize(num_parametric_constraints);
    parametric_out_.resize(num_parametric_constraints);
    parametric_out_.setZero();

    // user constraint bounds
    num_user_constraints_per_node_in_phase.clear(); /*TO Problem variable*/
    num_user_constraints = 0; /* NLP variable */

    for (int k = 0 ; k < this->num_phases ; k++) {

        int num_user_constraints_per_phase = _constraints[k]->getNumConstraints();
        num_user_constraints_per_node_in_phase.push_back(num_user_constraints_per_phase);

        //adding up the number of user constraints per phase projected to the nodes
        num_user_constraints += num_user_constraints_per_phase * this->number_nodes_phase[k];
    }
    user_constraints_out_.resize(num_user_constraints);
    user_constraints_out_.setZero();

    userConstraints_lb.resize(num_user_constraints);
    userConstraints_ub.resize(num_user_constraints);

    // guard function bounds
    if (_multiphaseproblem) {

        // guards are NOT necessarily scalar functions but user-defined
      // each constraint is going to be evaluated only in 1 point!
      for( int i = 0 ; i < num_phases - 1 ; i++) {
          size_guard[i] = this->_guards[i]->GetSize();
          num_guards += size_guard[i];
      }
        //num_guards =  (num_phases - 1) * 2;  // each guard should be evaluated in two nodes
        guard_lb.resize(num_guards);
        guard_ub.resize(num_guards);

        // interphase constraints are user-defined size (maybe state-size)
        // each neighbor state has half interphase constraint
        for(int i = 0 ; i < num_phases - 1 ; i++) {
            size_interphase_constraint[i] = this->_interphase_constraint[i]->GetSize();
            num_interphase_constraints += size_interphase_constraint[i];
        }
        interphase_lb.resize(num_interphase_constraints);
        interphase_ub.resize(num_interphase_constraints);

        guard_vector_.resize(num_guards);
        interface_vector_.resize(num_interphase_constraints);

        guard_vector_.setZero();
        interface_vector_.setZero();
    } else {
        num_guards = 0;
    }

    int num_F = num_defects + num_parametric_constraints + num_user_constraints + num_guards + num_interphase_constraints;

    myNLP.SetConstraintsSize(num_F);

    if (_verbose) {
        std::cout << " [VERBOSE] num_defects : " << num_defects << std::endl;
        std::cout << " [VERBOSE] num_parametric_constraints : " << num_parametric_constraints << std::endl;
        std::cout << " [VERBOSE] num_user_constraints : " << num_user_constraints << std::endl;
        std::cout << " [VERBOSE] num_guards : " << num_guards << std::endl;
        std::cout << " [VERBOSE] num_interphase_constraints : " << num_interphase_constraints << std::endl;
        std::getchar();
    }
}

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::SetTOConstraintsBounds() {

    // defects
        defects_lb.setConstant(0);
        defects_ub.setConstant(0);

    // parametric constraints
        parametricConstraints_lb(0) = _trajTimeRange(0);
        parametricConstraints_ub(0) = _trajTimeRange(1);

        if (_evenTimeIncr) {
            parametricConstraints_lb.segment(1, n_inc - 1).setConstant(0);
            parametricConstraints_ub.segment(1, n_inc - 1).setConstant(0);
        }

    // user constraints
        int aux_index = 0;
        for (int i = 0 ; i < this->num_phases ; i++) {
            int constraints_in_phase = number_nodes_phase[i]*num_user_constraints_per_node_in_phase[i];
            userConstraints_lb.segment(aux_index,constraints_in_phase) = _constraints[i]->getConstraintsLowerBound().replicate(
                    number_nodes_phase[i],1);
            userConstraints_ub.segment(aux_index,constraints_in_phase) = _constraints[i]->getConstraintsUpperBound().replicate(
                    number_nodes_phase[i],1);
            aux_index += constraints_in_phase;
        }

    // guard and interphase
        if (_multiphaseproblem) {
            int first_index = 0;
            int first_index_guard = 0;
            for (int i = 0 ; i < this->num_phases -1 ; i++) {
                interphase_lb.segment(first_index,this->size_interphase_constraint[i]) = _interphase_constraint[i]->getLowerBound();
                interphase_ub.segment(first_index,this->size_interphase_constraint[i]) = _interphase_constraint[i]->getUpperBound();
                first_index+=size_interphase_constraint[i];

                //left point
                guard_lb.segment(first_index_guard,this->size_guard[i])=_guards[i]->GetLowerBound();
                guard_ub.segment(first_index_guard,this->size_guard[i])=_guards[i]->GetUpperBound();
                first_index_guard +=size_guard[i];
            }
        }

    Eigen::VectorXd Flb,Fub;

    if (_multiphaseproblem) {
        Flb.resize(myNLP.num_F);
        Fub.resize(myNLP.num_F);
        Flb << defects_lb, parametricConstraints_lb, userConstraints_lb, guard_lb, interphase_lb;
        Fub << defects_ub, parametricConstraints_ub, userConstraints_ub, guard_ub, interphase_ub;
    } else {
        Flb.resize(myNLP.num_F);
        Fub.resize(myNLP.num_F);
        Flb << defects_lb, parametricConstraints_lb, userConstraints_lb;
        Fub << defects_ub, parametricConstraints_ub, userConstraints_ub;
    }

    myNLP.SetConstraintsBounds(Flb,Fub);

    if (_verbose) {
        std::cout << " [VERBOSE] defects_lb : " << defects_lb.transpose() << std::endl;
        std::cout << " [VERBOSE] defects_ub : " << defects_ub.transpose() << std::endl;
        std::cout << " [VERBOSE] parametricConstraints_lb : " << parametricConstraints_lb.transpose() << std::endl;
        std::cout << " [VERBOSE] parametricConstraints_ub : " << parametricConstraints_ub.transpose() << std::endl;
        std::cout << " [VERBOSE] guard_lb : " << guard_lb.transpose() << std::endl;
        std::cout << " [VERBOSE] guard_ub : " << guard_ub.transpose() << std::endl;
        std::cout << " [VERBOSE] interphase_lb : " << interphase_lb.transpose() << std::endl;
        std::cout << " [VERBOSE] interphase_ub : " << interphase_ub.transpose() << std::endl;
        std::getchar();
    }

}

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::SetTOVariablesBounds() {

    Eigen::VectorXd x_lb(myNLP.GetNumVars());
    Eigen::VectorXd x_ub(myNLP.GetNumVars());

    // Bounds on Decision variables
    x_lb.setConstant(-infy);
    x_ub.setConstant(infy);

    // time increment bounds
    x_lb.segment(0, n_inc).setConstant(minimum_increment_size);
    x_ub.segment(0, n_inc).setConstant(maximum_increment_size);

    // state bounds
    for (int j = 0 ; j < _numberOfNodes ; j++) {
        int index = node_to_phase_map[j];
        for (int i = 0 ; i < _numStates ; i++) {
            x_lb(y_inds(i, j)) = y_lb[index](i);
            x_ub(y_inds(i, j)) = y_ub[index](i);
        }
    }

    // control bounds
    for (int j = 0 ; j < _numberOfNodes ; j++) {
        int index = node_to_phase_map[j];
        for (int i = 0 ; i < _numControls ; i++) {
            x_lb(u_inds(i, j)) = u_lb[index](i);
            x_ub(u_inds(i, j)) = u_ub[index](i);
        }
    }

    // initial and final STATES values if required
    for (int i = 0; i < _numStates; i++) {

        if (_flagInitialState(i)) {
            x_lb(y_inds(i, 0)) = y_0(i);
            x_ub(y_inds(i, 0)) = y_0(i);
        }

        if (_flagFinalState(i)) {
            //ToDo : Add relaxation flag
            int last_node = _numberOfNodes - 1;
            x_lb(y_inds(i, last_node)) = y_f(i) - 0.01; //- (0.05 * fabs(y_f(i)));
            x_ub(y_inds(i, last_node)) = y_f(i) + 0.01; //(0.05 * fabs(y_f(i)));
        }
    }

    // initial and final CONTROL values if required
    for (int i = 0 ; i < _numControls ; i++) {

            if (_flagInitialControl(i)) {
                x_lb(u_inds(i,0)) = u_0(i);
                x_ub(u_inds(i,0)) = u_0(i);
            }
            if (_flagFinalControl(i)) {
                int last_node = _numberOfNodes - 1;
                x_lb(u_inds(i,last_node)) = u_f(i); // add relaxation
                x_ub(u_inds(i,last_node)) = u_f(i); // add relaxation
            }
    }

    myNLP.SetVariablesBounds(x_lb,x_ub);
}

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::SetupJacobianVariables() {

    setNLPnnzConstraintJacobian();

    setSparsityJacobianConstraint();

    setSparsityJacobianCost();
}

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::setNLPnnzConstraintJacobian() {

    int nnz_jac_g = 0;

    // Nnz of defect constraints
    // each defect is dependent on 5 variables: h, y_k, y_kp1, u_k, u_kp1,
    int nnz_defects = this->num_defects * (1 + 2 * this->_numStates + 2 * this->_numControls);
    nnz_jac_g += nnz_defects;

    // Nnz of parametric constraints
    // time constraint depends on all increments
    int nnz_parametric_constraints = this->n_inc;

    if (this->_evenTimeIncr) {
        //each equality-increment constraint depends on two increments
        nnz_parametric_constraints += 2 * (this->n_inc - 1);
    }
    nnz_jac_g += nnz_parametric_constraints;

    // Nnz of USER constraints
    this->nnz_user_constraints = this->getNumNonZeroUserJacobian();
    nnz_jac_g += this->nnz_user_constraints;

    // Nnz of GUARD constraints
    int nnz_guard_constraints = 0;
    int nnz_interphase_constraints = 0;
    if(this->_multiphaseproblem) {
        nnz_guard_constraints = this->getNumNonZeroGuardFunctionJacobian();
        nnz_jac_g += nnz_guard_constraints;

        nnz_interphase_constraints= this->getNumNonZeroInterphaseJacobian();
        nnz_jac_g += nnz_interphase_constraints;
    }

    myNLP.SetGSize(nnz_jac_g);

    if (this->_verbose) {
        std::cout   << "[VERBOSE] Nnz_Defects : " << nnz_defects << std::endl
                    << "[VERBOSE] nnz_parametric_constraints : "<< nnz_parametric_constraints << std::endl
                    << "[VERBOSE] nnz_user_constraints : " << this->nnz_user_constraints << std::endl
                    << "[VERBOSE] nnz_guard_constraints : " << nnz_guard_constraints << std::endl
                    << "[VERBOSE] nnz_interPhase_const : " << nnz_interphase_constraints << std::endl
                    << "[VERBOSE] Total nnz in Jacobian : " << nnz_jac_g << std::endl;
                    std::getchar();
    }
}

template<typename DIMENSIONS>
int DTOMethodInterface<DIMENSIONS>::getNumNonZeroUserJacobian() {

    int user_non_zero_G = 0;

    for (int i = 0 ; i < this->num_phases ; i++) {
        int num_nonzero_jacobian_user_per_node = _constraints[i]->getNnzJac();
        this->num_nonzero_jacobian_user_per_node_in_phase.push_back(num_nonzero_jacobian_user_per_node);
        int user_non_zero_this_phase = num_nonzero_jacobian_user_per_node * this->number_nodes_phase[i];
        this->user_non_zero_G_phase.push_back(user_non_zero_this_phase);
        user_non_zero_G += user_non_zero_this_phase;
    }

    return user_non_zero_G;
}

template<typename DIMENSIONS>
int DTOMethodInterface<DIMENSIONS>::getNumNonZeroGuardFunctionJacobian() {

    // guard functions only depend on state vectors at particular locations
    // and are scalar functions
    int guard_nonZeroElements = num_guards * _numStates;

    return guard_nonZeroElements;
}

template<typename DIMENSIONS>
int DTOMethodInterface<DIMENSIONS>::getNumNonZeroInterphaseJacobian() {

    int nnz_interphase = 0;
    for (int i = 0 ; i < this->num_phases - 1; i++) {
        nnz_interphase += _interphase_constraint[i]->GetNnzJacobian();
    }
    return nnz_interphase;
}

template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::setSparsityJacobianCost() {

    // states and controls at N nodes, plus increments (N-1)
    setNLPnnzCostFunctionJacobian();

    int g_index = 0;
    int inc_index = 0;
    int index_of_fist_node = 0;

    // sparsity given all the nodes but last one

    for(int p = 0 ; p < num_phases ; p++) {
        for (int k = 0 ; k < number_nodes_phase[p] - 1 ; k++) {
            myNLP.jG_cost(g_index) = h_inds(inc_index);
            g_index++;
            myNLP.jG_cost.segment(g_index,this->_numStates)   = y_inds.col(index_of_fist_node + k);
            g_index += this->_numStates;
            myNLP.jG_cost.segment(g_index,this->_numControls) = u_inds.col(index_of_fist_node + k);
            g_index += this->_numControls;
            inc_index++;
        }
        index_of_fist_node +=number_nodes_phase[p];

    }
    //sparsity of the last node of the trajectory
    myNLP.jG_cost.segment(g_index,this->_numStates)     = y_inds.col(index_of_fist_node-1);
    g_index += this->_numStates;
//  myNLP.jG_cost.segment(g_index,this->_numControls)   = u_inds.col(index_of_fist_node-1);
//  g_index += this->_numControls;



    if (_verbose) {
        std::cout << "[VERBOSE] lenG_cost : " << myNLP.lenG_cost << std::endl;
        std::cout << "[VERBOSE] jG_cost.size() : " << myNLP.jG_cost.size() << std::endl;
        std::cout << "[VERBOSE] jG_cost : " << myNLP.jG_cost.transpose() << std::endl;
        std::getchar();
    }
}

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::setNLPnnzCostFunctionJacobian() {

    //ToDo : Read this from _costFunction, but it depends on all states, controls and increments
    int cost_nonZeroElements = n_inc * (_numStates + _numControls) + _numStates + n_inc;
            // (_numberOfNodes - (this->num_phases-1)) * (_numStates + _numControls) + n_inc;

    myNLP.setGcostSize(cost_nonZeroElements);
}

/* System interface */

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::UpdateDecisionVariables(const double* x) {

    // mapping decision variables
    const Eigen::Map<Eigen::VectorXd> x_local(
            const_cast<double *>(x), myNLP.num_vars);

    UpdateDTOTrajectoriesFromDecisionVariables(x_local);

    //update all the dynamics and other structures before continuing
    for(int k = 0 ; k < this->_numberOfNodes ; k++) {
        _ydot_trajectory[k] = this->_dynamics[node_to_phase_map[k]]->systemDynamics(_y_trajectory[k],_u_trajectory[k]);
    }

    _costFunction->setCurrentTrajectory(
            _y_trajectory, _u_trajectory, _h_trajectory, leftphase_nodes);
}

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::getCompleteSolution(state_vector_array_t & yTraj,
                               state_vector_array_t & ydotTraj, control_vector_array_t & uTraj,
                               Eigen::VectorXd & hTraj, std::vector<int> & phaseId) const {

    int inc = 0;
    int node = 0;
    int phase_first_node = 0;
    std::vector<double> h_inc;

      for(int p = 0; p < this->num_phases ; p++ ){

          for(int n = 0 ; n < this->number_nodes_phase[p] - 1 ; n++) {

            node = phase_first_node + n;

            //compute the center point and its derivative
            state_vector_t y_mid = 0.5*(this->_y_trajectory[node] + this->_y_trajectory[node+1])
                + 0.125*this->_h_trajectory(inc)*(this->_ydot_trajectory[node] - this->_ydot_trajectory[node+1]);

            state_vector_t ymid_dot = (1.5/this->_h_trajectory(inc))*(this->_y_trajectory[node+1] - this->_y_trajectory[node])
                - 0.25*(this->_ydot_trajectory[node] + this->_ydot_trajectory[node+1]);

            control_vector_t u_mid = 0.5*(_u_trajectory[node] + _u_trajectory[node+1]);

            yTraj.push_back(_y_trajectory[node]);
            yTraj.push_back(y_mid);

            ydotTraj.push_back(_ydot_trajectory[node]);
            ydotTraj.push_back(ymid_dot);

            h_inc.push_back(_h_trajectory(inc) / 2);
            h_inc.push_back(_h_trajectory(inc) / 2);

            phaseId.push_back(node_to_phase_map[node]);
            phaseId.push_back(node_to_phase_map[node]);

            uTraj.push_back(_u_trajectory[node]);
            uTraj.push_back(u_mid);

            inc++;
        }

      yTraj.push_back(this->_y_trajectory[node+1]);
      phaseId.push_back(node_to_phase_map[node+1]);
      ydotTraj.push_back(this->_ydot_trajectory[node+1]);
      uTraj.push_back(_u_trajectory[node+1]);

      phase_first_node += this->number_nodes_phase[p];
  }

  hTraj = Eigen::Map<Eigen::VectorXd>(h_inc.data(),h_inc.size());

}

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::evalObjective(double *f) {

    *f = _costFunction->evaluate();
}

template<typename DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::evalConstraintFct(
        double* f_constraint_pointer,
        const int & constraint_pointer_size) {

    // mapping the output
    Eigen::Map<Eigen::VectorXd> constraints_fct(
            f_constraint_pointer, constraint_pointer_size);

    evaluateDefects(defect_out_);
    constraints_fct.segment(0, num_defects) = defect_out_;

    evaluateParametricConstraints(parametric_out_);
    constraints_fct.segment(num_defects, num_parametric_constraints) = parametric_out_;

    if(num_user_constraints > 0) {
        evaluateUserConstraints(user_constraints_out_);
        constraints_fct.segment(num_defects + num_parametric_constraints, num_user_constraints) = user_constraints_out_;
    }

    if (_multiphaseproblem) {

        evaluateGuardConstraints(guard_vector_);
        constraints_fct.segment(num_defects + num_parametric_constraints + num_user_constraints, num_guards) = guard_vector_;

        evaluateInterphaseConstraints(interface_vector_);
        constraints_fct.segment(num_defects+num_parametric_constraints+num_user_constraints+num_guards, num_interphase_constraints) = interface_vector_;
    }
}

template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::evaluateParametricConstraints(
        Eigen::VectorXd & parametric_vector) {

  parametric_vector.setZero();

    for (int i = 0; i < n_inc; i++) {
        parametric_vector(0) += _h_trajectory(i);
    }

    if (_evenTimeIncr) {
        for (int i = 0; i < n_inc - 1; i++) {
            parametric_vector(1 + i) =
                    _h_trajectory(i) - _h_trajectory(i + 1);
        }
    }
}

template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::evaluateUserConstraints(
        Eigen::VectorXd & constraints_vector) {

  constraints_vector.setZero();

    int current_row = 0;
    for (int k = 0; k < _numberOfNodes; k++) {

        int loca_num_constraints = this->num_user_constraints_per_node_in_phase[this->node_to_phase_map[k]];

        // at each point constraints depends on the whole trajectory
        // ToDo : Add parameters
        Eigen::VectorXd node_constraint = _constraints[this->node_to_phase_map[k]]->evalConstraintsFct(
                _y_trajectory[k], _u_trajectory[k]);

        constraints_vector.segment(current_row,
                loca_num_constraints) = node_constraint;

        current_row += loca_num_constraints;
    }
}

template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::evaluateGuardConstraints(
        Eigen::VectorXd & guardConstraint) {

  //guards only occur at the "leftphase nodes"
  guardConstraint.setZero();

    int count = 0;
    for (int it = 0 ; it < num_phases -1 ; it++ ) {
        guardConstraint.segment(count,size_guard[it]) = _guards[it]->evalConstraintsFct(_y_trajectory[leftphase_nodes(it)]);
        count += size_guard[it];
    }
}

template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::evaluateInterphaseConstraints(
        Eigen::VectorXd &interphaseVector) {

  interphaseVector.setZero();

    int index = 0;
    for(int i = 0 ; i < num_phases -1 ; i++ ) {
        interphaseVector.segment(index,size_interphase_constraint[i]) =
        _interphase_constraint[i]->evalConstraintsFct(
                _y_trajectory[leftphase_nodes(i)],_y_trajectory[rightphase_nodes(i)],
                _u_trajectory[leftphase_nodes(i)],_u_trajectory[rightphase_nodes(i)]);

        index += size_interphase_constraint[i];
    }

}

template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::evalSparseJacobianCost(double* val) {

    Eigen::Map<Eigen::VectorXd> jacobcost(val,myNLP.lenG_cost);

    jacobcost = _costFunction->evaluate_gradient();
}

//TODO : Deprecated I dont think we need this anymore
template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::getStateControlIncrementsTrajectoriesFromDecisionVariables(
        const Eigen::Map<Eigen::VectorXd> & x_local,
        state_vector_array_t & y_trajectory,
        control_vector_array_t & u_trajectory, Eigen::VectorXd & h_trajectory) {

    h_trajectory = x_local.segment(0, n_inc);

    y_trajectory.resize(_numberOfNodes);
    u_trajectory.resize(_numberOfNodes);

    for (int k = 0; k < _numberOfNodes; k++) {
        y_trajectory[k] = x_local.segment<DIMENSIONS::kTotalStates>(y_inds(0,k));
        u_trajectory[k] = x_local.segment<DIMENSIONS::kTotalControls>(u_inds(0,k));
    }
}

//TODO : Deprecated I dont think we need this anymore
template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::getStateControlIncrementsTrajectoriesFromDecisionVariables(
        const Eigen::VectorXd & x_local,
        state_vector_array_t & y_trajectory,
        control_vector_array_t & u_trajectory,
        Eigen::VectorXd & h_trajectory) {


    // To Do: We know these sizes from the very beginning!!! use
    //        member variables instead
    //h_trajectory.resize(n_inc);

    h_trajectory = x_local.segment(0, n_inc);

    y_trajectory.resize(_numberOfNodes);
    u_trajectory.resize(_numberOfNodes);

    for (int k = 0; k < _numberOfNodes; k++) {

        y_trajectory[k] = x_local.segment
                < DIMENSIONS::DimensionsSize::STATE_SIZE>(y_inds(0,k));
                //> (n_inc + (k * _numStates));

        u_trajectory[k] = x_local.segment< DIMENSIONS::DimensionsSize::CONTROL_SIZE>(u_inds(0,k));
                //> (n_inc + aux + (k * _numControls));
    }
}

template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::UpdateDTOTrajectoriesFromDecisionVariables(
        const Eigen::Map<Eigen::VectorXd> & x_local) {

    _h_trajectory = x_local.segment(0, n_inc);

    for (int k = 0; k < _numberOfNodes; k++) {

        _y_trajectory[k] = x_local.segment
                < DIMENSIONS::DimensionsSize::STATE_SIZE>(y_inds(0,k));
        _u_trajectory[k] = x_local.segment
                < DIMENSIONS::DimensionsSize::CONTROL_SIZE>(u_inds(0,k));
    }
}

template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::SetDecisionVariablesFromStateControlIncrementsTrajectories(
        Eigen::VectorXd & x_vector,
        const state_vector_array_t & y_sol,
        const control_vector_array_t & u_sol,
        const Eigen::VectorXd & h_sol) {

  x_vector.resize(this->myNLP.num_vars);

    int solution_nodes = y_sol.size();

    if (solution_nodes != _numberOfNodes) {
        std::cout << "Wrong size of initialization solution!" << std::endl;
        std::exit (EXIT_FAILURE);
    }

    x_vector.segment(0, n_inc) = h_sol;

    int aux = _numberOfNodes * _numStates;
    for (int k = 0; k < _numberOfNodes; k++) {
        x_vector.segment< DIMENSIONS::kTotalStates> (n_inc + (k * _numStates)) = y_sol[k];
        x_vector.segment< DIMENSIONS::kTotalControls>(n_inc + aux + (k * _numControls)) = u_sol[k];
    }
}

// utilities
template<class DIMENSIONS>
bool DTOMethodInterface<DIMENSIONS>::ValidateDuration() {

    if (_trajTimeRange[0] < min_traj_time
            || _trajTimeRange[1] < min_traj_time) {
        std::cout << "Minimum trajectory time is " << min_traj_time
                << std::endl;
        return false;
    } else if (_trajTimeRange[1] < _trajTimeRange[0]) {
        std::cout
                << "please use [t0 tf] to specify range of the trajectory time"
                << std::endl;
        return false;
    } else if (_verbose) {
        std::cout << "[VERBOSE] Time range : ok " << std::endl;
    }

    return true;
}

template<class DIMENSIONS>
bool DTOMethodInterface<DIMENSIONS>::ValidateStateControlBounds() {

    // if not provived set to infinity
    if (y_lb.empty() || y_ub.empty()) {
        state_vector_t lb, ub;
        lb.setConstant(-infy);
        ub.setConstant(infy);

        SetStateBounds(lb, ub);
    }
    if (u_lb.empty() || u_ub.empty()) {
        control_vector_t lb, ub;
        lb.setConstant(-infy);
        ub.setConstant(infy);

        SetControlBounds(lb, ub);
    }

    // upper bounds should be greater than lower bounds
    state_vector_t error_s;
    error_s.setConstant(0);
    for (int i = 0; i < num_phases; i++) {
        error_s += y_ub[i] - y_lb[i];
    }
    control_vector_t error_c;
    error_c.setConstant(0);
    for (int i = 0; i < num_phases; i++) {
        error_c += u_ub[i] - u_lb[i];
    }

    if (_verbose) {
        std::cout << " [VERBOSE] states bounds : " << std::endl;
        for(int i = 0; i < num_phases; i++) {
            std::cout << "ylb : " << y_lb[i].transpose() << std::endl;
            std::cout << "yub : " << y_ub[i].transpose() << std::endl;
        }

        std::cout << " [VERBOSE] control bounds: " << std::endl;
        for(int i = 0; i < num_phases; i++) {
            std::cout << "ulb : " << u_lb[i].transpose() << std::endl;
            std::cout << "uub : "<< u_ub[i].transpose() << std::endl;
        }
    }

    if ((error_s.array() < 0).any() || (error_c.array() < 0).any()) {
        std::cout << "Lower and Upper bounds are not consistent!" << std::endl;
        return false;
    }

    return true;
}

/* check w.r.t initialization of solver */
template<class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::InitializeNLPvector(Eigen::VectorXd & x,
        const state_vector_array_t & state_initial,
        const control_vector_array_t & control_initial,
        const Eigen::VectorXd & h_initial) {

    int total_state_indexes = _numberOfNodes * _numStates + n_inc;

    x.segment(0, n_inc) = h_initial;

    for (int i = 0; i < _numberOfNodes; i++) {
        int node_index = i * _numStates;

        for (int j = 0; j < _numStates; j++) {
            x(n_inc + node_index + j) = state_initial[i](j);
        }

        node_index = i * _numControls;

        for (int j = 0; j < _numControls; j++) {
            x(total_state_indexes + node_index + j) = control_initial[i](j);
        }
    }
    if (_verbose) {
        std::cout << "[VERBOSE] state_initial : " << state_initial << std::endl;
        std::cout << "[VERBOSE] control_initial : " << control_initial << std::endl;
        std::cout << "[VERBOSE] h_initial : " << h_initial.transpose() << std::endl;
        std::cout << "[VERBOSE] x : " << x.transpose() << std::endl;
    }
}

template <class DIMENSIONS>
void DTOMethodInterface<DIMENSIONS>::setSparsityJacobianConstraint(){

    // At this point these vectors should be at the right size
    myNLP.iGfun.setZero();
    myNLP.jGvar.setZero();

    int constraint_row = 0;
    int nnz_per_row = 0;
    int G_index = 0; // current index of the sparsity structure

    /* Indexes of defects */
        // each (this->_numStates-dimensional) defect is dependent on h_k, y_k, y_kp1, u_k, u_kp1 ...
        // this is for the "worst" case, when every 1-dim defect is dependent on the whole state and constraint vector y_k, y_kp1, u_k, u_kp1 and h

        nnz_per_row = this->_numStates*2 + this->_numControls*2 + 1; //+1 because of h

        int inc_index = 0;
        int phase_first_node = 0;
        int node = 0;

        // scanning by phase
        for (int p = 0 ; p < this->num_phases ; p++) {
            // all the v-defects in this phase
            for(int i = 0 ; i < this->number_nodes_phase[p] - 1 ; i++) {

                node = phase_first_node + i;
                // all the states in this v-defect
                for (int jj = 0 ; jj < this->_numStates ; jj++) {

                    myNLP.iGfun.segment(G_index,nnz_per_row) = Eigen::VectorXi::Constant(nnz_per_row, constraint_row);

                    //w.r.t h
                    myNLP.jGvar(G_index) = this->h_inds(inc_index);
                    G_index += 1;

                    //w.r.t y_k
                    myNLP.jGvar.segment(G_index, this->_numStates) = this->y_inds.col(node);
                    G_index += this->_numStates;
                    //w.r.t y_kp1s
                    myNLP.jGvar.segment(G_index, this->_numStates) = this->y_inds.col(node+1);
                    G_index += this->_numStates;

                     //w.r.t u_k
                    myNLP.jGvar.segment(G_index, this->_numControls) = this->u_inds.col(node);
                    G_index += this->_numControls;
                    //w.r.t u_kp1
                    myNLP.jGvar.segment(G_index, this->_numControls) = this->u_inds.col(node+1);
                    G_index += this->_numControls;

                    constraint_row++;
                }
                // next v-defect, i.e., new h_index
                inc_index++;
            }

            // new phase, i.e., new first node (skipping defect between last node of this phase and next node)
            phase_first_node += this->number_nodes_phase[p];
        }

        if (constraint_row != this->num_defects) {
            std::cout << "Error creating the indexes of constraint Jacobian" << std::endl;
            exit(EXIT_FAILURE);
        }

    /* Indexes of parametric constraints */

        // Time constraint
        nnz_per_row = this->n_inc;

        myNLP.iGfun.segment(G_index,nnz_per_row) = Eigen::VectorXi::Constant(this->n_inc,constraint_row);
        myNLP.jGvar.segment(G_index,this->n_inc) = Eigen::VectorXi::LinSpaced(this->n_inc,0,this->n_inc-1);

        G_index += this->n_inc;
        constraint_row++;

        // increments constraint
        nnz_per_row = 2;
        if (this->_evenTimeIncr) {
            for (int k = 0 ; k < this->n_inc -1 ; k++) {
                myNLP.iGfun.segment(G_index,nnz_per_row) = Eigen::VectorXi::Constant(nnz_per_row,constraint_row);

                Eigen::VectorXi aux_index(2);
                aux_index << this->h_inds(k) , this->h_inds(k+1);

                myNLP.jGvar.segment(G_index,nnz_per_row) = aux_index;

                G_index+=nnz_per_row;

                constraint_row++;
            }
        }

        if (constraint_row != this->num_defects + this->num_parametric_constraints) {
            std::cout << "Error creating the indexes of Jacobian of constraints" << std::endl;
            exit(EXIT_FAILURE);
        }

    // Indexes of User Constraints

        //ToDO: assuming user constraints depend on ALL states and controls but NOT on h

        int constant_offset = 0;
        int constraints_before_user_constraints = constraint_row;

        // arrange iGfun and jGvar for each node
        for (int k = 0 ; k < this->_numberOfNodes ; k++) {

            // relative user's constraint rows (e.g, 192)
            Eigen::VectorXi iRow_user_per_node = this->_constraints[node_to_phase_map[k]]->getSparseRow();

            // auxiliary vector for adding offset
            Eigen::VectorXi offsetRow = Eigen::VectorXi::Constant(this->num_nonzero_jacobian_user_per_node_in_phase[node_to_phase_map[k]],
                    constraints_before_user_constraints);

            // adding offset,
            iRow_user_per_node = iRow_user_per_node + offsetRow;

            Eigen::VectorXi node_offset = Eigen::VectorXi::Constant(this->num_nonzero_jacobian_user_per_node_in_phase[node_to_phase_map[k]],
                    constant_offset);

            constant_offset += this->num_user_constraints_per_node_in_phase[node_to_phase_map[k]];

            Eigen::VectorXi iRow_user_updated = iRow_user_per_node + node_offset;

            //the number of the constraints changes at each node
            myNLP.iGfun.segment(G_index,this->num_nonzero_jacobian_user_per_node_in_phase[node_to_phase_map[k]]) = iRow_user_updated;

            int state_dependent_row = 0;
            for (int state = 0 ; state < this->_numStates ; state++) {
                // Repetition of the States
                int current_index = G_index + state_dependent_row;
                myNLP.jGvar.segment(current_index,this->num_user_constraints_per_node_in_phase[node_to_phase_map[k]]).setConstant(this->y_inds(state,k));
                state_dependent_row += this->num_user_constraints_per_node_in_phase[node_to_phase_map[k]];
            }
            for (int control = 0 ; control < this->_numControls ; control++) {
                // Repetition of the Controls
                int current_index = G_index + state_dependent_row;
                myNLP.jGvar.segment(current_index,this->num_user_constraints_per_node_in_phase[node_to_phase_map[k]]).setConstant(this->u_inds(control,k));
                state_dependent_row += this->num_user_constraints_per_node_in_phase[node_to_phase_map[k]];
            }

            if ((state_dependent_row) != this->num_nonzero_jacobian_user_per_node_in_phase[node_to_phase_map[k]]) {
                std::cout << "**total added rows should be equal to num_nonzero_jacobian_user_per_node!" << std::endl;
                std::cout << "total added rows :  " << state_dependent_row << std::endl;
                std::cout << "num_nonzero_jacobian_user_per_node : " << this->num_nonzero_jacobian_user_per_node_in_phase[node_to_phase_map[k]] << std::endl;

                std::exit(EXIT_FAILURE);
            }

            G_index+=this->num_nonzero_jacobian_user_per_node_in_phase[node_to_phase_map[k]];

            constraint_row+=this->num_user_constraints_per_node_in_phase[node_to_phase_map[k]];
        }

        if (constraint_row != this->num_defects + this->num_parametric_constraints + this->num_user_constraints) {
            std::cout << "*** ERROR: wrong number of nonzero jacobian elements of G [USER - Sparsity]" << std::endl;
            exit(EXIT_FAILURE);
        }

        if (this->_multiphaseproblem) {

            /* Guard Constraints */
            // each guard constraint depends on the states at the given point
            nnz_per_row = this->_numStates;
            //Eigen::VectorXi jCol_guard(nnz_per_row);

            // at each phase border there are two guards constraints
            // now the guard is used-defined size
            for (int i = 0; i < this->num_phases - 1; i++) {

              for(int row = 0 ; row < this->size_guard[i]; row ++) {

          // left side
          myNLP.iGfun.segment(G_index,nnz_per_row).setConstant(constraint_row);
          myNLP.jGvar.segment(G_index,nnz_per_row) = y_inds.col(leftphase_nodes(i));
          constraint_row +=1;
          G_index += nnz_per_row;
              }

//        for(int row = 0 ; row < this->size_guard[i]; row ++) {
//
//          //right side (consider removing)
//          myNLP.iGfun.segment(G_index,nnz_per_row).setConstant(constraint_row);
//          myNLP.jGvar.segment(G_index,nnz_per_row) = y_inds.col(rightphase_nodes(i));
//          constraint_row += 1;
//          G_index += nnz_per_row;
//
//        }
            }

            /* Interphase constraints */

            // at each phase border there is one vector-size 'InterPhase' constraint
            // the size of the InterPhase constraint is user-provided
            // each constraint depends on the state and control of k and k+1
            nnz_per_row = 2 * this->_numStates + 2* this->_numControls;

            // NOW assuming Interphase constraints depend on states and controls!

            Eigen::VectorXi x_f_v(_numStates);
            Eigen::VectorXi u_f_v(_numControls);
            Eigen::VectorXi x_s_v(_numStates);
            Eigen::VectorXi u_s_v(_numControls);

            for(int i = 0 ; i < this->num_phases -1; i++) {

                x_f_v = y_inds.col(leftphase_nodes(i));
                u_f_v = u_inds.col(leftphase_nodes(i));

                x_s_v = y_inds.col(rightphase_nodes(i));
                u_s_v = u_inds.col(rightphase_nodes(i));

                for(int c_row = 0 ;  c_row < this->size_interphase_constraint[i]; c_row++) {

                    myNLP.iGfun.segment(G_index,nnz_per_row).setConstant(constraint_row);

                    myNLP.jGvar.segment(G_index,_numStates) = x_f_v;
                    G_index += _numStates;
                    myNLP.jGvar.segment(G_index,_numControls) = u_f_v;
          G_index += _numControls;

                    myNLP.jGvar.segment(G_index,_numStates) = x_s_v;
                    G_index += _numStates;
                    myNLP.jGvar.segment(G_index,_numControls) = u_s_v;
          G_index += _numControls;

                    constraint_row++;
                }
            }
        }

    if (G_index != myNLP.lenG) {
        std::cout << "lenG = " << myNLP.lenG << "G_index = " << G_index << std::endl;
        std::cout << "*** ERROR: wrong number of nonzero jacobian elements of G [USER - Sparsity]" << std::endl;
        exit(EXIT_FAILURE);
    }

    if (this->_verbose) {
        std::cout
                    << "[VERBOSE] lenG : " << myNLP.lenG << std::endl
                    << "[VERBOSE] G_index : " << G_index << std::endl
                    << "[VERBOSE] constraint_row : " << constraint_row << std::endl
                    << "[VERBOSE] jGvar.size() : " << myNLP.jGvar.size() << std::endl
                    << "[VERBOSE] iGfun.size() : " << myNLP.iGfun.size() << std::endl;
                    getchar();
    }
}

} // namespace DirectTrajectoryOptimization