#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <vector>
#include <algorithm>
#include <math.h>

#include "NRGclasses.hpp"
#include "NRGfunctions.hpp"
using namespace std;

//////////////

int CNRGarray::NumBlocks(){

  return((int)(QNumbers.size()/NQNumbers));

}

//////////////
double CNRGarray::EniBlockj(int i, int j){

  // returns the energy of the i-th state in block j

  double aux=0.0;

  if ( (2*j-2)<BlockBegEnd.size() )
    {
      if ( (BlockBegEnd[2*j-2]+(i-1))<dEn.size() )
	{
	  aux=dEn[BlockBegEnd[2*j-2]+(i-1)];
	}
    }

  return(aux);

}
//////////////

//////////////
void CNRGarray::PrintQNumbers(){

  int ii;
  cout << "Qnumbers : " << endl;
  for (ii=0;ii<QNumbers.size();ii++)
    {
      cout << fixed << QNumbers[ii] << "  ";
      if ((ii+1) % NQNumbers == 0) cout << endl;
    }
  cout << endl;
  cout << resetiosflags (ios_base::floatfield);

}

//////////////
void CNRGarray::PrintEn(){

  int ii;
  int iblock,pos_in_block;
  for (ii=0;ii<dEn.size();ii++){
    iblock=GetBlockFromSt(ii,pos_in_block);
    cout << "En = " << dEn[ii] << " ---| " ;
    for (int iQn=0;iQn<NQNumbers;iQn++){
      cout << GetQNumber(iblock,iQn) << "  ";
    }
    if (ii<Kept.size()){if(Kept[ii]){cout << "(K)";}}
    cout << ">_N= ";
    cout << Nshell << endl;
  }
}

//////////////
void CNRGarray::PrintBlockQNumbers(int iblock){

  cout << "Block no. " << iblock << " : ";
  for (int ii=0;ii<NQNumbers;ii++)
    cout << fixed << GetQNumber(iblock,ii) << " ";
  cout << endl;
  cout << resetiosflags (ios_base::floatfield);

}

//////////////
void CNRGarray::PrintBlockEn(int iblock){

  PrintBlockQNumbers(iblock);

  int iBeg=GetBlockLimit(iblock,0);
  int iEnd=GetBlockLimit(iblock,1);
  cout << " dEn : " << endl;
  if (dEn.size()>0)
    {
      for (int ii=iBeg;ii<=iEnd;ii++)
	cout << ii << " : " << scientific << dEn[ii] << endl;
    }
  //end if
  cout << resetiosflags (ios_base::floatfield);
  cout << "Kept States: "<< endl;
  if (Kept.size()>0){
    for (int ii=iBeg;ii<=iEnd;ii++){
      cout << Kept[ii] << " ";
    }
    cout << endl;
  }
  // end if

}

//////////////
void CNRGarray::PrintBlock(int iblock){

  PrintBlockEn(iblock);

  int iBeg=GetBlockLimitEigv(iblock,0);
  int iEnd=GetBlockLimitEigv(iblock,1);

  int NstBl=GetBlockLimit(iblock,1)-GetBlockLimit(iblock,0)+1;
  cout << " dEigVec (rows): " << endl;;
  if (dEigVec.size()>0){
    int i1=1;
    for (int ii=iBeg;ii<=iEnd;ii++){
      cout << scientific << dEigVec[ii] << "  ";
      if (i1%NstBl==0) cout << endl;
//       // complex
//       cout << scientific << cEigVec[ii] << "  ";
//       if (i1%NstBl==0) cout << endl;
      i1++;
    }
  } else if (cEigVec.size()>0){
    int i1=1;
    for (int ii=iBeg;ii<=iEnd;ii++){
      // complex
      cout << scientific << cEigVec[ii] << "  ";
      if (i1%NstBl==0) cout << endl;
      i1++;
    }
  }
  //end if

  cout << resetiosflags (ios_base::floatfield);

}

//////////////
void CNRGarray::PrintAll(){

  int ii;
  //void CNRGarray::PrintQNumbers();

  PrintQNumbers();

  cout << " Any totalS QNs? = " << totalS << endl;
  if (Sqnumbers.size()>0){
    cout << "  totalS QNs positions : ";
    for (ii=0;ii<Sqnumbers.size();ii++)
      cout << Sqnumbers[ii] << " ";
    cout << endl;
  }
  cout << "dEn : " << endl;
  for (ii=0;ii<dEn.size();ii++)
    cout << scientific << dEn[ii] << " ";
  cout << endl;

  cout << "dEigVec (rows): " << endl;
  for (ii=0;ii<dEigVec.size();ii++)
    cout << scientific << dEigVec[ii] << " ";
  cout << endl;
  for (ii=0;ii<cEigVec.size();ii++)
    cout << scientific << cEigVec[ii] << " ";
  cout << endl;

  cout << "BlockBegEnd : " << endl;
  for (ii=0;ii<BlockBegEnd.size();ii+=2)
    cout << BlockBegEnd[ii] << " "<< BlockBegEnd[ii+1] << ", ";
  cout << endl;

  cout << "Size Blocks : " << endl;
  for (ii=0;ii<NumBlocks();ii++)
    cout << GetBlockSize(ii) << " ";
  cout << endl;

  cout << "Child States (size= "<< ChildStates.size() <<" ):"<< endl;
  // Why clear?
  //ChildStates.clear();
  if (ChildStates.size()>0){
    for (ii=0;ii<ChildStates.size();ii++){
      if (ChildStates[ii].size()>0){
	for (int jj=0;jj<ChildStates[ii].size();jj++){
	  cout << ChildStates[ii][jj] << " ";
	}
      cout << endl;
      }
      // end if
    }
    cout << endl;
  }
  // end if

  cout << "Kept States (size= "<< Kept.size() <<" ):"<< endl;
  if (Kept.size()>0){
    for (ii=0;ii<Kept.size();ii++){
      cout << Kept[ii] << " ";
    }
    cout << endl;
  }
  // end if

  cout << resetiosflags (ios_base::floatfield);
}
//////////////
int CNRGarray::Nstates(){

  // Changing this:
  //return(BlockBegEnd[NQNumbers*(NumBlocks()-1)+1]-BlockBegEnd[0]+1);
  return(BlockBegEnd.back()-BlockBegEnd.front()+1);

}

//////////////
void CNRGarray::SetE0zero(){

  double Emin=*min_element(dEn.begin(),dEn.end());
  cout <<  "Emin = " << Emin << endl;

  for (int ii=0;ii<dEn.size();ii++) dEn[ii]-=Emin;

}

//////////////
void CNRGarray::SetDegen(){

  // int nst,ii;

  // Search for near-degeneracies in the spectrum
  // and set the energy of the states to be the same
  int ii=0;
  while ( ii<dEn.size() ){
    for (int jj=ii+1;jj<dEn.size();jj++){
//     if (fabs(dEn[ii]-dEn[ii-1])<1.0E-10) nst=iDegen[ii-1]+1;
//     else nst=1;
//     iDegen.push_back(nst);
      if (dEqualPrec(dEn[jj],dEn[ii],MyEPS)){
	if (dEn[jj]>dEn[ii])
	  dEn[jj]=dEn[ii];
	else
	  dEn[ii]=dEn[jj];
      }
      //end if equal
    }
    // end for loop in jj
    ii++;
  }
  // end while loop
}

///////////////////////
void CNRGarray::FilterQNumbers(){

  int equal=0;
  int ii,i1;

  vector<double>::iterator qnums_iter,qnums_iter2,qnums_ii1,qnums_ii2;

  for (qnums_iter=QNumbers.begin(); qnums_iter<QNumbers.end();
       qnums_iter+=NQNumbers)
    {

      //cout << " QN1 = " << (*qnums_iter) << " , " << *(qnums_iter+1)<< endl;
      ii=1;
      qnums_iter2=qnums_iter+ii*NQNumbers;
      while (qnums_iter2<QNumbers.end())
	{
	  //cout << " QN2 = " << (*qnums_iter2) << " , " << *(qnums_iter2+1)<< endl;
	  equal=1;
	  i1=0;
	  for (qnums_ii2=qnums_iter2;
	       qnums_ii2<qnums_iter2+NQNumbers;qnums_ii2++)
	    {
	      qnums_ii1=qnums_iter+i1;
	      if ( (*qnums_ii2==*qnums_ii1)&&(equal==1) ) equal=1;
	      else equal=0;
	      i1++;
	    }
	  if (equal==1)
	    {
	      //cout << "Found equal " << endl;
	      QNumbers.erase(qnums_iter2,qnums_iter2+NQNumbers);
	      ii--;
	    }
	  ii++;
	  qnums_iter2=qnums_iter+ii*NQNumbers;
	}
    }

}

//////////////
void CNRGarray::ClearAll(){

  QNumbers.clear();
  dEn.clear();
  dEigVec.clear();
  iDegen.clear();

  BlockBegEnd.clear();

  ChildStates.clear();

  Kept.clear();

}

//////////////
double CNRGarray::GetQNumber(int iblock, int whichqn){

  int iq=iblock*NQNumbers+whichqn;

  if (QNumbers.size()>0)
     return(QNumbers[iq]);
  else
     return(0.0);

}

//////////////
int CNRGarray::GetBlockLimit(int iblock, int whichlimit){

  if(iblock<0)
    return(0);
  else
    return(BlockBegEnd[2*iblock+whichlimit]);

}

//////////////
int CNRGarray::GetBlockSize(int iblock){

  return(BlockBegEnd[2*iblock+1]-BlockBegEnd[2*iblock]+1);

}

//////////////
int CNRGarray::GetBlockSize(int iblock, bool kp){

  int Nstkp=0;

  if (Kept.size()<=BlockBegEnd[2*iblock+1]) return(0);

  for (int ii=BlockBegEnd[2*iblock];ii<=BlockBegEnd[2*iblock+1];ii++){

    if (Kept[ii]==kp) Nstkp++;

  }

  return(Nstkp);

}

//////////////
int CNRGarray::GetBlockFromSt(int ist,int &pos_in_block){

  // Find which block ist belongs to:
  // FInds iblock such that
  // BlockBegEnd[2*iblock]<=ist<=BlockBegEnd[2*iblock+1]

  int iblock=0;

  while (ist>BlockBegEnd[2*iblock+1]) {iblock++;}

  pos_in_block=ist-BlockBegEnd[2*iblock];

  // if state is not there (holes in BlockBegEnd, pos_in_block is negative.

  return(iblock);
}
/////////////
int CNRGarray::GetBlockLimitEigv(int iblock, int whichlimit){

  // Returns position in dEigVec corresponding to block iblock

  int posEig=0;
  int NstBl=0;

  for (int ii=0;ii<iblock;ii++)
    {
      NstBl=GetBlockSize(ii);
      posEig+=NstBl*NstBl;
    }

  NstBl=GetBlockSize(iblock);
  posEig+=whichlimit*(NstBl*NstBl - 1);

  return(posEig);

}

/////////////
double CNRGarray::GetBlockEmin(int iblock){

  double Emin = *min_element(dEn.begin()+GetBlockLimit(iblock,0),
			     dEn.begin()+GetBlockLimit(iblock,1)+1);

  return(Emin);
}

/////////////
double CNRGarray::GetBlockEmax(int iblock){

  double Emax = *max_element(
			     dEn.begin()+GetBlockLimit(iblock,0),
			     dEn.begin()+GetBlockLimit(iblock,1)+1);

  return(Emax);
}

/////////////
/////////////

// bool GTEcut (double E, double Ecut) {
//   return (E>Ecut);
// }

int CNRGarray::GetBlockEcutPos(int iblock,double Ecut){

  vector<double>::iterator FindEnGTEcut=dEn.begin()+GetBlockLimit(iblock,0);

   while ( (*FindEnGTEcut<=Ecut)
 	  &&(FindEnGTEcut<=dEn.begin()+GetBlockLimit(iblock,1)) )
//  to allow close-by states to be retained (did not work)
//   while ( ( dLTPrec(*FindEnGTEcut,Ecut,0.01/fabs(Ecut)) )
// 	  &&(FindEnGTEcut<=dEn.begin()+GetBlockLimit(iblock,1)) )
    {FindEnGTEcut++;}

//   =find_if(dEn.begin()+GetBlockLimit(iblock,0),
// 	      dEn.begin()+GetBlockLimit(iblock,1),
// 	      GTEcut); // doesnt work

  return((int)(FindEnGTEcut-dEn.begin()));
}

/////////////
void CNRGarray::SetKept(int Ncutoff){

  // New (2019)

  SetDegen();

  //

  double dEcut=Ecut(Ncutoff);

  Kept.clear();
  vector<double>::iterator it;

  cout << "SetKept: Ncutoff = " << Ncutoff
       << " Ecut = " << dEcut << endl;

  int Nkept=0;
  int Ndisc=0;

  for (it=dEn.begin();it<dEn.end();it++){
    if ((*it)<=dEcut){
      Kept.push_back(true); Nkept++;
    }else{Kept.push_back(false);Ndisc++;}

  }

  cout << "SetKept: Nkept = " << Nkept << " Ndisc = " << Ndisc << endl;

}
////////////
bool CNRGarray::CheckKept(int ist, bool kp){
  //
  // Checks if Keep[ist] matches kp

  if (Kept.size()<=ist){return(false);}
  else{
    if (Kept[ist]==kp) return(true);
    else return(false);
  }
}

bool CNRGarray::CheckKept(int iblock, int istbl, bool kp){

  int ist=GetBlockLimit(iblock,0)+istbl;

  return(CheckKept(ist,kp));
}

/////////////
int CNRGarray::GetBlockFromQNumbers(double *qnums){

  // uses predicate "dEqual" defined NRGfunctions.hpp

  vector <double>::iterator it=QNumbers.begin()-1;
  bool ok=false;

  // watch out for false alarms
  while (!ok)
   {
  it = search (it+1, QNumbers.end(), qnums, qnums+NQNumbers,dEqual);

    ok=(((int)(it-QNumbers.begin()))%NQNumbers==0)||((int)(it-QNumbers.end())==0)?true:false;
   }

  //return (int(it-QNumbers.begin())/NQNumbers);
  // Adding safeguards
  int ibl=(int)(it-QNumbers.begin())/NQNumbers;
  if (ibl>NumBlocks()-1){
    //cout << "GetBlockFromQNumbers: ibl larger than NumBlocks()-1. Returning -1" << endl;
    ibl=-1;
  }

  return (ibl);

  // Adding safeguards

}

