#include "diffuclass.h"
#include <iostream>
#include <math.h>
#include <boost/filesystem.hpp>

//------------------------------------------------------------------------------
//                           Declare functions
//------------------------------------------------------------------------------

extern PetscErrorCode ComputeMatrix(KSP, Mat, Mat, void*);

PetscErrorCode MyMatMult(Mat,Vec,Vec);

//------------------------------------------------------------------------------
//							  Public Methods
//------------------------------------------------------------------------------
//TODO: include all boundary condition elements and individually parallelized grids
void diffusionPETSc::initDiffusion(eQ::diffusionSolver::params &initParams)
//void diffusionPETSc::initDiffusion(MPI_Comm comm, std::vector<std::string> filePaths, int argc, char* argv[])
{
    auto argc = initParams.argc;
    auto argv = initParams.argv;
    int 	 i, j;

    /*
    //Split input communicator into subcommunicators for each desired diffusion grid
    MPI_Comm subComm;
    int 	 myRankMPI, commSize, i, j;

    MPI_Comm_size(comm, &commSize);
    MPI_Comm_rank(comm, &myRankMPI);

    MPI_Comm_split(comm, myRankMPI, 0, &subComm);

    initData.subCommunicator = subComm;
    //Record vector of diffusion constants and choose the appropriate constant for each processor
    std::vector<double>		d_vector;
    d_vector = std::vector<double>(eQ::data::parameters["D_HSL"].get<std::vector<double>>());
    initData.diffusionConstant = d_vector.at(myRankMPI % d_vector.size());

    //Set domain size based on parameters
    initData.xLengthMicrons = int(eQ::data::parameters["simulationTrapWidthMicrons"]);
    initData.yLengthMicrons = int(eQ::data::parameters["simulationTrapHeightMicrons"]);

    //Set time step
    initData.dt = eQ::data::parameters["dt"];

    //Set grid spacing in domain
    initData.h = 1 / double(eQ::data::parameters["nodesPerMicronSignaling"]);

    //Set the appropriate filepath to each grid for data recording
    initData.directoryName = filePaths.at(myRankMPI % d_vector.size());
*/
    initData.subCommunicator = initParams.comm;
    initData.diffusionConstant = initParams.D_HSL;//direct read from init params
    initData.xLengthMicrons = initParams.trapWidthMicrons;
    initData.yLengthMicrons = initParams.trapHeightMicrons;
    initData.dt = initParams.dt;
    initData.h = 1.0 / initParams.nodesPerMicron;

    initData.directoryName = initParams.filePath + "petsc";
     boost::filesystem::path dstFolder = initData.directoryName;
     boost::filesystem::create_directory(dstFolder);

     initData.objectName = "grid";

    //Set initial boundary conditions
    if("DIRICHLET_0" == eQ::data::parameters["boundaryType"]){
        initData.homogeneousDirichlet = PETSC_TRUE;

        initData.topDirichletCoefficient = 1;
        initData.bottomDirichletCoefficient = 1;
        initData.leftDirichletCoefficient = 1;
        initData.rightDirichletCoefficient = 1;

        initData.topNeumannCoefficient = 0;
        initData.bottomNeumannCoefficient = 0;
        initData.leftNeumannCoefficient = 0;
        initData.rightNeumannCoefficient = 0;

        initData.topBoundaryValue = 0;
        initData.bottomBoundaryValue = 0;
        initData.leftBoundaryValue = 0;
        initData.rightBoundaryValue = 0;
    }

    //Initialize PETSc element of diffusion
    InitializeDiffusion(&initData, argc, argv);

    //Resize and populate vectors for data transfer
    solution_vector.resize(gridNodesX * gridNodesY);
    allXCoordinates.resize(gridNodesX * gridNodesY);
    allYCoordinates.resize(gridNodesX * gridNodesY);

    for (j=0; j < gridNodesY; j++){
        for (i=0; i < gridNodesX; i++){
            allXCoordinates.at(i + j * gridNodesX) = i*initData.h;
            allYCoordinates.at(i + j * gridNodesX) = j*initData.h;
            solution_vector.at(i + j * gridNodesX) = 0;
        }
    }

    //Copy initial conditions to solution vector
    ReadGridValues(allXCoordinates, allYCoordinates, &solution_vector);
}

void diffusionPETSc::stepDiffusion(void)
{
    //Copy modified solution vector to u0
    //solution_vector is our public vector of all solution data - copy that over.

    WriteGridValues(allXCoordinates, allYCoordinates, solution_vector);

    TimeStep();

    ReadGridValues(allXCoordinates, allYCoordinates, &solution_vector);
}

//TODO: implement each wall separately
void diffusionPETSc::setBoundaryValues(const eQ::data::parametersType &bvals)
{
    //JW: over-ride for now: set boundaries explicitly by writing the data structures
    return;

    if(bool(bvals["allBoundaries"]) == true){
        gridData->topBoundaryValue = double(bvals["boundaryValue"]);
        gridData->bottomBoundaryValue = double(bvals["boundaryValue"]);
        gridData->leftBoundaryValue = double(bvals["boundaryValue"]);
        gridData->rightBoundaryValue = double(bvals["boundaryValue"]);
    }
}

//TODO: verify accuracy, check about choosing walls
eQ::data::parametersType diffusionPETSc::getBoundaryFlux(void)
{
    //Compute and return boundary flux across all walls
    eQ::data::parametersType 	fluxData;
    double 			   	totalBoundaryFlux, boundarySlope;
    int 				i, j;

    for (i = 0; i < gridNodesX; i++){
        for (j = 0; j < gridNodesY; j++){
            if (i == 0){
                boundarySlope = boundarySlope + (solution_vector.at(2 + (gridNodesX) * j) - \
                        solution_vector.at((gridNodesX)*j))/(2*gridData->h);
            }
            else if (i == gridNodesX - 1){
                boundarySlope = boundarySlope + (solution_vector.at(i - 2 + (gridNodesX-1)*j) - \
                        solution_vector.at(i + (gridNodesX-1)*j))/(2*gridData->h);
            }
            else if (j == 0){
                boundarySlope = boundarySlope + (solution_vector.at(i + (gridNodesX-1) * 2) - \
                        solution_vector.at(i))/(2*gridData->h);
            }
            else if (j == gridNodesY - 1){
                boundarySlope = boundarySlope + (solution_vector.at(i + (gridNodesX-1) * (j - 2)) - \
                        solution_vector.at(i + (gridNodesX-1)*j))/(2*gridData->h);
            }
        }
    }

    totalBoundaryFlux = gridData->diffusionConstant * gridData->dt * boundarySlope;
    fluxData["totalFlux"] = totalBoundaryFlux;
    return fluxData;
}

void diffusionPETSc::writeDiffusionFiles(double timestamp)
{
    //Work in timestamp
    RecordData();
}

/*
void diffusionPETSc::writeDataFiles(double dt)
{
    //Need to write full function
}
*/

void diffusionPETSc::finalize(void)
{
    DiffusionFinalize(PETSC_TRUE);
}

//------------------------------------------------------------------------------
//                            Private Methods
//------------------------------------------------------------------------------

