LCPtoLP.cpp

Go to the documentation of this file.

#include "lcptolp.h"
#include <algorithm>
#include <armadillo>
#include <boost/log/trivial.hpp>
#include <boost/program_options.hpp>
#include <cmath>
#include <gurobi_c++.h>
#include <iostream>
#include <memory>
#include <random>
#include <set>
#include <string>

using namespace std;
using namespace Utils;

bool operator==(vector<short int> Fix1, vector<short int> Fix2)
{
  if (Fix1.size() != Fix2.size())
    return false;
  for (unsigned int i = 0; i < Fix1.size(); i++) {
    if (Fix1.at(i) != Fix2.at(i))
      return false;
  }
  return true;
}

bool operator<(vector<short int> Fix1, vector<short int> Fix2)
{
  if (Fix1.size() != Fix2.size())
    return false;
  for (unsigned int i = 0; i < Fix1.size(); i++) {
    if (Fix1.at(i) != Fix2.at(i) && Fix1.at(i) * Fix2.at(i) != 0) {
      return false; // Fix1 is not a child of Fix2
    }
  }
  return true; // Fix1 is a child of Fix2
}

bool operator>(vector<int> Fix1, vector<int> Fix2) { return (Fix2 < Fix1); }

void Game::LCP::defConst(GRBEnv *env)
{
  this->Ai = unique_ptr<spmat_Vec>(new spmat_Vec());
  this->bi = unique_ptr<vec_Vec>(new vec_Vec());
  this->RlxdModel.set(GRB_IntParam_OutputFlag, VERBOSE);
  this->env = env;
  this->nR = this->M.n_rows;
  this->nC = this->M.n_cols;
}

Game::LCP::LCP(
    GRBEnv *env,    
    arma::sp_mat M, 
    arma::vec q,    
    perps Compl,    
    arma::sp_mat A, 
    arma::vec b     
    )
    : M{M}, q{q}, _A{A}, _b{b}, RlxdModel(*env)
{
  defConst(env);
  this->Compl = perps(Compl);
  std::sort(
      this->Compl.begin(), this->Compl.end(),
      [](pair<unsigned int, unsigned int> a,
         pair<unsigned int, unsigned int> b) { return a.first < b.first; });
  for (auto p : this->Compl)
    if (p.first != p.second) {
      this->LeadStart = p.first;
      this->LeadEnd = p.second - 1;
      this->nLeader = this->LeadEnd - this->LeadStart + 1;
      this->nLeader = this->nLeader > 0 ? this->nLeader : 0;
      break;
    }
  this->initializeNotProcessed();
}

Game::LCP::LCP(
    GRBEnv *env,            
    arma::sp_mat M,         
    arma::vec q,            
    unsigned int LeadStart, 
    unsigned LeadEnd, 
    arma::sp_mat A,   
    arma::vec b       
    )
    : M{M}, q{q}, _A{A}, _b{b}, RlxdModel(*env)

{
  defConst(env);
  this->LeadStart = LeadStart;
  this->LeadEnd = LeadEnd;
  this->nLeader = this->LeadEnd - this->LeadStart + 1;
  this->nLeader = this->nLeader > 0 ? this->nLeader : 0;
  for (unsigned int i = 0; i < M.n_rows; i++) {
    unsigned int count = i < LeadStart ? i : i + nLeader;
    this->Compl.push_back({i, count});
  }
  std::sort(
      this->Compl.begin(), this->Compl.end(),
      [](pair<unsigned int, unsigned int> a,
         pair<unsigned int, unsigned int> b) { return a.first < b.first; });
  this->initializeNotProcessed();
}

Game::LCP::LCP(GRBEnv *env, const NashGame &N)
    : RlxdModel(*env)
{
  arma::sp_mat M;
  arma::vec q;
  perps Compl;
  N.FormulateLCP(M, q, Compl);
  // LCP(env, M, q, Compl, N.RewriteLeadCons(), N.getMCLeadRHS());

  this->M = M;
  this->q = q;
  this->_A = N.RewriteLeadCons();
  this->_b = N.getMCLeadRHS();
  defConst(env);
  this->Compl = perps(Compl);
  sort(this->Compl.begin(), this->Compl.end(),
       [](pair<unsigned int, unsigned int> a,
          pair<unsigned int, unsigned int> b) { return a.first < b.first; });
  // Delete no more!
  for (auto p : this->Compl) {
    if (p.first != p.second) {
      this->LeadStart = p.first;
      this->LeadEnd = p.second - 1;
      this->nLeader = this->LeadEnd - this->LeadStart + 1;
      this->nLeader = this->nLeader > 0 ? this->nLeader : 0;
      break;
    }
  }
  this->initializeNotProcessed();
}

Game::LCP::~LCP()
{}