///////////// April 08
double CNRGarray::GetQNumberFromSt(int ist, int whichqn){

  int pos=0;
  int ibl=GetBlockFromSt(ist,pos);

  return(GetQNumber(ibl,whichqn));

}
/////////////
boost::numeric::ublas::matrix<double> CNRGarray::EigVec2BLAS(int iblock){

  // Converts the section in dEigVec corresponding to block iblock to uBLAS
  // Format: each eigenvector in a ROW!

  int Nst=GetBlockSize(iblock);
  int pos=GetBlockLimitEigv(iblock,0);

  //cout << "NstBlock = " << Nst << "  Pos in dEigVec = " << pos << endl;

  boost::numeric::ublas::matrix<double> Maux(Nst,Nst);

//   copy(dEigVec.begin()+pos,dEigVec.begin()+pos+Nst*Nst,
// 	   Maux.begin2());

  // Let me try this:

  vector<double>::iterator it;

  it=dEigVec.begin()+pos;
  for (int ii=0;ii<Nst;ii++)
    {
      for (int jj=0;jj<Nst;jj++)
	{
	  Maux(ii,jj)=*it;
	  it++;
	}
    }

  //return(boost::numeric::ublas::trans(Maux)); // In Columns
  return(Maux);          // In Rows

}
/////////////
boost::numeric::ublas::matrix<complex<double> > CNRGarray::cEigVec2BLAS(int iblock){

  // Converts the section in cEigVec corresponding to block iblock to uBLAS
  // Format: each eigenvector in a ROW! COMPLEX MATRIX

  int Nst=GetBlockSize(iblock);
  int pos=GetBlockLimitEigv(iblock,0);

  //cout << "NstBlock = " << Nst << "  Pos in dEigVec = " << pos << endl;

  boost::numeric::ublas::matrix<complex<double> > Maux(Nst,Nst);

//   copy(dEigVec.begin()+pos,dEigVec.begin()+pos+Nst*Nst,
// 	   Maux.begin2());

  // Let me try this:

  vector<complex<double> >::iterator it;

  it=cEigVec.begin()+pos;
  for (int ii=0;ii<Nst;ii++)
    {
      for (int jj=0;jj<Nst;jj++)
	{
	  Maux(ii,jj)=*it;
	  it++;
	}
    }

  //return(boost::numeric::ublas::trans(Maux)); // In Columns
  return(Maux);          // In Rows

}
///////

///////////// April 09
double CNRGarray::GetEigVecComponent(int ist,int istbasis){

  // For "uncut" vectors only!
  // A CNRGbasisarray version for this one and for
  // GetBlockLimitEigv should do the job
  // in cut objects.
  //

  if (dEigVec.size()==0){
    cout << "GetEigVecComponent error: No dEigVec. Return 0." << endl;
    return(0.0);
  }

  int posBl;
  int posBlbasis;
  int ibl=GetBlockFromSt(ist,posBl);
  int ibl_basis=GetBlockFromSt(istbasis,posBlbasis);
  int bl_size=GetBlockSize(ibl);
  int ieigv0=GetBlockLimitEigv(ibl,0);

  if (ibl==ibl_basis){
    vector<double>::iterator it;
    it=dEigVec.begin()+ieigv0+posBl*bl_size+posBlbasis;

    return(*it);
  }
  else{return(0.0);}

}

/////////////
double CNRGarray::Ecut(int Ncut){

  vector<double> Eaux=dEn;
  double aux,aux2;

  sort(Eaux.begin(),Eaux.end());

  //  if (Ncut>Eaux.size()) aux=Eaux.back();
  //  else aux=Eaux[Ncut];

// Let's try this
  if ( Ncut>=(Eaux.size()-1) ) aux=Eaux.back();
  else{
    aux=Eaux[Ncut-1]; // state Ncut
    aux2=Eaux[Ncut];  // state Ncut+1
    cout << " CNRGarray::Ecut : Original Ecut = "<< aux
	 << " Ecut+1 = " << aux2 << endl;
    //check for near-degenerate levels
    int ii=1;
//     while ( (aux2>0.0)&&(fabs((aux2-aux)/aux)<0.001)
    while ( (aux2>0.0)&&(fabs((aux2-aux)/aux)<0.01)
	    &&(ii<Eaux.size()) ){aux2=Eaux[Ncut+ii];ii++;}
    if (ii>1) {aux=Eaux[Ncut+ii-2];aux2=Eaux[Ncut+ii-1];}
    cout << " CNRGarray::Ecut : New Ecut = "<< aux
	 << " New Ecut+1 = " << aux2
         << " Nkept = " << Ncut+ii-1 << endl;
  }

  return(aux);
}

///////////// May 08
int CNRGarray::GetiGS(){

  // returns *position* of the minimum element in dEn

  vector<double>::iterator it=min_element(dEn.begin(),dEn.end());

  int iGS=0;
  while (it>dEn.begin()){it--;iGS++;}

  return(iGS);

}

void CNRGarray::SaveQSParameters(){

  cout << "Saving in old format " << endl;

  // outstream
  ofstream OutFile;
  char arqname[32];
  char CNsites[8];

  sprintf(CNsites,"%d",Nshell);

  //
  // Saving QS parameters
  //

  strcpy(arqname,"DataQSN");
  strcat(arqname,CNsites);
  strcat(arqname,".dat");

  cout << "File is " << arqname << endl;

  OutFile.open(arqname);

  int iGS=GetiGS();
  int pos=0;
  int blockGS=GetBlockFromSt(iGS,pos);

  cout << " Nsites " << Nshell+1 << " Nstates " << Nstates()
       << " iGS = " << iGS << " BlockGS = " << blockGS
       << " NumQSblocks = " << NumBlocks()
       << endl;

  OutFile << Nshell+1 << " " << Nstates()
       << " " << iGS << " " << blockGS
       << " " << NumBlocks()
       << endl;

  for (int iblock=0;iblock<NumBlocks();iblock++)
    {
      OutFile << fixed << GetQNumber(iblock,0) << "  " << GetQNumber(iblock,1) << endl;
    }
  OutFile << resetiosflags (ios_base::floatfield);
  for (int iblock=0;iblock<NumBlocks();iblock++)
    {
      OutFile << GetBlockLimit(iblock,0) << "  " << GetBlockLimit(iblock,1) << endl;
    }

  OutFile.close();

  //
  // Saving Energies
  //

  strcpy(arqname,"DataEQSN");
  strcat(arqname,CNsites);
  strcat(arqname,".bin");

  // Using C format...
  FILE *pFile;

  pFile=fopen(arqname,"wb");
  fwrite (&dEn[0], sizeof(double), dEn.size() ,pFile);
  fclose(pFile);

}
//

void CNRGarray::SaveBin(char arqname[]){

  vector<double>::iterator dit;
  vector<int>::iterator iit;
  vector<bool>::iterator boolit;
  vector<complex<double> >::iterator cit;

  ofstream OutFile;
  double daux;
  int iaux;
  bool auxbool;

  complex<double> caux;

  OutFile.open(arqname, ios::out | ios::binary);

  if (!OutFile){cout << "SaveBin: Cannot save data in " << arqname << endl; return;}

  // Save Nshell
  OutFile.write((char*)&Nshell, sizeof(int));
  // Save NQNumbers
  OutFile.write((char*)&NQNumbers, sizeof(int));
  // Save QNumbers
  iaux=QNumbers.size();
  OutFile.write((char*)&iaux, sizeof(int));
  for(dit=QNumbers.begin();dit<QNumbers.end();dit++){
    daux=*dit;
    OutFile.write(reinterpret_cast<char*>(&daux), sizeof(double));
  }
  // Save dEn
  iaux=dEn.size();
  OutFile.write((char*)&iaux, sizeof(int));
  for(dit=dEn.begin();dit<dEn.end();dit++){
    // (&(*dit)) is weird but it is right...
    OutFile.write(reinterpret_cast<char*>(&(*dit)), sizeof(double));
  }
  // Save dEigVec
  iaux=dEigVec.size();
  OutFile.write((char*)&iaux, sizeof(int));
  for(dit=dEigVec.begin();dit<dEigVec.end();dit++){
    // (&(*dit)) is weird but it is right...
    OutFile.write(reinterpret_cast<char*>(&(*dit)), sizeof(double));
  }
  // Save cEigVec
  iaux=cEigVec.size();
  OutFile.write((char*)&iaux, sizeof(int));
  for(cit=cEigVec.begin();cit<cEigVec.end();cit++){
    // (&(*cit)) is weird but it is right...
    OutFile.write(reinterpret_cast<char*>(&(*cit)), sizeof(complex<double> ));
  }
  // Save BlockBegEnd
  iaux=BlockBegEnd.size();
  OutFile.write((char*)&iaux, sizeof(int));
  for(iit=BlockBegEnd.begin();iit<BlockBegEnd.end();iit++){
    OutFile.write(reinterpret_cast<char*>(&(*iit)), sizeof(int));
  }
  // Save iDegen
  iaux=iDegen.size();
  OutFile.write((char*)&iaux, sizeof(int));
  for(iit=iDegen.begin();iit<iDegen.end();iit++){
    OutFile.write(reinterpret_cast<char*>(&(*iit)), sizeof(int));
  }
  // Save Kept
  iaux=Kept.size();
  OutFile.write((char*)&iaux, sizeof(int));
  for(boolit=Kept.begin();boolit<Kept.end();boolit++){
    //OutFile.write(reinterpret_cast<char*>(&(*boolit)), sizeof(bool));
    auxbool=*boolit;
    OutFile.write((char*)&auxbool, sizeof(bool));
  }
  // Save totalS,Sqnumbers
  OutFile.write((char*)&totalS, sizeof(bool));
  iaux=Sqnumbers.size();
  OutFile.write((char*)&iaux, sizeof(int));
  for(iit=Sqnumbers.begin();iit<Sqnumbers.end();iit++){
    OutFile.write(reinterpret_cast<char*>(&(*iit)), sizeof(int));
  }

  OutFile.close();

  if (!OutFile.good()){cout << "SaveBin: error saving" << endl;}

}
///

void CNRGarray::ReadBin(char arqname[]){

  // outstream
  ifstream InFile;

  InFile.open(arqname, ios::in | ios::binary);

  ReadBinInStream(InFile);

  InFile.close();

  if (!InFile.good()){cout << "ReadBin: error reading" << endl;}

}
///

void CNRGarray::ReadBinInStream(ifstream &InFile){

  int isize,iaux, iaux2;
  double daux;
  bool boolaux;
  complex<double> caux;

  // Read Nshell
  InFile.read((char*)&Nshell, sizeof(int));
  // Read NQNumbers
  InFile.read((char*)&NQNumbers, sizeof(int));
  // Read QNumbers
  QNumbers.clear();
  InFile.read((char*)&isize, sizeof(int));
  for(iaux=0;iaux<isize;iaux++){
    InFile.read(reinterpret_cast<char*>(&daux), sizeof(double));
    QNumbers.push_back(daux);
  }
  // Read dEn
  dEn.clear();
  InFile.read((char*)&isize, sizeof(int));
  for(iaux=0;iaux<isize;iaux++){
    InFile.read(reinterpret_cast<char*>(&daux), sizeof(double));
    dEn.push_back(daux);
  }
  // Read dEigVec
  dEigVec.clear();
  InFile.read((char*)&isize, sizeof(int));
  for(iaux=0;iaux<isize;iaux++){
    InFile.read(reinterpret_cast<char*>(&daux), sizeof(double));
    dEigVec.push_back(daux);
  }
  // Read cEigVec
  cEigVec.clear();
  InFile.read((char*)&isize, sizeof(int));
  for(iaux=0;iaux<isize;iaux++){
    InFile.read(reinterpret_cast<char*>(&caux), sizeof(complex<double> ));
    cEigVec.push_back(caux);
  }
  // Read BlockBegEnd
  BlockBegEnd.clear();
  InFile.read((char*)&isize, sizeof(int));
  for(iaux=0;iaux<isize;iaux++){
    InFile.read(reinterpret_cast<char*>(&iaux2), sizeof(int));
    BlockBegEnd.push_back(iaux2);
  }
  // Read iDegen
  iDegen.clear();
  InFile.read((char*)&isize, sizeof(int));
  for(iaux=0;iaux<isize;iaux++){
    InFile.read(reinterpret_cast<char*>(&iaux2), sizeof(int));
    iDegen.push_back(iaux2);
  }
  // Read Kept
  Kept.clear();
  InFile.read((char*)&isize, sizeof(int));
  for(iaux=0;iaux<isize;iaux++){
    //InFile.read(reinterpret_cast<char*>(&boolaux), sizeof(bool));
    InFile.read((char*)(&boolaux), sizeof(bool));
    Kept.push_back(boolaux);
  }
  // Read totalS,Sqnumbers
  InFile.read((char*)&totalS, sizeof(bool));
  Sqnumbers.clear();
  InFile.read((char*)&isize, sizeof(int));
  for(iaux=0;iaux<isize;iaux++){
    InFile.read(reinterpret_cast<char*>(&iaux2), sizeof(int));
    Sqnumbers.push_back(iaux2);
  }

}

//////////////
double CNRGarray::PartitionFuncTeq0(){

  // Works only for Q,Sz basis (not anymore)
  // 04/2010: Including 2S+1 degeneracy factor for SU(2) symmetries
  //

  double sum=0.0;

//   vector<double>::iterator dit;

//   for (dit=dEn.begin();dit<dEn.end();dit++){
//     if (dEqualPrec(abs((*dit)),0.0,1e-10)) sum+=1.0;
//   }

  for (int ibl=0;ibl<NumBlocks();ibl++){
    double Si=0.0;
    if (totalS){
      Si=GetQNumber(ibl,Sqnumbers[0]); // only a single SU(2) for now
    }

    for (int ist=GetBlockLimit(ibl,0);ist<=GetBlockLimit(ibl,1);ist++){
      if ( dEqualPrec(abs(dEn[ist]),0.0,1e-10) ) sum+=2.0*Si+1.0;
    }
    // end loop in states
  }
  // end loop in blocks

//
 return(sum);

}