PetscErrorCode diffusionPETSc::ApplyBoundaryConditions()
{
  //Applys Neumann/Robin Boundary Conditions to RHS vector
  PetscInt       i,j,xm,ym,xs,ys;
  PetscScalar    **RHSarray;

  //Get subgrid information for each processor and get arrays from vector
  ierr = DMDAGetCorners(distributedArray,&xs,&ys,0,&xm,&ym,0);CHKERRQ(ierr);
  ierr = DMDAVecGetArray(distributedArray, globalVector, &RHSarray);CHKERRQ(ierr);

  //JW NOTE:  each node can check its range whether it has boundary points
  //  then, traverse the boundary explicitly, rather than traversing the entire i,j space
    const double twoFh = 2 * gridData->fourierNumber * gridData->h;//
//    double topD = gridData->topDirichletCoefficient;
//    double bottomD = gridData->bottomDirichletCoefficient;
//    double leftD = gridData->leftDirichletCoefficient;
//    double rightD = gridData->rightDirichletCoefficient;
        const double topN = gridData->topNeumannCoefficient;
        const double bottomN = gridData->bottomNeumannCoefficient;
        const double leftN = gridData->leftNeumannCoefficient;
        const double rightN = gridData->rightNeumannCoefficient;

  //Alter values in arrays to match Boundary Conditions
  for (j = ys; j < ys+ym; j++){
    for (i = xs; i < xs+xm; i++){
//      if (i == 0 || i == gridNodesX-1 || j == 0 || j == gridNodesY-1){

          //TOP WALL
          if(j == gridNodesY-1)
          {
              const double topBV = topBoundaryValue[i];
              //Set non-Dirichlet for top wall
              if (topN != 0)
              {//if TL (TR) corner, only set to N if left (right) wall is N
                  if ((i != 0 || leftN != 0) && (i != gridNodesX-1 || rightN != 0))
                        RHSarray[j][i] += (twoFh * topBV) / topN;
              }
              else
//                  RHSarray[j][i] = topBV / topD;
                  RHSarray[j][i] = topBV;//if N==0, then D must be 1
          }
          //BOTTOM WALL
          else if(j == 0)
          {
              const double bottomBV = bottomBoundaryValue[i];
              //Set non-Dirichlet for bottom wall
              if (bottomN != 0)
              {
                  if ((i != 0 || leftN != 0) && (i != gridNodesX-1 || rightN != 0))
                        RHSarray[j][i] +=  (twoFh * bottomBV) / bottomN;
              }
              else
//                  RHSarray[j][i] = bottomBV / bottomD;
                  RHSarray[j][i] = bottomBV;//if N==0, then D must be 1
          }
          //RIGHT WALL
          if(i == gridNodesX-1)
          {
              const double rightBV = gridData->rightBoundaryValue;
              //Set non-Dirichlet for right wall
              if (rightN != 0)
              {
                  if ((j != 0 || bottomN != 0) && (j != gridNodesY-1 || topN != 0))
                      RHSarray[j][i] += (twoFh * rightBV) / rightN;
              }
              else
//                  RHSarray[j][i] = rightBV / rightD;
                  RHSarray[j][i] = rightBV;//if N==0, then D must be 1
          }
          //LEFT WALL
          else if(i == 0)
          {
              const double leftBV = gridData->leftBoundaryValue;
              //Set non-Dirichlet for left wall
              if (leftN != 0)
              {
                  if ((j != 0 || bottomN != 0) && (j != gridNodesY-1 || topN != 0))
                      RHSarray[j][i] += (twoFh * leftBV) / leftN;
              }
              else
//                  RHSarray[j][i] = leftBV / leftD;
                  RHSarray[j][i] = leftBV;//if N==0, then D must be 1
          }
//      }
    }
  }
/*
  //Alter values in arrays to match Boundary Conditions
  for (j = ys; j < ys+ym; j++){
    for (i = xs; i < xs+xm; i++){
      if (i == 0 || i == gridNodesX-1 || j == 0 || j == gridNodesY-1){
        //Set non-Dirichlet for top wall
        if (gridData->topNeumannCoefficient != 0 && j == gridNodesY-1){
          //Only change corners if they are also Neumann Boundary Conditions
          if ((i != 0 || gridData->leftNeumannCoefficient != 0) &&
              (i != gridNodesX-1 || gridData->rightNeumannCoefficient != 0)){
                RHSarray[j][i] = RHSarray[j][i] + (2 * gridData->fourierNumber * gridData->h * \
                  gridData->topBoundaryValue) / gridData->topNeumannCoefficient;
          }
        }
        //Set non-Dirichlet for bottom wall
        if (gridData->bottomNeumannCoefficient != 0 && j == 0){
          //Only change corners if they are also Neumann Boundary Conditions
          if ((i != 0 || gridData->leftNeumannCoefficient != 0) &&
              (i != gridNodesX-1 || gridData->rightNeumannCoefficient != 0)){
            RHSarray[j][i] = RHSarray[j][i] + (2 * gridData->fourierNumber * gridData->h * \
              gridData->bottomBoundaryValue) / gridData->bottomNeumannCoefficient;
          }
        }
        //Set non-Dirichlet for left wall
        if (gridData->leftNeumannCoefficient != 0 && i == 0){
          //Only change corners if they are also Neumann Boundary Conditions
          if ((j != 0 || gridData->bottomNeumannCoefficient != 0) &&
              (j != gridNodesY-1 || gridData->topNeumannCoefficient != 0)){
            RHSarray[j][i] = RHSarray[j][i] + (2 * gridData->fourierNumber * gridData->h * \
              gridData->leftBoundaryValue) / gridData->leftNeumannCoefficient;
          }
        }
        //Set non-Dirichlet for right wall
        if (gridData->rightNeumannCoefficient != 0 && i == gridNodesX-1){
          //Only change corners if they are also Neumann Boundary Conditions
          if ((j != 0 || gridData->bottomNeumannCoefficient != 0) &&
              (j != gridNodesY-1 || gridData->topNeumannCoefficient != 0)){
            RHSarray[j][i] = RHSarray[j][i] + (2 * gridData->fourierNumber * gridData->h * \
              gridData->rightBoundaryValue) / gridData->rightNeumannCoefficient;
          }
        }

        //Otherwise, set Dirichlet conditions
        if (gridData->topNeumannCoefficient == 0 && j == gridNodesY-1){
          RHSarray[j][i] = gridData->topBoundaryValue / gridData->topDirichletCoefficient;
        }
        else if (gridData->bottomNeumannCoefficient == 0 && j == 0){
          RHSarray[j][i] = gridData->bottomBoundaryValue / gridData->bottomDirichletCoefficient;
        }
        else if (gridData->leftNeumannCoefficient == 0 && i == 0){
          RHSarray[j][i] = gridData->leftBoundaryValue / gridData->leftDirichletCoefficient;
        }
        else if (gridData->rightNeumannCoefficient == 0 && i == gridNodesX-1){
          RHSarray[j][i] = gridData->rightBoundaryValue / gridData->rightDirichletCoefficient;
        }
      }
    }
  }
*/
  ierr = DMDAVecRestoreArray(distributedArray, globalVector, &RHSarray);CHKERRQ(ierr);

  return ierr;
}

//Figure out argc and argv
PetscErrorCode diffusionPETSc::InitializeDiffusion (DiffusionData *dataStruct, int argc, char **argv)
{
  //Initializes sets data values, inializes PETSc, and otherwise takes care of
  //all preparation needed in order to iterate. It should be noted that this
  //function does NOT set any initial conditions.

  gridData = dataStruct;
  //Initialize PETSc and assign passed communicator to DIFFU_COMM
  DIFFU_COMM = gridData->subCommunicator;
  ierr = PetscInitialize(&argc,&argv,(char*)0,NULL);
  MPI_Comm_size(DIFFU_COMM, &commSize);

  //Set up viewer for print statements
  printViewer = PETSC_VIEWER_STDOUT_(DIFFU_COMM);

  //Calculate crucial values for grid
  gridNodesX = (gridData->xLengthMicrons/gridData->h) + 1;
  gridNodesY = (gridData->yLengthMicrons/gridData->h) + 1;
  gridData->totalNodes = gridNodesX * gridNodesY;
  gridData->fourierNumber = (gridData->diffusionConstant*gridData->dt) / (gridData->h*gridData->h);

  //Set boundary and stencil types for distributed array
  boundary = DM_BOUNDARY_NONE;
  stencilType = DMDA_STENCIL_STAR;

  //Create Krylov Subspace method(KSP) context and Distributed Array(DMDA)
  ierr = KSPCreate(DIFFU_COMM, &krylovSolver);CHKERRQ(ierr);
  ierr = DMDACreate2d(DIFFU_COMM, boundary, boundary, stencilType, gridNodesX, gridNodesY, PETSC_DECIDE, \
    PETSC_DECIDE, 1, 1, NULL, NULL, &distributedArray);CHKERRQ(ierr);

  //Configure DMDA
  ierr = DMSetFromOptions(distributedArray);CHKERRQ(ierr);
  ierr = DMSetUp(distributedArray);CHKERRQ(ierr);
  ierr = DMDASetUniformCoordinates(distributedArray, 0, gridData->xLengthMicrons, 0, \
    gridData->yLengthMicrons, 0, 0);CHKERRQ(ierr);
  ierr = DMView(distributedArray, printViewer);CHKERRQ(ierr);

  //Create solution vector and local helper vector for scattering
  ierr = DMCreateGlobalVector(distributedArray, &globalVector);CHKERRQ(ierr);
  ierr = DMCreateLocalVector(distributedArray, &gridData->localVector);CHKERRQ(ierr);

  //Obtain additional needed values for data structure
  ierr = VecGetOwnershipRange(globalVector, &gridData->localMinimum, &gridData->localMaximum);CHKERRQ(ierr);

  //Configure KSP
  ierr = KSPSetDM(krylovSolver, distributedArray);CHKERRQ(ierr);
  ierr = KSPSetComputeOperators(krylovSolver, ComputeMatrix, gridData);CHKERRQ(ierr);
  ierr = KSPGetPC(krylovSolver, &preconditioner);CHKERRQ(ierr);
  ierr = PCSetType(preconditioner, PCNONE);CHKERRQ(ierr);
  ierr = KSPSetType(krylovSolver, KSPFBCGSR);
  ierr = KSPSetFromOptions(krylovSolver);CHKERRQ(ierr);
  ierr = KSPSetUp(krylovSolver);CHKERRQ(ierr);

  //Prepare Application Ordering (AO)
  ierr = DMDAGetAO(distributedArray, &appOrder);CHKERRQ(ierr);

  ierr = PetscPrintf(DIFFU_COMM, "\nPETSC CLASS InitializeDiffusion(): %s \n", gridData->directoryName.c_str());CHKERRQ(ierr);

  //Set initial timestep number
  step = 0;
  return ierr;
}

PetscErrorCode diffusionPETSc::TimeStep()
{
  //Takes a step forward in time in the simulation
  step++;
  ierr = ApplyBoundaryConditions();CHKERRQ(ierr);
  ierr = KSPSolve(krylovSolver, globalVector, globalVector);CHKERRQ(ierr);
  ierr = PetscPrintf(DIFFU_COMM,"\r%d steps complete", step);CHKERRQ(ierr);
  return ierr;
}

