/*
! Program for 2D explicit finite element analysis of incompressible Navier-Stokes
!
!
! Author: Dr. Chennakesava Kadapa
! Date  : 17-May-2018
! Place : Swansea, UK
!
!
*/

#include "headersVTK.h"
#include "headersBasic.h"
#include "headersEigen.h"
#include "elementutilitiescfd.h"
#include "SolutionData.h"
#include "BernsteinElem2DINSTria6Node.h"
#include "BernsteinElem2DINSQuad9Node.h"
#include <Eigen/SuperLUSupport>

using namespace std;

int main(int argc, char* argv[])
{
    double tstart, tend;

    int ndim=2, ndof=3, npElem=6;
    double fact, xNode[50], yNode[50];

    vector<vector<int> > ElemDofArray;

    int  nElem, nNode, nDBC, nFBC;
    int  ee, ii, jj, kk, ind, count, row, col;
    int  n1, n2, n3, n4, n5, n6, nsize;
    int  nn, dof;

    string  infileNodes, infileElems, infileDBCs, infileOutput;
    string  infileFBCs, charTemp, outFileName;

    //Set file names
    //The file names are specified as inputs from the command line
    if(argc == 0)
    {
        cerr << " Error in input data " << endl;
        cerr <<  "Number of input files is not sufficient " << endl;
        cerr <<  "You must enter names of THREE files" << endl;
        cerr <<  "a.) Node file, b.) Element file, and c.) Dirichlet BC file" << endl;
        cerr << "Aborting..." << endl;
    }
    else
    {
       infileNodes = argv[1];
       infileElems = argv[2];
       infileDBCs  = argv[3];

       //if(argc == 5)
         //infileFBCs  = argv[3];

       if(argc == 5)
         infileOutput = argv[4];
    }

    // Read nodal data files
    /////////////////////////////////////

    std::ifstream  infile_nodes(infileNodes);
    std::ifstream  infile_elems(infileElems);
    std::ifstream  infile_DBCs(infileDBCs);
    std::ifstream  infile_FBCs(infileFBCs);

    if(infile_nodes.fail())
    {
       cout << " Could not open the input nodes file " << endl;
       exit(1);
    }

    double  val[50];
    int  val2[50];

    std::string line;

    // read nodal coordinates
    ////////////////////////////////////////////

    cout << " reading nodes " << endl;

    nNode = 0;
    while (std::getline(infile_nodes, line))
      ++nNode;

    vector<vector<double> >  node_coords(nNode, vector<double>(3));

    infile_nodes.clear();
    infile_nodes.seekg(0, infile_nodes.beg);

      ii=0;
      while(infile_nodes >> val[0] >> val[1] >> val[2] )
      {
        //printf("%12.6f \t %12.6f \t %12.6f \n", val[0], val[1], val[2]);

        node_coords[ii][0] = val[1];
        node_coords[ii][1] = val[2];
        node_coords[ii][2] = 0.0;

        ii++;
      }

    // read elements
    ////////////////////////////////////////////
    cout << " reading elements " << endl;

    if(infile_elems.fail())
    {
       cout << " Could not open the input elements file " << endl;
       exit(1);
    }

    nElem = 0;
    while (std::getline(infile_elems, line))
      ++nElem;

    cout << " nElem   " << nElem << endl;

    infile_elems.clear();
    infile_elems.seekg(0, infile_elems.beg);

    vector<vector<int> >  elemNodeConn(nElem, vector<int>(npElem));

    ee=0;
    if(npElem == 6)
    {
      while(infile_elems >> val2[0] >> val2[1] >> val2[2] >> val2[3] >> val2[4] >> val2[5] >> val2[6] >> val2[7] >> val2[8] >> val2[9] )
      {
        //printf("%6d \t %6d \t %6d \t %6d \n", val2[4], val2[5], val2[6], val2[7]);

        for(ii=0; ii<npElem; ii++)
          elemNodeConn[ee][ii] = val2[4+ii]-1;

        ee++;
      }
    }
    else if(npElem == 9)
    {
      while(infile_elems >> val2[0] >> val2[1] >> val2[2] >> val2[3] >> val2[4] >> val2[5] >> val2[6] >> val2[7] >> val2[8] >> val2[9] >> val2[10] >> val2[11] >> val2[12] )
      {
        //printf("%6d \t %6d \t %6d \t %6d \n", val2[4], val2[5], val2[6], val2[7]);

        for(ii=0; ii<npElem; ii++)
          elemNodeConn[ee][ii] = val2[4+ii]-1;

        ee++;
      }
    }
    else
    {
      cerr << " Invalid npElem " << npElem << endl;
      exit(-1);
    }
    //
    // Read Dirichlet BC data
    //
    ////////////////////////////////////////////
    cout << " reading DBCs " << endl;

    if(infile_DBCs.fail())
    {
       cout << " Could not open the input elements file " << endl;
       exit(1);
    }

    vector<vector<double> >  DirichletBCs;
    vector<double>  vecDblTemp(3);

    nDBC = 0;
    while(infile_DBCs >> val[0] >> val[1] >> val[2] )
    {
      vecDblTemp[0] = val[0]-1;
      vecDblTemp[1] = val[1]-1;
      vecDblTemp[2] = val[2];

      //if( val[1] <= ndof )
      //{
        DirichletBCs.push_back(vecDblTemp);
        nDBC++;
      //}
    }

    cout << " nDBC  = " << '\t' << nDBC << endl;

    infile_nodes.close();
    infile_elems.close();
    infile_DBCs.close();

    //
    // Read Output data
    //
    ////////////////////////////////////////////

    vector<int>  OutputData;

    if( !infileOutput.empty() )
    {
      std::ifstream  infile_output(infileOutput);

      cout << " reading output data " << endl;

      if(infile_output.fail())
      {
        cout << " Could not open the input elements file " << endl;
        exit(1);
      }

      ii = 0;
      while (std::getline(infile_output, line))
        ++ii;

      infile_output.clear();
      infile_output.seekg(0, infile_output.beg);

      OutputData.resize(ii);

      ee=0;
      while(infile_output >> val[0] )
      {
        OutputData[ee] = val[0]-1;
        ee++;
      }

      infile_output.close();
    }

    cout << " Input files have been read successfully \n\n " << endl;

      vector<vector<int> >  midNodeData;

      midNodeData.resize(nNode);

      for(ii=0; ii<nNode; ii++)
      {
        midNodeData[ii].resize(3);

        midNodeData[ii][0] = 0;  midNodeData[ii][1] = 0;  midNodeData[ii][2] = 0;
      }

    if(npElem == 6)
    {
      for(ee=0; ee<nElem; ee++)
      {
        ii = elemNodeConn[ee][3];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][0];
        midNodeData[ii][2] = elemNodeConn[ee][1];

        ii = elemNodeConn[ee][4];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][1];
        midNodeData[ii][2] = elemNodeConn[ee][2];

        ii = elemNodeConn[ee][5];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][2];
        midNodeData[ii][2] = elemNodeConn[ee][0];
      }
    }
    else if(npElem == 9)
    {
      for(ee=0; ee<nElem; ee++)
      {
        ii = elemNodeConn[ee][4];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][0];
        midNodeData[ii][2] = elemNodeConn[ee][1];

        ii = elemNodeConn[ee][5];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][1];
        midNodeData[ii][2] = elemNodeConn[ee][2];

        ii = elemNodeConn[ee][6];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][2];
        midNodeData[ii][2] = elemNodeConn[ee][3];

        ii = elemNodeConn[ee][7];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][3];
        midNodeData[ii][2] = elemNodeConn[ee][0];
      }
    }

    vector<vector<int> >  ID;
    vector<vector<bool> >  NodeType;
    vector<int>  assyForSoln;

    NodeType.resize(nNode);

    ID.resize(nNode);

    for(ii=0;ii<nNode;ii++)
    {
      NodeType[ii].resize(ndof);
      ID[ii].resize(ndof);

      for(jj=0;jj<ndof;jj++)
      {
        NodeType[ii][jj] = false;
        ID[ii][jj] = -1;
      }
    }

    // fix the pressure at all the mid nodes
    for(ii=0; ii<nNode; ii++)
    {
      if(midNodeData[ii][0])
        NodeType[ii][2] = true;
    }

    if(npElem == 9)
    {
      for(ee=0; ee<nElem; ee++)
      {
        NodeType[elemNodeConn[ee][8]][2] = true;
      }
    }

    // fix the specified Dirichlet BCs
    for(ii=0; ii<nDBC; ii++)
    {
      //cout << ii << '\t' << DirichletBCs[ii][0] << '\t' << DirichletBCs[ii][1] << endl;
      NodeType[DirichletBCs[ii][0]][DirichletBCs[ii][1]] = true;
    }

    //for(ii=0; ii<totalDOF; ii++)
      //cout << ii << '\t' << assyForSoln[ii] << endl;

    int totalDOF = 0;
    for(ii=0;ii<nNode;ii++)
    {
      for(jj=0;jj<ndof;jj++)
      {
        //cout << ii << '\t' << jj << '\t' << NodeType[ii][jj] << endl;
        if(!NodeType[ii][jj])
        {
          ID[ii][jj] = totalDOF++;
          assyForSoln.push_back(ii*ndof+jj);
        }
      }
    }

      cout << " Mesh statistics .....\n" << endl;
      cout << " nElem          = " << '\t' << nElem << endl;
      cout << " nNode          = " << '\t' << nNode  << endl;
      cout << " npElem         = " << '\t' << npElem << endl;
      cout << " ndof           = " << '\t' << ndof << endl;
      cout << " Total DOF      = " << '\t' << totalDOF << endl;

      vector<vector<int> >  LM, forAssyMat;

      LM.resize(nElem);

      for(ee=0;ee<nElem;ee++)
      {
        npElem = elemNodeConn[ee].size();

        //printVector(IEN[ee]);

        ind = ndof*npElem;
        LM[ee].resize(ind);

        for(ii=0;ii<npElem;ii++)
        {
          ind = ndof*ii;

          kk = elemNodeConn[ee][ii];

          for(jj=0;jj<ndof;jj++)
          {
            LM[ee][ind+jj] = ID[kk][jj];
          }
        }
      }

      printf("\n element DOF values initialised \n\n");
      printf("\n Preparing matrix pattern \n\n");

      // compute matrix pattern needs to be prepared only for the implicit solver

      forAssyMat.clear();
      forAssyMat.resize(totalDOF);

      int *tt, r, c;

    for(ee=0;ee<nElem;ee++)
    {
      tt = &(LM[ee][0]);
      nsize = LM[ee].size();

      for(ii=0;ii<nsize;ii++)
      {
        r = tt[ii];

        if(r != -1)
        {
          for(jj=0;jj<nsize;jj++)
          {
            if(tt[jj] != -1)
            {
              //printf("ii.... %5d \t %5d \t %5d \t %5d \n",ii, jj, r, tt[jj]);
              forAssyMat[r].push_back(tt[jj]);
            }
          }
        }
      }
    }

    printf("\n Preparing matrix pattern DONE \n\n");

    VectorXi  nnzVec(totalDOF);

    int nnz = 0;
    for(ii=0;ii<totalDOF;ii++)
    {
      findUnique(forAssyMat[ii]);

      nnzVec[ii] = forAssyMat[ii].size();
      nnz += nnzVec[ii];
    }
    cout << " nnz " << nnz << endl;

    bool pp1=false;
    //pp1=true;
    if(pp1)
    {
       printf("   Number of non-zeros = %5d \n\n", nnz);
       printf("   dof to dof connectivity ...:  \n\n");
       for(ii=0;ii<totalDOF;ii++)
       {
          cout << " dof # " << ii << " : ";
          for(jj=0;jj<forAssyMat[ii].size();jj++)
            cout << '\t' << forAssyMat[ii][jj];
          cout << endl;
       }
       printf("\n\n\n");
    }

      /////////////////////////////////////////
      //
      // Eigen based solver
      //
      /////////////////////////////////////////

      cout << " Eigen based solver " << totalDOF << endl;

      //solver->rhsVec.resize(nRow);

      SparseMatrixXd  mtx(totalDOF, totalDOF);

      mtx.reserve(nnz);
      mtx.reserve(nnzVec);

      for(ii=0;ii<totalDOF;ii++)
      {
        for(jj=0;jj<forAssyMat[ii].size();jj++)
        {
          mtx.coeffRef(ii, forAssyMat[ii][jj]) = 0.0;
        }
      }

      mtx.makeCompressed();

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

      VectorXd  pres, presCur, presPrev, presPrev2, presPrev3;
      VectorXd  presDot, presDotPrev, presDotCur, presDiff;
      VectorXd  velo, veloCur, veloDiff, veloPrev, veloPrev2, veloPrev3;
      VectorXd  veloDot, veloDotPrev, acceCur;

      VectorXd  solnApplied, solnVTK, soln, solnCur, solnTemp, rhsVec;

      ind = nNode*ndim;

      velo.resize(ind);
      velo.setZero();

      veloPrev  = velo;
      veloCur   = velo;
      veloPrev2 = velo;
      veloPrev3 = velo;

      veloDot     = velo;
      veloDotPrev = veloDot;
      acceCur  = veloDot;

      pres.resize(nNode);
      pres.setZero();

      presPrev  = pres;
      presPrev2 = pres;
      presPrev3 = pres;
      presCur   = pres;

      presDot     = velo;
      presDotPrev = pres;
      presDotCur  = pres;

      soln.resize(nNode*ndof);
      soln.setZero();

      solnCur = soln;
      solnApplied = soln;

      solnTemp.resize(totalDOF);
      solnTemp.setZero();

      rhsVec = solnTemp;

      ////////////////////////////////////////////////
      // Computations start from here
      ////////////////////////////////////////////////

      char fname[200];
      //sprintf(fname,"(),"-explicit.dat");

      ofstream fout("convergence-data2.dat");

      if(fout.fail())
      {
        cout << " Could not open the Output file" << endl;
        exit(1);
      }

      fout.setf(ios::fixed);
      fout.setf(ios::showpoint);
      fout.precision(14);

      double  xx, yy;