void Game::LCP::makeRelaxed()
{
  try {
    if (this->madeRlxdModel)
      return;
    BOOST_LOG_TRIVIAL(trace)
        << "Game::LCP::makeRelaxed: Creating a model with : " << nR
        << " variables and  " << nC << " constraints";
    GRBVar x[nC], z[nR];
    BOOST_LOG_TRIVIAL(trace)
        << "Game::LCP::makeRelaxed: Initializing variables";
    for (unsigned int i = 0; i < nC; i++)
      x[i] = RlxdModel.addVar(0, GRB_INFINITY, 1, GRB_CONTINUOUS,
                              "x_" + to_string(i));
    for (unsigned int i = 0; i < nR; i++)
      z[i] = RlxdModel.addVar(0, GRB_INFINITY, 1, GRB_CONTINUOUS,
                              "z_" + to_string(i));
    BOOST_LOG_TRIVIAL(trace) << "Game::LCP::makeRelaxed: Added variables";
    for (unsigned int i = 0; i < nR; i++) {
      GRBLinExpr expr = 0;
      for (auto v = M.begin_row(i); v != M.end_row(i); ++v)
        expr += (*v) * x[v.col()];
      expr += q(i);
      RlxdModel.addConstr(expr, GRB_EQUAL, z[i], "z_" + to_string(i) + "_def");
    }
    BOOST_LOG_TRIVIAL(trace)
        << "Game::LCP::makeRelaxed: Added equation definitions";
    // If @f$Ax \leq b@f$ constraints are there, they should be included too!
    if (this->_A.n_nonzero != 0 && this->_b.n_rows != 0) {
      if (_A.n_cols != nC || _A.n_rows != _b.n_rows) {
        BOOST_LOG_TRIVIAL(trace) << "(" << _A.n_rows << "," << _A.n_cols
                                 << ")\t" << _b.n_rows << " " << nC;
        throw string("Game::LCP::makeRelaxed: A and b are incompatible! Thrown "
                     "from makeRelaxed()");
      }
      for (unsigned int i = 0; i < _A.n_rows; i++) {
        GRBLinExpr expr = 0;
        for (auto a = _A.begin_row(i); a != _A.end_row(i); ++a)
          expr += (*a) * x[a.col()];
        RlxdModel.addConstr(expr, GRB_LESS_EQUAL, _b(i),
                            "commonCons_" + to_string(i));
      }
      BOOST_LOG_TRIVIAL(trace)
          << "Game::LCP::makeRelaxed: Added common constraints";
    }
    RlxdModel.update();
    this->madeRlxdModel = true;
  } catch (const char *e) {
    cerr << "Error in Game::LCP::makeRelaxed: " << e << '\n';
    throw;
  } catch (string e) {
    cerr << "String: Error in Game::LCP::makeRelaxed: " << e << '\n';
    throw;
  } catch (exception &e) {
    cerr << "Exception: Error in Game::LCP::makeRelaxed: " << e.what() << '\n';
    throw;
  } catch (GRBException &e) {
    cerr << "GRBException: Error in Game::LCP::makeRelaxed: "
         << e.getErrorCode() << "; " << e.getMessage() << '\n';
    throw;
  }
}

unique_ptr<GRBModel> Game::LCP::LCP_Polyhed_fixed(
    vector<unsigned int>
        FixEq, 
    vector<unsigned int>
        FixVar 
    )
{
  makeRelaxed();
  unique_ptr<GRBModel> model(new GRBModel(this->RlxdModel));
  for (auto i : FixEq) {
    if (i >= nR)
      throw "Game::LCP::LCP_Polyhed_fixed: Element in FixEq is greater than nC";
    model->getVarByName("z_" + to_string(i)).set(GRB_DoubleAttr_UB, 0);
  }
  for (auto i : FixVar) {
    if (i >= nC)
      throw "Game::LCP::LCP_Polyhed_fixed: Element in FixEq is greater than nC";
    model->getVarByName("z_" + to_string(i)).set(GRB_DoubleAttr_UB, 0);
  }
  return model;
}

unique_ptr<GRBModel> Game::LCP::LCP_Polyhed_fixed(
    arma::Col<int> FixEq, 
    arma::Col<int> FixVar 
    )
{
  makeRelaxed();
  unique_ptr<GRBModel> model{new GRBModel(this->RlxdModel)};
  for (unsigned int i = 0; i < nC; i++)
    if (FixVar[i])
      model->getVarByName("x_" + to_string(i)).set(GRB_DoubleAttr_UB, 0);
  for (unsigned int i = 0; i < nR; i++)
    if (FixEq[i])
      model->getVarByName("z_" + to_string(i)).set(GRB_DoubleAttr_UB, 0);
  model->update();
  return model;
}

unique_ptr<GRBModel> Game::LCP::LCPasMIP(
    vector<short int>
        Fixes, 
    bool solve 
    )
{
  if (Fixes.size() != this->nR)
    throw string(
        "Game::LCP::LCPasMIP: Bad size for Fixes in Game::LCP::LCPasMIP");
  vector<unsigned int> FixVar, FixEq;
  for (unsigned int i = 0; i < nR; i++) {
    if (Fixes[i] == 1)
      FixEq.push_back(i);
    if (Fixes[i] == -1)
      FixVar.push_back(i > this->LeadStart ? i + this->nLeader : i);
  }
  return this->LCPasMIP(FixEq, FixVar, solve);
}

unique_ptr<GRBModel> Game::LCP::LCPasMIP(
    vector<unsigned int> FixEq,  
    vector<unsigned int> FixVar, 
    bool solve 
    )
{
  makeRelaxed();
  unique_ptr<GRBModel> model{new GRBModel(this->RlxdModel)};
  // Creating the model
  try {
    GRBVar x[nC], z[nR], u[nR], v[nR];
    // Get hold of the Variables and Eqn Variables
    for (unsigned int i = 0; i < nC; i++)
      x[i] = model->getVarByName("x_" + to_string(i));
    for (unsigned int i = 0; i < nR; i++)
      z[i] = model->getVarByName("z_" + to_string(i));
    // Define binary variables for bigM
    for (unsigned int i = 0; i < nR; i++)
      u[i] = model->addVar(0, 1, 0, GRB_BINARY, "u_" + to_string(i));
    if (this->useIndicators)
      for (unsigned int i = 0; i < nR; i++)
        v[i] = model->addVar(0, 1, 0, GRB_BINARY, "v_" + to_string(i));
    // Include ALL Complementarity constraints using bigM

    if (this->useIndicators) {
      BOOST_LOG_TRIVIAL(trace)
          << "Using indicator constraints for complementarities.";
    } else {
      BOOST_LOG_TRIVIAL(trace)
          << "Using bigM for complementarities with M=" << this->bigM;
    }

    GRBLinExpr expr = 0;
    for (const auto p : Compl) {
      // z[i] <= Mu constraint

      // u[j]=0 --> z[i] <=0
      if (!this->useIndicators) {
        expr = bigM * u[p.first];
        model->addConstr(expr, GRB_GREATER_EQUAL, z[p.first],
                         "z" + to_string(p.first) + "_L_Mu" +
                             to_string(p.first));
      } else {
        model->addGenConstrIndicator(
            u[p.first], 1, z[p.first], GRB_LESS_EQUAL, 0,
            "z_ind_" + to_string(p.first) + "_L_Mu_" + to_string(p.first));
      }
      // x[i] <= M(1-u) constraint
      if (!this->useIndicators) {
        expr = bigM - bigM * u[p.first];
        model->addConstr(expr, GRB_GREATER_EQUAL, x[p.second],
                         "x" + to_string(p.first) + "_L_MuDash" +
                             to_string(p.first));
      } else {
        model->addGenConstrIndicator(
            v[p.first], 1, x[p.second], GRB_LESS_EQUAL, 0,
            "x_ind_" + to_string(p.first) + "_L_MuDash_" + to_string(p.first));
      }

      if (this->useIndicators)
        model->addConstr(u[p.first] + v[p.first], GRB_EQUAL, 1,
                         "uv_sum_" + to_string(p.first));
    }
    // If any equation or variable is to be fixed to zero, that happens here!
    for (auto i : FixVar)
      model->addConstr(x[i], GRB_EQUAL, 0.0);
    for (auto i : FixEq)
      model->addConstr(z[i], GRB_EQUAL, 0.0);
    model->update();
    if (!this->useIndicators) {
      model->set(GRB_DoubleParam_IntFeasTol, this->eps_int);
      model->set(GRB_DoubleParam_FeasibilityTol, this->eps);
      model->set(GRB_DoubleParam_OptimalityTol, this->eps);
    }
    // Get first Equilibrium
    model->set(GRB_IntParam_SolutionLimit, 1);
    if (solve)
      model->optimize();
    return model;
  } catch (const char *e) {
    cerr << "Error in Game::LCP::LCPasMIP: " << e << '\n';
    throw;
  } catch (string e) {
    cerr << "String: Error in Game::LCP::LCPasMIP: " << e << '\n';
    throw;
  } catch (exception &e) {
    cerr << "Exception: Error in Game::LCP::LCPasMIP: " << e.what() << '\n';
    throw;
  } catch (GRBException &e) {
    cerr << "GRBException: Error in Game::LCP::LCPasMIP: " << e.getErrorCode()
         << "; " << e.getMessage() << '\n';
    throw;
  }
  return nullptr;
}