//Ensure vector references work as desired
PetscErrorCode diffusionPETSc::WriteGridValues(vector<double> xCoordinates, vector<double> \
  yCoordinates, vector<double> values)
{
  //Takes values and their coordinates in vector form, converts coordinates to
  //a discretized form, and then inserts the values into the grid based on these
  //coordinates.

  //Declare temporary variables for use in method
  PetscInt      xm, ym, xs, ys, xindex, yindex;
  unsigned int  size, i;
  PetscScalar   **globalarray;

  //Record size variable and check for size consistency across vectors
  size = xCoordinates.size();
  if (size != yCoordinates.size() || size != values.size()){
    ierr = PetscPrintf(PETSC_COMM_SELF, \
        "Error: vector sizes for WriteGridValues must match\n");CHKERRQ(ierr);
  }

  //Get coordinate range and array for all data on processor
  ierr = DMDAGetCorners(distributedArray,&xs,&ys,0,&xm,&ym,0);CHKERRQ(ierr);
  ierr = DMDAVecGetArray(distributedArray, globalVector, &globalarray);CHKERRQ(ierr);

  //Iterate through all coordinates, filter out those not on processor, and
  //insert values into grid
  for (i=0; i<size; i++){
    xindex = round(xCoordinates.at(i)/gridData->h);
    yindex = round(yCoordinates.at(i)/gridData->h);
    if (xindex >= xs && xindex < xs+xm && yindex >= ys && yindex < ys+ym){
        globalarray[yindex][xindex] = values.at(i);
    }
  }

  //Restore array to vector, finalizing the insert of these new values.
  ierr = DMDAVecRestoreArray(distributedArray, globalVector, &globalarray);CHKERRQ(ierr);

  return ierr;
}

//TODO: Ensure vector references work as desired
//Extensively test commSize > 1 case - need MPI to properly share data?
PetscErrorCode diffusionPETSc::ReadGridValues(vector<double> xCoordinates, vector<double> \
  yCoordinates, vector<double> *values)
{
  //Given some coordinates and a vector for values, discretizes the coordinates
  //and finds the values associated with them in the grid. Puts these values
  //into the given vector. Note that this only does so for on-process values.

  //Record size variable and check for size consistency across vectors
  unsigned int  readSize;
  readSize = xCoordinates.size();
  if (readSize != yCoordinates.size() || readSize != values->size()){
    ierr = PetscPrintf(PETSC_COMM_SELF, \
    "Error: vector sizes for ReadGridValues must match\n");CHKERRQ(ierr);
    return ierr;
  }

  //Read values from grid - use alternative algorithm if grid is parallelized
  if (commSize == 1){
    //Declare temporary variables for use in method
    PetscInt            xm, ym, xs, ys, xindex, yindex;
    unsigned int        i;
    PetscScalar         **readarray;

    //Get coordinate range and array for all data on processor
    ierr = DMDAGetCorners(distributedArray,&xs,&ys,0,&xm,&ym,0);CHKERRQ(ierr);
    ierr = DMDAVecGetArrayRead(distributedArray, globalVector, &readarray);CHKERRQ(ierr);

    //Get list of natural grid coordinates
    for (i=0; i < readSize; i++){
        xindex = round(xCoordinates.at(i)/gridData->h);
        yindex = round(yCoordinates.at(i)/gridData->h);
        if (xindex >= xs && xindex < xs+xm && yindex >= ys && yindex < ys+ym){
            values->at(i) = readarray[yindex][xindex];
        }
    }

    //Restore read array to vector
    ierr = DMDAVecRestoreArrayRead(distributedArray, globalVector, &readarray);CHKERRQ(ierr);
  }

  else{
    //Declare temporary variables for use in method
    PetscInt 		   	xindex, yindex, globalIndex[readSize],
                        localIndex[readSize];
    unsigned int		i;
    const PetscScalar	*readarray;
    IS					from, to;
    Vec					sequentialVector;
    VecScatter			scatter;

    //Create index vector for later PETSc vector scattering
    for (i=0; i < readSize; i++){
        localIndex[i] = i;
    }

    //Get list of natural grid coordinates
    for (i=0; i < readSize; i++){
        xindex = round(xCoordinates.at(i)/gridData->h);
        yindex = round(yCoordinates.at(i)/gridData->h);
        globalIndex[i] = yindex*gridNodesX + xindex;
    }

    //Replace natural grid coordinates with PETSc grid coordinates
    ierr = AOApplicationToPetsc(appOrder, readSize, globalIndex);CHKERRQ(ierr);

    //Prepares index sets for PETSc vector scattering
    ierr = ISCreateGeneral(DIFFU_COMM, readSize, globalIndex, PETSC_COPY_VALUES, &from);
        CHKERRQ(ierr);
    ierr = ISCreateGeneral(PETSC_COMM_SELF, readSize, localIndex, PETSC_COPY_VALUES, &to);
        CHKERRQ(ierr);

    //Create PETSc vector to be scattered to
    ierr = VecCreateSeq(PETSC_COMM_SELF, readSize, &sequentialVector);CHKERRQ(ierr);

    //Create PETSc vector scatter and execute it
    ierr = VecScatterCreate(globalVector, from, sequentialVector, to, &scatter);CHKERRQ(ierr);
    ierr = VecScatterBegin(scatter, globalVector, sequentialVector, INSERT_VALUES, SCATTER_FORWARD);
        CHKERRQ(ierr);
    ierr = VecScatterEnd(scatter, globalVector, sequentialVector, INSERT_VALUES, SCATTER_FORWARD);
        CHKERRQ(ierr);

    //Obtain data array from scattered PETSc vector
    ierr = VecGetArrayRead(sequentialVector, &readarray);CHKERRQ(ierr);

    //Copy data array into vector (non-PETSc vector)
    for (i=0; i < readSize; i++){
        values->at(i) = readarray[i];
    }

    //Restore array to vector
    ierr = VecRestoreArrayRead(sequentialVector, &readarray);CHKERRQ(ierr);

    //Destroy unnecessary objects
    ierr = ISDestroy(&from);CHKERRQ(ierr);
    ierr = ISDestroy(&to);CHKERRQ(ierr);
    ierr = VecScatterDestroy(&scatter);CHKERRQ(ierr);
    ierr = VecDestroy(&sequentialVector);CHKERRQ(ierr);
  }

  return ierr;
}

PetscErrorCode diffusionPETSc::RecordData()
{
  //Writes grid data into a .vtr file to be read by ParaView

  //Declare temporary variables for use in method
  char filename[100];
  PetscViewer vtrviewer;

  //Get name of file to be written
  sprintf(filename, "%s/%s%04d.vtr", gridData->directoryName.c_str(), \
          gridData->objectName.c_str(), step);

    //    sprintf(filename, "%s/%s%d.vtr", gridData->directoryName.c_str(),

  //Open viewer and write data to file
  ierr = PetscViewerVTKOpen(DIFFU_COMM, filename, FILE_MODE_WRITE, \
    &vtrviewer);CHKERRQ(ierr);
  ierr = PetscViewerPushFormat(vtrviewer, PETSC_VIEWER_VTK_VTR);CHKERRQ(ierr);
  ierr = VecView(globalVector, vtrviewer);CHKERRQ(ierr);

  //Destroy viewer and return ierr
  ierr = PetscViewerDestroy(&vtrviewer);CHKERRQ(ierr);
  return ierr;
}

PetscErrorCode diffusionPETSc::DiffusionFinalize(PetscBool last)
{
    //Destroys all remaining objects in class and finalizes PETSc if "last" is set
    //to true.

    //Destroy objects created for an instance of this class
    ierr = DMDestroy(&distributedArray);CHKERRQ(ierr);
    ierr = KSPDestroy(&krylovSolver);CHKERRQ(ierr);
    ierr = VecDestroy(&globalVector);CHKERRQ(ierr);
    ierr = VecDestroy(&gridData->localVector);CHKERRQ(ierr);

    ierr = PetscPrintf(DIFFU_COMM, "\nDone\n");CHKERRQ(ierr);

    //If this is the last instance of this class to be finished, finalize PETSc.
    if (last){
        ierr = PetscFinalize();
    }

    return ierr;
}

//------------------------------------------------------------------------------
//                    Helper functions (external to class)
//------------------------------------------------------------------------------

PetscErrorCode ComputeMatrix(KSP krylovSolver, Mat A, Mat jac, void *user){
    //Establishes shell matrix for KSP operation

    //Initialize function---------------------------------------------------------
    //Declare variables
    PetscErrorCode ierr;
    DiffusionData  *gridData = (DiffusionData*)user;

    //Begin function
    PetscFunctionBegin;

    //Create matrices-------------------------------------------------------------
    //Form matrix for KSP process
    ierr = MatSetSizes(A, gridData->localMaximum-gridData->localMinimum, gridData->localMaximum-\
        gridData->localMinimum, gridData->totalNodes, gridData->totalNodes);CHKERRQ(ierr);
    ierr = MatSetType(A,MATSHELL);CHKERRQ(ierr);
    ierr = MatShellSetContext(A,gridData);CHKERRQ(ierr);
    ierr = MatSetUp(A);CHKERRQ(ierr);

    //Set multiplication function for KSP process matrix
    ierr = MatShellSetOperation(A,MATOP_MULT,(void(*)(void))MyMatMult);
        CHKERRQ(ierr);

    //Return error code-----------------------------------------------------------
    PetscFunctionReturn(ierr);
}