//
      // loop over the nodes and adjust nodal coordinates
      for(nn=0; nn<nNode; nn++)
      {
        if( midNodeData[nn][0] )
        {
          n1 = midNodeData[nn][1];
          n2 = midNodeData[nn][2];

          xx = 0.25*node_coords[n1][0] + 0.25*node_coords[n2][0];
          yy = 0.25*node_coords[n1][1] + 0.25*node_coords[n2][1];

          node_coords[nn][0] = 2.0*(node_coords[nn][0] - xx);
          node_coords[nn][1] = 2.0*(node_coords[nn][1] - yy);
        }
      }
//

      double  elemData[50], timeData[50];
      double  norm_rhs=1000.0, norm_pres=1000.0;

      //time integration parameters
      timeData[1] = 1.0;   timeData[2] = 0.0;

      //density
      elemData[0] = 1.0;
      //viscosity
      //elemData[1] = 1.0;
      //elemData[1] = 0.025;
      elemData[1] = 1.0/229.0;
      //Body force in X-, Y- and Z- direction
      elemData[2] = 0.0;   elemData[3] = 0.0; elemData[4] = 0.0;
      //beta
      elemData[5] = 2.0;

      double  rhoInf;
      double  am = 1.0;
      double  gamm = 0.5+am;

      int  stepsMax = 4000000;
      int  stepsCompleted=0;
      int  outputFreq = 10;
      int  fileCount=1;

      double  dt = 0.01;
      double  timeFact=0.0;
      double  timeNow=0.0;
      double  timeFinal = 4000.0;
      double  num, denom;
      double  fact1, fact2;

      // create elements and prepare element data
      BernsteinElem2DINSTria6Node   **elems;
      //BernsteinElem2DINSQuad9Node   **elems;

      elems = new BernsteinElem2DINSTria6Node* [nElem];
      //elems = new BernsteinElem2DINSQuad9Node* [nElem];

      for(ee=0;ee<nElem;ee++)
      {
        elems[ee] = new BernsteinElem2DINSTria6Node;
        //elems[ee] = new BernsteinElem2DINSQuad9Node;

        elems[ee]->nodeNums = elemNodeConn[ee];

        //elems[ee]->SolnData = &(SolnData);

        elems[ee]->prepareElemData(node_coords);

        elems[ee]->forAssyVec = LM[ee];
      }

      cout << " elements are created and prepated " << endl;
      cout << " Computing the solution \n" << endl;

      //SimplicialLDLT<SparseMatrix<double> > solver;
      SuperLU<SparseMatrixXd > solver;

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

      velo.setZero();      veloPrev.setZero();      veloCur.setZero();
      veloDot.setZero();      veloDotPrev.setZero();      acceCur.setZero();
      pres.setZero();      presPrev.setZero();      presCur.setZero();
      presDot.setZero();   presDotPrev.setZero();   presDotCur.setZero();

      //for(ii=0; ii<nNode_Velo; ii++)
      //{
        //veloPrev(ii*2) = 1.0;
      //}

      //velo = veloPrev;

        //SolnData.applyDirichletBCs(timeFact);

        solnApplied.setZero();

        //Add specified Dirichlet BC
        for(ii=0; ii<nDBC; ii++)
        {
          nn  = DirichletBCs[ii][0];
          dof = DirichletBCs[ii][1];

          jj = nn*ndof+dof;

          solnApplied(jj) = DirichletBCs[ii][2];
        }

        //printVector(soln);

        for(ii=0; ii<nDBC; ii++)
        {
          nn   = (int) (DirichletBCs[ii][0]);
          dof  = (int) (DirichletBCs[ii][1]);

          if( midNodeData[nn][0] )
          {
            fact = 0.25*solnApplied(midNodeData[nn][1]*ndof+dof) + 0.25*solnApplied(midNodeData[nn][2]*ndof+dof);

            solnApplied[nn*ndof+dof] = 2.0*(solnApplied(nn*ndof+dof) - fact);
          }
        }
        //printVector(solnApplied);

      //Time loop
      //while( (stepsCompleted < stepsMax ) && (timeNow < timeFinal) )

      vector<int>  vecTempInt;

      ind = npElem*ndof;

      VectorXd  Flocal(ind);
      MatrixXd  Klocal(ind, ind);

      int iter=0, loadIncr;

      soln.setZero();

      for(loadIncr=0; loadIncr<10; loadIncr++)
      {
        //timeFact = 0.5*(1-cos(PI*stepsCompleted/2000));
        timeFact = 0.1*(loadIncr+1);

        for(ii=0; ii<nDBC; ii++)
        {
          nn   = (int) (DirichletBCs[ii][0]);
          dof  = (int) (DirichletBCs[ii][1]);

          jj = nn*ndof + dof;

          soln[jj] = timeFact*solnApplied(jj);
        }

        //cout << " aaaaaaaaaaa " << endl;

      for(iter=0; iter<20; iter++)
      {
        //Loop over elements and compute the RHS

        mtx *= 0.0;
        rhsVec.setZero();

        for(ee=0; ee<nElem; ee++)
        {
            fact = timeNow - dt;

            //Compute the element force vector, including residual force
            elems[ee]->StiffnessAndResidual(node_coords, elemData, timeData, soln, Klocal, Flocal, fact);

            vecTempInt = elems[ee]->forAssyVec;
            ind = vecTempInt.size();

            //Assemble the element vector
            for(ii=0; ii<ind; ii++)
            {
              r = vecTempInt[ii];
              if( r != -1)
              {
                rhsVec(r)  += Flocal(ii);

                for(jj=0; jj<ind; jj++)
                {
                  c = vecTempInt[jj];
                  if( c != -1)
                  {
                    mtx.coeffRef(r, c)  += Klocal(ii, jj);
                  }
                }
              }
            }
        } //LoopElem

        // Add specified nodal force
        //SolnData.addNodalForces(timeFact);

        norm_rhs = rhsVec.norm();

        cout << " RHS norm = " << norm_rhs << endl;

        if(norm_rhs < 1.0e-8)
        {
          cout << " Solution converged below the specified tolerance " << endl;
          break;
        }
        else
        {
          solver.compute(mtx);

          solnTemp = solver.solve(rhsVec);

          for(ii=0; ii<totalDOF; ii++)
          {
            soln(assyForSoln[ii]) += solnTemp(ii);
          }

          //printVector(soln);
        }
      } // iteration loop

          // write the solution to the VTK file

          velo.resize(nNode*3);
          velo.setZero();
          pres.setZero();

          /*
          for(ii=0; ii<nNode; ii++)
          {
            n1 = ii*2;
            n2 = ii*3;

            velo(n1)   = soln(n2);
            velo(n1+1) = soln(n2+1);

            pres(ii)   = soln(n2+2);

            //pres(ii)   = soln(ii);
          }

          for(ii=0; ii<nNode; ii++)
          {
            if(midNodeData[ii][0])
            {
              jj = ii*2;
              n1 = midNodeData[ii][1]*2;
              n2 = midNodeData[ii][2]*2;

              velo(jj)    = 0.5*velo(jj)   + 0.25*velo(n1)   + 0.25*velo(n2);
              velo(jj+1)  = 0.5*velo(jj+1) + 0.25*velo(n1+1) + 0.25*velo(n2+1);

              pres(ii)    = 0.5*(pres(midNodeData[ii][1]) + pres(midNodeData[ii][2]) );
            }
          }
          */

          //writevtk(ndim, node_coords, elemNodeConn, midNodeData, soln, fileCount);

          fileCount = fileCount+1;

          //fact1 = 0.0;
          //fact2 = 0.0;
          //for(ii=0; ii<OutputData.size(); ii++)
          //{
            //jj = OutputData[ii]*2;

            //fact1 += rhsVec(jj);
            //fact2 += rhsVec(jj+1);
          //}

          //fout << timeNow << '\t' << stepsCompleted << '\t' << norm_rhs << '\t' << norm_pres ;
          //fout << '\t' << fact1 << '\t' << fact2 << endl;

        // store the variables
        //SolnData.timeUpdate();
        veloPrev3  = veloPrev2;
        veloPrev2  = veloPrev;
        veloPrev   = velo;

        veloDotPrev  = veloDot;

        presPrev3  = presPrev2;
        presPrev2  = presPrev;
        presPrev   = pres;
        presDotPrev  = presDot;

      } //Time loop

    //
    cout << " Computing errors \n " << endl;
    double totalError = 0.0;
    //cout << " index = " << index << endl;
    for(int index=0; index<4; index++)
    {
      totalError = 0.0;
      for(ee=0; ee<nElem; ee++)
      {
        //Compute the element force vector, including residual force
        totalError += elems[ee]->CalculateError(node_coords, elemData, timeData, soln, veloDot, pres, timeNow, index);
      }

      totalError = sqrt(totalError);

      if(index == 0)
        printf(" \n\n \t L2 Error in X-velocity = %12.6E \n\n " , totalError);
      else if(index == 1)
        printf(" \n\n \t L2 Error in Y-velocity = %12.6E \n\n " , totalError);
      else if(index == 2)
        printf(" \n\n \t L2 Error in pressure   = %12.6E \n\n " , totalError);
      else
        printf(" \n\n \t H1 Error in velocity   = %12.6E \n\n " , totalError);
    }
    //

    //tend = MPI_Wtime()
    //write(*,*) "That took ", (tend-tstart), "seconds"

    if(elems != NULL)
    {
      for(ii=0;ii<nElem;ii++)
        delete elems[ii];

      delete [] elems;
      elems = NULL;
    }

    cout << " Program is successful \n " << endl;

    return 1;
}