bool Game::LCP::errorCheck(
    bool throwErr 
) const
{
  const unsigned int nR = M.n_rows;
  const unsigned int nC = M.n_cols;
  if (throwErr) {
    if (nR != q.n_rows)
      throw "Game::LCP::errorCheck: M and q have unequal number of rows";
    if (nR + nLeader != nC)
      throw "Game::LCP::errorCheck: Inconsistency between number of leader "
            "vars " +
          to_string(nLeader) + ", number of rows " + to_string(nR) +
          " and number of cols " + to_string(nC);
  }
  return (nR == q.n_rows && nR + nLeader == nC);
}

void Game::LCP::print(string end) {
  cout << "LCP with " << this->nR << " rows and " << this->nC << " columns."
       << end;
}

unsigned int
Game::LCP::ConvexHull(arma::sp_mat &A, 
                      arma::vec &b) 

{
  const std::vector<arma::sp_mat *> tempAi = [](spmat_Vec &uv) {
    std::vector<arma::sp_mat *> v{};
    for (const auto &x : uv)
      v.push_back(x.get());
    return v;
  }(*this->Ai);
  const std::vector<arma::vec *> tempbi = [](vec_Vec &uv) {
    std::vector<arma::vec *> v{};
    std::for_each(uv.begin(), uv.end(),
                  [&v](const std::unique_ptr<arma::vec> &ptr) {
                    v.push_back(ptr.get());
                  });
    return v;
  }(*this->bi);
  arma::sp_mat A_common;
  A_common = arma::join_cols(this->_A, -this->M);
  arma::vec b_common = arma::join_cols(this->_b, this->q);
  if (Ai->size() == 1) {
    A.zeros(Ai->at(0)->n_rows + A_common.n_rows,
            Ai->at(0)->n_cols + A_common.n_cols);
    b.zeros(bi->at(0)->n_rows + b_common.n_rows);
    A = arma::join_cols(*Ai->at(0), A_common);
    b = arma::join_cols(*bi->at(0), b_common);
    return 1;
  } else
    return Game::ConvexHull(&tempAi, &tempbi, A, b, A_common, b_common);
};

unsigned int Game::ConvexHull(
    const vector<arma::sp_mat *>
        *Ai, 
    const vector<arma::vec *>
        *bi,         
    arma::sp_mat &A, 
    arma::vec &b,    
    const arma::sp_mat
        Acom,            
    const arma::vec bcom 
    )
{
  // Count number of polyhedra and the space we are in!
  const unsigned int nPoly{static_cast<unsigned int>(Ai->size())};
  // Error check
  if (nPoly == 0)
    throw string(
        "Game::ConvexHull: Empty vector of polyhedra given! Problem might be "
        "infeasible."); // There should be at least 1 polyhedron to
                        // consider
  const unsigned int nC{static_cast<unsigned int>(Ai->front()->n_cols)};
  const unsigned int nComm{static_cast<unsigned int>(Acom.n_rows)};

  if (nComm > 0 && Acom.n_cols != nC)
    throw string("Game::ConvexHull: Inconsistent number of variables in the "
                 "common polyhedron");
  if (nComm > 0 && nComm != bcom.n_rows)
    throw string("Game::ConvexHull: Inconsistent number of rows in LHS and RHS "
                 "in the common polyhedron");

  // Count the number of variables in the convex hull.
  unsigned int nFinCons{0}, nFinVar{0};
  if (nPoly != bi->size())
    throw string(
        "Game::ConvexHull: Inconsistent number of LHS and RHS for polyhedra");
  for (unsigned int i = 0; i != nPoly; i++) {
    if (Ai->at(i)->n_cols != nC)
      throw string("Game::ConvexHull: Inconsistent number of variables in the "
                   "polyhedra ") +
          to_string(i) + "; " + to_string(Ai->at(i)->n_cols) +
          "!=" + to_string(nC);
    if (Ai->at(i)->n_rows != bi->at(i)->n_rows)
      throw string("Game::ConvexHull: Inconsistent number of rows in LHS and "
                   "RHS of polyhedra ") +
          to_string(i) + ";" + to_string(Ai->at(i)->n_rows) +
          "!=" + to_string(bi->at(i)->n_rows);
    nFinCons += Ai->at(i)->n_rows;
  }
  // For common constraint copy
  nFinCons += nPoly * nComm;

  const unsigned int FirstCons = nFinCons;

  // 2nd constraint in Eqn 4.31 of Conforti - twice so we have 2 ineq instead of
  // 1 eq constr
  nFinCons += nC * 2;
  // 3rd constr in Eqn 4.31. Again as two ineq constr.
  nFinCons += 2;
  // Common constraints
  // nFinCons += Acom.n_rows;

  nFinVar = nPoly * nC + nPoly +
            nC; // All x^i variables + delta variables+ original x variables
  A.zeros(nFinCons, nFinVar);
  b.zeros(nFinCons);
  // A.zeros(nFinCons, nFinVar); b.zeros(nFinCons);
  // Implements the first constraint more efficiently using better constructors
  // for sparse matrix
  Game::compConvSize(A, nFinCons, nFinVar, Ai, bi, Acom, bcom);

  // Counting rows completed
  /****************** SLOW LOOP BEWARE *******************/
  for (unsigned int i = 0; i < nPoly; i++) {
    BOOST_LOG_TRIVIAL(trace) << "Game::ConvexHull: Handling Polyhedron "
                             << i + 1 << " out of " << nPoly;
    // First constraint in (4.31)
    // A.submat(complRow, i*nC, complRow+nConsInPoly-1, (i+1)*nC-1) =
    // *Ai->at(i); // Slowest line. Will arma improve this? First constraint RHS
    // A.submat(complRow, nPoly*nC+i, complRow+nConsInPoly-1, nPoly*nC+i) =
    // -*bi->at(i); Second constraint in (4.31)
    for (unsigned int j = 0; j < nC; j++) {
      A.at(FirstCons + 2 * j, nC + (i * nC) + j) = 1;
      A.at(FirstCons + 2 * j + 1, nC + (i * nC) + j) = -1;
    }
    // Third constraint in (4.31)
    A.at(FirstCons + nC * 2, nC + nPoly * nC + i) = 1;
    A.at(FirstCons + nC * 2 + 1, nC + nPoly * nC + i) = -1;
  }
  /****************** SLOW LOOP BEWARE *******************/
  // Second Constraint RHS
  for (unsigned int j = 0; j < nC; j++) {
    A.at(FirstCons + 2 * j, j) = -1;
    A.at(FirstCons + 2 * j + 1, j) = 1;
  }
  // Third Constraint RHS
  b.at(FirstCons + nC * 2) = 1;
  b.at(FirstCons + nC * 2 + 1) = -1;
  return nPoly; 
}