PetscErrorCode MyMatMult(Mat A, Vec X, Vec Y){
    //Defines the multiplication function for shell matrix

    //Initialize function---------------------------------------------------------
    //Declare variables
    PetscErrorCode ierr;
    void           *ptr;
    DiffusionData  *user;
    PetscScalar    **yarray, **xarray;
    DM             distributedArray;
    PetscInt       i,j,xm,ym,xs,ys,gxm,gym,gxs,gys;

    //Begin function
    PetscFunctionBegin;

    //Retrieve context and DM
    ierr = MatShellGetContext(A,&ptr);CHKERRQ(ierr);
    ierr = VecGetDM(X,&distributedArray);CHKERRQ(ierr);
    user = (DiffusionData*)ptr;

    //Prepare for computation-----------------------------------------------------
    //Begin scattering
    ierr = DMGlobalToLocalBegin(distributedArray, X, INSERT_VALUES, user->localVector);CHKERRQ(ierr);

    //Get local domain in grid
    ierr = DMDAGetCorners(distributedArray,&xs,&ys,0,&xm,&ym,0);CHKERRQ(ierr);
    ierr = DMDAGetGhostCorners(distributedArray,&gxs,&gys,0,&gxm,&gym,0);CHKERRQ(ierr);

    //Get local array for product vector
    ierr = DMDAVecGetArray(distributedArray, Y, &yarray);CHKERRQ(ierr);

    //Finish scattering and get local array from scattered vector
    ierr = DMGlobalToLocalEnd(distributedArray, X, INSERT_VALUES, user->localVector);CHKERRQ(ierr);
    ierr = DMDAVecGetArrayRead(distributedArray, user->localVector, &xarray);CHKERRQ(ierr);

    //Compute product-------------------------------------------------------------
/*
    const double h = user->h;
    const double F = user->fourierNumber;
    const double twoF = 2*user->fourierNumber;
    const double topD = user->topDirichletCoefficient;
    const double bottomD = user->bottomDirichletCoefficient;
    const double leftD = user->leftDirichletCoefficient;
    const double rightD = user->rightDirichletCoefficient;
        const double topN = user->topNeumannCoefficient;
        const double bottomN = user->bottomNeumannCoefficient;
        const double leftN = user->leftNeumannCoefficient;
        const double rightN = user->rightNeumannCoefficient;

    for (j=ys; j<ys+ym; j++) {
        for (i=xs; i<xs+xm; i++) {
            //Sets value for edge points
//            if (i == gxs || j == gys || i == gxs+gxm-1 || j == gys+gym-1){

            //TOP WALL
            if(j == gys+gym-1)
            {
                if (topN != 0)
                {
                    //Evaluate left corner when left wall is Neumann/Robin
                    if (i == gxs && leftN != 0)
                    {
                        yarray[j][i] =
                                -twoF * (xarray[j][i+1] + xarray[j-1][i])
                            + (1 + (2 + (h * topD/topN) + (h * leftD/leftN)) * twoF) * xarray[j][i];
                    }
                    //Evaluate right corner when right wall is Neumann/Robin
                    else if (i == gxs+gxm-1 && rightN != 0)
                    {
                        yarray[j][i] =
                                -twoF * (xarray[j][i-1] + xarray[j-1][i])
                            + (1 + (2 + (h * topD/topN) + (h * rightD/rightN)) * twoF) * xarray[j][i];
                    }
                    //Evaluate all other edge points on top wall
                    else{
                        yarray[j][i] =
                                -F * (2*xarray[j-1][i] + xarray[j][i-1] + xarray[j][i+1])
                                + (1 + (2 + h*topD/topN) * twoF) * xarray[j][i];
                    }
                }
                //Evaluate wall in case of Dirichlet condition
                else
                    yarray[j][i] = xarray[j][i];
            }
            //BOTTOM WALL
            else if(j == gys)
            {
                if (bottomN != 0)
                {
                    //Evaluate left corner when left wall is Neumann/Robin
                    if (i == gxs && leftN != 0){
                        yarray[j][i] =
                                -twoF * (xarray[j][i+1] + xarray[j+1][i])
                            + (1 + (2 + (h * leftD/leftN) +  (h * bottomD/bottomN)) * twoF) * xarray[j][i];
                    }
                    //Evaluate right corner when right wall is Neumann/Robin
                    else if (i == gxs+gxm-1 && rightN != 0){
                        yarray[j][i] =
                                -twoF * (xarray[j][i-1] + xarray[j+1][i])
                            + ( 1 + (2 + (h * rightD/rightN) + (h * bottomD/bottomN)) * twoF) * xarray[j][i];
                    }
                    //Evaluate all other edge points on bottom wall
                    else{
                        yarray[j][i] =
                                -F * (2*xarray[j+1][i] + xarray[j][i-1] + xarray[j][i+1])
                                + (1 + (2 + h*bottomD/bottomN) * twoF) * xarray[j][i];
                    }
                }
                //Evaluate wall in case of Dirichlet condition
                else
                    yarray[j][i] = xarray[j][i];
            }
            //LEFT WALL
            else if(i == gxs)
            {
//                else if ((user->leftNeumannCoefficient != 0 && i == gxs) && (j != gys && j != gys+gym-1)){
                if (leftN != 0)
                    yarray[j][i] =
                            -F * (xarray[j-1][i] + xarray[j+1][i] + 2*xarray[j][i+1])
                            + (1 + (2 + (user->h*leftD/leftN)) * twoF) * xarray[j][i];
                //Evaluate wall in case of Dirichlet condition
                else
                    yarray[j][i] = xarray[j][i];
            }
            //RIGHT WALL
            else if(i == gxs+gxm-1)
            {
//                else if ((user->rightNeumannCoefficient != 0 && i == gxs+gxm-1) && (j != gys && j != gys+gym-1)){
                if (rightN != 0)
                    yarray[j][i] =
                            -F * (xarray[j-1][i] + xarray[j+1][i] -  2*xarray[j][i-1])
                            + (1 + (2 + (h*rightD/rightN)) * twoF) * xarray[j][i];
                //Evaluate wall in case of Dirichlet condition
                else
                    yarray[j][i] = xarray[j][i];
            }

            //Sets value for non-edge points
            else
                yarray[j][i] = 1.0 - F*(
                        xarray[j-1][i] + xarray[j+1][i] + xarray[j][i-1] + xarray[j][i+1] - 4*xarray[j][i]);
                //        yarray[j][i] = -user->fourierNumber*xarray[j-1][i]-user->fourierNumber*xarray[j+1][i]-\
                //            user->fourierNumber*xarray[j][i-1] -user->fourierNumber*xarray[j][i+1]+\
                //            (1+4*user->fourierNumber)*xarray[j][i];
        }
    }
*/

///*
    for (j=ys; j<ys+ym; j++) {
        for (i=xs; i<xs+xm; i++) {
        //Sets value for edge points
          if (i == gxs || j == gys || i == gxs+gxm-1 || j == gys+gym-1){
                //Evaluate in case where top wall is Neumann/Robin
                if (user->topNeumannCoefficient != 0 && j == gys+gym-1){
                    //Evaluate left corner when left wall is Neumann/Robin
                    if (i == gxs && user->leftNeumannCoefficient != 0){
                        yarray[j][i] = -2*user->fourierNumber*xarray[j][i+1]-2*user->fourierNumber*xarray[j-1][i]\
                            +(1+(4 + (2*user->h*user->topDirichletCoefficient/user->topNeumannCoefficient)\
                            +(2*user->h*user->leftDirichletCoefficient/user->leftNeumannCoefficient))*\
                            user->fourierNumber)*xarray[j][i];
                    }
                    //Evaluate right corner when right wall is Neumann/Robin
                    else if (i == gxs+gxm-1 && user->rightNeumannCoefficient != 0){
                        yarray[j][i] = -2*user->fourierNumber*xarray[j][i-1]-2*user->fourierNumber*xarray[j-1][i]\
                            +(1+(4 + (2*user->h*user->topDirichletCoefficient/user->topNeumannCoefficient)\
                            +(2*user->h*user->rightDirichletCoefficient/user->rightNeumannCoefficient))*\
                            user->fourierNumber)*xarray[j][i];
                    }
                    //Evaluate all other edge points on top wall
                    else{
                        yarray[j][i] = -2*user->fourierNumber*xarray[j-1][i]-user->fourierNumber*xarray[j][i-1]\
                            -user->fourierNumber*xarray[j][i+1]+(1 + (4 + 2*user->h*user->topDirichletCoefficient\
                            /user->topNeumannCoefficient)*user->fourierNumber)*xarray[j][i];
                    }
                }
                //Evaluate in case where bottom wall is Neumann/Robin
                else if (user->bottomNeumannCoefficient != 0 && j == gys){
                    //Evaluate left corner when left wall is Neumann/Robin
                    if (i == gxs && user->leftNeumannCoefficient != 0){
                        yarray[j][i] = -2*user->fourierNumber*xarray[j][i+1]-2*user->fourierNumber*xarray[j+1][i]\
                            +(1+(4 + (2*user->h*user->leftDirichletCoefficient/user->leftNeumannCoefficient) + \
                            (2*user->h*user->bottomDirichletCoefficient/user->bottomNeumannCoefficient))*\
                            user->fourierNumber)*xarray[j][i];
                    }
                    //Evaluate right corner when right wall is Neumann/Robin
                    else if (i == gxs+gxm-1 && user->rightNeumannCoefficient != 0){
                        yarray[j][i] = -2*user->fourierNumber*xarray[j][i-1]-2*user->fourierNumber*xarray[j+1][i]\
                            +(1+(4 + (2*user->h*user->rightDirichletCoefficient/user->rightNeumannCoefficient) + \
                            (2*user->h*user->bottomDirichletCoefficient/user->bottomNeumannCoefficient))*\
                            user->fourierNumber)*xarray[j][i];
                    }
                    //Evaluate all other edge points on bottom wall
                    else{
                        yarray[j][i] =
                                -2*user->fourierNumber*xarray[j+1][i]
                                -user->fourierNumber*xarray[j][i-1]
                            -user->fourierNumber*xarray[j][i+1]
                                +(1+(4 + (2*user->h*user->bottomDirichletCoefficient\
                            /user->bottomNeumannCoefficient))*user->fourierNumber)*xarray[j][i];
                    }
                }
                //Evaluate in case where left wall is Neumann/Robin
                else if ((user->leftNeumannCoefficient != 0 && i == gxs) && (j != gys && j != gys+gym-1)){
                    yarray[j][i] = -user->fourierNumber*xarray[j-1][i]-user->fourierNumber*xarray[j+1][i]-\
                        2*user->fourierNumber*xarray[j][i+1]+(1+(4 + (2*user->h*user->leftDirichletCoefficient\
                        /user->leftNeumannCoefficient))*user->fourierNumber)*xarray[j][i];
                }
                //Evaluate in case where right wall is Neumann/Robin
                else if ((user->rightNeumannCoefficient != 0 && i == gxs+gxm-1) && (j != gys && j != gys+gym-1)){
                    yarray[j][i] = -user->fourierNumber*xarray[j-1][i]-user->fourierNumber*xarray[j+1][i]-\
                        2*user->fourierNumber*xarray[j][i-1]+(1+(4 + (2*user->h*user->rightDirichletCoefficient\
                        /user->rightNeumannCoefficient))*user->fourierNumber)*xarray[j][i];
                }
                //Evaluate wall in case of Dirichlet condition
                else{
                    yarray[j][i] = xarray[j][i];
                }
            }
            //Sets value for non-edge points
            else
                yarray[j][i] = -user->fourierNumber*xarray[j-1][i]-user->fourierNumber*xarray[j+1][i]-\
                    user->fourierNumber*xarray[j][i-1] -user->fourierNumber*xarray[j][i+1]+\
                    (1+4*user->fourierNumber)*xarray[j][i];
        }
    }
//*/

    //Finish function-------------------------------------------------------------
    //Restore arrays
    ierr = DMDAVecRestoreArray(distributedArray, Y, &yarray);CHKERRQ(ierr);
    ierr = DMDAVecRestoreArrayRead(distributedArray, user->localVector, &xarray);CHKERRQ(ierr);

    //Return error code
    PetscFunctionReturn(ierr);
}