/**
 *  @file eigen_isnan.cpp
 *  @author Maximilian Harr <maximilian.harr@daimler.com>
 *  @date 29.06.2017
 *
 *  @brief Check if matrix has nan values
 *
 *
 *
 *          Coding Standard:
 *          wiki.ros.org/CppStyleGuide
 *          https://google.github.io/styleguide/cppguide.html
 *
 *
 *  @bug
 *
 *
 *  @todo
 *
 *
 */

// PRAGMA

// SYSTEM INCLUDES
#include <Eigen/Core>
#include <iostream>

// PROJECT INCLUDES

// LOCAL INCLUDES

// FORWARD REFERENCES

// FUNCTION PROTOTYPES

// GLOBAL VARIABLES

using namespace Eigen;
using namespace std;

//// MAIN //////////////////////////////////////////////////////////////////////////////////////////
int main(int argc, char* argv[])
{
	Eigen::Matrix3d v;
	v << 0, 1, 2, 3, 4, 5, 6, 7, 8;

	if( !isnan(v.array()).isZero(0) || !isinf(v.array()).isZero(0) ){
		std::cout << "Matrix has a nan/inf value." << std::endl;
	}
	else{
		std::cout << "Matrix has no nan/inf value." << std::endl;
	}
	v(0,0) *= 0.0/0.0;
	v(1,1) /= 0.0;
	std::cout << v << std::endl << std::endl;
	std::cout << "isinf:" << std::endl << Eigen::isinf(v.array()) << std::endl;
	std::cout << "isnan:" << std::endl << Eigen::isnan(v.array()) << std::endl;
	if( !isnan(v.array()).isZero(0) ){
		std::cout << "Matrix has a nan/inf value." << std::endl;
	}
	else{
		std::cout << "Matrix has no nan/inf value." << std::endl;
	}

}

//// FUNCTION DEFINITIONS //////////////////////////////////////////////////////////////////////////


#include "ConnectFour.hpp"
#include "Graph.hpp"

#include <set>
#include <fstream>
#include <boost/timer/timer.hpp>
#include <boost/range/adaptor/reversed.hpp>

#include <boost/serialization/vector.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

#include <boost/variant.hpp>

/*
        We can solve connect as follows. First to comprehand the computational
        complexity, note that for every square on a 7x6 board we can have
        either {empty,hero,villian}. This means the number of boards is
        bounded above by 3^{6*7} < 2^66, and I observe about
        2.1^n number.
                All in all, this means that we can't simply let my computer
        solve as it won't fit in memory (I need to store the result
        somehow, because of the way boards are constructed I would
        be recomputing the same board from different paths).
                The way solve solve this computation problem, is to first
        work forwards, creating a database of boards, starting from
        an empty board db_0, and then iterativley produce
                        db_{i+1} = f(db_i),
        untill we reach the end of the game tree. One we have the set
                        DB = {db_i},
        we then create another database
                        s_i = f(db_i, s_{i+1}),
        where s_i as a database, evaulating each configuration. This
        iterative step.

        The algebra for evaluating the result database, is as follows.
        For each board b, where we have i moves, if b is a terminal
        board, we assign r(b) one of {Win,Lose,Draw} depending on the
        evaulation, and we are done.
                If b is a non-terminal, we then take the set all

 */