//////////////
double CNRGarray::PartitionFunc(double betabar){

  // Works only for Q,Sz basis (not anymore)
  // 04/2010: Including 2S+1 degeneracy factor for SU(2) symmetries
  //

  double sum=0.0;

//   vector<double>::iterator dit;

//   for (dit=dEn.begin();dit<dEn.end();dit++){
//     sum+=exp(-betabar*(*dit));
//   }

  for (int ibl=0;ibl<NumBlocks();ibl++){
    double Si=0.0;
    if (totalS){
      Si=GetQNumber(ibl,Sqnumbers[0]); // only a single SU(2) for now
    }

    for (int ist=GetBlockLimit(ibl,0);ist<=GetBlockLimit(ibl,1);ist++){
      sum+=(2.0*Si+1.0)*exp(-betabar*(dEn[ist]));
    }
    // end loop in states
  }
  // end loop in blocks

  return(sum);
}

//////////////
//////////////
double CNRGarray::PartitionFuncDisc(double betabar){

  double sum=0.0;

  if (Kept.size()==0){
    cout << "PartitionFuncDisc: Kept not defined. Returning -1.0 " << endl;
    return(-1.0);
  }

  for (int ibl=0;ibl<NumBlocks();ibl++){
    double Si=0.0;
    if (totalS){
      Si=GetQNumber(ibl,Sqnumbers[0]); // only a single SU(2) for now
    }

    for (int ist=GetBlockLimit(ibl,0);ist<=GetBlockLimit(ibl,1);ist++){
      if (Kept[ist]==false)
	sum+=(2.0*Si+1.0)*exp(-betabar*(dEn[ist]));
    }
    // end loop in states
  }
  // end loop in blocks

  return(sum);
}

//////////////

void CNRGarray::SetEigVecToOne(){

  // Sets eigenvectors to (1 0 0, 0 1 0, 0 0 1... on each block in the basis)
  // Creates an "uncut" dEigVec with Nbl x Nbl block components
  dEigVec.clear();

  for(int ibl=0;ibl<NumBlocks();ibl++){
    // within each block
    int ist0=GetBlockLimit(ibl,0);
    int istF=GetBlockLimit(ibl,1);
    for (int ist1=ist0;ist1<=istF;ist1++){
      for (int ist2=ist0;ist2<=istF;ist2++){
	if (ist1==ist2)
	  dEigVec.push_back(1.0); // diagonal
	else
	  dEigVec.push_back(0.0); // off-diagonal
      }
    }
    // end loops in block states
  }
  // end block loops

}

///////////////

boost::numeric::ublas::matrix<double> CNRGarray::ExpEi2BLAS(int iblock, double betabar){

  int Nst1=GetBlockSize(iblock);

  int ist=GetBlockLimit(iblock,0);

  // NO NEED!
//   double Si=0.0;
//   if (totalS){
//     Si=GetQNumber(iblock,Sqnumbers[0]);
//     // only a single SU(2) for now
//   }

  boost::numeric::ublas::matrix<double> dMatAux(Nst1,Nst1);

  for (int ii=0;ii<Nst1;ii++){
    for (int jj=0;jj<Nst1;jj++){
      if (ii==jj)
	// WRONG
// 	dMatAux(ii,jj)=(2.0*Si+1.0)*exp(-betabar*dEn[ist]);
	dMatAux(ii,jj)=exp(-betabar*dEn[ist]);
      else dMatAux(ii,jj)=0.0;
    }
    ist++;
  }
  // end loop

  return(dMatAux);

}
//

///////////////

bool CNRGarray::CheckComplex(){

  bool result=false;

  if ( (cEigVec.size()>0)&&(dEigVec.size()==0) ) result=true;

  return(result);

}

///////////////

//////////////////////////////////////////////////
//////////////////////////////////////////////////
//                                              //
//                                              //
//     Class CNRGbasisarray functions           //
//                                              //
//                                              //
//////////////////////////////////////////////////
//////////////////////////////////////////////////

//////////////
//void CNRGbasisarray::ClearVecsBasis(){
void CNRGbasisarray::ClearAll(){

  CNRGarray::ClearAll();

  iType.clear();
  StCameFrom.clear();
  BlockBegEndBC.clear();
  BlockBegEndEigVec.clear();

  StCameFromQNumbers.clear();

}
//////////////
void CNRGbasisarray::PrintAll(){

  CNRGarray::PrintAll();

  cout << "Type : " << endl;
  for (int ii=0;ii<iType.size();ii++)
    cout << iType[ii] << " ";
  cout << endl;

  cout << "StCameFrom : " << endl;
  for (int ii=0;ii<StCameFrom.size();ii++)
    cout << StCameFrom[ii] << " ";
  cout << endl;

  cout << "iDegen : " << endl;
  for (int ii=0;ii<iDegen.size();ii++)
    cout << iDegen[ii] << " ";
  cout << endl;

  cout << "BlockBegEndBC : " << endl;
  for (int ii=0;ii<BlockBegEndBC.size();ii+=2)
    cout << BlockBegEndBC[ii] << " "<< BlockBegEndBC[ii+1] << ", ";
  cout << endl;

  cout << "BlockBegEndEigVec : " << endl;
  for (int ii=0;ii<BlockBegEndEigVec.size();ii++)
    cout << BlockBegEndEigVec[ii] << " ";
  cout << endl;

  cout << "StCameFromQNumbers : " << endl;
  for (int ii=0;ii<StCameFromQNumbers.size();ii++)
    {
      if ( (ii%NQNumbers_stcf==0) ) cout << ii/NQNumbers_stcf << ": ";
      //cout << ii << " -  " << StCameFromQNumbers[ii] << " ";
      cout << StCameFromQNumbers[ii] << " ";
      if ( ((ii+1)%NQNumbers_stcf==0) ) cout << endl;
    }

}
///////////////////
//////////////
void CNRGbasisarray::PrintBlockBasis(int iblock){

  CNRGarray::PrintBlockQNumbers(iblock);

  int iBeg=GetBlockLimit(iblock,0);
  int iEnd=GetBlockLimit(iblock,1);
  cout << " Beg: " << iBeg << " End: " << iEnd << endl;
  for (int ii=iBeg;ii<=iEnd;ii++){
    cout << ii << " ";
    if (dEn.size()>0)
      cout << scientific << "Eold = " << dEn[ii] << "  ";
    if (iType.size()>0)
      cout << fixed << "type = " << iType[ii] << "  ";
    if (StCameFrom.size()>0)
      cout << fixed << "StCFrom = " << StCameFrom[ii]  << "  ";
    if (iDegen.size()>ii)
      cout << fixed << "iDeg = " << iDegen[ii] << "  ";
    cout << resetiosflags (ios_base::floatfield);
    if (StCameFromQNumbers.size()>0)
      {
	cout << "SCF_QNbrs: ";
	for (int jj=ii*NQNumbers_stcf;jj<(ii+1)*NQNumbers_stcf;jj++)
	  cout << StCameFromQNumbers[jj] << " ";
      }
    cout << endl;

  }
  // end loop in ii

}
/////////////
void  CNRGbasisarray::SetSCFfromdEn(){

  // Sets StCameFrom

  StCameFrom.clear();
  for (int ii=0;ii<dEn.size();ii++) StCameFrom.push_back(ii);

}

/////////////

//////////////
int CNRGbasisarray::GetBlockSizeBC(int iblock){

  if(BlockBegEndBC.size()==0){return(GetBlockSize(iblock));}

  return(BlockBegEndBC[2*iblock+1]-BlockBegEndBC[2*iblock]+1);

}

//////////////
int CNRGbasisarray::GetBlockFromStBC(int ist,int &pos_in_block){

  // Find which block ist belongs in the BC structure:
  // FInds iblock such that
  // BlockBegEnd[2*iblock]<=ist<=BlockBegEnd[2*iblock+1]

  if (BlockBegEndBC.size()==0){
    int auxst=CNRGarray::GetBlockFromSt(ist,pos_in_block);
    return(auxst);
  }
  else{

    int iblock=0;

    while (ist>BlockBegEndBC[2*iblock+1]) {iblock++;}

    pos_in_block=ist-BlockBegEndBC[2*iblock];

    return(iblock);
  }
  // end if else
}

//////////////
void CNRGbasisarray::SetBlockBegEndEigVec(){

  BlockBegEndEigVec.clear();

  int Nbls=(int)BlockBegEndBC.size()/2;
  if (Nbls==0) return;

  int aux=0;
  for (int ibl=0;ibl<Nbls;ibl++)
    {
      BlockBegEndEigVec.push_back(aux);
      aux+=GetBlockSizeBC(ibl)*GetBlockSizeBC(ibl)-1;
      BlockBegEndEigVec.push_back(aux);
      aux++;
    }

}

/////////////
void CNRGbasisarray::RemoveBlock(int iblock){

  // Checks StCameFrom

  if ( (dEn.size()==0)||( (dEigVec.size()==0)&&(cEigVec.size()==0) )
       ||(StCameFrom.size()==0) ){
      cout << "Cant remove block: " << iblock << endl;
      return;
    }
  //CNRGbasisarray::PrintAll();

  // Erase elements in dEn, dEigVec, StCameFrom, Kept

  dEn.erase(dEn.begin()+GetBlockLimit(iblock,0),
	    dEn.begin()+GetBlockLimit(iblock,1)+1);

  StCameFrom.erase(StCameFrom.begin()+GetBlockLimit(iblock,0),
		   StCameFrom.begin()+GetBlockLimit(iblock,1)+1);

  Kept.erase(Kept.begin()+GetBlockLimit(iblock,0),
	    Kept.begin()+GetBlockLimit(iblock,1)+1);

  // Watch out
  if (dEigVec.size()>0)
  dEigVec.erase(dEigVec.begin()+BlockBegEndEigVec[2*iblock],
		dEigVec.begin()+BlockBegEndEigVec[2*iblock+1]+1);

  if (cEigVec.size()>0)
  cEigVec.erase(cEigVec.begin()+BlockBegEndEigVec[2*iblock],
		cEigVec.begin()+BlockBegEndEigVec[2*iblock+1]+1);

  // Erase elements in QNumbers

  QNumbers.erase(QNumbers.begin()+NQNumbers*iblock,
		 QNumbers.begin()+NQNumbers*(iblock+1));

  // Re-arrange BlockBegEnd
  // Re-arrange BlockBegEndEigVec

  for (int ibl1=iblock;ibl1<NumBlocks();ibl1++)
    {
      int Size_next=GetBlockSize(ibl1+1);
      BlockBegEnd[2*ibl1+1]=BlockBegEnd[2*ibl1]+Size_next-1;
      BlockBegEnd[2*ibl1+2]=BlockBegEnd[2*ibl1]+Size_next;

      int Size_next2=Size_next*Size_next;
      BlockBegEndEigVec[2*ibl1+1]=BlockBegEndEigVec[2*ibl1]+Size_next2-1;
      BlockBegEndEigVec[2*ibl1+2]=BlockBegEndEigVec[2*ibl1]+Size_next2;

    }
  BlockBegEnd.pop_back();
  BlockBegEnd.pop_back();

  BlockBegEndEigVec.pop_back();
  BlockBegEndEigVec.pop_back();

  // Erase elements in BlockBegEndBC

  BlockBegEndBC.erase(BlockBegEndBC.begin()+2*iblock,
		      BlockBegEndBC.begin()+2*(iblock+1));

}

/////////////

/////////////
void CNRGbasisarray::RemoveState(int ist){

  // Now it does cut egenvectors as well!!

  // Checks vectors

  if ( (dEn.size()==0)||(StCameFrom.size()==0) )
    return;

  int pos_in_bl;
  int iblock=CNRGarray::GetBlockFromSt(ist,pos_in_bl);
  int bl_size=CNRGarray::GetBlockSize(iblock);
  int bl_sizeBC=GetBlockSizeBC(iblock);

  if (BlockBegEnd[2*iblock]==
      BlockBegEnd[2*iblock+1]) CNRGbasisarray::RemoveBlock(iblock);
  else{

    dEn.erase(dEn.begin()+ist);

    StCameFrom.erase(StCameFrom.begin()+ist);

    Kept.erase(Kept.begin()+ist);

    // This doesn't work:
    //       vector<double>::iterator it=dEigVec.begin()+GetBlockLimitEigv(iblock,0)+bl_size*pos_in_bl;

    //       dEigVec.erase(it,it+bl_size);
    //
    //
    // bl_size should be the original block size but
    // no information on the original block length is available anymore.
    //
    // Needs a "BlockBegEndBC" or something.
    //
    // But we won't be needing the eigenvectors to generate the basis anyway.
    //

    // Now this should work:

    vector<double>::iterator it=dEigVec.begin()+
      BlockBegEndEigVec[2*iblock]+bl_sizeBC*pos_in_bl;

    vector<complex<double> >::iterator cit=cEigVec.begin()+
      BlockBegEndEigVec[2*iblock]+bl_sizeBC*pos_in_bl;

//     if (it+bl_sizeBC>dEigVec.end()){
    if ( (it+bl_sizeBC>dEigVec.end())&&(cit+bl_sizeBC>cEigVec.end()) ) {
      cout << " Ops, Problems in removing eigvec! ";
      cout << "Removing section of EigVec. bl_sizeBC = " << bl_sizeBC;
      cout << " BegEigVec at : " << BlockBegEndEigVec[2*iblock];
      cout << " EndEigVec at : " << BlockBegEndEigVec[2*iblock+1];
      cout << " bl_sizeBC*pos_in_bl : " << bl_sizeBC*pos_in_bl;
      cout << endl;
      exit(0);
    }

    if (it+bl_sizeBC<=dEigVec.end())
      dEigVec.erase(it,it+bl_sizeBC);
    if (cit+bl_sizeBC<=cEigVec.end())
      cEigVec.erase(cit,cit+bl_sizeBC);

    //Update BlockBegEnd
    //Update BlockBegEndEigVec
    BlockBegEnd[2*iblock+1]-=1;
    BlockBegEndEigVec[2*iblock+1]-=bl_sizeBC;

    for (int ibl1=iblock+1;ibl1<NumBlocks();ibl1++){
      BlockBegEnd[2*ibl1]-=1;
      BlockBegEnd[2*ibl1+1]-=1;

      BlockBegEndEigVec[2*ibl1]-=bl_sizeBC;
      BlockBegEndEigVec[2*ibl1+1]-=bl_sizeBC;
    }

  }

}