#include "mwoibn/hierarchical_control/controllers/continous.h"
#include <cmath>
#include <boost/range/adaptor/reversed.hpp>

void mwoibn::hierarchical_control::controllers::Continous::addTask(tasks::BasicTask& new_task, mwoibn::VectorN gain, double damping){
        mwoibn::hierarchical_control::controllers::Default::addTask(new_task, gain, damping);
        _resize();
        _last_stack_change = 0;
}

void mwoibn::hierarchical_control::controllers::Continous::addTask(tasks::BasicTask& new_task, double gain, double damping){
        mwoibn::hierarchical_control::controllers::Default::addTask(new_task, gain, damping);
        _resize();
        _last_stack_change = 0;
}

void mwoibn::hierarchical_control::controllers::Continous::addTask(tasks::BasicTask& new_task, unsigned int i, mwoibn::VectorN gain, double damping){
        mwoibn::hierarchical_control::controllers::Default::addTask(new_task, i, gain, damping);
        _resize();
        _last_stack_change = 0;
}

void mwoibn::hierarchical_control::controllers::Continous::addTask(tasks::BasicTask& new_task, unsigned int i, double gain, double damping){
        mwoibn::hierarchical_control::controllers::Default::addTask(new_task, i, gain, damping);
        _resize();
        _last_stack_change = 0;
}

void mwoibn::hierarchical_control::controllers::Continous::_resize()
{
        int size = 0;

        for (auto& task : _tasks)
        {
                size += task.get().getTaskSize();
        }

        _e.setZero(size);
        _g.setZero(_robot.getDofs(), size);
}

void mwoibn::hierarchical_control::controllers::Continous::removeTask(
        int i)
{
        mwoibn::hierarchical_control::controllers::Default::removeTask(i);

        _resize();
        _last_stack_change = 0;
}

const mwoibn::VectorN &mwoibn::hierarchical_control::controllers::Continous::update()
{
        for (auto& task : _tasks)
        {
                task.get().update();
        }
        _previous_joint_states.noalias() = _joint_states;

        compute();

        return getCommand();
}

void mwoibn::hierarchical_control::controllers::Continous::compute()
{
        _command.setZero();
        _P.setIdentity();
        int i = 0;
        for (auto& task : _tasks)
        {
                _updateTask(i, task);
                ++i;
        }

        if (!_checkStack()) {
                _last_stack_change = 0;
                std::cout << "stack change detected" << std::endl;
        }

        if (!_last_stack_change)
                _resetCorrection();

        _g.setZero();
        int cols = _g.cols();
        _g_it.setIdentity();
        i--;
        for (auto& task : boost::adaptors::reverse(_tasks))
        {
                cols -= task.get().getTaskSize();

                __n_by_n1.setIdentity();
//    std::cout << "inverser\n" << _inversers_ptrs[i]->getInverse().rows() << "x" << _inversers_ptrs[i]->getInverse().cols() << std::endl;
//    std::cout << "jacobian\n" << task->getJacobian().rows() << "x" << task->getJacobian().cols() << std::endl;
                __n_by_n2.noalias() = _inversers_ptrs[i]->getInverse() * task.get().getJacobian();
                __n_by_n1.noalias() -= __n_by_n2;
                __n_by_n2.noalias() = _g_it * __n_by_n1;

                _g.block(0, cols, _robot.getDofs(), task.get().getTaskSize()) =
                        _g_it * _inversers_ptrs[i]->getInverse();
                i--;
        }

        ++_last_stack_change;

        // change with respect to the original
        _command.noalias() +=
                _g * _e * exp(-_mu * _last_stack_change *
                              _robot.rate()); //! \todo ** frequency should be an outside argument
}

void mwoibn::hierarchical_control::controllers::Continous::_resetCorrection()
{
//  std::cout << "_resetCorrection" << std::endl;

        int i = 0;
        int size = 0;
        for (auto& task : _tasks)
        {
//    std::cout << "error " <<  _gains[i].cwiseProduct(task->getPreviousError()) << std::endl;
//    std::cout << "d error " << task->getPreviousJacobian() * _previous_joint_states << std::endl;

//    std::cout << _e.size() << std::endl;
//    std::cout << _e.segment(size, task->getTaskSize()) << std::endl;
                _e.segment(size, task.get().getTaskSize()) =
                        _gains[i].cwiseProduct(task.get().getError());
                _e.segment(size, task.get().getTaskSize()) +=
                        task.get().getJacobian() * _joint_states;
                size += task.get().getTaskSize();
                i++;
        }
}