void Driver0(){
        //BoardInputOutput io;
        //auto board = io.ParseBoard(6,5,"      "
                                       //"      "
                                       //"      "
                                       //"      "
                                       //"      ");
        //auto root = GenerateGameTree(board.get());
        auto root = GenerateGameTree();
        NodeMarker marker;
        marker.Run(root);

        PRINT( marker.Lookup(root.GetStart() ) );
        Profiler prof;
        prof.Run(root);

}

/*
        This represents the level of the board,
        ie for
                (width,height,n)

                (7,6,0) => P =  {(0,0,0,0,0,0,0)}
                (7,6,1) => P =  {(1,0,0,0,0,0,0),
                                 (0,1,0,0,0,0,0),
                                 (0,1,1,0,0,0,0),
                                 ...
                                 }
                (7,6,2) => P =  {(1,0,0,0,0,0,0),
                                 (0,1,0,0,0,0,0),
                                 (0,1,1,0,0,0,0),
                                 ...
                                 }
 */

template<class T>
std::string tostring(T const& iter){
        std::stringstream sstr;
        sstr << "{";
        bool first = false;
        for( auto const& _ : iter){
                sstr
                        << ( first ? (first=false,"") : ", ")
                        << (int)_;
        }
        sstr << "}";
        return sstr.str();
}

struct LevelCombinationSet{
        using CombinationType = std::vector<char>;

        explicit LevelCombinationSet(char width, char height, char n)
                :width_{width}
                ,height_{height}
        {
                CombinationType iter(width, 0 );
                Recurse_( iter, n );
                #if 0
                char tmp = n;
                for(auto& _ : iter){
                        _ = std::min(height, tmp);
                        tmp -= _;
                        if( _ == 0 )
                                break;
                }
                #endif

        }
        auto begin()const{ return set_.begin(); }
        auto end()const{ return set_.end(); }
private:
        void Recurse_( std::vector<char> proto, char n ){
                if( n == 0 ){
                        set_.insert(std::move(proto));
                        return;
                }
                for(size_t i=0;i!=proto.size();++i){
                        if( ! (proto[i] < height_ ) )
                                continue;
                        auto next = proto;
                        ++next[i];
                        Recurse_(std::move(next), n-1);
                }
        }
        char width_;
        char height_;
        std::set<CombinationType> set_;
};

std::string Bits(unsigned long long val){
        std::bitset<sizeof(val)*8> aux{val};
        return aux.to_string();
}

void PermuattionTest(){

        #if 0
        for(int k=0;;++k){
                boost::timer::auto_cpu_timer at;
                std::vector<int> proto(7*6, 0);
                for(int i=0;i!=k;++i)
                        proto[proto.size()-1-i] = 1;
                for(;;){
                        if( ! std::next_permutation(proto.begin(), proto.end()))
                                break;
                }
                PRINT(k);
                std::cout << "--------------\n";
        }
        #endif
        unsigned long long upper = 1;
        for(int i=0;i!=6*7;++i)
                upper *= 2;
        for(int k=0;;++k){
                boost::timer::auto_cpu_timer at;
                unsigned long long iter = 0;
                int n = 0;
                PRINT_SEQ((k)(iter)(upper));
                for(;iter < upper;++iter){
                        if( __builtin_popcountll(iter) == k ){
                                PRINT_SEQ( (Bits(iter))(__builtin_popcountll(iter)) );
                                ++n;
                        }
                }
                PRINT_SEQ((upper)(k)(n));
        }

        #if 0
        LevelCombinationSet ps(7,6,10);
        for( auto const& _ : ps )
                PRINT( tostring(_));
                #endif
        /*
                want sequence
                        s1 .. sn
                where there are only K sn for sn = Empty.
                This imples can take sequence
                        t1 ... tm  where m = n -k
                where ti \in {Hero, Villian},
                        and take
                        q1 .. qk
                are index positions for inset the Empy ones

         */

}

struct SubGraph{
        void Push(Node* ptr){
                start_.push_back(ptr);
        }
        auto begin()const{ return start_.begin(); }
        auto end()const{ return start_.end(); }
        void Debug()const{
                PRINT( start_.size() );
                #if 0
                for( auto ptr : start_){
                        ptr->Ctx().Display();
                }
                #endif
        }
private:
        std::vector<Node*> start_;
};

struct Graph{
        void Push(SubGraph* sub){
                subs_.push_back(sub);
        }
        auto begin()const{ return subs_.begin(); }
        auto end()const{ return subs_.end(); }
        auto begin(){ return subs_.begin(); }
        auto end(){ return subs_.end(); }
        auto Back(){  return subs_.back(); }
        // Assume first one only have one entry
        auto Start(){ return *subs_.front()->begin(); }
        auto At(size_t idx)const{ return subs_[idx]; }
        auto Size()const{ return subs_.size(); }
private:
        // increasing popcount
        std::vector<SubGraph*> subs_;
};

#include <boost/pool/object_pool.hpp>

/*
        The essence of the problem is that I can't easily work backwards
        in this game. This is because I can't git the game tree
        with depth of 42 into memory. I also have to keep results in
        memory, because otherwise I'll be re-computing the result
        for a certain board for each path from it.
                What I need to do is create auxiallary sets of all the
        board at each depth. From this, I can work backwards.

*/

#if 0
struct BoardAllocator{
        using hash_t = decltype(std::declval<Board>().Hash());
        // (was_allocated, ptr)
        std::pair<bool, Board*> FindOrAllocate(Board const& brd){
                auto h = brd.Hash();
                auto iter = nodes_.find(h);
                if( iter == nodes_.end()){
                        auto tmp = pool_.construct(brd);
                        nodes_.emplace(h, tmp);
                        return std::make_pair(true, tmp);
                }
                return std::make_pair(false, iter->second);
        }
private:
        boost::object_pool<Board> pool_;
        std::map<hash_t, Board*> nodes_;
};

SubGraph* CreateSubGraph(BoardAllocator& alloc, SubGraph const& entry){
        ConnectFourLogic logic;

        SubGraph* result = new SubGraph;

        for( auto node : entry){
                auto const& ctx{node->Ctx()};

                // try to place a tile
                for( unsigned x=0;x!=ctx.BoardWidth();++x){
                        if( ! CanPlace( ctx.GetBoard(), x) ){
                                continue;
                        }
                        GameContext nextCtx{ctx};
                        nextCtx.Place(logic, x);

                        //nextCtx.Display();

                        auto ret = alloc.FindOrAllocate(nextCtx);
                        if( ret.first) {
                                result->Push(ret.second);
                        }

                        node->AddEdge( x, ret.second );
                }
        }

        return result;
}
int main(){

        Graph graph;
        SubGraph* start = new SubGraph;
        start->Push( new Node{GameContext{}} );
        //start->Push( alloc.FindOrAllocate(GameContext{}).second );
        graph.Push(start);

        std::vector< std::unique_ptr<BoardAllocator> > alloc;

        for(int i=0;;++i){
                //if( alloc.size())
                        //alloc.back().reset();
                alloc.emplace_back( std::make_unique<BoardAllocator>() );
                boost::timer::auto_cpu_timer at;
                graph.Back()->Debug();

                auto next = CreateSubGraph(*alloc.back(), *graph.Back() );

                graph.Push(next);
                //if( i == 10 )
                        //break;
                PRINT(i);

                if( alloc.size() >= 3 ){
                        alloc[alloc.size()-2].reset();
                        delete graph.At(alloc.size()-2);
                }
                PRINT( sizeof(Node) );
        }

}
#endif

template<class BoardType>
struct FileBackedBoardSet{
private:
        std::vector<BoardType> boards_;
};

template<class BoardType>
struct LevelGroup{
        using HashType = decltype(std::declval<BoardType>().Hash());
        LevelGroup():player_{Player_NotAPlayer}{}
        template<class IterType>
        LevelGroup(Player p, IterType first, IterType last)
                :player_{p}
                ,boards_{first,last}
        {
                std::sort(boards_.begin(), boards_.end());
        }
        auto GetPlayer()const{ return player_; }
        BoardType const& Find(HashType const& hash)const{
                auto iter = std::lower_bound( boards_.begin(),
                                              boards_.end(),
                                              hash );
                if( iter->Hash() == hash )
                        return *iter;
                throw std::domain_error("hash doesn't exist");
        }
        auto begin()const{ return boards_.begin(); }
        auto end()const{ return boards_.end(); }
        auto Size()const{ return boards_.size(); }
        auto const& operator[](size_t idx)const{
                return boards_.at(idx);
        }
private:
        friend class boost::serialization::access;

        template <typename Archive>
        void serialize(Archive &ar, const unsigned int version){
                ar & player_;
                ar & boards_;
        }
private:
        Player player_;
        std::vector<BoardType> boards_;
};

#if 0
template<class BoardType>
struct LevelGroupProducer{
        using LevelGroupType = LevelGroup<BoardType>;
        virtual ~LevelGroupProducer()=default;

        virtual
        std::unique_ptr<LevelGroupType> ProduceNextInSequence(LevelGroupType const& prev)const=0;
};