/////////////
void CNRGbasisarray::RemoveStatesFromBlock(int iblock, int ist1,int ist2){

  // Now it does cut egenvectors as well!!
  // Similar as RemoveState but does that in blocks

  // Checks vectors

  if ( (dEn.size()==0)||(StCameFrom.size()==0) )
    return;

  int NstGone=ist2-ist1+1;

  // Make sure that ist1 and ist2 belong to iblock
  int pos_in_bl[2];
  int ibl_check1=CNRGarray::GetBlockFromSt(ist1,pos_in_bl[0]);
  int ibl_check2=CNRGarray::GetBlockFromSt(ist2,pos_in_bl[1]);
  if ( (ibl_check1!=iblock)||(ibl_check2!=iblock) ){
    cout << " Error in RemoveStatesFromBlock: ist1, ist2  are not within block " << endl;
    return;
  }

  int bl_size=CNRGarray::GetBlockSize(iblock);
  int bl_sizeBC=GetBlockSizeBC(iblock);

  if (BlockBegEnd[2*iblock]==
      BlockBegEnd[2*iblock+1]) CNRGbasisarray::RemoveBlock(iblock);
  else{

    dEn.erase(dEn.begin()+ist1,dEn.begin()+ist2+1); // Check if works

    StCameFrom.erase(StCameFrom.begin()+ist1,StCameFrom.begin()+ist2+1);

    Kept.erase(Kept.begin()+ist1,Kept.begin()+ist2+1);
    //
    // Now the tricky part: the eigenvector
    //

    //
    // Iterator set at begining of first vector
    //
    vector<double>::iterator it=dEigVec.begin()+
      BlockBegEndEigVec[2*iblock]+bl_sizeBC*pos_in_bl[0];
    vector<complex<double> >::iterator cit=cEigVec.begin()+
      BlockBegEndEigVec[2*iblock]+bl_sizeBC*pos_in_bl[0];

//     if (it+bl_sizeBC>dEigVec.end()){
    if ( (it+bl_sizeBC>dEigVec.end())&&(cit+bl_sizeBC>cEigVec.end()) ){
      cout << " Ops, Problems in removing eigvec! ";
      cout << "Removing section of EigVec. bl_sizeBC = " << bl_sizeBC;
      cout << " BegEigVec at : " << BlockBegEndEigVec[2*iblock];
      cout << " EndEigVec at : " << BlockBegEndEigVec[2*iblock+1];
      cout << " bl_sizeBC*pos_in_bl[0] : " << bl_sizeBC*pos_in_bl[0];
      cout << endl;
      return;
    }

    if (it+bl_sizeBC<=dEigVec.end())
      dEigVec.erase(it,it+bl_sizeBC*NstGone); // Should work

    if (cit+bl_sizeBC<=cEigVec.end())
      cEigVec.erase(cit,cit+bl_sizeBC*NstGone); // Should work

    // Re-struct
    //Update BlockBegEnd
    //Update BlockBegEndEigVec
    BlockBegEnd[2*iblock+1]-=NstGone;
    BlockBegEndEigVec[2*iblock+1]-=bl_sizeBC*NstGone;

    for (int ibl1=iblock+1;ibl1<NumBlocks();ibl1++){
      BlockBegEnd[2*ibl1]-=NstGone;
      BlockBegEnd[2*ibl1+1]-=NstGone;

      BlockBegEndEigVec[2*ibl1]-=bl_sizeBC*NstGone;
      BlockBegEndEigVec[2*ibl1+1]-=bl_sizeBC*NstGone;
    }

  }
  // If not RemoveBlock

}
///////////// April 09
int CNRGbasisarray::GetBlockLimitEigv(int iblock, int whichlimit){

  // Returns position in dEigVec corresponding to block iblock
  // USES BlockBegEndEigVec if available!!

  if (BlockBegEndEigVec.size()==0){
    return(CNRGarray::GetBlockLimitEigv(iblock,whichlimit));
  }
  else{
//    cout << "Overloaded GetBlockLimitEigv (CNRGbasisarray)" << endl;
    return(BlockBegEndEigVec[2*iblock+whichlimit]);
  }

}
///////////////
///////////// April 09
double CNRGbasisarray::GetEigVecComponent(int ist,int istbasis){

  // Overloads in the case of "cut" vectors
  // For "uncut" vectors only!
  // A CNRGbasisarray version for this one and for
  // GetBlockLimitEigv should do the job
  // in cut objects.
  //

  if  (dEigVec.size()==0){
    cout << "GetEigVecComponent: No dEigVec. Return 0." << endl;
    return(0.0);
  }

  int posBl;
  int posBlbasis;
  int ibl=GetBlockFromSt(ist,posBl);
  int iblbasis=GetBlockFromStBC(istbasis,posBlbasis);
  int bl_sizeBC=GetBlockSizeBC(ibl);

  //  cout << "GetEigVecComponent: Using overloaded version" << endl;

  if (ibl==iblbasis){
    int ieigv0=GetBlockLimitEigv(ibl,0);
    vector<double>::iterator it;
    it=dEigVec.begin()+ieigv0+posBl*bl_sizeBC+posBlbasis;
    return(*it);
  }
  else{return(0.0);}

}

///////////////
///////////////
boost::numeric::ublas::matrix<double> CNRGbasisarray::EigVecCut2BLAS(int iblock){

  // Converts the section in dEigVec corresponding to block iblock to uBLAS
  // Format: each eigenvector in a ROW!

  int Nst=GetBlockSize(iblock);
  int pos=BlockBegEndEigVec[2*iblock];

  int NstBC=GetBlockSizeBC(iblock);

  //cout << "NstBlock = " << Nst << "  Pos in dEigVec = " << pos << endl;

  boost::numeric::ublas::matrix<double> Maux(Nst,NstBC);

  // Let me try this:

  vector<double>::iterator it;

  it=dEigVec.begin()+pos;
  for (int ii=0;ii<Nst;ii++){
    for (int jj=0;jj<NstBC;jj++){
      Maux(ii,jj)=*it;
      it++;
    }
  }
  //return(boost::numeric::ublas::trans(Maux)); // In Columns
  return(Maux);          // In Rows

}
/// Now the complex one
boost::numeric::ublas::matrix<complex<double> > CNRGbasisarray::cEigVecCut2BLAS(int iblock){

  // Converts the section in cEigVec corresponding to block iblock to uBLAS
  // Format: each eigenvector in a ROW!

  int Nst=GetBlockSize(iblock);
  int pos=BlockBegEndEigVec[2*iblock];

  int NstBC=GetBlockSizeBC(iblock);

  //cout << "NstBlock = " << Nst << "  Pos in dEigVec = " << pos << endl;

  boost::numeric::ublas::matrix<complex<double> > Maux(Nst,NstBC);

  vector<complex<double> >::iterator it;

  it=cEigVec.begin()+pos;
  for (int ii=0;ii<Nst;ii++){
    for (int jj=0;jj<NstBC;jj++){
      Maux(ii,jj)=*it;
      it++;
    }
  }
  //return(boost::numeric::ublas::trans(Maux)); // In Columns
  return(Maux);          // In Rows (complex)

}

///////////////
boost::numeric::ublas::matrix<double> CNRGbasisarray::EigVecCut2BLAS(int iblock,
								     bool kp){

  // Converts the section in dEigVec corresponding to block iblock to uBLAS
  // Format: each eigenvector in a ROW!

  // The assumption: NO CUTTING WAS DONE in the basisarray object

  int Nst=GetBlockSize(iblock);
  int pos=BlockBegEndEigVec[2*iblock];

  int NstBC=GetBlockSizeBC(iblock); // Should be equal to Nst

  int Nstkp=CNRGarray::GetBlockSize(iblock,kp);

  //cout << "NstBlock = " << Nst << "  Pos in dEigVec = " << pos << endl;

  boost::numeric::ublas::matrix<double> Maux(Nstkp,Nstkp);
  // if Nst != NstBC, then we will hake a problem here:
  // this matrix will not be square

  vector<double>::iterator it;

  it=dEigVec.begin()+pos;
  int istkp=0;
  for (int ii=0;ii<Nst;ii++){
    int jstkp=0;
    for (int jj=0;jj<NstBC;jj++){
      if (CheckKept(iblock,ii,kp)){
	Maux(istkp,jstkp)=*it;
	jstkp++;
      }
      // if state kept==kp
      it++;
    }
    // end loop in components
    if (CheckKept(iblock,ii,kp)){istkp++;}
  }
  // end loop in states

  //return(boost::numeric::ublas::trans(Maux)); // In Columns
  return(Maux);          // In Rows

}

///////////////
///////////////
///////////////
///////////////
///////////// April 08
double CNRGbasisarray::GetStCameFromQNumberFromSt(int ist, int whichqn){

  int stcf=StCameFrom[ist];

  return(StCameFromQNumbers[NQNumbers_stcf*ist+whichqn]);

}
/////////////

///////////////
///////////// June 08
/////////////
void CNRGbasisarray::CopyBlock(int iblock,int NoCopies){

  // Checks StCameFrom

  if ( (NoCopies<1)||(NoCopies>100) )
    {
      cout << " CopyBlock : NoCopies not valid " << endl;
      return;
    }

  // Set iDegen if not set yet
  if (iDegen.size()==0)
    for (int ii=0;ii<Nstates();ii++) iDegen.push_back(0);

  // Set iType if not set yet
  if (iType.size()==0)
    for (int ii=0;ii<Nstates();ii++) iType.push_back(0);

  int BlSize=GetBlockSize(iblock);

  vector<double> dAux;
  vector<int> iAux,iAux2;

//   cout << " Bl Size = " << BlSize
//        << " Limit 0: " << GetBlockLimit(iblock,0)
//        << " Limit 1: " << GetBlockLimit(iblock,1)
//        << endl;

  dAux.assign(dEn.begin()+GetBlockLimit(iblock,0),
 	      dEn.begin()+GetBlockLimit(iblock,1)+1);

  iAux.assign(iType.begin()+GetBlockLimit(iblock,0),
 	      iType.begin()+GetBlockLimit(iblock,1)+1);

  iAux2.assign(StCameFrom.begin()+GetBlockLimit(iblock,0),
 	      StCameFrom.begin()+GetBlockLimit(iblock,1)+1);

  for (int ic=1;ic<=NoCopies;ic++)
    {
      // Copy to dEn
      dEn.insert(dEn.begin()+GetBlockLimit(iblock,1)+1,dAux.begin(),dAux.end());
      // Copy to iType
      iType.insert(iType.begin()+GetBlockLimit(iblock,1)+1,iAux.begin(),iAux.end());
      // Copy to StCameFrom
      StCameFrom.insert(StCameFrom.begin()+GetBlockLimit(iblock,1)+1,iAux2.begin(),iAux2.end());

      // Update iDegen

      iDegen.insert(iDegen.begin()+GetBlockLimit(iblock,1)+1,BlSize,ic);

      // Re-arrange BlockBegEnd, Get ready for next copy
      BlockBegEnd[2*iblock+1]+=BlSize;
      for (int ibl1=iblock+1;ibl1<NumBlocks();ibl1++)
	{
	  BlockBegEnd[2*ibl1]+=BlSize;
	  BlockBegEnd[2*ibl1+1]+=BlSize;
	}
      // end loop in blocks

    }
  iAux.clear();
  dAux.clear();

}

///////////////
/////////////
///////////// September 08
/////////////
void CNRGbasisarray::SetLastQNumber(vector<double> InputVec){

  // Based on the iDegen values, add QNumber and rearrange
  // dEn, QNumbers and BlockBegEnd. Good for phonons.
  // It will re-order the blocks according with iDegen values
  // (it may come down to a single state per block)

  if (InputVec.size()!=Nstates()){
    cout << "SetLastQNumber : Input vec size does not match" << endl;
    return;
  }

  // assume it is the latest qn

  vector<double> auxdEn;
  vector<double> auxQNumbers;
  vector<int> auxBlockBegEnd;
  vector<int> auxiDegen;

  vector<int> auxiType;
  vector<int> auxStCameFrom;

  vector<double>::iterator it0;
  vector<double>::iterator it1;

  vector<int>::iterator it_ideg;

  double *auxdQN=new double [NQNumbers];

  int NoBlocksOri=NumBlocks(); // will increase QNumbers so need this one
  int Nadded=0;

  // Add new blocks to QNumbers

  auxQNumbers=QNumbers;

  auxdQN[0]=0.0;
  for (int ibl=0;ibl<NoBlocksOri;ibl++){
    int ist0=GetBlockLimit(ibl,0);
    int ist1=GetBlockLimit(ibl,1);

    for (int ist=ist0;ist<=ist1;ist++){
      it0=QNumbers.begin()+(ibl+Nadded)*NQNumbers;
      it1=QNumbers.begin()+(ibl+Nadded+1)*NQNumbers;
      // 	  auxdQN[0]=(double)iDegen[ist];
      auxdQN[0]=InputVec[ist];
      QNumbers[(Nadded*NQNumbers)+((ibl+1)*NQNumbers)-1]=auxdQN[0];
      if (ist<ist1)
	{QNumbers.insert(it0,it0,it1);Nadded++;}
    }
    // end loop in states
  }
  // end loop in blocks

  FilterQNumbers();

  // Update Block structure

  auxdEn.clear();
  auxiDegen.clear();
  auxBlockBegEnd.clear();
  auxiType.clear();
  auxStCameFrom.clear();

  Nadded=0;

  // Loop in new block structure
  for (int ibl=0;ibl<NumBlocks();ibl++){
    for (int iqn=0;iqn<NQNumbers;iqn++){auxdQN[iqn]=GetQNumber(ibl,iqn);}

    // Loop in the old block structure
    auxBlockBegEnd.push_back(Nadded);
    for (int ibl1=0;ibl1<NoBlocksOri;ibl1++){
      bool ok_match=true;

      int ist0=GetBlockLimit(ibl1,0);
      int ist1=GetBlockLimit(ibl1,1);

      for (int iqn=0;iqn<NQNumbers-1;iqn++)
	if(dNEqual(auxdQN[iqn],auxQNumbers[ibl1*NQNumbers+iqn]))
	  ok_match=false;
      if (ok_match){
	//cout << " Blocks Match: " << " ibl1= " << ibl1 << " ibl= " << ibl  << endl;
	for (int ist=ist0;ist<=ist1;ist++){
	  // 		  if (dEqual(auxdQN[NQNumbers-1],(double)iDegen[ist]))
	  if (dEqual(auxdQN[NQNumbers-1],InputVec[ist])){

	    if (ist<dEn.size()) auxdEn.push_back(dEn[ist]);
	    if (ist<iDegen.size()) auxiDegen.push_back(iDegen[ist]);
	    if (ist<iType.size()) auxiType.push_back(iType[ist]);
	    if (ist<StCameFrom.size()) auxStCameFrom.push_back(StCameFrom[ist]);

	    Nadded++;
	    //cout << " Adding ist = " << ist << " to block " << ibl << endl;
	  }
	}
	// end loop in ist in matchin blocks
      }
      // if match
    }
    // loop in old blocks

    auxBlockBegEnd.push_back(Nadded-1);
  }
  // loop in new blocks

  // Update other vectors
  dEn.clear();
  BlockBegEnd.clear();
  iDegen.clear();
  iType.clear();
  StCameFrom.clear();

  dEn=auxdEn;
  BlockBegEnd=auxBlockBegEnd;
  iDegen=auxiDegen;

  iType=auxiType;
  StCameFrom=auxStCameFrom;

  delete[] auxdQN;

}