//==================================================================================================
/**
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================

#ifndef BOOST_SIMD_FUNCTION_SCALAR_COSD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_COSD_HPP_INCLUDED

#include <boost/simd/function/definition/cosd.hpp>
#include <boost/simd/arch/common/scalar/function/cosd.hpp>

#endif


//***************************************************************************
//* Copyright (c) 2015 Saint Petersburg State University
//* Copyright (c) 2011-2014 Saint Petersburg Academic University
//* All Rights Reserved
//* See file LICENSE for details.
//***************************************************************************

#pragma once

#include "assembly_graph/core/graph.hpp"
#include "assembly_graph/graph_support/chimera_stats.hpp"
#include "assembly_graph/graph_support/basic_edge_conditions.hpp"
#include "assembly_graph/graph_support/parallel_processing.hpp"
#include "assembly_graph/graph_support/detail_coverage.hpp"

#include "modules/simplification/tip_clipper.hpp"
#include "modules/simplification/complex_tip_clipper.hpp"
#include "modules/simplification/bulge_remover.hpp"
#include "modules/simplification/complex_bulge_remover.hpp"
#include "modules/simplification/erroneous_connection_remover.hpp"
#include "modules/simplification/relative_coverage_remover.hpp"
#include "modules/simplification/mf_ec_remover.hpp"
#include "modules/simplification/parallel_simplification_algorithms.hpp"
#include "stages/simplification_pipeline/simplification_settings.hpp"

#include "modules/graph_read_correction.hpp"

#include "pipeline/config_struct.hpp"

#include "utils/perf/timetracer.hpp"

#include <boost/algorithm/string.hpp>

namespace debruijn {

namespace simplification {

//todo remove this line
using namespace debruijn_graph;

template<class Graph>
class ConditionParser {
private:
    typedef typename Graph::EdgeId EdgeId;

    const Graph &g_;
    std::string next_token_, input_;
    const SimplifInfoContainer settings_;
    /*(curr_iter + 1) / total_iter*/
    double iter_run_progress_;
    std::queue<std::string> tokenized_input_;

    size_t max_length_bound_;
    double max_coverage_bound_;
    int requested_iterations_;

    std::string ReadNext() {
        if (!tokenized_input_.empty()) {
            next_token_ = tokenized_input_.front();
            tokenized_input_.pop();
        } else {
            next_token_ = "";
        }
        return next_token_;
    }

    template<typename T>
    bool RelaxMax(T &cur_max, T t) {
        if (t > cur_max) {
            cur_max = t;
            return true;
        }
        return false;
    }

    template<typename T>
    bool RelaxMin(T &cur_min, T t) {
        if (t < cur_min) {
            cur_min = t;
            return true;
        }
        return false;
    }

    double GetCoverageBound() {
        if (next_token_ == "auto") {
            return settings_.detected_coverage_bound();
        } else {
            return std::stod(next_token_);
        }
    }

    func::TypedPredicate<EdgeId> ParseCondition(size_t &min_length_bound,
                                               double &min_coverage_bound) {
        if (next_token_ == "tc_lb") {
            double length_coeff = std::stod(ReadNext());

            DEBUG("Creating tip length bound. Coeff " << length_coeff);
            size_t length_bound = LengthThresholdFinder::MaxTipLength(
                settings_.read_length(), g_.k(), length_coeff);

            DEBUG("Length bound " << length_bound);

            RelaxMin(min_length_bound, length_bound);
            DEBUG("Min length bound - " << min_length_bound);
            return LengthUpperBound<Graph>(g_, length_bound);

        } else if (next_token_ == "rlmk") {
            //Read length minus k
            CHECK_FATAL_ERROR(settings_.read_length() > g_.k(), "Read length was shorter than K");
            double length_coeff = std::stod(ReadNext());
            DEBUG("Creating (rl - k) bound. Multiplicative coefficient: " << length_coeff);
            size_t length_bound = size_t(math::round(length_coeff * double(settings_.read_length() - g_.k())));
            RelaxMin(min_length_bound, length_bound);
            DEBUG("Min length bound - " << min_length_bound);
            return LengthUpperBound<Graph>(g_, length_bound);
        } else if (next_token_ == "rl") {
            //Read length
            CHECK_FATAL_ERROR(settings_.read_length() > 0, "Read was zero");
            double length_coeff = std::stod(ReadNext());
            DEBUG("Creating rl bound. Multiplicative coefficient: " << length_coeff);
            size_t length_bound = size_t(math::round(length_coeff * double(settings_.read_length())));
            RelaxMin(min_length_bound, length_bound);
            DEBUG("Min length bound - " << min_length_bound);
            return LengthUpperBound<Graph>(g_, length_bound);
        } else if (next_token_ == "to_ec_lb") {
            double length_coeff = std::stod(ReadNext());

            DEBUG( "Creating length bound for erroneous connections originated from tip merging. Coeff " << length_coeff);
            size_t length_bound =
                    LengthThresholdFinder::MaxTipOriginatedECLength(
                        settings_.read_length(), g_.k(), length_coeff);

            DEBUG("Length bound " << length_bound);

            RelaxMin(min_length_bound, length_bound);
            DEBUG("Min length bound - " << min_length_bound);
            return LengthUpperBound<Graph>(g_, length_bound);

        } else if (next_token_ == "ec_lb") {
            size_t length_coeff = std::stoll(ReadNext());

            DEBUG("Creating ec length bound. Coeff " << length_coeff);
            size_t length_bound =
                    LengthThresholdFinder::MaxErroneousConnectionLength(
                        g_.k(), length_coeff);

            DEBUG("Length bound " << length_bound);

            RelaxMin(min_length_bound, length_bound);
            DEBUG("Min length bound - " << min_length_bound);
            return LengthUpperBound<Graph>(g_, length_bound);
        } else if (next_token_ == "lb") {
            size_t length_bound = std::stoll(ReadNext());

            DEBUG("Creating length bound. Value " << length_bound);

            RelaxMin(min_length_bound, length_bound);
            DEBUG("Min length bound - " << min_length_bound);
            return LengthUpperBound<Graph>(g_, length_bound);
        } else if (next_token_ == "cb") {
            ReadNext();
            double cov_bound = GetCoverageBound();
            DEBUG("Creating coverage upper bound " << cov_bound);
            RelaxMin(min_coverage_bound, cov_bound);
            return CoverageUpperBound<Graph>(g_, cov_bound);
        } else if (next_token_ == "icb") {
            ReadNext();
            double cov_bound = GetCoverageBound();
            cov_bound = cov_bound * iter_run_progress_;
            DEBUG("Creating iterative coverage upper bound " << cov_bound);
            RelaxMin(min_coverage_bound, cov_bound);
            return CoverageUpperBound<Graph>(g_, cov_bound);
        } else if (next_token_ == "nbr") {
            return NotBulgeECCondition<Graph>(g_);
        } else if (next_token_ == "rcec_cb") {
            ReadNext();
            double rcec_ratio = std::stod(next_token_);
            INFO("Creating relative coverage EC condition with threshold " << rcec_ratio);
            return RelativeCoverageECCondition<Graph>(g_, rcec_ratio);
        } else if (next_token_ == "rctc") {
            ReadNext();
            DEBUG("Creating relative cov tip cond " << next_token_);
            return RelativeCoverageTipCondition<Graph>(g_, std::stod(next_token_));
        } else if (next_token_ == "disabled") {
            DEBUG("Creating disabling condition");
            return func::AlwaysFalse<EdgeId>();
        } else if (next_token_ == "mmm") {
            ReadNext();
            DEBUG("Creating max mismatches cond " << next_token_);
            return MismatchTipCondition<Graph>(g_, std::stod(next_token_));
        } else {
            VERIFY(false);
            return func::AlwaysTrue<EdgeId>();
        }
    }

    func::TypedPredicate<EdgeId> ParseConjunction(size_t &min_length_bound,
                                                  double &min_coverage_bound) {
        func::TypedPredicate<EdgeId> answer = func::AlwaysTrue<EdgeId>();
        VERIFY(next_token_ == "{");
        ReadNext();
        while (next_token_ != "}") {
            answer = func::And(answer,
                              ParseCondition(min_length_bound, min_coverage_bound));
            ReadNext();
        }
        return answer;
    }

public:

    ConditionParser(const Graph &g, std::string input, const SimplifInfoContainer &settings,
                    double iter_run_progress = 1.)
                    //size_t curr_iteration = 0, size_t iteration_cnt = 1)
            : g_(g),
              input_(std::move(input)),
              settings_(settings),
              iter_run_progress_(iter_run_progress),
              //iter_run_progress_((double) (curr_iteration + 1) / (double) iteration_cnt),
              max_length_bound_(0),
              max_coverage_bound_(0.),
              requested_iterations_(1) {
        DEBUG("Creating parser for string " << input);
        std::vector<std::string> tmp_tokenized_input;
        boost::split(tmp_tokenized_input, input_, boost::is_any_of(" ,;"), boost::token_compress_on);
        for (const auto &s : tmp_tokenized_input) {
            tokenized_input_.push(s);
        }
        ReadNext();
    }

    func::TypedPredicate<EdgeId> operator()() {
        DEBUG("Parsing");
        func::TypedPredicate<EdgeId> answer = func::AlwaysFalse<EdgeId>();
        if (next_token_ == "loop") {
            requested_iterations_ = std::stoi(ReadNext());
            ReadNext();
        }

        CHECK_FATAL_ERROR(next_token_ == "{", "Expected \"{\", but next token was " << next_token_);
        while (next_token_ == "{") {
            size_t min_length_bound = std::numeric_limits<size_t>::max();
            double min_coverage_bound = std::numeric_limits<double>::max();
            answer = func::Or(answer,
                             ParseConjunction(min_length_bound, min_coverage_bound));
            RelaxMax(max_length_bound_, min_length_bound);
            RelaxMax(max_coverage_bound_, min_coverage_bound);
            ReadNext();
        }
        return answer;
    }

    size_t max_length_bound() const {
        return max_length_bound_;
    }

    double max_coverage_bound() const {
        return max_coverage_bound_;
    }

    int requested_iterations() const {
        return requested_iterations_;
    }

private:
    DECL_LOGGER("ConditionParser");
};

template<class Graph>
class EditDistanceTrackingCallback {
    typedef typename Graph::EdgeId EdgeId;
    const Graph &g_;

public:
    EditDistanceTrackingCallback(const Graph &g)
            : g_(g) {
    }