template<class BoardType>
struct LevelGroupProducerFactory{
        using LevelGroupProducerType = LevelGroupProducer<BoardType>;
        std::unique_ptr<LevelGroupProducerType> MakeForLevel(int level)const=0;
};
#endif

/*
        G0 -> G1 -> G2

        G1 = Generate(G0)
        G2 = Generate(G1)
        ...

        Because a single group is too large to have in memory
 */
template<class BoardType>
struct Group
{
        using LevelType = LevelGroup<BoardType>;

        struct MemoryManager{
                MemoryManager(std::string filename,
                              std::unique_ptr<LevelType> ptr):
                        filename_{std::move(filename)},
                        ptr_{std::move(ptr)}
                {
                        //PRINT(filename_);
                }
                void Close(){
                        //std::cerr << "Closing " << filename_ << "\n";
                        ptr_.reset();
                }
                bool Load(){
                        if( !! ptr_.get() )
                                return false;
                        //std::cerr << "Loading " << filename_ << "\n";
                        std::ifstream ifs(filename_);
                        if( ! ifs.is_open()){
                                std::cerr << "Unable to open " << filename_ << "\n";
                                return false;
                        }
                        boost::archive::text_iarchive ia{ifs};
                        ptr_.reset(new LevelType{});
                        ia >> *ptr_;
                        return true;
                }
                bool Save()const{
                        //std::cerr << "Saving " << filename_ << "\n";
                        if( ! ptr_.get() ){
                                std::cerr << "Nothing to save\n";
                                return false;
                        }
                        std::ofstream of(filename_);
                        if( ! of.is_open()){
                                std::cerr << "Unable to open " << filename_ << "\n";
                                return false;
                        }
                        boost::archive::text_oarchive oa{of};
                        oa << *ptr_;
                        return true;
                }
                LevelType const* operator->(){
                        // Autoload
                        Load();
                        return ptr_.get();
                }
                LevelType const& operator*(){
                        // Autoload
                        Load();
                        return *ptr_;
                }
        private:
                std::string filename_;
                mutable std::unique_ptr<LevelType> ptr_;
        };

        auto& DeclSubGroup(std::string const& filename, std::unique_ptr<LevelType> ptr){
                groups_.emplace_back(filename, std::move(ptr));
                return groups_.back();
        }
        auto Size()const{ return groups_.size(); }
        auto& operator[](size_t idx){
                return groups_.at(idx);
        }

private:
        mutable std::vector<MemoryManager> groups_;
};

void Driver2(){
        using BoardType = GenericBoard<7,6>;
        //using BoardType = GenericBoard<4,4>;
        ConnectFourLogic logic;
        BoardInputOutput io;

        std::vector<Player> p;
        std::vector< std::set< BoardType > > b;

        p.emplace_back(Player_Hero);
        b.emplace_back();
        b.back().emplace();

        Group<BoardType> group;

        int level=0;

        auto namebroker = [](int lvl){
                std::stringstream sstr;
                sstr << BoardType::Width() << "x" << BoardType::Height() << "Depth" << lvl << ".bin";
                return sstr.str();
        };

        auto& firstSub = group.DeclSubGroup(namebroker(level++), std::make_unique<LevelGroup<BoardType> >(p.back(), b.back().begin(), b.back().end() ));
        firstSub.Save();

        for(;;){
                boost::timer::auto_cpu_timer at;

                p.emplace_back( NextPlayer(p.back()));
                b.emplace_back();

                auto width = b.back().begin()->Width();
                auto height = b.back().begin()->Height();

                auto t = TileForPlayer(NextPlayer(p.back()));

                for( auto const& board : b[b.size()-2]){

                        auto e = logic.Evaluate(board);
                        if( e != Eval_NotFinished )
                                continue;

                        for( unsigned x=0;x!=width;++x){
                                auto level = Level(board, x);
                                if( level == height )
                                        continue;
                                BoardType next(board);
                                next.Set(x, level, t);

                                b.back().emplace(next);

                                //io.Display(next);
                                //std::stringstream sstr;
                                //sstr
                                        //<< io.ToString(board) << " => "
                                        //<< io.ToString(next);
                                //std::cout << sstr.str() << "\n";
                        }
                }

                if( b.back().size() == 0 ){
                        p.pop_back();
                        b.pop_back();
                        break;
                }

                auto& sub = group.DeclSubGroup(namebroker(level++), std::make_unique<LevelGroup<BoardType> >(p.back(), b.back().begin(), b.back().end() ));
                sub.Save();

                if( group.Size() > 2 ){
                        group[group.Size()-2].Close();
                }

                PRINT(level);
                PRINT(b.back().size());
                io.Display(*b.back().begin());
                PRINT(EvalToString(logic.Evaluate(*b.back().begin())));
        }

        std::vector<
                std::map<
                        decltype( std::declval<BoardType>().Hash() ),
                        int
                >
        > m(b.size());

        auto marking_lt = [](auto left, auto right){
                static std::map<int, int> order = [](){
                        int precedence[] = {
                                Marked_Win,
                                Marked_Win  | Marked_Draw,
                                Marked_Draw,
                                Marked_Win  | Marked_Draw | Marked_Lose,
                                Marked_Win  | Marked_Lose,
                                Marked_Draw | Marked_Lose,
                                Marked_Lose,
                                Marked_Zero,
                        };
                        std::map<int, int> ret;
                        int index = 0;
                        for( auto mark : precedence ){
                                ret.emplace(mark, index++);
                        }
                        return std::move(ret);
                }();
                return order[left] < order[right];
        };

        for(size_t idx=group.Size();idx!=0;){
                --idx;

                auto& ctrl = group[idx];
                ctrl.Load();
                auto const& sub = *ctrl;

                auto cp = sub.GetPlayer();

                PRINT(sub.Size());
                PRINT(cp);

                for(size_t i=0;i!=sub.Size();++i){
                        auto const& board = sub[i];
                        auto h = board.Hash();
                        auto e = logic.Evaluate(board);

                        //std::cout << "processing " << io.ToString(board) << "\n";
                        //PRINT(EvalToString(e));
                        switch(e){
                        case Eval_Hero:
                                m[idx][h] = Marked_Win;
                                break;
                        case Eval_Villian:
                                m[idx][h] = Marked_Lose;
                                break;
                        case Eval_Draw:
                                m[idx][h] = Marked_Draw;
                                break;
                        case Eval_NotFinished:{
                                int aggregate = 0;
                                for( unsigned x=0;x!=board.Width();++x){
                                        auto level = Level(board, x);
                                        if( level == board.Height() )
                                                continue;
                                        BoardType next(board);
                                        next.Set(x, level, TileForPlayer(cp));

                                        auto nh = next.Hash();

                                        if( m[idx+1][nh] == 0 ){
                                                PRINT(x);
                                                std::cout << "####### FROM #######\n";
                                                io.Display(board);
                                                std::cout << io.ToString(board) << "\n";
                                                std::cout << "####### TO   #######\n";
                                                io.Display(next);
                                                std::cout << io.ToString(next) << "\n";
                                                PRINT_SEQ((std::get<0>(nh))(std::get<1>(nh)));
                                                std::cout << "bad marking----------------\n";
                                                std::cout << "####### END  #######\n";
                                        }
                                        if( cp == Player_Hero ){
                                                auto cand = m[idx+1][nh];
                                                if( marking_lt( cand, aggregate) )
                                                        aggregate = cand;
                                        } else {
                                                aggregate |= m[idx+1][nh];
                                        }
                                }
                                m[idx][h] = aggregate;
                        }
                                break;
                        }
                        //io.Display(board);
                        //PRINT_SEQ((std::get<0>(h))(std::get<1>(h))(m[idx][h]));
                }
                std::cout << "\n\n";
                ctrl.Close();

                //if( idx == group.Size()-4)
                        //break;
        }

        PRINT( m[0].size() );
        PRINT( m[0].begin()->second );

}

int main(){
        Driver2();
}


#include <iostream> // cerr
#include <random> // mt19937_64, uniform_x_distribution
#include <vector>
#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>

#include "johnson.hpp"

graph_t *johnson_init(const int n, const double p, const unsigned long seed) {
  static std::uniform_real_distribution<double> flip(0, 1);
  static std::uniform_int_distribution<int> choose_weight(1, 100);

  std::mt19937_64 rand_engine(seed);

  int *adj_matrix = new int[n * n];
  size_t E = 0;
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
      if (i == j) {
        adj_matrix[i*n + j] = 0;
      } else if (flip(rand_engine) < p) {
        adj_matrix[i*n + j] = choose_weight(rand_engine);
        E ++;
      } else {
        adj_matrix[i*n + j] = INT_MAX;
      }
    }
  }
  Edge *edge_array = new Edge[E];
  int *weights = new int[E];
  int ei = 0;
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
      if (adj_matrix[i*n + j] != 0
          && adj_matrix[i*n + j] != INT_MAX) {
        edge_array[ei] = Edge(i,j);
        weights[ei] = adj_matrix[i*n + j];
        ei++;
      }
    }
  }

  delete[] adj_matrix;

  graph_t *gr = new graph_t;
  gr->V = n;
  gr->E = E;
  gr->edge_array = edge_array;
  gr->weights = weights;

  return gr;
}