void Game::compConvSize(
    arma::sp_mat &A,             
    const unsigned int nFinCons, 
    const unsigned int nFinVar,  
    const vector<arma::sp_mat *>
        *Ai, 
    const vector<arma::vec *>
        *bi, 
    const arma::sp_mat
        &Acom,            
    const arma::vec &bcom 
    )
{
  const unsigned int nPoly{static_cast<unsigned int>(Ai->size())};
  const unsigned int nC{static_cast<unsigned int>(Ai->front()->n_cols)};
  unsigned int N{0}; // Total number of nonzero elements in the final matrix
  const unsigned int nCommnz{
      static_cast<unsigned int>(Acom.n_nonzero + bcom.n_rows)};
  for (unsigned int i = 0; i < nPoly; i++) {
    N += Ai->at(i)->n_nonzero;
    N += bi->at(i)->n_rows;
  }
  N += nCommnz *
       nPoly; // The common constraints have to be copied for each polyhedron.

  // Now computed N which is the total number of nonzeros.
  arma::umat locations; // location of nonzeros
  arma::vec val;        // nonzero values
  locations.zeros(2, N);
  val.zeros(N);

  unsigned int count{0}, rowCount{0}, colCount{nC};
  for (unsigned int i = 0; i < nPoly; i++) {
    for (auto it = Ai->at(i)->begin(); it != Ai->at(i)->end();
         ++it) // First constraint
    {
      locations(0, count) = rowCount + it.row();
      locations(1, count) = colCount + it.col();
      val(count) = *it;
      ++count;
    }
    for (unsigned int j = 0; j < bi->at(i)->n_rows;
         ++j) // RHS of first constraint
    {
      locations(0, count) = rowCount + j;
      locations(1, count) = nC + nC * nPoly + i;
      val(count) = -bi->at(i)->at(j);
      ++count;
    }
    rowCount += Ai->at(i)->n_rows;

    // For common constraints
    for (auto it = Acom.begin(); it != Acom.end(); ++it) // First constraint
    {
      locations(0, count) = rowCount + it.row();
      locations(1, count) = colCount + it.col();
      val(count) = *it;
      ++count;
    }
    for (unsigned int j = 0; j < bcom.n_rows; ++j) // RHS of first constraint
    {
      locations(0, count) = rowCount + j;
      locations(1, count) = nC + nC * nPoly + i;
      val(count) = -bcom.at(j);
      ++count;
    }
    rowCount += Acom.n_rows;

    colCount += nC;
  }
  A = arma::sp_mat(locations, val, nFinCons, nFinVar);
}

arma::vec
Game::LPSolve(const arma::sp_mat &A, 
              const arma::vec &b,    
              const arma::vec &c,    
              int &status, 
              bool Positivity 
              )
{
  unsigned int nR, nC;
  nR = A.n_rows;
  nC = A.n_cols;
  if (c.n_rows != nC)
    throw "Game::LPSolve: Inconsistency in no of Vars in isFeas()";
  if (b.n_rows != nR)
    throw "Game::LPSolve: Inconsistency in no of Constr in isFeas()";

  arma::vec sol = arma::vec(c.n_rows, arma::fill::zeros);
  const double lb = Positivity ? 0 : -GRB_INFINITY;

  GRBEnv env;
  GRBModel model = GRBModel(env);
  GRBVar x[nC];
  GRBConstr a[nR];
  // Adding Variables
  for (unsigned int i = 0; i < nC; i++)
    x[i] = model.addVar(lb, GRB_INFINITY, c.at(i), GRB_CONTINUOUS,
                        "x_" + to_string(i));
  // Adding constraints
  for (unsigned int i = 0; i < nR; i++) {
    GRBLinExpr lin{0};
    for (auto j = A.begin_row(i); j != A.end_row(i); ++j)
      lin += (*j) * x[j.col()];
    a[i] = model.addConstr(lin, GRB_LESS_EQUAL, b.at(i));
  }
  model.set(GRB_IntParam_OutputFlag, VERBOSE);
  model.set(GRB_IntParam_DualReductions, 0);
  model.optimize();
  status = model.get(GRB_IntAttr_Status);
  if (status == GRB_OPTIMAL)
    for (unsigned int i = 0; i < nC; i++)
      sol.at(i) = x[i].get(GRB_DoubleAttr_X);
  return sol;
}