    bool operator()(EdgeId edge, const std::vector<EdgeId> &path) const {
        Sequence path_sequence = MergeSequences(g_, path);
        size_t dist = EditDistance(g_.EdgeNucls(edge), path_sequence);
        TRACE( "Bulge sequences with distance " << dist << " were " << g_.EdgeNucls(edge) << " and " << path_sequence);
        return true;
    }

private:
    DECL_LOGGER("EditDistanceTrackingCallback");
};

//enabling tip projection
template<class gp_t>
EdgeRemovalHandlerF<typename gp_t::graph_t> WrapWithProjectionCallback(
    gp_t &gp,
    EdgeRemovalHandlerF<typename gp_t::graph_t> removal_handler) {
    typedef typename gp_t::graph_t Graph;
    typedef typename Graph::EdgeId EdgeId;
    TipsProjector<gp_t> tip_projector(gp);
    return [=](EdgeId e) {
        if (removal_handler) removal_handler(e);
        tip_projector.ProjectTip(e);
    };
}

template<class Graph>
class LowCoverageEdgeRemovingAlgorithm : public PersistentProcessingAlgorithm<Graph,
                                                                              typename Graph::EdgeId,
                                                                              omnigraph::CoverageComparator<Graph>> {
    typedef typename Graph::EdgeId EdgeId;
    typedef PersistentProcessingAlgorithm<Graph, EdgeId, omnigraph::CoverageComparator<Graph>> base;

    const SimplifInfoContainer simplif_info_;
    const std::string condition_str_;
    EdgeRemover<Graph> edge_remover_;

    func::TypedPredicate<EdgeId> remove_condition_;
    func::TypedPredicate<EdgeId> proceed_condition_;

protected:

    void PrepareIteration(double iter_run_progress) override {
        TRACE("Preparing iteration for iterative progress " << iter_run_progress);
        ConditionParser<Graph> parser(this->g(), condition_str_,
                                      simplif_info_, iter_run_progress);
        remove_condition_ = omnigraph::AddAlternativesPresenceCondition(this->g(), parser());
        TRACE("Updated remove condition");
        proceed_condition_ = CoverageUpperBound<Graph>(this->g(), parser.max_coverage_bound());
        TRACE("Updated proceed condition up to coverage " << parser.max_coverage_bound());
    }

    bool Proceed(EdgeId e) const override {
        return proceed_condition_(e);
    }

    bool Process(EdgeId e) override {
        TRACE("Checking edge " << this->g().str(e) << " for the removal condition");
        if (remove_condition_(e)) {
            TRACE("Check passed, removing");
            edge_remover_.DeleteEdge(e);
            return true;
        }
        TRACE("Check not passed");
        return false;
    }

public:
    LowCoverageEdgeRemovingAlgorithm(Graph &g,
                                     const std::string &condition_str,
                                     const SimplifInfoContainer &simplif_info,
                                     std::function<void(EdgeId)> removal_handler = nullptr,
                                     bool canonical_only = true,
                                     bool track_changes = true)
            : base(g, nullptr,
                   canonical_only,
                   omnigraph::CoverageComparator<Graph>(g),
                   track_changes),
              simplif_info_(simplif_info),
              condition_str_(condition_str),
              edge_remover_(g, removal_handler),
              remove_condition_(func::AlwaysFalse<EdgeId>()),
              proceed_condition_(func::AlwaysTrue<EdgeId>()) {

        ConditionParser<Graph> parser(g, condition_str, simplif_info);
        this->interest_el_finder_ =
                std::make_shared<omnigraph::ParallelInterestingElementFinder<Graph>>(
                        AddAlternativesPresenceCondition(g, parser()),
                        simplif_info.chunk_cnt());
    }

private:
    DECL_LOGGER("LowCoverageEdgeRemovingAlgorithm");
};

template<class Graph>
AlternativesAnalyzer<Graph> ParseBRConfig(const Graph &g,
                                          const config::debruijn_config::simplification::bulge_remover &config) {
    size_t max_length = LengthThresholdFinder::MaxBulgeLength(
        g.k(), config.max_bulge_length_coefficient,
        config.max_additive_length_coefficient);

    DEBUG("Length bound " << max_length);

    return AlternativesAnalyzer<Graph>(g, config.max_coverage,
                                       max_length,
                                       config.max_relative_coverage,
                                       config.max_delta,
                                       config.max_relative_delta,
                                       config.max_number_edges,
                                       config.dijkstra_vertex_limit, config.min_identity);
}

template<class Graph>
AlgoPtr<Graph> SelfConjugateEdgeRemoverInstance(Graph &g, const std::string &condition_str,
                const SimplifInfoContainer &info,
                EdgeRemovalHandlerF<Graph> removal_handler = 0) {
    if (condition_str.empty())
        return nullptr;
    ConditionParser<Graph> parser(g, condition_str, info);
    auto condition = func::And(SelfConjugateCondition<Graph>(g), parser());

    return std::make_shared<omnigraph::ParallelEdgeRemovingAlgorithm<Graph>>(g,
                                                                  condition,
                                                                  info.chunk_cnt(),
                                                                  removal_handler,
                                                                  /*canonical_only*/true);
}

template<class Graph>
AlgoPtr<Graph> RelativeCoverageComponentRemoverInstance (
        Graph &g,
        const FlankingCoverage<Graph> &flanking_cov,
        const config::debruijn_config::simplification::relative_coverage_comp_remover &rcc_config,
        const SimplifInfoContainer &info,
        typename ComponentRemover<Graph>::HandlerF removal_handler = nullptr) {
    if (!rcc_config.enabled) {
        return nullptr;
        INFO("Removal of relatively low covered connections disabled");
    }

    //     INFO("Removing relatively low covered connections");
    size_t connecting_path_length_bound = LengthThresholdFinder::MaxErroneousConnectionLength(
            g.k(), rcc_config.max_ec_length_coefficient);

    std::string pics_dir = "";

    double max_coverage = math::ge(rcc_config.max_coverage_coeff, 0.)
                          ? info.detected_coverage_bound() * rcc_config.max_coverage_coeff
                          : std::numeric_limits<double>::max();

    return std::make_shared<omnigraph::simplification::relative_coverage::
        RelativeCoverageComponentRemover<Graph>>(g,
                                                 info.chunk_cnt(),
                                                 flanking_cov,
                                                 rcc_config.coverage_gap,
                                                 size_t(double(info.read_length()) * rcc_config.length_coeff),
                                                 size_t(double(info.read_length()) * rcc_config.tip_allowing_length_coeff),
                                                 connecting_path_length_bound,
                                                 max_coverage,
                                                 removal_handler, rcc_config.vertex_count_limit, pics_dir);
}

template<class Graph>
AlgoPtr<Graph> RelativelyLowCoverageDisconnectorInstance(Graph &g,
        const config::debruijn_config::simplification::relative_coverage_edge_disconnector &rced_config,
        const SimplifInfoContainer &info,
        EdgeRemovalHandlerF<Graph> removal_handler = nullptr) {
    if (!rced_config.enabled) {
        INFO("Disconnection of relatively low covered edges disabled");
        return nullptr;
    }

    using Condition=omnigraph::simplification::relative_coverage::RelativeCovDisconnectionCondition<Graph>;

    func::TypedPredicate<EdgeId> condition = Condition(g, rced_config.diff_mult, rced_config.edge_sum);

    if (math::gr(rced_config.unconditional_diff_mult, 0.))
        condition = func::Or(Condition(g, rced_config.unconditional_diff_mult, 0), condition);

    return std::make_shared<omnigraph::DisconnectionAlgorithm<Graph>>(g,
            condition,
            info.chunk_cnt(),
            removal_handler);
}

template<class Graph>
AlgoPtr<Graph> ComplexBRInstance(
    Graph &g,
    config::debruijn_config::simplification::complex_bulge_remover cbr_config,
    const SmartEdgeSet<std::unordered_set<typename Graph::EdgeId>, Graph> *restricted_edges,
    const SimplifInfoContainer &info) {
    if (!cbr_config.enabled)
        return nullptr;
    size_t max_length = (size_t) ((double) g.k() * cbr_config.max_relative_length);
    size_t max_diff = cbr_config.max_length_difference;
    return std::make_shared<omnigraph::complex_br::ComplexBulgeRemover<Graph>>(g, max_length, max_diff,
                                                                               restricted_edges, info.chunk_cnt());
}

template<class Graph>
AlgoPtr<Graph> ComplexTipClipperInstance(Graph &g,
                     config::debruijn_config::simplification::complex_tip_clipper ctc_conf,
                     const SimplifInfoContainer &info,
                     typename ComponentRemover<Graph>::HandlerF removal_handler = 0) {
    if (!ctc_conf.enabled) {
        INFO("Complex tip clipping disabled");
        return nullptr;
    }

    ConditionParser<Graph> parser(g, ctc_conf.condition, info);
    parser();

    return std::make_shared<omnigraph::ComplexTipClipper<Graph>>(g, ctc_conf.max_relative_coverage,
                                         ctc_conf.max_edge_len,
                                         parser.max_length_bound(), info.chunk_cnt(),
                                         "", removal_handler);
}