#ifdef CUDA
void free_graph_cuda(graph_cuda_t *g) {
  delete[] g->edge_array;
  delete[] g->weights;
  delete g;
}

void set_edge(edge_t *edge, int u, int v) {
  edge->u = u;
  edge->v = v;
}

graph_cuda_t *johnson_cuda_init(const int n, const double p, const unsigned long seed) {
  static std::uniform_real_distribution<double> flip(0, 1);
  static std::uniform_int_distribution<int> choose_weight(1, 100);

  std::mt19937_64 rand_engine(seed);

  int *adj_matrix = new int[n * n];
  size_t E = 0;
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
      if (i == j) {
        adj_matrix[i*n + j] = 0;
      } else if (flip(rand_engine) < p) {
        adj_matrix[i*n + j] = choose_weight(rand_engine);
        E ++;
      } else {
        adj_matrix[i*n + j] = INT_MAX;
      }
    }
  }
  edge_t *edge_array = new edge_t[E];
  int* starts = new int[n + 1];  // Starting point for each edge
  int* weights = new int[E];
  int ei = 0;
  for (int i = 0; i < n; i++) {
    starts[i] = ei;
    for (int j = 0; j < n; j++) {
      if (adj_matrix[i*n + j] != 0
          && adj_matrix[i*n + j] != INT_MAX) {
        set_edge(&edge_array[ei], i, j);
        weights[ei] = adj_matrix[i*n + j];
        ei++;
      }
    }
  }
  starts[n] = ei; // One extra

  delete[] adj_matrix;

  graph_cuda_t *gr = new graph_cuda_t;
  gr->V = n;
  gr->E = E;
  gr->edge_array = edge_array;
  gr->weights = weights;
  gr->starts = starts;

  return gr;
}

void free_cuda_graph(graph_cuda_t* g) {
  delete[] g->edge_array;
  delete[] g->weights;
  delete[] g->starts;
  delete g;
}

#endif

void free_graph(graph_t* g) {
  delete[] g->edge_array;
  delete[] g->weights;
  delete g;
}

inline bool bellman_ford(graph_t* gr, int* dist, int src) {
  int V = gr->V;
  int E = gr->E;
  Edge* edges = gr->edge_array;
  int* weights = gr->weights;

#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (int i = 0; i < V; i++) {
    dist[i] = INT_MAX;
  }
  dist[src] = 0;

  for (int i = 1; i <= V-1; i++) {
#ifdef _OPENMP
#pragma omp parallel for
#endif
    for (int j = 0; j < E; j++) {
      int u = std::get<0>(edges[j]);
      int v = std::get<1>(edges[j]);
      int new_dist = weights[j] + dist[u];
      if (dist[u] != INT_MAX && new_dist < dist[v])
        dist[v] = new_dist;
    }
  }

  bool no_neg_cycle = true;
#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (int i = 0; i < E; i++) {
    int u = std::get<0>(edges[i]);
    int v = std::get<1>(edges[i]);
    int weight = weights[i];
    if (dist[u] != INT_MAX && dist[u] + weight < dist[v])
      no_neg_cycle = false;
  }
  return no_neg_cycle;
}

void johnson_parallel(graph_t* gr, int* output) {

  int V = gr->V;

  // Make new graph for Bellman-Ford
  // First, a new node q is added to the graph, connected by zero-weight edges
  // to each of the other nodes.
  graph_t* bf_graph = new graph_t;
  bf_graph->V = V + 1;
  bf_graph->E = gr->E + V;
  bf_graph->edge_array = new Edge[bf_graph->E];
  bf_graph->weights = new int[bf_graph->E];

  std::memcpy(bf_graph->edge_array, gr->edge_array, gr->E  * sizeof(Edge));
  std::memcpy(bf_graph->weights, gr->weights, gr->E * sizeof(int));
  std::memset(&bf_graph->weights[gr->E], 0, V * sizeof(int));

#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (int e = 0; e < V; e++) {
    bf_graph->edge_array[e + gr->E] = Edge(V, e);
  }

  // Second, the Bellman–Ford algorithm is used, starting from the new vertex q,
  // to find for each vertex v the minimum weight h(v) of a path from q to v. If
  // this step detects a negative cycle, the algorithm is terminated.
  // TODO Can run parallel version?
  int* h = new int[bf_graph->V];
  bool r = bellman_ford(bf_graph, h, V);
  if (!r) {
    std::cerr << "\nNegative Cycles Detected! Terminating Early\n";
    exit(1);
  }
  // Next the edges of the original graph are reweighted using the values computed
  // by the Bellman–Ford algorithm: an edge from u to v, having length
  // w(u,v), is given the new length w(u,v) + h(u) − h(v).
#ifdef _OPENMP
#pragma omp parallel for
#endif
  for (int e = 0; e < gr->E; e++) {
    int u = std::get<0>(gr->edge_array[e]);
    int v = std::get<1>(gr->edge_array[e]);
    gr->weights[e] = gr->weights[e] + h[u] - h[v];
  }

  Graph G(gr->edge_array, gr->edge_array + gr->E, gr->weights, V);

#ifdef _OPENMP
#pragma omp parallel for schedule(dynamic)
#endif
  for (int s = 0; s < V; s++) {
    std::vector<int> d(num_vertices(G));
    dijkstra_shortest_paths(G, s, distance_map(&d[0]));
    for (int v = 0; v < V; v++) {
      output[s*V + v] = d[v] + h[v] - h[s];
    }
  }

  delete[] h;
  free_graph(bf_graph);
}


// Copyright (c) 2017 Graphcore Ltd. All rights reserved.
#include "Constraint.hpp"

#include "Scheduler.hpp"

#include <popsolver/Model.hpp>

#include <poplibs_support/Visitor.hpp>

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/range/iterator_range.hpp>

#include <limits>

using namespace popsolver;

using BigInteger = boost::multiprecision::uint128_t;
static_assert(
    sizeof(BigInteger) >= sizeof(DataType::UnderlyingType) * 2,
    "BigInteger isn't large enough to perform operations on DataType");

Constraint::~Constraint() = default;

bool Product::propagate(Scheduler &scheduler) {
  const Domains &domains = scheduler.getDomains();
  const auto result = vars[0];
  const auto left = vars[1];
  const auto right = vars[2];

  bool madeChange;
  do {
    madeChange = false;

    // Check if the result doesn't match the inputs
    // left[max] * right[max] = result[max]
    // left[min] * right[min] = result[min]
    const BigInteger maxProduct =
        BigInteger{*domains[left].max()} * BigInteger{*domains[right].max()};
    const BigInteger minProduct =
        BigInteger{*domains[left].min()} * BigInteger{*domains[right].min()};
    if (minProduct > *domains[result].max() ||
        maxProduct < *domains[result].min()) {
      return false;
    }
    if (minProduct > *domains[result].min()) {
      // This is fine to do unchecked because minProduct is less than the
      // value in *domains[result].max() which is limited to DataType. Otherwise
      // We would have already bailed out.
      scheduler.setMin(result,
                       popsolver::DataType{
                           minProduct.convert_to<DataType::UnderlyingType>()});
      madeChange = true;
    }
    if (maxProduct < *domains[result].max()) {
      // This is fine to do unchecked because we are less than the value of
      // *domains[result].max(), which is of type DataType.
      scheduler.setMax(result,
                       popsolver::DataType{
                           maxProduct.convert_to<DataType::UnderlyingType>()});
      madeChange = true;
    }

    // Check if the inputs do not match the result
    // We shortcut some calculations avoiding a costly divide by noting:
    // result[min] <= left[max] * right[min] <= result[max]
    // result[min] <= left[min] * right[max] <= result[max]

    // If we want to reduce the value of left[max] we can check if it
    // is valid by comparing against result[max].
    //
    // If (left[max] * right[min] <= result[max]) is not true,
    // we can reduce the value of left[max] to be result[max] / right[min]. And
    // likewise for all other permutations.

    // left[max] * right[min] <= result[max]
    if (domains[right].min() != popsolver::DataType{0} &&
        BigInteger{*domains[right].min()} * BigInteger{*domains[left].max()} >
            BigInteger{*domains[result].max()}) {
      auto newLeftMax = domains[result].max() / domains[right].min();
      assert(newLeftMax < domains[left].max());
      if (newLeftMax < domains[left].min())
        return false;
      scheduler.setMax(left, newLeftMax);
      madeChange = true;
    }
    // left[min] * right[max] <= result[max]
    if (domains[left].min() != popsolver::DataType{0} &&
        BigInteger{*domains[left].min()} * BigInteger{*domains[right].max()} >
            BigInteger{*domains[result].max()}) {
      auto newRightMax = domains[result].max() / domains[left].min();
      assert(newRightMax < domains[right].max());
      if (newRightMax < domains[right].min())
        return false;
      scheduler.setMax(right, newRightMax);
      madeChange = true;
    }

    // left[min] * right[max] >= result[min]
    if (domains[right].max() != popsolver::DataType{0} &&
        BigInteger{*domains[right].max()} * BigInteger{*domains[left].min()} <
            BigInteger{*domains[result].min()}) {
      auto newLeftMin =
          domains[result].min() / domains[right].max() +
          popsolver::DataType{(domains[result].min() % domains[right].max() !=
                               popsolver::DataType{0})};
      if (newLeftMin > domains[left].min()) {
        if (newLeftMin > domains[left].max())
          return false;
        scheduler.setMin(left, newLeftMin);
        madeChange = true;
      }
    }
    // left[max] * right[min] >= result[min]
    if (domains[left].max() != popsolver::DataType{0} &&
        BigInteger{*domains[left].max()} * BigInteger{*domains[right].min()} <
            BigInteger{*domains[result].min()}) {
      auto newRightMin =
          domains[result].min() / domains[left].max() +
          popsolver::DataType{(domains[result].min() % domains[left].max() !=
                               popsolver::DataType{0})};
      if (newRightMin > domains[right].min()) {
        if (newRightMin > domains[right].max())
          return false;
        scheduler.setMin(right, newRightMin);
        madeChange = true;
      }
    }
  } while (madeChange);
  return true;
}