bool Game::LCP::extractSols(
    GRBModel *model, 
    arma::vec &z, 
    arma::vec &x, 
    bool extractZ 
) const
{
  if (model->get(GRB_IntAttr_Status) == GRB_LOADED)
    model->optimize();
  auto status = model->get(GRB_IntAttr_Status);
  if (!(status == GRB_OPTIMAL || status == GRB_SUBOPTIMAL ||
        status == GRB_SOLUTION_LIMIT))
    return false;
  x.zeros(nC);
  if (extractZ)
    z.zeros(nR);
  for (unsigned int i = 0; i < nR; i++) {
    x[i] = model->getVarByName("x_" + to_string(i)).get(GRB_DoubleAttr_X);
    if (extractZ)
      z[i] = model->getVarByName("z_" + to_string(i)).get(GRB_DoubleAttr_X);
  }
  for (unsigned int i = nR; i < nC; i++)
    x[i] = model->getVarByName("x_" + to_string(i)).get(GRB_DoubleAttr_X);
  return true;
}

std::vector<short int> Game::LCP::solEncode(const arma::vec &x) const
{
  return this->solEncode(this->M * x + this->q, x);
}

vector<short int> Game::LCP::solEncode(const arma::vec &z, 
                                       const arma::vec &x  
) const
{
  vector<short int> solEncoded(nR, 0);
  for (const auto p : Compl) {
    unsigned int i, j;
    i = p.first;
    j = p.second;
    if (isZero(z(i)))
      solEncoded.at(i)++;
    if (isZero(x(j)))
      solEncoded.at(i)--;
    if (!isZero(x(j)) && !isZero(z(i)))
      BOOST_LOG_TRIVIAL(error) << "Infeasible point given! Stay alert! " << x(j)
                               << " " << z(i) << " with i=" << i;
  };
  // std::stringstream enc_str;
  // for(auto vv:solEncoded) enc_str << vv <<" ";
  // BOOST_LOG_TRIVIAL (debug) << "Game::LCP::solEncode: Handling deviation with
  // encoding: "<< enc_str.str() << '\n';
  return solEncoded;
}

vector<short int> Game::LCP::solEncode(GRBModel *model) const
{
  arma::vec x, z;
  if (!this->extractSols(model, z, x, true))
    return {}; // If infeasible model, return empty!
  else
    return this->solEncode(z, x);
}

LCP &Game::LCP::addPolyFromX(const arma::vec &x, bool &ret)
{
  const unsigned int nCompl = this->Compl.size();
  vector<short int> encoding = this->solEncode(x);
  std::stringstream enc_str;
  for (auto vv : encoding)
    enc_str << vv << " ";
  BOOST_LOG_TRIVIAL(trace)
      << "Game::LCP::addPolyFromX: Handling deviation with encoding: "
      << enc_str.str() << '\n';
  // Check if the encoding polyhedron is already in this->AllPolyhedra
  for (const auto &i : AllPolyhedra) {
    std::vector<short int> bin = num_to_vec(i, nCompl);
    if (encoding < bin) {
      BOOST_LOG_TRIVIAL(trace) << "LCP::addPolyFromX: Encoding " << i
                               << " already in All Polyhedra! ";
      ret = false;
      return *this;
    }
  }

  BOOST_LOG_TRIVIAL(trace)
      << "LCP::addPolyFromX: New encoding not in All Polyhedra! ";
  // If it is not in AllPolyhedra
  // First change any zero indices of encoding to 1
  for (short &i : encoding) {
    if (i == 0)
      ++i;
  }
  // And then add the relevant polyhedra
  ret = this->FixToPoly(encoding, false);
  // ret = true;
  return *this;
}

bool Game::LCP::FixToPoly(
    const vector<short int>
        Fix,        
    bool checkFeas, 
    bool custom,    
    spmat_Vec *custAi, 
    vec_Vec *custbi    
    )
{
  unsigned int FixNumber = vec_to_num(Fix);
  BOOST_LOG_TRIVIAL(trace) << "Game::LCP::FixToPoly: Working on polyhedron"
                           << FixNumber;

  if (knownInfeas.find(FixNumber) != knownInfeas.end()) {
    BOOST_LOG_TRIVIAL(trace) << "Game::LCP::FixToPoly: Previously known "
                                "infeasible polyhedron. Not added"
                             << FixNumber;
    return false;
  }

  if (!custom && !AllPolyhedra.empty()) {
    if (AllPolyhedra.find(FixNumber) != AllPolyhedra.end()) {
      BOOST_LOG_TRIVIAL(trace)
          << "Game::LCP::FixToPoly: Previously added polyhedron. Not added "
          << FixNumber;
      return false;
    }
  }

  unique_ptr<arma::sp_mat> Aii =
      unique_ptr<arma::sp_mat>(new arma::sp_mat(nR, nC));
  Aii->zeros();
  unique_ptr<arma::vec> bii =
      unique_ptr<arma::vec>(new arma::vec(nR, arma::fill::zeros));
  for (unsigned int i = 0; i < this->nR; i++) {
    if (Fix.at(i) == 0) {
      throw string(
          "Error in Game::LCP::FixToPoly. 0s not allowed in argument vector");
    }
    if (Fix.at(i) == 1) // Equation to be fixed top zero
    {
      for (auto j = this->M.begin_row(i); j != this->M.end_row(i); ++j)
        if (!this->isZero((*j)))
          Aii->at(i, j.col()) =
              (*j); // Only mess with non-zero elements of a sparse matrix!
      bii->at(i) = -this->q(i);
    } else // Variable to be fixed to zero, i.e. x(j) <= 0 constraint to be
    // added
    {
      unsigned int varpos = (i >= this->LeadStart) ? i + this->nLeader : i;
      Aii->at(i, varpos) = 1;
      bii->at(i) = 0;
    }
  }
  bool add = !checkFeas;
  if (checkFeas) {
    add = this->checkPolyFeas(Fix);
  }
  if (add) {
    if (custom) {
      custAi->push_back(std::move(Aii));
      custbi->push_back(std::move(bii));
    } else {
      AllPolyhedra.insert(FixNumber);
      this->Ai->push_back(std::move(Aii));
      this->bi->push_back(std::move(bii));
    }
    return true; // Successfully added
  }
  return false;
}