///////////

//
/////////////
///////////// February 09
/////////////

void CNRGbasisarray::SaveBin(char arqname[]){

  CNRGarray::SaveBin(arqname);

  vector<double>::iterator dit;
  vector<int>::iterator iit;

  ofstream OutFile(arqname, ios::out | ios::binary | ios::app);
  double daux;
  int iaux;

  if (!OutFile){cout << "SaveBin: Cannot save data in " << arqname << endl; return;}

  // Save iType
  iaux=iType.size();
  OutFile.write((char*)&iaux, sizeof(int));
  for(iit=iType.begin();iit<iType.end();iit++){
    OutFile.write(reinterpret_cast<char*>(&(*iit)), sizeof(int));
  }

  // Save StCameFrom
  iaux=StCameFrom.size();
  OutFile.write((char*)&iaux, sizeof(int));
  for(iit=StCameFrom.begin();iit<StCameFrom.end();iit++){
    OutFile.write(reinterpret_cast<char*>(&(*iit)), sizeof(int));
  }

  // Save BlockBegEndBC
  iaux=BlockBegEndBC.size();
  OutFile.write((char*)&iaux, sizeof(int));
  for(iit=BlockBegEndBC.begin();iit<BlockBegEndBC.end();iit++){
    OutFile.write(reinterpret_cast<char*>(&(*iit)), sizeof(int));
  }

  // Save BlockBegEndEigVec
  iaux=BlockBegEndEigVec.size();
  OutFile.write((char*)&iaux, sizeof(int));
  for(iit=BlockBegEndEigVec.begin();iit<BlockBegEndEigVec.end();iit++){
    OutFile.write(reinterpret_cast<char*>(&(*iit)), sizeof(int));
  }
  // Save NQNumbers_stcf
  OutFile.write((char*)&NQNumbers_stcf, sizeof(int));
  // Save QNumbers_stcf
  iaux=StCameFromQNumbers.size();
  OutFile.write((char*)&iaux, sizeof(int));
  for(dit=StCameFromQNumbers.begin();dit<StCameFromQNumbers.end();dit++){
    daux=*dit;
    OutFile.write(reinterpret_cast<char*>(&daux), sizeof(double));
  }

  OutFile.close();

  if (!OutFile.good()){cout << "SaveBin: error saving" << endl;}

}
///

void CNRGbasisarray::ReadBin(char arqname[]){

  // ifstream
  ifstream InFile;

  InFile.open(arqname, ios::in | ios::binary);
  //InFile.seekg(0,ios::beg);

  if (!InFile){cout << "ReadBin: Cannot read data in " << arqname << endl; return;}

  CNRGbasisarray::ReadBinInStream(InFile);

  InFile.close();

  if (!InFile.good()){cout << "ReadBin: overall error reading" << endl;}

}

/////
void CNRGbasisarray::ReadBinInStream(ifstream &InFile){

  int isize,iaux, iaux2;
  double daux;

  CNRGarray::ReadBinInStream(InFile);

   // Read iType
  iType.clear();
  InFile.read((char*)&isize, sizeof(int));
  for(iaux=0;iaux<isize;iaux++){
    InFile.read(reinterpret_cast<char*>(&iaux2), sizeof(int));
    iType.push_back(iaux2);
  }
  // Read StCameFrom
  StCameFrom.clear();
  InFile.read((char*)&isize, sizeof(int));
  for(iaux=0;iaux<isize;iaux++){
    InFile.read(reinterpret_cast<char*>(&iaux2), sizeof(int));
    StCameFrom.push_back(iaux2);
  }
  // Read BlockBegEndBC BlockBegEigVec
  BlockBegEndBC.clear();
  InFile.read((char*)&isize, sizeof(int));
  for(iaux=0;iaux<isize;iaux++){
    InFile.read(reinterpret_cast<char*>(&iaux2), sizeof(int));
    BlockBegEndBC.push_back(iaux2);
  }
  // Read BlockBegEndEigVec
  BlockBegEndEigVec.clear();
  InFile.read((char*)&isize, sizeof(int));
  for(iaux=0;iaux<isize;iaux++){
    InFile.read(reinterpret_cast<char*>(&iaux2), sizeof(int));
    BlockBegEndEigVec.push_back(iaux2);
  }
  // Read NQNumbers_stcf
  InFile.read((char*)&NQNumbers_stcf, sizeof(int));
  // Read QNumbers_stcf
  StCameFromQNumbers.clear();
  InFile.read((char*)&isize, sizeof(int));
  for(iaux=0;iaux<isize;iaux++){
    InFile.read(reinterpret_cast<char*>(&daux), sizeof(double));
    StCameFromQNumbers.push_back(daux);
  }

}
/////////////

///////////////


/* Copyright 2017 The sfcpp Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#ifndef SRC_GEO_VECTOR3D_HPP_
#define SRC_GEO_VECTOR3D_HPP_

#include <geo/CoordinateSystem.hpp>
#include <Eigen/Dense>
#include <memory>

namespace sfcpp {
namespace geo {

class Vector3D {
	Eigen::Vector3d vec;
	std::shared_ptr<CoordinateSystem> coordinateSystem;

	Eigen::Vector4d getVec4D() const;
public:
	Vector3D(Eigen::Vector3d const &vec, std::shared_ptr<CoordinateSystem> const &coordinateSystem);
	Vector3D(Vector3D const &other);

	Eigen::Vector3d getCoordinates(std::shared_ptr<CoordinateSystem> const &targetSystem) const;
	Eigen::Vector3d getWorldCoordinates() const;

	Vector3D &operator+=(Vector3D const &other);
	Vector3D &operator-=(Vector3D const &other);
	Vector3D &operator*=(double factor);

	friend Vector3D operator+(Vector3D const &first, Vector3D const &second);
	friend Vector3D operator-(Vector3D const &first, Vector3D const &second);
	friend Vector3D operator*(double factor, Vector3D const &vec);
};

} /* namespace geo */
} /* namespace utils */

#endif /* SRC_GEO_VECTOR3D_HPP_ */


#include <iostream>
#include <time.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/point_types.h>
#include <pcl/features/pfh.h>
#include <pcl/filters/passthrough.h>
#include <pcl/visualization/cloud_viewer.h>
#include <limits>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include "pcl/point_cloud.h"
#include "pcl/kdtree/kdtree_flann.h"
#include "pcl/filters/passthrough.h"
#include "pcl/filters/voxel_grid.h"
#include "pcl/features/fpfh.h"

#include "filters.h"
#include "features.h"
#include "sac_ia.h"
#include "visualization.h"

using namespace std;

const double FILTER_LIMIT = 1000.0;
const int MAX_SACIA_ITERATIONS = 2000;

const float VOXEL_GRID_SIZE = 3;
const double NORMALS_RADIUS = 20;
const double FEATURES_RADIUS = 50;
const double SAC_MAX_CORRESPONDENCE_DIST = 2000;
const double SAC_MIN_CORRESPONDENCE_DIST = 3;

int main(int argc, char *argv[]){
	time_t starttime = time(NULL);
	cout << "Loading clouds...\n";
	cout.flush();

	// open the clouds
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);

	pcl::io::loadPLYFile("pcd/blade_model.ply", *cloud1);
	pcl::io::loadPLYFile("pcd/blade_data.ply", *cloud2);

	// downsample the clouds
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1ds(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2ds(new pcl::PointCloud<pcl::PointXYZ>);

	voxelFilter(cloud1, cloud1ds, VOXEL_GRID_SIZE);
	voxelFilter(cloud2, cloud2ds, VOXEL_GRID_SIZE);

	// compute normals
	pcl::PointCloud<pcl::Normal>::Ptr normals1 = getNormals(cloud1ds, NORMALS_RADIUS);
	pcl::PointCloud<pcl::Normal>::Ptr normals2 = getNormals(cloud2ds, NORMALS_RADIUS);

	// compute local features
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr features1 = getFeatures(cloud1ds, normals1, FEATURES_RADIUS);
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr features2 = getFeatures(cloud2ds, normals2, FEATURES_RADIUS);

	//
	auto sac_ia = align(cloud1ds, cloud2ds, features1, features2,
		MAX_SACIA_ITERATIONS, SAC_MIN_CORRESPONDENCE_DIST, SAC_MAX_CORRESPONDENCE_DIST);

	Eigen::Matrix4f init_transform = sac_ia.getFinalTransformation();
	pcl::transformPointCloud(*cloud2, *cloud2, init_transform);
	pcl::PointCloud<pcl::PointXYZ> final = *cloud1;
	final += *cloud2;

	cout << init_transform << endl;
	cout << "done. Time elapsed: " << time(NULL) - starttime << " seconds\n";
	cout.flush();

	viewPair(cloud1ds, cloud2ds, cloud1, cloud2);
	pcl::io::savePCDFile("result.pcd", final);
	//view(final);
	return 0;
}


/*
  dice code with boost library
  (c) yukawa satoshi 2007
 $Id$
 */

#include <iostream>
#include <boost/random.hpp>

int main(int argc,char* argv[]){
  boost::mt19937 rnd(12345678);
  boost::uniform_int<> range(32,126);
  boost::variate_generator<boost::mt19937&, boost::uniform_int<> >
  die(rnd,range);
  for(int j=0;j<40;j++){
    for(int i=0;i<60;i++){
      std::cout  <<  (char) die() ;
    }
    std::cout <<'\n';
  }
  return 0;
}


#include "../../src/Tracker/Struck.h"
#include "../../src/Superpixels/Objectness.h"
#include "../../src/Tracker/LocationSampler.h"
#include <iostream>
#include <opencv2/opencv.hpp>
#include "armadillo"
#include <vector>
#include <boost/python.hpp>
#include "glog/logging.h"
class Objectness {

public:
  cv::Mat image;
  cv::Mat small_image;

  Straddling* straddle;
  EdgeDensity* edgeDensity;

  double downsample=1.03;
  int minScale=-2;
  int maxScale=4;

  int min_x;
  int min_y;

  double getStraddling(int x, int y, int width, int height){
        if (x<0){
          x=0;
        }

        if (y<0){
          y=0;
        }
        if (x + width >= image.cols) {
          width = image.cols - x - 1;
        }
        if (y + height >= image.rows) {
          height = image.rows - y - 1;
        }

        cv::Rect rect(x,y,width,height);

        return this->straddle->computeStraddling(rect);
    };

    double getEdgeness(int x, int y, int width, int height) {
        if (x<0){
            x=0;
        }

        if (y<0){
            y=0;
        }

        if (x + width >= image.cols) {
          width = image.cols - x - 1;
        }

        if (y + height >= image.rows) {
          height = image.rows - y - 1;
        }

        cv::Rect rect(x,y,width,height);

        return this->edgeDensity->computeEdgeDensity(rect);
    };

    double getEdgenessMultiscale(int x, int y,int width, int height){

        double sum=0;
        for (int j = minScale ;j <= maxScale; ++j) {

            int width_s=width*pow(downsample,j);
            int height_s=height*pow(downsample,j);

            int new_x=(int)(x+width/2.0-width_s/2.0);
            int new_y=(int)(y+height/2.0-height_s/2.0);

            sum+=getEdgeness(new_x,new_y,width_s,height_s);
        }

        sum=sum/(maxScale-minScale);
        return sum;
    }

    double getStraddlingMultiscale(int x, int y,int width, int height){

        double sum=0;
        for (int j = minScale ;j <= maxScale; ++j) {

            int width_s=width*pow(downsample,j);
            int height_s=height*pow(downsample,j);

            int new_x=(int)(x+width/2.0-width_s/2.0);
            int new_y=(int)(y+height/2.0-height_s/2.0);

            sum+=getStraddling(new_x,new_y,width_s,height_s);
        }

        sum=sum/(maxScale-minScale);
        return sum;
    }

    void plotObjectness(){
      cv::imshow("Image", this->small_image);
      //cv::waitKey();
      //cv::destroyAllWindows();

      cv::imshow("Objectness", this->straddle->canvas);
      cv::waitKey();
      cv::destroyAllWindows();

    };

  void readImage(std::string imName){
    cv::Mat im = cv::imread(imName);

    // setImageSize(im.cols,im.rows);
    this->image = im.clone();
  };

  void smallImage(int R, int x, int y, int width, int height ){
    int delta = R;

    cv::Rect lastLocation(x, y, width, height);
    int x_min = max(0, lastLocation.x - delta);
    int y_min = max(0, lastLocation.y - delta);

    int x_max = min(image.cols, lastLocation.x + lastLocation.width + delta);
    int y_max = min(image.rows, lastLocation.y + lastLocation.height + delta);

    cv::Rect big_box(x_min, y_min, x_max - x_min, y_max - y_min);
    // extract small image from 'image'
    cv::Mat small_image_crop(this->image, big_box);

    this->min_x = x_min;
    this->min_y = y_min;

    this->small_image = small_image_crop.clone();

  }