bool Sum::propagate(Scheduler &scheduler) {
  const Domains &domains = scheduler.getDomains();
  const auto result = vars[0];
  const auto args =
      boost::make_iterator_range(std::begin(vars) + 1, std::end(vars));

  // The data type used to store the max sum must be large enough to store
  // the maximum value of the result plus the maximum value of any operand so
  // that we can compute the max sum of a subset containing all but one variable
  // by subtracting the the maximum value of the variable from the max sum.
  BigInteger minSum = 0;
  BigInteger maxSum = 0;
  for (const auto &v : args) {
    minSum = minSum + *domains[v].min();
    maxSum = maxSum + *domains[v].max();
  }
  if (minSum > *domains[result].max() || maxSum < *domains[result].min()) {
    return false;
  }
  if (minSum > *domains[result].min()) {
    scheduler.setMin(
        result,
        popsolver::DataType{minSum.convert_to<DataType::UnderlyingType>()});
  }
  if (maxSum < *domains[result].max()) {
    scheduler.setMax(
        result,
        popsolver::DataType{maxSum.convert_to<DataType::UnderlyingType>()});
  }
  for (const auto &v : args) {
    auto &domain = domains[v];
    auto minOtherVarsSum = minSum - *domain.min();
    if (minOtherVarsSum > *domains[result].max())
      return false;
    auto newMax = *domains[result].max() - minOtherVarsSum;
    if (newMax < *domain.min())
      return false;
    if (newMax < *domain.max())
      scheduler.setMax(v, popsolver::DataType{
                              newMax.convert_to<DataType::UnderlyingType>()});
    auto maxOtherVarsSum = maxSum - *domain.max();
    if (maxOtherVarsSum < *domains[result].min()) {
      auto newMin = *domains[result].min() - maxOtherVarsSum;
      if (newMin > *domain.max())
        return false;
      if (newMin > *domain.min())
        scheduler.setMin(v, popsolver::DataType{
                                newMin.convert_to<DataType::UnderlyingType>()});
    }
  }
  return true;
}

bool Max::propagate(Scheduler &scheduler) {
  const Domains &domains = scheduler.getDomains();
  const auto result = vars[0];
  const auto args =
      boost::make_iterator_range(std::begin(vars) + 1, std::end(vars));

  // give A = max(B, C), we can deduce:
  //  - upperbound(A) = min(upperbound(A), max(upperbound(B), upperbound(C))),
  //  - lowerbound(A) = max(lowerbound(A), max(lowerbound(B), lowerbound(C))),
  //  - upperbound(B) = min(upperbound(A), upperbound(B)),
  //  - upperbound(C) = min(upperbound(A), upperbound(C))
  // propagation will fail if:
  //  - upperbound(A) < max(lowerbound(B), lowerbound(C)),
  //  - lowerbound(A) > max(upperbound(B), upperbound(C))

  const auto resultLower = domains[result].min();
  const auto resultUpper = domains[result].max();

  auto maxLowerBound = DataType::min();
  auto minLowerBound = DataType::max();
  auto maxUpperBound = DataType::min();
  auto minUpperBound = DataType::max();

  for (const auto &var : args) {
    if (domains[var].min() > resultUpper) {
      return false;
    } else if (domains[var].max() > resultUpper) {
      scheduler.setMax(var, resultUpper);
    }

    maxLowerBound = std::max(maxLowerBound, domains[var].min());
    minLowerBound = std::min(minLowerBound, domains[var].min());
    maxUpperBound = std::max(maxUpperBound, domains[var].max());
    minUpperBound = std::min(minUpperBound, domains[var].max());
  }

  assert(resultUpper >= maxLowerBound);
  if (resultLower > maxUpperBound) {
    return false;
  }

  if (resultUpper > maxUpperBound) {
    scheduler.setMax(result, maxUpperBound);
  }

  if (resultLower < maxLowerBound) {
    scheduler.setMin(result, maxLowerBound);
  }

  return true;
}

bool Min::propagate(Scheduler &scheduler) {
  const Domains &domains = scheduler.getDomains();
  const auto result = vars[0];
  const auto args =
      boost::make_iterator_range(std::begin(vars) + 1, std::end(vars));

  // give A = min(B, C), we can deduce:
  //  - upperbound(A) = min(upperbound(A), min(upperbound(B), upperbound(C))),
  //  - lowerbound(A) = max(lowerbound(A), min(lowerbound(B), lowerbound(C))),
  //  - lowerbound(B) = max(lowerbound(A), lowerbound(B)),
  //  - lowerbound(C) = max(lowerbound(A), lowerbound(C))
  // propagation will fail if:
  //  - upperbound(A) < max(lowerbound(B), lowerbound(C)),
  //  - lowerbound(A) > max(upperbound(B), upperbound(C))

  const auto resultLower = domains[result].min();
  const auto resultUpper = domains[result].max();

  auto maxLowerBound = DataType::min();
  auto minLowerBound = DataType::max();
  auto maxUpperBound = DataType::min();
  auto minUpperBound = DataType::max();

  for (const auto &var : args) {
    if (domains[var].max() < resultLower) {
      return false;
    } else if (domains[var].min() < resultLower) {
      scheduler.setMin(var, resultLower);
    }

    maxLowerBound = std::max(maxLowerBound, domains[var].min());
    minLowerBound = std::min(minLowerBound, domains[var].min());
    maxUpperBound = std::max(maxUpperBound, domains[var].max());
    minUpperBound = std::min(minUpperBound, domains[var].max());
  }

  assert(resultLower <= minUpperBound);
  if (resultUpper < minLowerBound) {
    return false;
  }

  if (resultUpper > minUpperBound) {
    scheduler.setMax(result, minUpperBound);
  }

  if (resultLower < minLowerBound) {
    scheduler.setMin(result, minLowerBound);
  }

  return true;
}

bool Less::propagate(Scheduler &scheduler) {
  const Domains &domains = scheduler.getDomains();
  const auto left = vars[0];
  const auto right = vars[1];

  if (domains[left].min() >= domains[right].max()) {
    return false;
  }
  if (domains[left].min() >= domains[right].min()) {
    scheduler.setMin(right, domains[left].min() + popsolver::DataType{1});
  }
  if (domains[right].max() <= domains[left].max()) {
    scheduler.setMax(left, domains[right].max() - popsolver::DataType{1});
  }
  return true;
}

bool LessOrEqual::propagate(Scheduler &scheduler) {
  const Domains &domains = scheduler.getDomains();
  const auto left = vars[0];
  const auto right = vars[1];

  if (domains[left].min() > domains[right].max()) {
    return false;
  }
  if (domains[left].min() > domains[right].min()) {
    scheduler.setMin(right, domains[left].min());
  }
  if (domains[right].max() < domains[left].max()) {
    scheduler.setMax(left, domains[right].max());
  }
  return true;
}

template <> bool GenericAssignment<DataType>::propagate(Scheduler &scheduler) {
  const Domains &domains = scheduler.getDomains();
  for (std::size_t i = 1; i != vars.size(); ++i) {
    const auto domain = domains[vars[i]];
    if (domain.size() > popsolver::DataType{1}) {
      return true;
    }
    values[i - 1] = domain.val();
  }

  const auto x = f(values);

  if (!x) {
    return false;
  }
  const auto result = vars[0];
  if (x.get() < domains[result].min() || x.get() > domains[result].max()) {
    return false;
  }
  if (domains[result].min() != x.get() || domains[result].max() != x.get()) {
    scheduler.set(result, x.get());
  }
  return true;
}