bool Game::LCP::checkPolyFeas(
    const unsigned long int
        &decimalEncoding 
) {
  return this->checkPolyFeas(num_to_vec(decimalEncoding, this->Compl.size()));
}

bool Game::LCP::checkPolyFeas(
    const vector<short int> &Fix 
) {

  unsigned long int FixNumber = vec_to_num(Fix);
  if (knownInfeas.find(FixNumber) != knownInfeas.end()) {
    BOOST_LOG_TRIVIAL(trace) << "Game::LCP::checkPolyFeas: Previously known "
                                "infeasible polyhedron. "
                             << FixNumber;
    return false;
  }

  if (feasiblePoly.find(FixNumber) != feasiblePoly.end()) {
    BOOST_LOG_TRIVIAL(trace) << "Game::LCP::checkPolyFeas: Previously known "
                                "feasible polyhedron."
                             << FixNumber;
    return true;
  }

  unsigned int count{0};
  try {
    makeRelaxed();
    GRBModel model(this->RlxdModel);
    for (auto i : Fix) {
      if (i > 0)
        model.getVarByName("z_" + to_string(count)).set(GRB_DoubleAttr_UB, 0);
      if (i < 0)
        model
            .getVarByName("x_" + to_string(count >= this->LeadStart
                                               ? count + nLeader
                                               : count))
            .set(GRB_DoubleAttr_UB, 0);
      count++;
    }
    model.set(GRB_IntParam_OutputFlag, VERBOSE);
    model.optimize();
    if (model.get(GRB_IntAttr_Status) == GRB_OPTIMAL) {
      feasiblePoly.insert(FixNumber);
      return true;
    } else {
      BOOST_LOG_TRIVIAL(trace)
          << "Game::LCP::checkPolyFeas: Detected infeasibility of " << FixNumber
          << " (GRB_STATUS=" << model.get(GRB_IntAttr_Status) << ")";
      knownInfeas.insert(FixNumber);
      return false;
    }
  } catch (const char *e) {
    cerr << "Error in Game::LCP::FixToPoly: " << e << '\n';
    throw;
  } catch (string e) {
    cerr << "String: Error in Game::LCP::FixToPoly: " << e << '\n';
    throw;
  } catch (exception &e) {
    cerr << "Exception: Error in Game::LCP::FixToPoly: " << e.what() << '\n';
    throw;
  } catch (GRBException &e) {
    cerr << "GRBException: Error in Game::LCP::FixToPoly: " << e.getErrorCode()
         << ": " << e.getMessage() << '\n';
    throw;
  }
  return false;
}

Game::LCP &Game::LCP::FixToPolies(
    const vector<short int>
        Fix,        
    bool checkFeas, 
    bool custom,    
    spmat_Vec *custAi, 
    vec_Vec *custbi    
    )
{
  bool flag = false; // flag that there may be multiple polyhedra, i.e. 0 in
                     // some Fix entry
  vector<short int> MyFix(Fix);
  unsigned int i;
  for (i = 0; i < this->nR; i++) {
    if (Fix.at(i) == 0) {
      flag = true;
      break;
    }
  }
  if (flag) {
    MyFix[i] = 1;
    this->FixToPolies(MyFix, checkFeas, custom, custAi, custbi);
    MyFix[i] = -1;
    this->FixToPolies(MyFix, checkFeas, custom, custAi, custbi);
  } else
    this->FixToPoly(Fix, checkFeas, custom, custAi, custbi);
  return *this;
}

unsigned long int Game::LCP::getNextPoly(Game::EPECAddPolyMethod method) {
  switch (method) {
  case Game::EPECAddPolyMethod::sequential: {
    while (this->sequentialPolyCounter < this->maxTheoreticalPoly) {
      const bool isAll =
          AllPolyhedra.find(this->sequentialPolyCounter) != AllPolyhedra.end();
      const bool isInfeas =
          knownInfeas.find(this->sequentialPolyCounter) != knownInfeas.end();
      this->sequentialPolyCounter++;
      if (!isAll && !isInfeas) {
        return this->sequentialPolyCounter - 1;
      }
    }
    return this->maxTheoreticalPoly;
  } break;
  case Game::EPECAddPolyMethod::reverse_sequential: {
    while (this->reverseSequentialPolyCounter >= 0) {
      const bool isAll =
          AllPolyhedra.find(this->reverseSequentialPolyCounter) !=
          AllPolyhedra.end();
      const bool isInfeas =
          knownInfeas.find(this->reverseSequentialPolyCounter) !=
          knownInfeas.end();
      this->reverseSequentialPolyCounter--;
      if (!isAll && !isInfeas) {
        return this->reverseSequentialPolyCounter + 1;
      }
    }
    return this->maxTheoreticalPoly;
  } break;
  case Game::EPECAddPolyMethod::random: {
    static std::mt19937 engine{this->addPolyMethodSeed};
    std::uniform_int_distribution<unsigned long int> dist(
        0, this->maxTheoreticalPoly - 1);
    if ((knownInfeas.size() + AllPolyhedra.size()) == this->maxTheoreticalPoly)
      return this->maxTheoreticalPoly;
    while (true) {
      unsigned long int randomPolyId = dist(engine);
      const bool isAll = AllPolyhedra.find(randomPolyId) != AllPolyhedra.end();
      const bool isInfeas = knownInfeas.find(randomPolyId) != knownInfeas.end();
      if (!isAll && !isInfeas)
        return randomPolyId;
    }
  }
  default: {
    BOOST_LOG_TRIVIAL(error)
        << "Error in Game::LCP::getNextPoly: unrecognized method "
        << to_string(method);
    throw;
  }
  }
}