  boost::python::list process(const int superpixels,
                              const double inner,
                              const int n,
                              const int R, const int scale_R,
                              const int min_size_half,
                              const int min_scales, const int max_scales,
                              const double downsample,
                              const double shrink_one_side_scale,
                              int x, int y, int width, int height){

    cv::Rect lastLocation(x, y, width, height);
    std::vector<int> radiuses;
    std::vector<int> widths;
    std::vector<int> heights;

    this->straddle=new Straddling(superpixels,inner);
    // add boxes and stuff
    std::vector<arma::mat> straddling_cube = LocationSampler::
      generateBoxesTensor(R, scale_R, min_size_half, min_scales,
                          max_scales, downsample, shrink_one_side_scale,
                          this->small_image.rows,
                          this->small_image.cols,
                          lastLocation, &radiuses, &widths, &heights);

    this->straddle->preprocessIntegral(this->small_image);

    CHECK_NOTNULL(&this->small_image);
    this->straddle->straddlingOnCube(this->small_image.rows,
                                     this->small_image.cols,
                                     x - this->min_x,
                                     y - this->min_y,
                                     radiuses, widths, heights,
                                     straddling_cube);

    if ( n > 0){
      std::vector<arma::mat> suppressed_cube;
      for (int i = 0; i<straddling_cube.size(); i++) {
        arma::mat suppressed =
          this->straddle->nonMaxSuppression(straddling_cube[i], n);

        suppressed_cube.push_back(suppressed);
      }

      std::cout << " Done."<< std::endl;
      straddling_cube = suppressed_cube;

    }

    // convert from vector<arma::mat> to python::list<nd a≈grray>
    boost::python::list output;

    for (int i = 0; i<straddling_cube.size(); i++) {
      int rows = straddling_cube[i].n_rows;
      int cols = straddling_cube[i].n_cols;

      boost::python::list np_array;
      for (int j = 0; j<rows; j++) {
        boost::python::list output_row;
        for (int k = 0; k< cols; k++) {
          output_row.append(straddling_cube[i](j,k));
        }
        np_array.append(output_row);
      }
      output.append(np_array);
    }

    return output;

  }

 boost::python::list processEdge(const int superpixels,
                              const double inner,
                              const int n,
                              const int R, const int scale_R,
                              const int min_size_half,
                              const int min_scales, const int max_scales,
                              const double downsample,
                              const double shrink_one_side_scale,
                              int x, int y, int width, int height){

    cv::Rect lastLocation(x, y, width, height);
    std::vector<int> radiuses;
    std::vector<int> widths;
    std::vector<int> heights;

    EdgeDensity* edge = new EdgeDensity(0.66*100, 1.33*100, inner, 0);
    // add boxes and stuff
    std::vector<arma::mat> straddling_cube = LocationSampler::
      generateBoxesTensor(R, scale_R, min_size_half, min_scales,
                          max_scales, downsample, shrink_one_side_scale,
                          this->small_image.rows,
                          this->small_image.cols,
                          lastLocation, &radiuses, &widths, &heights);

    edge->preprocessIntegral(this->small_image);

    CHECK_NOTNULL(&this->small_image);
    edge->edgeOnCube(this->small_image.rows,
                     this->small_image.cols,
                     x - this->min_x,
                     y - this->min_y,
                     radiuses, widths, heights,
                     straddling_cube);

    // convert from vector<arma::mat> to python::list<nd a≈grray>
    boost::python::list output;

    for (int i = 0; i<straddling_cube.size(); i++) {
      int rows = straddling_cube[i].n_rows;
      int cols = straddling_cube[i].n_cols;

      boost::python::list np_array;
      for (int j = 0; j<rows; j++) {
        boost::python::list output_row;
        for (int k = 0; k< cols; k++) {
          output_row.append(straddling_cube[i](j,k));
        }
        np_array.append(output_row);
      }
      output.append(np_array);
    }

    return output;

  }

  void initializeStraddling(int superpixels, double inner){

        this->straddle=new Straddling(superpixels,inner);

        arma::mat labels=this->straddle->getLabels(this->image );

        this->straddle->computeIntegralImages(labels);
       //this->tracker = &t;
  };

  boost::python::list getEdgenessList(boost::python::list& x,
                                      boost::python::list& y,
                                      boost::python::list& width,
                                      boost::python::list& height){

      boost::python::list output;
      for (int i = 0; i < len(x); ++i)
        {
            int x_val=boost::python::extract<int>(x[i]);
            int y_val=boost::python::extract<int>(y[i]);
            int w_val=boost::python::extract<int>(width[i]);
            int h_val=boost::python::extract<int>(height[i]);
            double r=this->getEdgeness(x_val,y_val,w_val,h_val);
            output.append(r);
        }

        return output;
  }

  boost::python::list getStraddlingList(boost::python::list& x,
                                        boost::python::list& y,
                                        boost::python::list& width,
                                        boost::python::list& height){

      boost::python::list output;
      for (int i = 0; i < len(x); ++i)
        {
            int x_val=boost::python::extract<int>(x[i]);
            int y_val=boost::python::extract<int>(y[i]);
            int w_val=boost::python::extract<int>(width[i]);
            int h_val=boost::python::extract<int>(height[i]);
            double r=this->getStraddling(x_val,y_val,w_val,h_val);
            output.append(r);
        }

        return output;
  }

~Objectness(){
    delete straddle;
    delete edgeDensity;
}

  void initializeEdgeDensity(double t1, double t2, double inner){
    this->edgeDensity=new EdgeDensity(t1,t2,inner,0);

    cv::Mat edges=this->edgeDensity->getEdges(this->image);

    this->edgeDensity->computeIntegrals(edges);
  };
};

using namespace boost::python;

BOOST_PYTHON_MODULE(objectness_python)

{
  class_<Objectness>("Objectness")
      //.add_property("getStraddling", &Objectness::getStraddling)
      //.add_property("getEdgeness", &Objectness::getEdgeness)
    .def("readImage",&Objectness::readImage)
    .def("smallImage", &Objectness::smallImage)
    .def("processEdge", &Objectness::processEdge)
    .def("process", &Objectness::process)
    .def("initializeStraddling", &Objectness::initializeStraddling)
    .def("initializeEdgeDensity", &Objectness::initializeEdgeDensity)
    .def("plot", &Objectness::plotObjectness)
    .def("getStraddling", &Objectness::getStraddling)
    .def("getEdgeness", &Objectness::getEdgeness)
    .def("getEdgenessList",&Objectness::getEdgenessList)
    .def("getStraddlingList",&Objectness::getStraddlingList)
    .def("getEdgenessMultiscale",&Objectness::getEdgenessMultiscale)
    .def("getStraddlingMultiscale",&Objectness::getStraddlingMultiscale)
    ;
}
// find how to write functions which return some values in c++/python boost
// framework


#ifndef FOOTSTEP_OBJECT_H
#define FOOTSTEP_OBJECT_H

#include <Eigen/Dense>
#include <iostream>
#include <string>

#define LEFT_FOOTSTEP 0
#define RIGHT_FOOTSTEP 1
#define MID_FOOTSTEP 2

// Data container for a footstep object

class Footstep{
public:
  Footstep();
  Footstep(const Eigen::Vector3d & pos_in, const Eigen::Quaterniond & quat_in, const int & robot_side_in);

  ~Footstep();

  // Position and orientation of the sole of the footstep
  Eigen::Vector3d position;
  Eigen::Quaternion<double> orientation;

  Eigen::Matrix3d R_ori;

  // Setters
  void setPosOriSide(const Eigen::Vector3d & pos_in, const Eigen::Quaterniond & quat_in, const int & robot_side_in);
  void setPosOri(const Eigen::Vector3d & pos_in, const Eigen::Quaterniond & quat_in);
  void setRightSide();
  void setLeftSide();
  void setMidFoot();

  // Left or right side
  int robot_side;

  void computeMidfeet(const Footstep & footstep1, const Footstep & footstep2, Footstep & midfeet);

  int getSide();
  void printInfo();

  // length and width of the soles
  double sole_length = 0.2;
  double sole_width = 0.1;

  std::vector<Eigen::Vector3d> local_contact_point_list;
  std::vector<Eigen::Vector3d> global_contact_point_list;

private:
  void common_initialization();
  void updateContactLocations();

};

#endif


//////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::normal::include.hpp                 //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_NORMAL_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_NORMAL_INCLUDE_HPP_ER_2009

#include <boost/math/distributions/normal.hpp>

#include <boost/statistics/detail/distribution_toolkit/distributions/normal/description.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/normal/derivative_log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/normal/log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/normal/is_math_distribution.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/normal/normalizing_constant.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/normal/random.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/normal/os.hpp>

#endif


#ifndef AIKIDO_PLANNER_PARABOLIC_PARABOLICTIMER_HPP_
#define AIKIDO_PLANNER_PARABOLIC_PARABOLICTIMER_HPP_

#include <Eigen/Dense>

#include "aikido/planner/TrajectoryPostProcessor.hpp"
#include "aikido/trajectory/Interpolated.hpp"
#include "aikido/trajectory/Spline.hpp"

namespace aikido {
namespace planner {
namespace parabolic {

/// Computes the time-optimal timing of a trajectory consisting of a sequence
/// Geodesic interpolations between states under velocity and acceleration
/// bounds. The output is a parabolic spline, encoded in cubic polynomials,
/// that \b exactly follows the input path.
///
/// The output trajectory consists of a sequence of trapezoidal velocity
/// profiles that implement bang-bang control. This trajectory must stop at
/// each waypoint that introduces a velocity discontinuity to satisfy finite
/// acceleration bounds. You should consider using a blending or smoothing
/// algorithm, which does \b not follow the exact input path, if this behavior
/// is undesirable.
///
/// This function curently only supports \c RealVector, \c SO2, and compound
/// state spaces of those types. Additionally, this function requires that
/// \c _inputTrajectory to be interpolated using a \c GeodesicInterpolator.
///
/// \param _inputTrajectory input piecewise Geodesic trajectory
/// \param _maxVelocity maximum velocity for each dimension
/// \param _maxAcceleration maximum acceleration for each dimension
/// \return time optimal trajectory that satisfies acceleration constraints
std::unique_ptr<aikido::trajectory::Spline> computeParabolicTiming(
    const aikido::trajectory::Interpolated& _inputTrajectory,
    const Eigen::VectorXd& _maxVelocity,
    const Eigen::VectorXd& _maxAcceleration);

/// Computes the time-optimal timing of a trajectory consisting of a linear
/// spline between states under velocity and acceleration
/// bounds. The output is a parabolic spline, encoded in cubic polynomials,
/// that \b exactly follows the input path.
///
/// The output trajectory consists of a sequence of trapezoidal velocity
/// profiles that implement bang-bang control. This trajectory must stop at
/// each waypoint that introduces a velocity discontinuity to satisfy finite
/// acceleration bounds. You should consider using a blending or smoothing
/// algorithm, which does \b not follow the exact input path, if this behavior
/// is undesirable.
///
/// This function curently only supports \c RealVector, \c SO2, and compound
/// state spaces of those types. Additionally, this function requires that
/// \c _inputTrajectory to be interpolated using a \c GeodesicInterpolator.
///
/// \param _inputTrajectory linear spline trajectory
/// \param _maxVelocity maximum velocity for each dimension
/// \param _maxAcceleration maximum acceleration for each dimension
/// \return time optimal trajectory that satisfies acceleration constraints
std::unique_ptr<aikido::trajectory::Spline> computeParabolicTiming(
    const aikido::trajectory::Spline& _inputTrajectory,
    const Eigen::VectorXd& _maxVelocity,
    const Eigen::VectorXd& _maxAcceleration);

/// Class for performing parabolic retiming on trajectories.
class ParabolicTimer : public aikido::planner::TrajectoryPostProcessor
{
public:
  /// \param _velocityLimits Maximum velocity for each dimension.
  /// \param _accelerationLimits Maximum acceleration for each dimension.
  ParabolicTimer(
      const Eigen::VectorXd& _velocityLimits,
      const Eigen::VectorXd& _accelerationLimits);

  /// Performs parabolic retiming on an input trajectory.
  /// \copydoc TrajectoryPostProcessor::postprocess
  std::unique_ptr<aikido::trajectory::Spline> postprocess(
      const aikido::trajectory::Interpolated& _inputTraj,
      const aikido::common::RNG& _rng,
      const aikido::constraint::TestablePtr& _constraint = nullptr) override;

  /// Performs parabolic retiming on an input *spline* trajectory.
  /// \copydoc TrajectoryPostProcessor::postprocess
  std::unique_ptr<aikido::trajectory::Spline> postprocess(
      const aikido::trajectory::Spline& _inputTraj,
      const aikido::common::RNG& _rng,
      const aikido::constraint::TestablePtr& _constraint = nullptr) override;

private:
  /// Set to the value of \c _velocityLimits.
  const Eigen::VectorXd mVelocityLimits;

  /// Set to the value of \c _accelerationLimits.
  const Eigen::VectorXd mAccelerationLimits;
};

} // namespace parabolic
} // namespace planner
} // namespace aikido

#endif // ifndef AIKIDO_PLANNER_PARABOLIC_PARABOLICTIMER_HPP_


//---------------------------------------------------------------------------//
//  MIT License
//
//  Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
//  Copyright (c) 2020-2021 Nikita Kaskov <nemo@nil.foundation>

//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in all
//  copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
//  SOFTWARE.
//---------------------------------------------------------------------------//

#include <boost/filesystem/path.hpp>

#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark.hpp>

namespace nil {
    namespace filecoin {
        template<typename SchemeType>
        struct scheme_params {
            typedef SchemeType scheme_type;
        };

        template<typename CurveType>
        struct scheme_params<crypto3::zk::snark::r1cs_gg_ppzksnark<CurveType>> {
            typedef CurveType curve_type;
            typedef typename curve_type::template g1_type<> g1_type;

            typedef crypto3::zk::snark::r1cs_gg_ppzksnark<CurveType> scheme_type;
            typedef typename scheme_type::verifying_key_type verifying_key_type;

            verifying_key_type vk;

            // Elements of the form ((tau^i * t(tau)) / delta) for i between 0 and
            // m-2 inclusive. Never contains points at infinity.
            std::vector<typename g1_type::value_type> h;