template <typename T>
bool GenericAssignment<T>::propagate(Scheduler &scheduler) {
  const Domains &domains = scheduler.getDomains();
  for (std::size_t i = 1; i != vars.size(); ++i) {
    // Input operands must be with the range allowed for the data type
    const auto &domain = domains[vars[i]];
    if (domain.min() > popsolver::DataType{std::numeric_limits<T>::max()} ||
        domain.max() < popsolver::DataType{std::numeric_limits<T>::min()}) {
      return false;
    }
    if (domain.min() < popsolver::DataType{std::numeric_limits<T>::min()}) {
      scheduler.setMin(vars[i],
                       popsolver::DataType{std::numeric_limits<T>::min()});
    }
    if (domain.max() > popsolver::DataType{std::numeric_limits<T>::max()}) {
      scheduler.setMax(vars[i],
                       popsolver::DataType{std::numeric_limits<T>::max()});
    }
  }

  for (std::size_t i = 1; i != vars.size(); ++i) {
    const auto domain = domains[vars[i]];
    if (domain.size() > popsolver::DataType{1}) {
      return true;
    }
    values[i - 1] = domain.val();
  }

  std::vector<T> castedValues{};
  castedValues.reserve(values.size());
  for (auto v : values) {
    castedValues.push_back(v.template getAs<T>());
  }

  const auto x = f(castedValues);

  if (!x) {
    return false;
  }
  const auto result = vars[0];
  if (x.get() < domains[result].min() || x.get() > domains[result].max()) {
    return false;
  }
  if (domains[result].min() != x.get() || domains[result].max() != x.get()) {
    scheduler.set(result, x.get());
  }
  return true;
}

template class popsolver::GenericAssignment<DataType>;
template class popsolver::GenericAssignment<unsigned>;
template class popsolver::GenericAssignment<uint64_t>;


// C_error_policy_example.cpp

// Copyright Paul A. Bristow 2007, 2010.
// Copyright John Maddock 2007.

// 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)

// Suppose we want a call to tgamma  to behave in a C-compatible way
// and set global ::errno rather than throw an exception.

#include <cerrno> // for ::errno

#include <boost/math/special_functions/gamma.hpp>
using boost::math::tgamma;

using boost::math::policies::policy;
// Possible errors
using boost::math::policies::overflow_error;
using boost::math::policies::underflow_error;
using boost::math::policies::domain_error;
using boost::math::policies::pole_error;
using boost::math::policies::denorm_error;
using boost::math::policies::evaluation_error;

using boost::math::policies::errno_on_error;
using boost::math::policies::ignore_error;

//using namespace boost::math::policies;
//using namespace boost::math; // avoid potential ambiguity with std:: <random>

// Define a policy:
typedef policy<
      domain_error<errno_on_error>, // 'bad' arguments.
      pole_error<errno_on_error>, // argument is pole value.
      overflow_error<errno_on_error>, // argument value causes overflow.
      evaluation_error<errno_on_error>  // evaluation does not converge and may be inaccurate, or worse,
      // or there is no way  known (yet) to implement this evaluation,
      // for example, kurtosis of non-central beta distribution.
      > C_error_policy;

// std
#include <iostream>
   using std::cout;
   using std::endl;

int main()
{
  // We can achieve this at the function call site
  // with the previously defined policy C_error_policy.
  double t = tgamma(4., C_error_policy());
  cout << "tgamma(4., C_error_policy() = " << t << endl; // 6

  // Alternatively we could use the function make_policy,
  // provided for convenience,
  // and define everything at the call site:
  t = tgamma(4., make_policy(
         domain_error<errno_on_error>(),
         pole_error<errno_on_error>(),
         overflow_error<errno_on_error>(),
         evaluation_error<errno_on_error>()
      ));
  cout << "tgamma(4., make_policy(...) = " << t << endl; // 6

  return 0;
} // int main()

/*

Output

  c_error_policy_example.cpp
  Generating code
  Finished generating code
  c_error_policy_example.vcxproj -> J:\Cpp\MathToolkit\test\Math_test\Release\c_error_policy_example.exe
  tgamma(4., C_error_policy() = 6
  tgamma(4., make_policy(...) = 6
  tgamma(4., C_error_policy() = 6
  tgamma(4., make_policy(...) = 6

*/


#include <bits/types/FILE.h>
#include <fstream>
#include <iostream>
#include <stdio.h>
#include <vector>
#include <string>
#include <tgmath.h>
#include "image_ppm.h"
#include <filesystem>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

typedef Matrix<u_char, Dynamic, Dynamic> MatrixImg;
typedef Matrix<double, Dynamic, Dynamic> TempMatrixImg;
typedef Vector<u_char,Dynamic> ImgLine;
typedef Vector<double,Dynamic> TempImgLine;
//typedef Matrix<ImgLine,Dynamic,Dynamic> FlattenedImages;

unsigned char max(u_char a, u_char b){
    if (a<b) return b;
    else return a;
}

unsigned char min(u_char a, u_char b){
    if (a>b) return b;
    else return a;
}

double max(double a, double b){
    if (a<b) return b;
    else return a;
}

double min(double a, double b){
    if (a>b) return b;
    else return a;
}

int max(int a, int b){
    if (a<b) return b;
    else return a;
}

int min(int a, int b){
    if (a>b) return b;
    else return a;
}

vector<double> projectOnEigenSpace(vector<TempImgLine> eigenfaces, TempImgLine imToProj,int K){
    vector<double> res = vector<double>();
    for (int i=0;i<K;i++){
        res.push_back(eigenfaces[i].dot(imToProj));
    }
    return res;
}

ImgLine octToVec(OCTET* im, int nH, int nW){
    ImgLine res(nH*nW);
    for (int i=0; i<nH*nW;i++){
        res(i)=im[i];
    }
    return res;
}

double eigenProjsDistance(vector<double> proj1, vector<double> proj2){

    double res =0.0;
    for (int i=0; i<min(proj1.size(),proj2.size());i++){
        res+=(proj1[i]-proj2[i])*(proj1[i]-proj2[i]);
    }
    return res;
}

int main(int argc, char* argv[]){

    //recup eigenfaces

    vector<TempImgLine> eigenfaces;
    int K =200; int nH;int nW;

    for (int i=0; i<K;i++){
        OCTET* im;
        char name[50];
        sprintf(name,"out/eigenfaces/im%d.pgm",i);
        lire_nb_lignes_colonnes_image_pgm(name,&nH,&nW);
        allocation_tableau(im,OCTET,nH*nW);
        lire_image_pgm(name,im,nH*nW);
        TempImgLine eigenFace(nH*nW) ;
        double sum=0.0;
        for (int j=0;j<nH*nW;j++){
            eigenFace(j)=im[j]-127;
            sum+=(double)(im[j]-127)*(double)(im[j]-127);
        }
        for (int j=0;j<nH*nW;j++){
            eigenFace(j)=eigenFace(j)/sqrt(sum);
            }
        eigenfaces.push_back(eigenFace);
        free(im);
    }

    string dirIn = string(argv[1]);

    //vector<vector<int*>> histos = vector<vector<int*>>();

    vector<vector<vector<double>>> registre = vector<vector<vector<double>>>();

    int countDirs =0;
    for (auto & dir : std::filesystem::directory_iterator(dirIn)){
        registre.push_back(vector<vector<double>>());
        int countFile=0;
        for (auto & file : std::filesystem::directory_iterator(dir.path())){
            OCTET* img;
            lire_nb_lignes_colonnes_image_pgm(file.path().c_str(),&nH,&nW);
            TempImgLine imgLine(nH*nW);
            allocation_tableau(img,OCTET, nH*nW);
            lire_image_pgm(file.path().c_str(),img,nH*nW);
            double sumIm=0.0;
            for (int i=0;i<nH*nW;i++){sumIm+=(double)(img[i]-127)*(double)(img[i]-127);}
            for (int i=0; i<nH*nW;i++){imgLine(i)=(double)(img[i]-127)/sumIm;}
            registre[countDirs].push_back(projectOnEigenSpace(eigenfaces,imgLine,K));

            countFile++;
        }
        countDirs++;
    }

    for (double threshold=0.0;threshold<0.0000005;threshold+=0.00000001){
        int VP=0,VN=0,FP=0,FN=0;

        for (int i=0;i<registre.size();i++){
            for (int j=0; j<registre[i].size();j++){
                for (int ip=0;ip<registre.size();ip++){
                    for (int jp=0;jp<registre[ip].size();jp++){
                        if (!(i==ip && j==jp)){
                            //cout<<eigenProjsDistance(registre[i][j],registre[ip][jp])<<endl;
                            if (eigenProjsDistance(registre[i][j],registre[ip][jp])<threshold){
                                if (i==ip){
                                    VP++;
                                }
                                else{
                                    FP++;
                                }
                            }

                        else{
                                if (i==ip){
                                    FN++;
                                }
                                else{
                                    VN++;
                                }
                            }

                        }
                    }
                }
            }
        }

        double sensitivite = (double)((double)VP/(double)(VP+FN));
        double antispecificite = 1.0-(double)((double)VN/(double)(VN+FP));
        double distance = antispecificite*antispecificite + (1-sensitivite)*(1-sensitivite);
        cout<<antispecificite<<" "<<sensitivite<<" "<<distance<<" "<<threshold<<endl;
    }

}