std::set<std::vector<short int>>
Game::LCP::addAPoly(unsigned long int nPoly, Game::EPECAddPolyMethod method,
                    std::set<std::vector<short int>> Polys) {
  // We already have polyhedrain AllPolyhedra and polyhedra in
  // knownInfeas, that are known to be infeasible.
  // Effective maximum of number of polyhedra that can be added
  // at most
  const unsigned int nCompl = this->Compl.size();

  if (this->maxTheoreticalPoly <
      nPoly) {                 // If you cannot add that many polyhedra
    BOOST_LOG_TRIVIAL(warning) // Then issue a warning
        << "Warning in Game::LCP::randomPoly: "
        << "Cannot add " << nPoly << " polyhedra. Promising a maximum of "
        << this->maxTheoreticalPoly;
    nPoly = this->maxTheoreticalPoly; // and update maximum possibly addable
  }

  if (nPoly == 0) // If nothing to be added, then nothing to be done
    return Polys;

  if (nPoly < 0) // There is no way that this can happen!
  {
    BOOST_LOG_TRIVIAL(error) << "nPoly can't be negative, i.e., " << nPoly;
    throw string(
        "Error in Game::LCP::addAPoly: nPoly reached a negative value!");
  }

  bool complete{false};
  while (!complete) {
    unsigned long int choice_decimal = this->getNextPoly(method);
    if (choice_decimal >= this->maxTheoreticalPoly)
      return Polys;

    const std::vector<short int> choice = num_to_vec(choice_decimal, nCompl);
    auto added = this->FixToPoly(choice, true);
    if (added) // If choice is added to All Polyhedra
    {
      Polys.insert(choice); // Add it to set of added polyhedra
      if (Polys.size() == nPoly) {
        complete = true;
        return Polys;
      }
    }
  }
  return Polys;
}
bool Game::LCP::addThePoly(const unsigned long int &decimalEncoding) {
  if (this->maxTheoreticalPoly < decimalEncoding) {
    // This polyhedron does not exist
    BOOST_LOG_TRIVIAL(warning)
        << "Warning in Game::LCP::addThePoly: Cannot add " << decimalEncoding
        << " polyhedra, since it does not exist!";
    return false;
  }
  const unsigned int nCompl = this->Compl.size();
  const std::vector<short int> choice = num_to_vec(decimalEncoding, nCompl);
  return this->FixToPoly(choice, true);
}

Game::LCP &Game::LCP::EnumerateAll(
    const bool
        solveLP 
    )
{
  vector<short int> Fix = vector<short int>(nR, 0);
  this->Ai->clear();
  this->bi->clear();
  this->FixToPolies(Fix, solveLP);
  if (this->Ai->empty()) {
    BOOST_LOG_TRIVIAL(warning)
        << "Empty vector of polyhedra given! Problem might be infeasible."
        << '\n';
    // 0 <= -1 for infeasability
    unique_ptr<arma::sp_mat> A(new arma::sp_mat(1, this->M.n_cols));
    unique_ptr<arma::vec> b(new arma::vec(1));
    b->at(0) = -1;
    this->Ai->push_back(std::move(A));
    this->bi->push_back(std::move(b));
  }
  return *this;
}

Game::LCP &Game::LCP::makeQP(
    Game::QP_objective
        &QP_obj, 
    Game::QP_Param &QP 

) {
  // Original sizes
  if (this->Ai->empty())
    return *this;
  const unsigned int Nx_old{static_cast<unsigned int>(QP_obj.C.n_cols)};

  Game::QP_constraints QP_cons;
  this->feasiblePolyhedra = this->ConvexHull(QP_cons.B, QP_cons.b);
  BOOST_LOG_TRIVIAL(trace) << "LCP::makeQP: No. feasible polyhedra: "
                           << this->feasiblePolyhedra;
  // Updated size after convex hull has been computed.
  const unsigned int Ncons{static_cast<unsigned int>(QP_cons.B.n_rows)};
  const unsigned int Ny{static_cast<unsigned int>(QP_cons.B.n_cols)};
  // Resizing entities.
  QP_cons.A.zeros(Ncons, Nx_old);
  QP_obj.c = resize_patch(QP_obj.c, Ny, 1);
  QP_obj.C = resize_patch(QP_obj.C, Ny, Nx_old);
  QP_obj.Q = resize_patch(QP_obj.Q, Ny, Ny);
  // Setting the QP_Param object
  QP.set(QP_obj, QP_cons);
  return *this;
}

unique_ptr<GRBModel> Game::LCP::LCPasQP(bool solve)
{
  this->makeRelaxed();
  unique_ptr<GRBModel> model(new GRBModel(this->RlxdModel));
  GRBQuadExpr obj = 0;
  GRBVar x[this->nR];
  GRBVar z[this->nR];
  for (const auto p : this->Compl) {
    unsigned int i = p.first;
    unsigned int j = p.second;
    z[i] = model->getVarByName("z_" + to_string(i));
    x[i] = model->getVarByName("x_" + to_string(j));
    obj += x[i] * z[i];
  }
  model->setObjective(obj, GRB_MINIMIZE);
  if (solve) {
    try {
      model->optimize();
      int status = model->get(GRB_IntAttr_Status);
      if (status != GRB_OPTIMAL ||
          model->get(GRB_DoubleAttr_ObjVal) > this->eps)
        throw "Game::LCP::LCPasQP: LCP infeasible";
    } catch (const char *e) {
      cerr << "Error in Game::LCP::LCPasQP: " << e << '\n';
      throw;
    } catch (string e) {
      cerr << "String: Error in Game::LCP::LCPasQP: " << e << '\n';
      throw;
    } catch (exception &e) {
      cerr << "Exception: Error in Game::LCP::LCPasQP: " << e.what() << '\n';
      throw;
    } catch (GRBException &e) {
      cerr << "GRBException: Error in Game::LCP::LCPasQP: " << e.getErrorCode()
           << "; " << e.getMessage() << '\n';
      throw;
    }
  }
  return model;
}

unique_ptr<GRBModel> Game::LCP::LCPasMIP(bool solve)
{
  return this->LCPasMIP({}, {}, solve);
}