            // Elements of the form (beta * u_i(tau) + alpha v_i(tau) + w_i(tau)) / delta
            // for all auxiliary inputs. Variables can never be unconstrained, so this
            // never contains points at infinity.
            std::vector<typename g1_type::value_type> l;

            // QAP "A" polynomials evaluated at tau in the Lagrange basis. Never contains
            // points at infinity: polynomials that evaluate to zero are omitted from
            // the CRS and the prover can deterministically skip their evaluation.
            std::vector<typename g1_type::value_type> a;

            // QAP "B" polynomials evaluated at tau in the Lagrange basis. Needed
            // in G1 and G2 for C/B queries, respectively. Never contains points at
            // infinity for the same reason as the "A" polynomials.
            std::vector<typename g1_type::value_type> b_g1;
            std::vector<typename g1_type::value_type> b_g2;
        };
    }    // namespace filecoin
}    // namespace nil


#include <boost/graph/closeness_centrality.hpp>


#include <cmath>
#include <iostream>

#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <Eigen/SVD>

#include "drake/solvers/fast_qp.h"
#include "drake/solvers/gurobi_qp.h"

#define MAX_CONSTRS 1000
#define MAX_STATE 1000
#define MAX_ITER 10

// TODO(jwnimmer-tri) Someone with gurobi needs to fix these.
// NOLINTNEXTLINE(build/namespaces)
using namespace Eigen;
// NOLINTNEXTLINE(build/namespaces)
using namespace std;

// template <typename tA, typename tB, typename tC, typename tD, typename tE,
// typename tF, typename tG>
// int fastQPThatTakesQinv(vector< MatrixBase<tA>* > QinvblkDiag, const
// MatrixBase<tB>& f, const MatrixBase<tC>& Aeq, const MatrixBase<tD>& beq,
// const MatrixBase<tE>& Ain, const MatrixBase<tF>& bin, set<int>& active,
// MatrixBase<tG>& x)
int fastQPThatTakesQinv(vector<MatrixXd*> QinvblkDiag, const VectorXd& f,
                        const MatrixXd& Aeq, const VectorXd& beq,
                        const MatrixXd& Ain, const VectorXd& bin,
                        // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
                        set<int>& active, VectorXd& x) {
  int i, d;
  int iterCnt = 0;

  int M_in = bin.size();
  int M = Aeq.rows();
  int N = Aeq.cols();

  if (f.rows() != N) {
    cerr << "size of f (" << f.rows() << " by " << f.cols()
         << ") doesn't match cols of Aeq (" << Aeq.rows() << " by "
         << Aeq.cols() << ")" << endl;
    return 2;
  }
  if (beq.rows() != M) {
    cerr << "size of beq doesn't match rows of Aeq" << endl;
    return 2;
  }
  if (Ain.cols() != N) {
    cerr << "cols of Ain doesn't match cols of Aeq" << endl;
    return 2;
  }
  if (bin.rows() != Ain.rows()) {
    cerr << "bin rows doesn't match Ain rows" << endl;
    return 2;
  }
  if (x.rows() != N) {
    cerr << "x doesn't match Aeq" << endl;
    return 2;
  }
  int n_active = active.size();

  MatrixXd Aact = MatrixXd(n_active, N);
  VectorXd bact = VectorXd(n_active);

  MatrixXd QinvAteq(N, M);
  VectorXd minusQinvf(N);

  // calculate a bunch of stuff that is constant during each iteration
  int startrow = 0;
  //  for (typename vector< MatrixBase<tA>* >::iterator
  //  iterQinv=QinvblkDiag.begin(); iterQinv!=QinvblkDiag.end(); iterQinv++) {
  //    MatrixBase<tA> *thisQinv = *iterQinv;
  for (vector<MatrixXd*>::iterator iterQinv = QinvblkDiag.begin();
       iterQinv != QinvblkDiag.end(); iterQinv++) {
    MatrixXd* thisQinv = *iterQinv;
    int numRow = thisQinv->rows();
    int numCol = thisQinv->cols();

    if (numRow == 1 || numCol == 1) {  // it's a vector
      d = numRow * numCol;
      if (M > 0)
        QinvAteq.block(startrow, 0, d, M) =
            thisQinv->asDiagonal() *
            Aeq.block(0, startrow, M, d)
                .transpose();  // Aeq.transpose().block(startrow, 0, d, N)
      minusQinvf.segment(startrow, d) =
          -thisQinv->cwiseProduct(f.segment(startrow, d));
      startrow = startrow + d;
    } else {  // potentially dense matrix
      d = numRow;
      if (numRow != numCol) {
        cerr << "Q is not square! " << numRow << "x" << numCol << "\n";
        return -2;
      }
      if (M > 0)
        QinvAteq.block(startrow, 0, d, M) = thisQinv->operator*(
            Aeq.block(0, startrow, M, d)
                .transpose());  // Aeq.transpose().block(startrow, 0, d, N)
      minusQinvf.segment(startrow, d) =
          -thisQinv->operator*(f.segment(startrow, d));
      startrow = startrow + d;
    }
    if (startrow > N) {
      cerr << "Q is too big!" << endl;
      return -2;
    }
  }
  if (startrow != N) {
    cerr << "Q is the wrong size.  Got " << startrow << "by" << startrow
         << " but needed " << N << "by" << N << endl;
    return -2;
  }

  MatrixXd A;
  VectorXd b;
  MatrixXd QinvAt;
  VectorXd lam, lamIneq;
  VectorXd violated(M_in);
  VectorXd violation;

  while (1) {
    iterCnt++;

    n_active = active.size();
    Aact.resize(n_active, N);
    bact.resize(n_active);

    i = 0;
    for (set<int>::iterator iter = active.begin(); iter != active.end();
         iter++) {
      if (*iter < 0 || *iter >= Ain.rows()) {
        return -3;  // active set is invalid.  exit quietly, because this is
                    // expected behavior in normal operation (e.g. it means I
                    // should immediately kick out to Gurobi)
      }
      Aact.row(i) = Ain.row(*iter);
      bact(i++) = bin(*iter);
    }

    A.resize(Aeq.rows() + Aact.rows(), N);
    b.resize(beq.size() + bact.size());
    A << Aeq, Aact;
    b << beq, bact;

    if (A.rows() > 0) {
      // Solve H * [x;lam] = [-f;b] using Schur complements, H = [Q, At';A, 0];
      QinvAt.resize(QinvAteq.rows(), QinvAteq.cols() + Aact.rows());

      if (n_active > 0) {
        startrow = 0;
        for (vector<MatrixXd*>::iterator iterQinv = QinvblkDiag.begin();
             iterQinv != QinvblkDiag.end(); iterQinv++) {
          MatrixXd* thisQinv = (*iterQinv);
          d = thisQinv->rows();
          int numCol = thisQinv->cols();

          if (numCol == 1) {  // it's a vector
            QinvAt.block(startrow, 0, d, M + n_active)
                << QinvAteq.block(startrow, 0, d, M),
                thisQinv->asDiagonal() *
                    Aact.block(0, startrow, n_active, d).transpose();
          } else {  // it's a matrix
            QinvAt.block(startrow, 0, d, M + n_active)
                << QinvAteq.block(startrow, 0, d, M),
                thisQinv->operator*(
                    Aact.block(0, startrow, n_active, d).transpose());
          }

          startrow = startrow + d;
        }
      } else {
        QinvAt = QinvAteq;
      }

      lam.resize(QinvAt.cols());
      lam =
          -(A * QinvAt).ldlt().solve(b + (f.transpose() * QinvAt).transpose());
      x = minusQinvf - QinvAt * lam;
      lamIneq = lam.tail(lam.size() - M);
    } else {
      x = minusQinvf;
      lamIneq.resize(0);
    }

    if (Ain.rows() == 0) {
      active.clear();
      break;
    }

    set<int> new_active;

    violation = Ain * x - bin;
    for (i = 0; i < M_in; i++)
      if (violation(i) >= 1e-6) new_active.insert(i);

    bool all_pos_mults = true;
    for (i = 0; i < n_active; i++) {
      if (lamIneq(i) < 0) {
        all_pos_mults = false;
        break;
      }
    }
    if (new_active.empty() && all_pos_mults) {
      // existing active was AOK
      break;
    }

    i = 0;
    set<int>::iterator iter = active.begin(), tmp;
    while (iter != active.end()) {  // to accommodating inloop erase
      tmp = iter++;
      if (lamIneq(i++) < 0) {
        active.erase(tmp);
      }
    }
    active.insert(new_active.begin(), new_active.end());

    if (iterCnt > MAX_ITER) {
      // Default to calling this method
      //      cout << "FastQP max iter reached." << endl;
      //       mexErrMsgIdAndTxt("Drake:approximateIKmex:Error", "Max iter
      //       reached. Problem is likely infeasible");
      return -1;
    }
  }
  return iterCnt;
}

// template <typename tA, typename tB, typename tC, typename tD, typename tE,
// typename tF, typename tG>
// int fastQP(vector< MatrixBase<tA>* > QblkDiag, const MatrixBase<tB>& f, const
// MatrixBase<tC>& Aeq, const MatrixBase<tD>& beq, const MatrixBase<tE>& Ain,
// const MatrixBase<tF>& bin, set<int>& active, MatrixBase<tG>& x)
int fastQP(vector<MatrixXd*> QblkDiag, const VectorXd& f, const MatrixXd& Aeq,
           const VectorXd& beq, const MatrixXd& Ain, const VectorXd& bin,
           // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
           set<int>& active, VectorXd& x) {
  /* min 1/2 * x'QblkDiag'x + f'x s.t A x = b, Ain x <= bin
   * using active set method.  Iterative solve a linearly constrained
   * quadratic minimization problem where linear constraints include
   * Ain(active,:)x == bin(active).  Quit if all dual variables associated
   * with these equations are positive (i.e. they satisfy KKT conditions).
   *
   * Note:
   * fails if QP is infeasible.
   * active == initial rows of Ain to treat as equations.
   * Frank Permenter - June 6th 2013
   *
   * @retval  if feasible then iterCnt, else -1 for infeasible, -2 for input
   *error
   */

  int N = f.rows();

  MatrixXd* Qinv = new MatrixXd[QblkDiag.size()];
  vector<MatrixXd*> Qinvmap;

#define REG 1e-13
  // calculate a bunch of stuff that is constant during each iteration
  int startrow = 0;
  // typedef typename vector< MatrixBase<tA> >::iterator Qiterator;

  int i = 0;
  for (vector<MatrixXd*>::iterator iterQ = QblkDiag.begin();
       iterQ != QblkDiag.end(); iterQ++) {
    MatrixXd* thisQ = *iterQ;
    int numRow = thisQ->rows();
    int numCol = thisQ->cols();

    if (numCol == 1) {  // it's a vector
      VectorXd Qdiag_mod =
          thisQ->operator+(VectorXd::Constant(numRow, REG));  // regularize
      Qinv[i] = Qdiag_mod.cwiseInverse();
      Qinvmap.push_back(&Qinv[i]);
      startrow = startrow + numRow;
    } else {  // potentially dense matrix
      if (numRow != numCol) {
        if (numRow == 1)
          cerr << "diagonal Q's must be set as column vectors" << endl;
        else
          cerr << "Q is not square! " << numRow << "x" << numCol << endl;
        return -2;
      }

      MatrixXd Q_mod =
          thisQ->operator+(REG * MatrixXd::Identity(numRow, numRow));
      Qinv[i] = Q_mod.inverse();
      Qinvmap.push_back(&Qinv[i]);
      startrow = startrow + numRow;
    }
    // cout << "Qinv{" << i << "} = " << Qinv[i] << endl;
    if (startrow > N) {
      cerr << "Q is too big!" << endl;
      return -2;
    }
    i++;
  }
  if (startrow != N) {
    cerr << "Q is the wrong size.  Got " << startrow << "by" << startrow
         << " but needed " << N << "by" << N << endl;
    return -2;
  }

  int info = fastQPThatTakesQinv(Qinvmap, f, Aeq, beq, Ain, bin, active, x);

  delete[] Qinv;
  return info;
}

/* Example call (allocate inequality matrix, call function, resize inequalites:
  VectorXd binBnd = VectorXd(2*N);
  AinBnd.setZero();
  int numIneq = boundToIneq(ub, lb, AinBnd, binBnd);
  AinBnd.resize(numIneq, N);
  binBnd.resize(numIneq);
*/
/*
int boundToIneq(const VectorXd& uB, const VectorXd& lB, MatrixXd& Ain, VectorXd&
bin)
{
    int rCnt = 0;
    int cCnt = 0;

    if (uB.rows()+lB.rows() > A.rows() ) {
        cerr << "not enough memory allocated";
    }

    if (uB.rows()+lB.rows() > b.rows() ) {
        cerr << "not enough memory allocated";
    }

    for (int i = 0; i < lB.rows(); i++ ) {
        if (!isinf(lB(i))) {
            cout << lB(i);
            cout << i;
            Ain(rCnt, cCnt++) = -1;// lB(i);
            bin(rCnt++) = -lB(i);
        }
    }
    cCnt = 0;
    for (int i = 0; i < uB.rows(); i++ ) {
        if (!isinf(uB(i))) {
            Ain(rCnt, cCnt++) = 1;// uB(i);
            bin(rCnt++) = uB(i);
        }
    }

    // resizing inside function all causes exception (why??)
    // A.resize(rCnt, uB.rows());
    return rCnt;
}
*/

template <typename DerivedA, typename DerivedB>
int myGRBaddconstrs(GRBmodel* model, MatrixBase<DerivedA> const& A,
                    MatrixBase<DerivedB> const& b, char sense,
                    double sparseness_threshold = 1e-14) {
  int i, j, nnz, error = 0;
  /*
    // todo: it seems like I should just be able to do something like this:
    SparseMatrix<double, RowMajor> sparseAeq(Aeq.sparseView());
    sparseAeq.makeCompressed();
    error = GRBaddconstrs(
        model, nq_con, sparseAeq.nonZeros(), sparseAeq.InnerIndices(),
        sparseAeq.OuterStarts(), sparseAeq.Values(), beq.data(), NULL);
  */

  int* cind = new int[A.cols()];
  double* cval = new double[A.cols()];
  for (i = 0; i < A.rows(); i++) {
    nnz = 0;
    for (j = 0; j < A.cols(); j++) {
      if (abs(A(i, j)) > sparseness_threshold) {
        cval[nnz] = A(i, j);
        cind[nnz++] = j;
      }
    }
    error = GRBaddconstr(model, nnz, cind, cval, sense, b(i), NULL);
    if (error) break;
  }

  delete[] cind;
  delete[] cval;
  return error;
}