template<class Graph>
AlgoPtr<Graph> IsolatedEdgeRemoverInstance(Graph &g,
                                           config::debruijn_config::simplification::isolated_edge_remover ier,
                                           const SimplifInfoContainer &info,
                                           EdgeRemovalHandlerF<Graph> removal_handler = 0) {
    if (!ier.enabled)
        return nullptr;

    size_t max_length_any_cov = ier.use_rl_for_max_length_any_cov ?
                                std::max(info.read_length() + ier.rl_threshold_increase, ier.max_length_any_cov) :
                                ier.max_length_any_cov;
    size_t max_length = ier.use_rl_for_max_length ?
                                std::max(info.read_length() + ier.rl_threshold_increase, ier.max_length) :
                                ier.max_length;

    auto condition = func::And(IsolatedEdgeCondition<Graph>(g),
                              func::Or(LengthUpperBound<Graph>(g, max_length_any_cov),
                                      func::And(LengthUpperBound<Graph>(g, max_length),
                                               CoverageUpperBound<Graph>(g, ier.max_coverage))));

    return std::make_shared<omnigraph::ParallelEdgeRemovingAlgorithm<Graph>>(g,
                                                                  condition,
                                                                  info.chunk_cnt(),
                                                                  removal_handler,
                                                                  /*canonical_only*/true);
}

template<class Graph>
AlgoPtr<Graph> RelativeECRemoverInstance(Graph &g,
                                         const config::debruijn_config::simplification::relative_coverage_ec_remover &rcec_config,
                                         const SimplifInfoContainer &info,
                                         EdgeRemovalHandlerF<Graph> removal_handler) {
    if (!rcec_config.enabled)
        return nullptr;

    return std::make_shared<omnigraph::ParallelEdgeRemovingAlgorithm<Graph>>(g,
            AddRelativeCoverageECCondition(g, rcec_config.rcec_ratio,
                                           AddAlternativesPresenceCondition(g, func::TypedPredicate<typename Graph::EdgeId>
                                                   (LengthUpperBound<Graph>(g, rcec_config.max_ec_length)))),
            info.chunk_cnt(), removal_handler, /*canonical_only*/true);
}

template<class Graph>
AlgoPtr<Graph> ECRemoverInstance(Graph &g,
                                 const config::debruijn_config::simplification::erroneous_connections_remover &ec_config,
                                 const SimplifInfoContainer &info,
                                 EdgeRemovalHandlerF<Graph> removal_handler = nullptr) {
    if (ec_config.condition.empty())
        return nullptr;

    return std::make_shared<LowCoverageEdgeRemovingAlgorithm<Graph>>(
            g, ec_config.condition, info, removal_handler);
}

template<class Graph>
AlgoPtr<Graph> TipClipperInstance(Graph &g,
                                  const EdgeConditionT<Graph> &condition,
                                  const SimplifInfoContainer &info,
                                  EdgeRemovalHandlerF<Graph> removal_handler = nullptr,
                                  bool track_changes = true) {
    return std::make_shared<omnigraph::ParallelEdgeRemovingAlgorithm<Graph, omnigraph::LengthComparator<Graph>>>(g,
                                                                        AddTipCondition(g, condition),
                                                                        info.chunk_cnt(),
                                                                        removal_handler,
                                                                        /*canonical_only*/true,
                                                                        LengthComparator<Graph>(g),
                                                                        track_changes);
}

template<class Graph>
AlgoPtr<Graph> TipClipperInstance(Graph &g,
                                  const config::debruijn_config::simplification::tip_clipper &tc_config,
                                  const SimplifInfoContainer &info,
                                  EdgeRemovalHandlerF<Graph> removal_handler = nullptr) {
    if (tc_config.condition.empty())
        return nullptr;

    ConditionParser<Graph> parser(g, tc_config.condition, info);
    auto condition = parser();
    auto algo = TipClipperInstance(g, condition, info, removal_handler);
    CHECK_FATAL_ERROR(parser.requested_iterations() != 0, "To disable tip clipper pass empty string");
    if (parser.requested_iterations() == 1)
        return algo;

    return std::make_shared<LoopedAlgorithm<Graph>>(g, algo, 1, size_t(parser.requested_iterations()),
                                                    /*force primary for all*/ true);
}

template<class Graph>
AlgoPtr<Graph> DeadEndInstance(Graph &g,
                               const config::debruijn_config::simplification::dead_end_clipper &dead_end_config,
                               const SimplifInfoContainer &info,
                               EdgeRemovalHandlerF<Graph> removal_handler) {
    if (!dead_end_config.enabled || dead_end_config.condition.empty())
        return nullptr;

    ConditionParser<Graph> parser(g, dead_end_config.condition, info);
    auto condition = parser();
    return std::make_shared<omnigraph::ParallelEdgeRemovingAlgorithm<Graph, omnigraph::LengthComparator<Graph>>>(g,
            AddDeadEndCondition(g, condition), info.chunk_cnt(), removal_handler, /*canonical_only*/true,
            LengthComparator<Graph>(g), /*track changes*/true);
}

template<class Graph>
AlgoPtr<Graph> TopologyTipClipperInstance(
    Graph &g,
    const config::debruijn_config::simplification::topology_tip_clipper &ttc_config,
    const SimplifInfoContainer &info,
    EdgeRemovalHandlerF<Graph> removal_handler = nullptr) {

    auto condition
            = func::And(LengthUpperBound<Graph>(g,
                                               LengthThresholdFinder::MaxTipLength(info.read_length(), g.k(), ttc_config.length_coeff)),
                       DefaultUniquenessPlausabilityCondition<Graph>(g,
                                                                     ttc_config.uniqueness_length, ttc_config.plausibility_length));

    return TipClipperInstance(g,
                              condition, info, removal_handler, /*track changes*/false);
}

typedef std::function<bool(EdgeId edge, const std::vector<EdgeId>& path)> BulgeCallbackF;

template<class Graph>
AlgoPtr<Graph> BRInstance(Graph &g,
                          const config::debruijn_config::simplification::bulge_remover &br_config,
                          const SimplifInfoContainer &info,
                          BulgeCallbackF callback = nullptr,
                          EdgeRemovalHandlerF<Graph> removal_handler = nullptr) {
    if (!br_config.enabled || (br_config.main_iteration_only && !info.main_iteration())) {
        return nullptr;
    }

    auto alternatives_analyzer = ParseBRConfig(g, br_config);

    if (br_config.parallel) {
        INFO("Creating parallel br instance");
        return std::make_shared<omnigraph::ParallelBulgeRemover<Graph>>(g,
                info.chunk_cnt(),
                br_config.buff_size,
                br_config.buff_cov_diff,
                br_config.buff_cov_rel_diff,
                alternatives_analyzer,
                callback,
                removal_handler,
                /*track_changes*/true);
    } else {
        INFO("Creating br instance");
        return std::make_shared<omnigraph::BulgeRemover<Graph>>(g,
                info.chunk_cnt(),
                alternatives_analyzer,
                callback,
                removal_handler,
                /*track_changes*/true);
    }
}

template<class Graph>
AlgoPtr<Graph> LowFlankDisconnectorInstance(Graph &g,
                                           const FlankingCoverage<Graph> &flanking_cov,
                                           double cov_bound,
                                           const SimplifInfoContainer &info,
                                           EdgeRemovalHandlerF<Graph> removal_handler) {
    if (math::ls(cov_bound, 0.)) {
        INFO("Flanking coverage based disconnection disabled");
        return nullptr;
    }

    auto condition = [&,cov_bound] (EdgeId e) {
        return g.OutgoingEdgeCount(g.EdgeStart(e)) > 1 &&
                math::le(flanking_cov.CoverageOfStart(e), cov_bound);
    };

    return std::make_shared<omnigraph::DisconnectionAlgorithm<Graph>>(g, condition,
                                                                      info.chunk_cnt(),
                                                                      removal_handler);
}

template<class Graph>
AlgoPtr<Graph> LowCoverageEdgeRemoverInstance(Graph &g,
                                              const config::debruijn_config::simplification::low_covered_edge_remover &lcer_config,
                                              const SimplifInfoContainer &info) {
    if (!lcer_config.enabled)
        return nullptr;

    VERIFY(info.read_length() > g.k());
    double threshold = lcer_config.coverage_threshold * double(info.read_length() - g.k()) / double(info.read_length());
    INFO("Low coverage edge removal (LCER) activated and will remove edges of coverage lower than " << threshold);
    return std::make_shared<ParallelEdgeRemovingAlgorithm<Graph, CoverageComparator<Graph>>>
                        (g,
                        CoverageUpperBound<Graph>(g, threshold),
                        info.chunk_cnt(),
                        (EdgeRemovalHandlerF<Graph>)nullptr,
                        /*canonical_only*/true,
                        CoverageComparator<Graph>(g));
}

template<class Graph>
bool RemoveHiddenLoopEC(Graph &g,
                        const FlankingCoverage<Graph> &flanking_cov,
                        double determined_coverage_threshold,
                        double relative_threshold,
                        EdgeRemovalHandlerF<Graph> removal_handler) {
    TIME_TRACE_SCOPE();

    INFO("Removing loops and rc loops with erroneous connections");
    ECLoopRemover<Graph> hc(g, flanking_cov,
                            determined_coverage_threshold,
                            relative_threshold, removal_handler);
    bool res = hc.Run();
    hc.PrintLoopStats();
    return res;
}

}
}