unique_ptr<GRBModel>
Game::LCP::MPECasMILP(const arma::sp_mat &C, const arma::vec &c,
                      const arma::vec &x_minus_i, bool solve)
{
  unique_ptr<GRBModel> model = this->LCPasMIP(true);
  // Reset the solution limit. We need to solve to optimality
  model->set(GRB_IntParam_SolutionLimit, GRB_MAXINT);
  if (C.n_cols != x_minus_i.n_rows)
    throw string("Game::LCP::LCPasMILP: Bad size of x_minus_i");
  if (c.n_rows != C.n_rows)
    throw string("Game::LCP::LCPasMILP: Bad size of c");
  arma::vec Cx(c.n_rows, arma::fill::zeros);
  try {
    Cx = C * x_minus_i;
  } catch (exception &e) {
    cerr << "Exception in Game::LCP::MPECasMIQP: " << e.what() << '\n';
    throw;
  } catch (string &e) {
    cerr << "Exception in Game::LCP::MPECasMIQP: " << e << '\n';
    throw;
  }
  arma::vec obj = c + Cx;
  GRBLinExpr expr{0};
  for (unsigned int i = 0; i < obj.n_rows; i++)
    expr += obj.at(i) * model->getVarByName("x_" + to_string(i));
  model->setObjective(expr, GRB_MINIMIZE);
  model->set(GRB_IntParam_OutputFlag, VERBOSE);
  if (solve)
    model->optimize();
  return model;
}

unique_ptr<GRBModel>
Game::LCP::MPECasMIQP(const arma::sp_mat &Q, const arma::sp_mat &C,
                      const arma::vec &c, const arma::vec &x_minus_i,
                      bool solve)
{
  auto model = this->MPECasMILP(C, c, x_minus_i, false);
  if (Q.n_nonzero != 0) // If Q is zero, then just solve MIP as opposed to MIQP!
  {
    GRBLinExpr linexpr = model->getObjective(0);
    GRBQuadExpr expr{linexpr};
    for (auto it = Q.begin(); it != Q.end(); ++it)
      expr += (*it) * model->getVarByName("x_" + to_string(it.row())) *
              model->getVarByName("x_" + to_string(it.col()));
    model->setObjective(expr, GRB_MINIMIZE);
  }
  if (solve)
    model->optimize();
  return model;
}

void Game::LCP::write(string filename, bool append) const {
  ofstream outfile(filename, append ? ios::app : ios::out);

  outfile << nR << " rows and " << nC << " columns in the LCP\n";
  outfile << "LeadStart: " << LeadStart << " \nLeadEnd: " << LeadEnd
          << " \nnLeader: " << nLeader << "\n\n";

  outfile << "M: " << this->M;
  outfile << "q: " << this->q;
  outfile << "Complementarity: \n";
  for (const auto &p : this->Compl)
    outfile << "<" << p.first << ", " << p.second << ">"
            << "\t";
  outfile << "A: " << this->_A;
  outfile << "b: " << this->_b;
  outfile.close();
}

void Game::LCP::save(string filename, bool erase) const {
  Utils::appendSave(string("LCP"), filename, erase);
  Utils::appendSave(this->M, filename, string("LCP::M"), false);
  Utils::appendSave(this->q, filename, string("LCP::q"), false);

  Utils::appendSave(this->LeadStart, filename, string("LCP::LeadStart"), false);
  Utils::appendSave(this->LeadEnd, filename, string("LCP::LeadEnd"), false);

  Utils::appendSave(this->_A, filename, string("LCP::_A"), false);
  Utils::appendSave(this->_b, filename, string("LCP::_b"), false);

  BOOST_LOG_TRIVIAL(trace) << "Saved LCP to file " << filename;
}

long int Game::LCP::load(string filename, long int pos) {
  if (!this->env)
    throw string("Error in LCP::load: To load LCP from file, it has to be "
                 "constructed using LCP(GRBEnv*) constructor");

  string headercheck;
  pos = Utils::appendRead(headercheck, filename, pos);
  if (headercheck != "LCP")
    throw string("Error in LCP::load: In valid header - ") + headercheck;

  arma::sp_mat M, A;
  arma::vec q, b;
  unsigned int LeadStart, LeadEnd;
  pos = Utils::appendRead(M, filename, pos, string("LCP::M"));
  pos = Utils::appendRead(q, filename, pos, string("LCP::q"));
  pos = Utils::appendRead(LeadStart, filename, pos, string("LCP::LeadStart"));
  pos = Utils::appendRead(LeadEnd, filename, pos, string("LCP::LeadEnd"));
  pos = Utils::appendRead(A, filename, pos, string("LCP::_A"));
  pos = Utils::appendRead(b, filename, pos, string("LCP::_b"));

  this->M = M;
  this->q = q;
  this->_A = A;
  this->_b = b;
  defConst(env);
  this->LeadStart = LeadStart;
  this->LeadEnd = LeadEnd;

  this->nLeader = this->LeadEnd - this->LeadStart + 1;
  this->nLeader = this->nLeader > 0 ? this->nLeader : 0;
  for (unsigned int i = 0; i < M.n_rows; i++) {
    unsigned int count = i < LeadStart ? i : i + nLeader;
    Compl.push_back({i, count});
  }
  std::sort(Compl.begin(), Compl.end(),
            [](std::pair<unsigned int, unsigned int> a,
               std::pair<unsigned int, unsigned int> b) {
              return a.first <= b.first;
            });
  this->initializeNotProcessed();
  return pos;
}

std::string Game::LCP::feas_detail_str() const {
  std::stringstream ss;
  ss << "\tProven feasible: ";
  for (auto vv : this->AllPolyhedra)
    ss << vv << ' ';
  // ss << "\tProven infeasible: ";
  // for (auto vv : this->knownInfeas)
  // ss << vv << ' ';

  return ss.str();
}

unsigned int Game::LCP::conv_Npoly() const {
  return this->AllPolyhedra.size();
}

unsigned int Game::LCP::conv_PolyPosition(const unsigned long int i) const {
  const unsigned int nPoly = this->conv_Npoly();
  if (i > nPoly) {
    BOOST_LOG_TRIVIAL(error) << "Error in Game::LCP::conv_PolyPosition: "
                                "Invalid argument. Out of bounds for i";
    throw std::string("Error in Game::LCP::conv_PolyPosition: Invalid "
                      "argument. Out of bounds for i");
  }
  const unsigned int nC = this->M.n_cols;
  return nC + i * nC;
}

unsigned int Game::LCP::conv_PolyWt(const unsigned long int i) const {
  const unsigned int nPoly = this->conv_Npoly();
  if (nPoly <= 1) {
    return 0;
  }
  if (i > nPoly) {
    throw std::string("Error in Game::LCP::conv_PolyWt: "
                      "Invalid argument. Out of bounds for i");
  }
  const unsigned int nC = this->M.n_cols;

  return nC + nPoly * nC + i;
}