// template <typename tA, typename tB, typename tC, typename tD, typename tE>
// GRBmodel* gurobiQP(GRBenv *env, vector< MatrixBase<tA>* > QblkDiag, VectorXd&
// f, const MatrixBase<tB>& Aeq, const MatrixBase<tC>& beq, const
// MatrixBase<tD>& Ain, const MatrixBase<tE>& bin, VectorXd& lb, VectorXd& ub,
// set<int>& active, VectorXd& x)
GRBmodel* gurobiQP(GRBenv* env, vector<MatrixXd*> QblkDiag,
                   // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
                   VectorXd& f,
                   const MatrixXd& Aeq, const VectorXd& beq,
                   const MatrixXd& Ain, const VectorXd& bin,
                   // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
                   VectorXd& lb, VectorXd& ub, set<int>& active, VectorXd& x,
                   double active_set_slack_tolerance) {
  // Note: f, lb, and ub are VectorXd instead of const MatrixBase templates
  // because i want to be able to call f.data() on them

  // NOTE:  this allocates memory for a new GRBmodel and returns it. (you should
  // delete this object when you're done with it)
  // NOTE:  by convention here, the active set indices correspond to Ain, bin
  // first, then lb, then ub.

  GRBmodel* model = NULL;

  int method;
  GRBgetintparam(env, "method", &method);

  int i, j, nparams = f.rows(), Qi, Qj;
  double* lbdata = NULL, * ubdata = NULL;
  if (lb.rows() == nparams) lbdata = lb.data();
  if (ub.rows() == nparams) ubdata = ub.data();
  CGE(GRBnewmodel(env, &model, "QP", nparams, NULL, lbdata, ubdata, NULL, NULL),
      env);

  int startrow = 0, d;
  for (vector<MatrixXd*>::iterator iterQ = QblkDiag.begin();
       iterQ != QblkDiag.end(); iterQ++) {
    MatrixXd* Q = *iterQ;

    // WARNING:  If there are no constraints, then Gurobi clearly solves a
    // different problem: min 1/2 x'Qx + f'x
    // This is very strange; see the solveWGUROBI method in QuadraticProgram
    if (method == 2)  //&& (Aeq.rows()+Ain.rows()>0))
      *Q = .5 * (*Q);

    if (Q->rows() == 1 || Q->cols() == 1) {  // it's a vector
      d = Q->rows() * Q->cols();
      for (i = 0; i < d; i++) {
        Qi = i + startrow;
        double& qval = Q->operator()(i);
        CGE(GRBaddqpterms(model, 1, &Qi, &Qi, &qval), env);
      }
      startrow = startrow + d;
    } else {  // potentially dense matrix
      d = Q->rows();
      if (d != Q->cols()) {
        cerr << "Q is not square! " << Q->rows() << "x" << Q->cols() << "\n";
        return NULL;
      }

      for (i = 0; i < d; i++)
        for (j = 0; j < d; j++) {
          Qi = i + startrow;
          Qj = j + startrow;
          double& qval = Q->operator()(i, j);
          CGE(GRBaddqpterms(model, 1, &Qi, &Qj, &qval), env);
        }
      startrow = startrow + d;
    }
    if (startrow > nparams) {
      cerr << "Q is too big!" << endl;
      return NULL;
    }
  }

  CGE(GRBsetdblattrarray(model, "Obj", 0, nparams, f.data()), env);

  if (Aeq.rows() > 0)
    CGE(myGRBaddconstrs(model, Aeq, beq, GRB_EQUAL, 1e-18), env);
  if (Ain.rows() > 0)
    CGE(myGRBaddconstrs(model, Ain, bin, GRB_LESS_EQUAL, 1e-18), env);

  CGE(GRBupdatemodel(model), env);
  CGE(GRBoptimize(model), env);

  CGE(GRBgetdblattrarray(model, GRB_DBL_ATTR_X, 0, nparams, x.data()), env);

  VectorXd slack(Ain.rows());
  CGE(GRBgetdblattrarray(model, "Slack", Aeq.rows(), Ain.rows(), slack.data()),
      env);

  int offset = 0;
  active.clear();
  for (int k = 0; k < Ain.rows(); k++) {
    if (slack(k) < active_set_slack_tolerance) active.insert(k);
  }
  offset = Ain.rows();
  if (lb.rows() == nparams) {
    for (int k = 0; k < nparams; k++) {
      if (x(k) - lb(k) < active_set_slack_tolerance) active.insert(offset + k);
    }
  }
  if (ub.rows() == nparams) {
    for (int k = 0; k < nparams; k++) {
      if (ub(k) - x(k) < active_set_slack_tolerance) {
        active.insert(offset + k + nparams);
      }
    }
  }

  return model;
}

GRBmodel* gurobiActiveSetQP(GRBenv* env, vector<MatrixXd*> QblkDiag,
                            // TODO(#2274) NOLINTNEXTLINE(runtime/references).
                            VectorXd& f,
                            const MatrixXd& Aeq,
                            const VectorXd& beq, const MatrixXd& Ain,
                            const VectorXd& bin,
                            // TODO(#2274) NOLINTNEXTLINE(runtime/references).
                            VectorXd& lb, VectorXd& ub,
                            // TODO(#2274) NOLINTNEXTLINE(runtime/references).
                            int*& vbasis, int vbasis_len,
                            // TODO(#2274) NOLINTNEXTLINE(runtime/references).
                            int*& cbasis, int cbasis_len,
                            // TODO(#2274) NOLINTNEXTLINE(runtime/references).
                            VectorXd& x) {
  // NOTE:  this allocates memory for a new GRBmodel and returns it. (you should
  // delete this object when you're done with it)
  // NOTE:  by convention here, the active set indices correspond to Ain, bin
  // first, then lb, then ub.
  GRBmodel* model = NULL;

  int method;
  GRBgetintparam(env, "method", &method);
  if (!(method == 0 || method == 1)) {
    cerr << "gurobiActiveSetQP: method should be 0 or 1" << endl;
    return NULL;
  }

  int i, j, nparams = f.rows(), Qi, Qj;
  double* lbdata = NULL, * ubdata = NULL;
  if (lb.rows() == nparams) lbdata = lb.data();
  if (ub.rows() == nparams) ubdata = ub.data();
  CGE(GRBnewmodel(env, &model, "QP", nparams, NULL, lbdata, ubdata, NULL, NULL),
      env);

  int startrow = 0, d;
  for (vector<MatrixXd*>::iterator iterQ = QblkDiag.begin();
       iterQ != QblkDiag.end(); iterQ++) {
    MatrixXd* Q = *iterQ;

    *Q = .5 * (*Q);

    if (Q->rows() == 1 || Q->cols() == 1) {  // it's a vector
      d = Q->rows() * Q->cols();
      for (i = 0; i < d; i++) {
        Qi = i + startrow;
        double& qval = Q->operator()(i);
        CGE(GRBaddqpterms(model, 1, &Qi, &Qi, &qval), env);
      }
      startrow = startrow + d;
    } else {  // potentially dense matrix
      d = Q->rows();
      if (d != Q->cols()) {
        cerr << "Q is not square! " << Q->rows() << "x" << Q->cols() << "\n";
        return NULL;
      }

      for (i = 0; i < d; i++)
        for (j = 0; j < d; j++) {
          Qi = i + startrow;
          Qj = j + startrow;
          double& qval = Q->operator()(i, j);
          CGE(GRBaddqpterms(model, 1, &Qi, &Qj, &qval), env);
        }
      startrow = startrow + d;
    }
    if (startrow > nparams) {
      cerr << "Q is too big!" << endl;
      return NULL;
    }
  }

  CGE(GRBsetdblattrarray(model, "Obj", 0, nparams, f.data()), env);

  if (Aeq.rows() > 0)
    CGE(myGRBaddconstrs(model, Aeq, beq, GRB_EQUAL, 1e-18), env);
  if (Ain.rows() > 0)
    CGE(myGRBaddconstrs(model, Ain, bin, GRB_LESS_EQUAL, 1e-18), env);

  CGE(GRBupdatemodel(model), env);

  int numvars;
  CGE(GRBgetintattr(model, "NumVars", &numvars), env);
  if (numvars == vbasis_len) {
    CGE(GRBsetintattrarray(model, "VBasis", 0, numvars, vbasis), env);
  } else {
    delete[] vbasis;
    vbasis = new int[numvars];
  }

  int numconstr;
  CGE(GRBgetintattr(model, "NumConstrs", &numconstr), env);
  if (numconstr == cbasis_len) {
    CGE(GRBsetintattrarray(model, "CBasis", 0, numconstr, cbasis), env);
  } else {
    delete[] cbasis;
    cbasis = new int[numconstr];
  }
  CGE(GRBoptimize(model), env);

  CGE(GRBgetdblattrarray(model, GRB_DBL_ATTR_X, 0, nparams, x.data()), env);

  CGE(GRBgetintattrarray(model, "VBasis", 0, numvars, vbasis), env);
  CGE(GRBgetintattrarray(model, "CBasis", 0, numconstr, cbasis), env);

  return model;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/unique.hpp>
#include <boost/geometry/multi/algorithms/unique.hpp>

#include <boost/geometry/domains/gis/io/wkt/wkt.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <boost/geometry/multi/geometries/multi_point.hpp>
#include <boost/geometry/multi/geometries/multi_linestring.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>

#include <algorithms/test_unique.hpp>

template <typename P>
void test_all()
{
    // Multi point, should happen nothing, even if there are duplicate points
    test_geometry<bg::model::multi_point<P> >(
        "MULTIPOINT((0 0),(0 0),(1 1))",
        "MULTIPOINT((0 0),(0 0),(1 1))");

    test_geometry<bg::model::multi_linestring<bg::model::linestring<P> > >(
        "MULTILINESTRING((0 0,1 1,1 1),(3 3,3 3,4 4))",
        "MULTILINESTRING((0 0,1 1),(3 3,4 4))");

    typedef bg::model::multi_polygon<bg::model::polygon<P> > mp;
    test_geometry<mp>(
        "MULTIPOLYGON(((0 0,0 1,1 1,1 1,1 1,1 0,0 0,0 0)))",
        "MULTIPOLYGON(((0 0,0 1,1 1,1 0,0 0)))");

    // With holes
    test_geometry<mp>(
        "MULTIPOLYGON(((0 0,0 10,10 10,10 10,10 10,10 0,0 0,0 0)))",
        "MULTIPOLYGON(((0 0,0 10,10 10,10 0,0 0)))");

}

int test_main( int , char* [] )
{
    test_all<bg::model::d2::point_xy<int> >();
    test_all<bg::model::d2::point_xy<double> >();

#ifdef HAVE_TTMATH
    test_all<bg::model::d2::point_xy<ttmath_big> >();
#endif

    return 0;
}


#include "CHOLMODSolver.hpp"
#include <Eigen/CholmodSupport>
#include <iostream>

namespace polysolve
{

    cholmod_sparse eigen2cholmod(StiffnessMatrixL &mat)
    {
        cholmod_sparse res;
        res.nzmax   = mat.nonZeros();
        res.nrow    = mat.rows();
        res.ncol    = mat.cols();
        res.p       = mat.outerIndexPtr();
        res.i       = mat.innerIndexPtr();
        res.x       = mat.valuePtr();
        res.z       = 0;
        res.sorted  = 1;
        if(mat.isCompressed())
        {
            res.packed  = 1;
            res.nz = 0;
        }
        else
        {
            res.packed  = 0;
            res.nz = mat.innerNonZeroPtr();
        }

        res.dtype = CHOLMOD_DOUBLE;

        res.itype = CHOLMOD_LONG;
        res.stype = 1;
        res.xtype = CHOLMOD_REAL;

        return res;
    }

    cholmod_dense eigen2cholmod(Eigen::VectorXd &mat)
    {
        cholmod_dense res;
        res.nrow   = mat.size();
        res.ncol   = 1;
        res.nzmax  = res.nrow * res.ncol;
        res.d      = mat.derived().size();
        res.x      = (void*)(mat.derived().data());
        res.z      = 0;
        res.xtype = CHOLMOD_REAL;
        res.dtype   = 0;

        return res;
    }
    ////////////////////////////////////////////////////////////////////////////////

    CHOLMODSolver::CHOLMODSolver()
    {
        cm = (cholmod_common*)malloc(sizeof(cholmod_common));
        cholmod_l_start (cm);
        L = NULL;

        cm->useGPU = 1;
        cm->maxGpuMemBytes = 2e9;
        cm->print = 4;
        cm->supernodal = CHOLMOD_SUPERNODAL;
    }

    // void CHOLMODSolver::getInfo(json &params) const
    // {
    //     params["num_iterations"] = num_iterations;
    //     params["final_res_norm"] = final_res_norm;
    // }

    void CHOLMODSolver::analyzePattern(StiffnessMatrixL &Ain)
    {
        if(!Ain.isCompressed()) {
            Ain.makeCompressed();
        }
        A = eigen2cholmod(Ain);
        L = cholmod_l_analyze (&A, cm);
    }

    void CHOLMODSolver::factorize(StiffnessMatrixL &Ain)
    {
        cholmod_l_factorize (&A, L, cm);
    }

    void CHOLMODSolver::solve(Eigen::VectorXd &rhs, Eigen::VectorXd &result)
    {
        b = eigen2cholmod(rhs);
        x = cholmod_l_solve (CHOLMOD_A, L, &b, cm);
        result.conservativeResize(rhs.size());
        memcpy(result.data(), x->x, result.size() * sizeof(result[0]));
    }

    ////////////////////////////////////////////////////////////////////////////////

    CHOLMODSolver::~CHOLMODSolver()
    {
        // cholmod_l_gpu_stats(cm);
        cholmod_l_free_factor (&L, cm);
        cholmod_l_free_dense (&x, cm);
    }

} // namespace polysolve