text: // Copyright (c) 2012 Pierre MOULON.
  
  // This Source Code Form is subject to the terms of the Mozilla Public
  // License, v. 2.0. If a copy of the MPL was not distributed with this
  // file, You can obtain one at http://mozilla.org/MPL/2.0/.
  
  #ifdef I23DSFM_HAVE_MOSEK
  
  #include "i23dSFM/linearProgramming/linearProgrammingMOSEK.hpp"
  #include <iostream>
  
  namespace i23dSFM {
  namespace linearProgramming {
  
  using namespace std;
  
  // This function prints log output from MOSEK to the terminal.
  static void MSKAPI printstr(void *handle,
                              char str[])
  {
    std::cout << str;
  }
  
  MSKenv_t     env = NULL;
  
  MOSEK_SolveWrapper::MOSEK_SolveWrapper(int nbParams) : LP_Solver(nbParams)
  {
    task = NULL;
    //env  = NULL;
  
    //-- Initialize MOSEK framework
  
    // Create the mosek environment.
    /*MSKrescodee r = MSK_makeenv(&env,NULL,NULL,NULL,NULL);
    if ( r!=MSK_RES_OK )  {
      std::cerr << "Cannot create the MOSEK environment" << std::endl;
    }*/
    if (env == NULL)
    {
       MSKrescodee r = MSK_makeenv(&env,NULL,NULL,NULL,NULL);
  
      // Directs the env log stream to the 'printstr' function.
      if ( r==MSK_RES_OK )
        MSK_linkfunctoenvstream(env,MSK_STREAM_LOG,NULL,printstr);
  
      // Initialize the environment.
      if ( r==MSK_RES_OK )
        r = MSK_initenv(env);
      else  {
        std::cerr << "Cannot create the MOSEK environment" << std::endl;
      }
    }
  
  }
  
  MOSEK_SolveWrapper::~MOSEK_SolveWrapper()
  {
    // Memory cleaning.
    MSK_deletetask(&task);
    //MSK_deleteenv(&env);
  }
  
  
  inline MSKboundkey_enum convertSign(LP_Constraints::eLP_SIGN sign) {
  
    switch(sign) {
      case LP_Constraints::LP_LESS_OR_EQUAL:    // = 1,  // cst (<=) VAL
        return MSK_BK_UP;
      case LP_Constraints::LP_GREATER_OR_EQUAL: // = 2,  // cst (>=) VAL
        return MSK_BK_LO;
      case LP_Constraints::LP_EQUAL:            // = 3,  // cst (=) VAL
        return MSK_BK_FX;
      case LP_Constraints::LP_FREE:
        return MSK_BK_FR;
      default:
        std::cerr << "Error unknow constraint sign : " << sign << "\n";
    }
  }
  
  bool MOSEK_SolveWrapper::setup(const LP_Constraints & cstraints) //cstraints <-> constraints
  {
    assert(_nbParams == cstraints._nbParams);
  
    MSK_deletetask(&task);
  
    int NUMVAR = cstraints._constraintMat.cols();
    int NUMCON = cstraints._constraintMat.rows();
    int NUMANZ = cstraints._constraintMat.cols() * cstraints._constraintMat.rows(); //DENSE MATRIX
  
    MSKrescodee r = MSK_RES_OK;
    if ( r==MSK_RES_OK )
    {
      // Create the optimization task.
      r = MSK_maketask(env,NUMCON,NUMVAR,&task);
  
      // Directs the log task stream to the 'printstr' function.
      if ( r==MSK_RES_OK )
        MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr);
  
      // Give MOSEK an estimate of the size of the input data.
      //This is done to increase the speed of inputting data.
      // However, it is optional.
      if (r == MSK_RES_OK)
        r = MSK_putmaxnumvar(task,NUMVAR);
  
      if (r == MSK_RES_OK)
        r = MSK_putmaxnumcon(task,NUMCON);
  
      if (r == MSK_RES_OK)
        r = MSK_putmaxnumanz(task,NUMANZ);
  
      // Append 'NUMCON' empty constraints. The constraints will initially have no bounds.
      if ( r == MSK_RES_OK )
        r = MSK_append(task,MSK_ACC_CON,NUMCON);
  
      // Append 'NUMVAR' variables. The variables will initially be fixed at zero (x=0).
      if ( r == MSK_RES_OK )
        r = MSK_append(task,MSK_ACC_VAR,NUMVAR);
    }
  
    this->_nbParams = NUMVAR;
  
    if (cstraints._bminimize) {
      r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MINIMIZE);
    }
    else  {
      r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MAXIMIZE);
    }
  
    // Optionally add a constant term to the objective.
    if ( r ==MSK_RES_OK )
      r = MSK_putcfix(task,0.0);
  
    // Add the objective function if any
    if (!cstraints._vec_cost.empty())  {
      // Set objective
      for (size_t i = 0; i < cstraints._vec_cost.size(); ++i)
        MSK_putcj(task, i, cstraints._vec_cost[i]);
    }
  
    //Add constraint row by row
    const Mat & A = cstraints._constraintMat;
  
    for (int i=0; i<A.rows() && r == MSK_RES_OK; ++i)
    {
      std::vector<int> vec_colno(A.cols(), 0);
      for (int ii=0; ii<A.cols(); ++ii){
        vec_colno[ii] = ii;
      }
  
      // Insert a row
      Vec temp = A.row(i);
      r = MSK_putavec(task,
                      MSK_ACC_CON,       // Input row of A.
                      MSKidxt (i),       // Variable row index.
                      vec_colno.size(),  // Number of non-zeros in row(i) A.
                      &vec_colno[0],     // Pointer to row indexes of row i.
                      (double*)temp.data());    // Pointer to Values of row i.
    }
  
    //Add bound on variables:
    if (cstraints._vec_bounds.size() == 1) {
      for (size_t i = 0; i < NUMVAR; ++i) {
        if (r == MSK_RES_OK)
          r = MSK_putbound(task,
                           MSK_ACC_VAR, // Put bounds on variables.
                           i,           // Index of variable.
                           convertSign(cstraints._vec_sign[0]),      // Bound key.
                           cstraints._vec_bounds[0].first,  // Numerical value of lower bound.
                           cstraints._vec_bounds[0].second); // Numerical value of upper bound.
      }
    }
    else{
      for (size_t i = 0; i < NUMVAR; ++i) {
      // Set the bounds on variable j.
      //    lowerbound <= x_j <= upper bound
        if (r == MSK_RES_OK)
          r = MSK_putbound(task,
                           MSK_ACC_VAR, // Put bounds on variables.
                           i,           // Index of variable.
                           convertSign(cstraints._vec_sign[i]),      // Bound key.
                           cstraints._vec_bounds[i].first,  // Numerical value of lower bound.
                           cstraints._vec_bounds[i].second); // Numerical value of upper bound.
      // in order to add sparse bounds use: MSK_putboundlist
      }
    }
  
    // Add bounds on constraint
    for (size_t i=0; i<NUMCON && r==MSK_RES_OK; ++i)
    {
      r = MSK_putbound(task,
                       MSK_ACC_CON, // Put bounds on constraints.
                       i,           // Index of constraint.
                       convertSign(cstraints._vec_sign[i]),      // Bound key.
                       -MSK_INFINITY,  // Numerical value of lower bound.
                       cstraints._Cst_objective(i)); // Numerical value of upper bound.
    }
  
    return r == MSK_RES_OK;
  }
  
  bool MOSEK_SolveWrapper::setup(const LP_Constraints_Sparse & cstraints) //cstraints <-> constraints
  {
    assert(_nbParams == cstraints._nbParams);
  
    MSK_deletetask(&task);
  
    int NUMVAR = this->_nbParams;
    int NUMCON = cstraints._constraintMat.rows();
    int NUMANZ = cstraints._constraintMat.nonZeros();
  
    MSKrescodee r = MSK_RES_OK;
    if ( r==MSK_RES_OK )
    {
      // Directs the log task stream to the 'printstr' function.
      if ( r==MSK_RES_OK )
        MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr);
  
      // Create the optimization task.
      r = MSK_maketask(env,NUMCON,NUMVAR,&task);
  
      // Give MOSEK an estimate of the size of the input data.
      //This is done to increase the speed of inputting data.
      // However, it is optional.
      if (r == MSK_RES_OK)
        r = MSK_putmaxnumvar(task,NUMVAR);
  
      if (r == MSK_RES_OK)
        r = MSK_putmaxnumcon(task,NUMCON);
  
      if (r == MSK_RES_OK)
        r = MSK_putmaxnumanz(task,NUMANZ);
  
      // Append 'NUMCON' empty constraints. The constraints will initially have no bounds.
      if ( r == MSK_RES_OK )
        r = MSK_append(task,MSK_ACC_CON,NUMCON);
  
      // Append 'NUMVAR' variables. The variables will initially be fixed at zero (x=0).
      if ( r == MSK_RES_OK )
        r = MSK_append(task,MSK_ACC_VAR,NUMVAR);
    }
  
    this->_nbParams = NUMVAR;
  
    if (cstraints._bminimize) {
      r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MINIMIZE);
    }
    else  {
      r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MAXIMIZE);
    }
  
    // Optionally add a constant term to the objective.
    if ( r ==MSK_RES_OK )
      r = MSK_putcfix(task,0.0);
  
    // Add the objective function if any
    if (!cstraints._vec_cost.empty())  {
      // Set objective
      for (size_t i = 0; i < cstraints._vec_cost.size(); ++i)
        r = MSK_putcj(task, i, cstraints._vec_cost[i]);
    }
  
    //Add constraint row by row
    const sRMat & A = cstraints._constraintMat;
    std::vector<int> vec_colno;
    std::vector<double> vec_value;
    for (int i=0; i<A.rows(); ++i)
    {
      vec_colno.resize(0);
      vec_value.resize(0);
      for (sRMat::InnerIterator it(A,i); it; ++it)
      {
        vec_colno.push_back(it.col());
        vec_value.push_back(it.value());
      }
      // Insert a row
      r = MSK_putavec(task,
                      MSK_ACC_CON,      // Input row of A.
                      MSKidxt(i),       // Variable row index.
                      vec_colno.size(), // Number of non-zeros in row(i) A.
                      &vec_colno[0],    // Pointer to row indexes of row i.
                      &vec_value[0]);   // Pointer to Values of row i.
    }
  
    //Add bound on variables:
    if (cstraints._vec_bounds.size() == 1) {
  
      for( size_t i = 0; i < NUMVAR; ++i) {
        if(r == MSK_RES_OK)
          r = MSK_putbound(task,
                           MSK_ACC_VAR,                         // Put bounds on variables.
                           MSKidxt(i),                          // Index of variable.
                           MSK_BK_RA, // Bound key.
                           cstraints._vec_bounds[0].first,      // Numerical value of lower bound.
                           cstraints._vec_bounds[0].second);    // Numerical value of upper bound.
      }
    }
    else{
  
      for( size_t i = 0; i < NUMVAR; ++i) {
      // Set the bounds on variable j.
      //    lower bound <= x_j <= upper bound
  
        if(r == MSK_RES_OK)
          r = MSK_putbound(task,
                           MSK_ACC_VAR,                         // Put bounds on variables.
                           MSKidxt(i),                          // Index of variable.
                           MSK_BK_RA, // Bound key.
                           cstraints._vec_bounds[i].first,      // Numerical value of lower bound.
                           cstraints._vec_bounds[i].second);    // Numerical value of upper bound.
      // in order to add sparse bounds use: MSK_putboundlist
      }
    }
  
    // Add bounds on constraint
    for(size_t i=0; i<NUMCON && r==MSK_RES_OK; ++i)
    {
      MSKboundkey_enum cst = convertSign(cstraints._vec_sign[i]);
      if (cst == MSK_BK_UP || cst == MSK_BK_RA)
        r = MSK_putbound(task,
                       MSK_ACC_CON,   // Put bounds on constraints.
                       MSKidxt(i),    // Index of constraint.
                       cst,           // Bound key.
                       -MSK_INFINITY, // Numerical value of lower bound.
                       cstraints._Cst_objective(i));      //upper bound.
      else if (cst == MSK_BK_LO)
        r = MSK_putbound(task,
                       MSK_ACC_CON,   // Put bounds on constraints.
                       MSKidxt(i),    // Index of constraint.
                       cst,           // Bound key.
                       cstraints._Cst_objective(i),
                      +MSK_INFINITY);
      else if (cst == MSK_BK_FX)
        r = MSK_putbound(task,
                       MSK_ACC_CON,   // Put bounds on constraints.
                       MSKidxt(i),    // Index of constraint.
                       cst,           // Bound key.
                       cstraints._Cst_objective(i),
                       cstraints._Cst_objective(i));
    }
  
    return r == MSK_RES_OK;
  }
  
  bool MOSEK_SolveWrapper::solve()
  {
    MSKrescodee trmcode;
  
    // Run optimizer
    //MSK_putintparam(task,MSK_IPAR_OPTIMIZER,MSK_OPTIMIZER_DUAL_SIMPLEX);
    MSKrescodee r = MSK_optimizetrm(task,&trmcode);
  
    // Print a summary containing information
    // about the solution for debugging purposes.
    MSK_solutionsummary (task,MSK_STREAM_LOG);
  
    if ( r== MSK_RES_OK )
    {
      MSKsolstae solsta;
      MSK_getsolutionstatus (task,
                             MSK_SOL_BAS,
                             NULL,
                             &solsta);
      switch(solsta)
      {
        case MSK_SOL_STA_OPTIMAL:
        case MSK_SOL_STA_NEAR_OPTIMAL:{
          return true;
          break;
        }
        case MSK_SOL_STA_DUAL_INFEAS_CER:
        case MSK_SOL_STA_PRIM_INFEAS_CER:
        case MSK_SOL_STA_NEAR_DUAL_INFEAS_CER:
        case MSK_SOL_STA_NEAR_PRIM_INFEAS_CER:
          //printf("Primal or dual infeasibility certificate found.\n");
          break;
        case MSK_SOL_STA_UNKNOWN:
          //printf("The status of the solution could not be determined.\n");
          break;
        default:
          //printf("Other solution status.");
          break;
      }
    }
    else
    {
      printf("Error while optimizing.\n");
    }
    return false;
  }
  
  bool MOSEK_SolveWrapper::getSolution(std::vector<double> & estimatedParams)
  {
    bool bRet = false;
    MSKsolstae solsta;
    MSK_getsolutionstatus (task,
                             MSK_SOL_BAS,
                             NULL,
                             &solsta);
    switch(solsta)
    {
      case MSK_SOL_STA_OPTIMAL:
      case MSK_SOL_STA_NEAR_OPTIMAL:
        MSK_getsolutionslice(task,
                             MSK_SOL_BAS,    // Request the basic solution.
                             MSK_SOL_ITEM_XX,// Which part of solution.
                             0,              // Index of first variable.
                             estimatedParams.size(), // Index of last variable+1.
                             &estimatedParams[0]);
        bRet = true;
      //MSK_writedata(task,"taskdump.opf");
  
        /*printf("Optimal primal solution\n");
        for(size_t j=0; j<estimatedParams.size(); ++j)
          std::cout << estimatedParams[j] << " ";
        std::cout << std::endl;*/
  
        break;
      case MSK_SOL_STA_DUAL_INFEAS_CER:
      case MSK_SOL_STA_PRIM_INFEAS_CER:
      case MSK_SOL_STA_NEAR_DUAL_INFEAS_CER:
      case MSK_SOL_STA_NEAR_PRIM_INFEAS_CER:
        //printf("Primal or dual infeasibility certificate found.\n");
        break;
  
      case MSK_SOL_STA_UNKNOWN:
        //printf("The status of the solution could not be determined.\n");
        break;
      default:
        //printf("Other solution status.");
        break;
    }
    return bRet;
  }
  
  } //namespace linearProgramming
  } //namespace i23dSFM
  
  #endif // I23DSFM_HAVE_MOSEK
  