#ifndef INCLUDED_STDDEFX
#include "stddefx.h"
#define INCLUDED_STDDEFX
#endif

#ifndef INCLUDED_CALC
#include "calc.h"
#define INCLUDED_CALC
#endif

// Library headers.
#ifndef INCLUDED_IOSTREAM
#include <iostream>
#define INCLUDED_IOSTREAM
#endif
#ifndef INCLUDED_BOOST_NONCOPYABLE
#include <boost/noncopyable.hpp>
#define INCLUDED_BOOST_NONCOPYABLE
#endif

// PCRaster library headers.
#ifndef INCLUDED_FIELDAPI_INTERFACE
#include "fieldapi_interface.h"
#define INCLUDED_FIELDAPI_INTERFACE
#endif

#ifndef INCLUDED_FIELDAPI_SCALARDOMAINCHECK
#include "fieldapi_scalardomaincheck.h"
#define INCLUDED_FIELDAPI_SCALARDOMAINCHECK
#endif

#ifndef INCLUDED_GEO_CELLLOCVISITOR
#include "geo_celllocvisitor.h"
#define INCLUDED_GEO_CELLLOCVISITOR
#endif

#ifndef INCLUDED_GEO_SIMPLERASTER
#include "geo_simpleraster.h"
#define INCLUDED_GEO_SIMPLERASTER
#endif

#ifndef INCLUDED_MISC
#include "misc.h"
#define INCLUDED_MISC
#endif

#ifndef INCLUDED_COM_INTERVALTYPES
#include "com_intervaltypes.h"
#define INCLUDED_COM_INTERVALTYPES
#endif

// Module headers.

//------------------------------------------------------------------------------
// Implicit finite difference according to:
// Introduction to groundwater modeling
// Wang & Anderson
// Crank-Nicolson method, Gauss-Seidel iteration
//------------------------------------------------------------------------------

namespace calc {

struct CalculationArgs {

  double d_area;     // Cell area, constant.
  double d_alpha;    // Constant.
  double d_nrCells;  // Number of surrounding cells contributing to current cel.
  double d_interval;
  double d_currentElevation;
  double d_recharge;
  double d_transmissivity;   // Average t for all directions.
  double d_storageCoefficient;
  double d_h1;       // h * t for all directions, timestep n
  double d_h2;       // h * t for all directions, timestep n + 1.

};

double calcElevation(const CalculationArgs& args)
{
  double f1 = (args.d_area * args.d_storageCoefficient) / args.d_interval;
  double f2 = 1.0 / (f1 + args.d_nrCells * args.d_transmissivity *
                   args.d_alpha);

  return f2 * (args.d_alpha * args.d_h2 + f1 * args.d_currentElevation +
                   (1.0 - args.d_alpha) *
                   (args.d_h1 - args.d_currentElevation * args.d_nrCells *
                   args.d_transmissivity) + args.d_area * args.d_recharge);
}

class DiffuseAlgorithm: public boost::noncopyable
{

public:

  enum Direction {
    NoDirection = 0x000,
    Left = 0x0001,
    Top = 0x0002,
    Right = 0x0004,
    Bottom = 0x0008,
    UpperLeft = Top | Left,
    UpperRight = Top | Right,
    LowerLeft = Bottom | Left,
    LowerRight = Bottom | Right,
    Enclosed = Left | Top | Right | Bottom
  };

  enum FlowCondition  {
    NOFLOW = 0,
    CALCULATE = 1,
    FIXED = 2
  };

private:

  fieldapi::ReadWriteReal8&  d_resultElevation;
  const fieldapi::ReadOnlyReal8& d_elevation;
  const fieldapi::ReadOnlyReal8& d_recharge;
  const fieldapi::ReadOnlyReal8& d_transmissivity;
  const fieldapi::ReadOnlyInt4& d_flowCondition;
  const fieldapi::ReadOnlyReal8& d_storageCoefficient;

  CalculationArgs d_args;
  geo::CellLoc d_location;    // A cell location.

  geo::SimpleRaster<double> d_tmRight; // Average transm. values, right.
  geo::SimpleRaster<double> d_tmBottom; // Average transm. values, bottom.

  geo::SimpleRaster<unsigned int> d_noFlowBoundaries;

  double calcElevationWithNoFlowBoundary(size_t r, size_t c, unsigned int direction)
  {
    if(direction == Enclosed) {
      // Current cell is surrounded by no flow boundaries. Current elevation
      // remains unchanged.
      return d_elevation.value(r, c);
    }

    // Calculate new elevation.
    d_args.d_h1 = 0.0;
    d_args.d_h2 = 0.0;
    d_args.d_nrCells = 0;

    // Harmonic means of transmissivities.
    double tmLeft(0), tmTop(0), tmRight(0), tmBottom(0);

    // Cell to the left.
    if(!(direction & Left)) {
      tmLeft = d_tmRight.cell(r, c - 1);
      d_args.d_h1 += tmLeft * d_elevation.value(r, c - 1);
      d_args.d_h2 += tmLeft * d_resultElevation.value(r, c - 1);
      ++d_args.d_nrCells;
    }
    else if(!(direction & Right)) {
      // Copy value from Right cell.
      tmLeft = d_tmRight.cell(r, c);
      d_args.d_h1 += tmLeft * d_elevation.value(r, c + 1);
      d_args.d_h2 += tmLeft * d_resultElevation.value(r, c + 1);
      ++d_args.d_nrCells;
    }

    // Cell to the top.
    if(!(direction & Top)) {
      tmTop = d_tmBottom.cell(r - 1, c);
      d_args.d_h1 += tmTop * d_elevation.value(r - 1, c);
      d_args.d_h2 += tmTop * d_resultElevation.value(r - 1, c);
      ++d_args.d_nrCells;
    }
    else if(!(direction & Bottom)) {
      // Copy value from Bottom cell.
      tmTop = d_tmBottom.cell(r, c);
      d_args.d_h1 += tmTop * d_elevation.value(r + 1, c);
      d_args.d_h2 += tmTop * d_resultElevation.value(r + 1, c);
      ++d_args.d_nrCells;
    }

    // Cell to the right.
    if(!(direction & Right)) {
      tmRight = d_tmRight.cell(r, c);
      d_args.d_h1 += tmRight * d_elevation.value(r, c + 1);
      d_args.d_h2 += tmRight * d_resultElevation.value(r, c + 1);
      ++d_args.d_nrCells;
    }
    else if(!(direction & Left)) {
      // Copy value from Left cell.
      tmRight = d_tmRight.cell(r, c - 1);
      d_args.d_h1 += tmRight * d_elevation.value(r, c - 1);
      d_args.d_h2 += tmRight * d_resultElevation.value(r, c - 1);
      ++d_args.d_nrCells;
    }

    // Cell to the bottom.
    if(!(direction & Bottom)) {
      tmBottom = d_tmBottom.cell(r, c);
      d_args.d_h1 += tmBottom * d_elevation.value(r + 1, c);
      d_args.d_h2 += tmBottom * d_resultElevation.value(r + 1, c);
      ++d_args.d_nrCells;
    }
    else if(!(direction & Top)) {
      // Copy value from Top cell.
      tmBottom = d_tmBottom.cell(r - 1, c);
      d_args.d_h1 += tmBottom * d_elevation.value(r - 1, c);
      d_args.d_h2 += tmBottom * d_resultElevation.value(r - 1, c);
      ++d_args.d_nrCells;
    }

    PRECOND(d_args.d_nrCells > 0);

    d_args.d_currentElevation = d_elevation.value(r, c);
    d_args.d_recharge = d_recharge.value(r, c);
    // Arithmic mean.
    d_args.d_transmissivity = (tmLeft + tmTop + tmRight + tmBottom) /
                   d_args.d_nrCells;
    d_args.d_storageCoefficient = d_storageCoefficient.value(r, c);

    return calc::calcElevation(d_args);
  }

  double calcElevationWithoutNoFlowBoundary(size_t r, size_t c)
  {
    d_args.d_h1 = 0.0;       // Elevation at t = n, * transmissivities.
    d_args.d_h2 = 0.0;       // Elevation at t = n + 1, * transmissivities.
    d_args.d_nrCells = 0;    // Nr of cells contributing to the current value.
    static double tmLeft, tmTop, tmRight, tmBottom; // Harm. means of transm.

    // The current cell has no no flow boundaries. All neighbours have
    // valid values.

    // Cell to the left.
    tmLeft = d_tmRight.cell(r, c - 1);
    d_args.d_h1 += tmLeft * d_elevation.value(r, c - 1);
    d_args.d_h2 += tmLeft * d_resultElevation.value(r, c - 1);
    ++d_args.d_nrCells;

    // Cell to the top.
    tmTop = d_tmBottom.cell(r - 1, c);
    d_args.d_h1 += tmTop * d_elevation.value(r - 1, c);
    d_args.d_h2 += tmTop * d_resultElevation.value(r - 1, c);
    ++d_args.d_nrCells;

    // Cell to the right.
    tmRight = d_tmRight.cell(r, c);
    d_args.d_h1 += tmRight * d_elevation.value(r, c + 1);
    d_args.d_h2 += tmRight * d_resultElevation.value(r, c + 1);
    ++d_args.d_nrCells;

    // Cell to the bottom.
    tmBottom = d_tmBottom.cell(r, c);
    d_args.d_h1 += tmBottom * d_elevation.value(r + 1, c);
    d_args.d_h2 += tmBottom * d_resultElevation.value(r + 1, c);
    ++d_args.d_nrCells;

    PRECOND(d_args.d_nrCells > 0);
    PRECOND(d_args.d_nrCells == 4);

    d_args.d_currentElevation = d_elevation.value(r, c);
    d_args.d_recharge = d_recharge.value(r, c);
    // Arithmic mean.
    d_args.d_transmissivity = (tmLeft + tmTop + tmRight + tmBottom) /
                   d_args.d_nrCells;
    d_args.d_storageCoefficient = d_storageCoefficient.value(r, c);

    return calc::calcElevation(d_args);
  }

public:

  DiffuseAlgorithm(fieldapi::ReadWriteReal8& resultElevation,
         const fieldapi::ReadOnlyReal8& elevation,
         const fieldapi::ReadOnlyReal8& recharge,
         const fieldapi::ReadOnlyReal8& transmissivity,
         const fieldapi::ReadOnlyInt4& flowCondition,
         const fieldapi::ReadOnlyReal8& storageCoefficient,
         double area, double alpha, double interval)

    : d_resultElevation(resultElevation), d_elevation(elevation),
      d_recharge(recharge), d_transmissivity(transmissivity),
      d_flowCondition(flowCondition), d_storageCoefficient(storageCoefficient),
      d_tmRight(resultElevation.nrRows(), resultElevation.nrCols()),
      d_tmBottom(resultElevation.nrRows(), resultElevation.nrCols()),
      d_noFlowBoundaries(resultElevation.nrRows(), resultElevation.nrCols(),
      static_cast<const unsigned int>(NoDirection))

  {
    d_args.d_area = area;
    d_args.d_alpha = alpha;
    d_args.d_interval = interval;

    geo::CellLoc loc;
    size_t r, c; // comes from C ya know

    //--------------------------------------------------------------------------
    // Calculate transmissivities. These stay constant within a call to the
    // function and are needed for each iteration so we calculate them here
    // once. We trade memory space for speed.

    // Harmonic mean transmissivities current and right cell.
    // For every row.
    for( r = 0; r < d_tmRight.nrRows(); ++r) {

      loc.setRow(r);

      // For every but the last col.
      for( c = 0; c < d_tmRight.nrCols() - 1; ++c) {

        loc.setCol(c);

        if(!d_transmissivity.isMV(loc) &&
                   !d_transmissivity.isMV(geo::CellLoc(r, c + 1))) {
          d_tmRight.cell(r, c) =
              2 * d_transmissivity.value(r, c + 1) *
              d_transmissivity.value(r, c) /
              (d_transmissivity.value(r, c + 1) + d_transmissivity.value(r, c));
        }
      }
    }

    // Harmonic mean transmissivities current and bottom cell.
    // For every but the last row.
    for( r = 0; r < d_tmBottom.nrRows() - 1; ++r) {

      loc.setRow(r);

      // For every col.
      for( c = 0; c < d_tmBottom.nrCols(); ++c) {

        loc.setCol(c);

        if(!d_transmissivity.isMV(loc) &&
                   !d_transmissivity.isMV(geo::CellLoc(r + 1, c))) {
          d_tmBottom.cell(r, c) =
              2 * d_transmissivity.value(r + 1, c) *
              d_transmissivity.value(r, c) /
              (d_transmissivity.value(r + 1, c) + d_transmissivity.value(r, c));
        }
      }
    }

    //--------------------------------------------------------------------------
    // Calculate no flow boundaries. These stay constant within a call to the
    // function and are needed for each iteration so we calculate them here
    // once. We trade memory space for speed.
    // d_noFlowBoundaries is already initialized with NoDirection.
    // A bounding cell is regarded as a now flow boundary if:
    // - it is missing (border cell of the raster)
    // - it contains a missing value
    // - if its flow condition is NOFLOW

    PRECOND(d_noFlowBoundaries.nrRows() >= 1);
    PRECOND(d_noFlowBoundaries.nrCols() >= 1);

    // For every but the first and last row.
    for( r = 1; r < d_noFlowBoundaries.nrRows() - 1; ++r) {

      // For every but the first and last col.
      for( c = 1; c < d_noFlowBoundaries.nrCols() - 1; ++c) {

        // Cell to the left.
        loc.setRow(r);
        loc.setCol(c - 1);
        if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r, c - 1) == NOFLOW) {
          d_noFlowBoundaries.cell(r, c) |= Left;
        }

        // Cell to the top.
        loc.setRow(r - 1);
        loc.setCol(c);
        if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r - 1, c) == NOFLOW) {
          d_noFlowBoundaries.cell(r, c) |= Top;
        }

        // Cell to the right.
        loc.setRow(r);
        loc.setCol(c + 1);
        if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r, c + 1) == NOFLOW) {
          d_noFlowBoundaries.cell(r, c) |= Right;
        }

        // Cell to the bottom.
        loc.setRow(r + 1);
        loc.setCol(c);
        if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r + 1, c) == NOFLOW) {
          d_noFlowBoundaries.cell(r, c) |= Bottom;
        }
      }
    }

    // Handle no-flow conditions on the borders of the area. The value of head
    // along the fictious column reflects across the border.

    // Process borders of raster.

    // First col.
    c = 0;

    // For every but the first and last row.
    for(r = 1; r < d_noFlowBoundaries.nrRows() - 1; ++r) {

      // Cell to the left.
      d_noFlowBoundaries.cell(r, c) |= Left;

      // Cell to the top.
      loc.setRow(r - 1);
      loc.setCol(c);
      if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r - 1, c) == NOFLOW) {
        d_noFlowBoundaries.cell(r, c) |= Top;
      }

      // Cell to the right.
      loc.setRow(r);
      loc.setCol(c + 1);
      if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r, c + 1) == NOFLOW) {
        d_noFlowBoundaries.cell(r, c) |= Right;
      }

      // Cell to the bottom.
      loc.setRow(r + 1);
      loc.setCol(c);
      if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                 d_flowCondition.value(r + 1, c) == NOFLOW) {
        d_noFlowBoundaries.cell(r, c) |= Bottom;
      }
    }

    // Last col.
    c = d_noFlowBoundaries.nrCols() - 1;

    // For every but the first and last row.
    for(r = 1; r < d_noFlowBoundaries.nrRows() - 1; ++r) {

      // Cell to the left.
      loc.setRow(r);
      loc.setCol(c - 1);
      if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r, c - 1) == NOFLOW) {
        d_noFlowBoundaries.cell(r, c) |= Left;
      }

      // Cell to the top.
      loc.setRow(r - 1);
      loc.setCol(c);
      if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r - 1, c) == NOFLOW) {
        d_noFlowBoundaries.cell(r, c) |= Top;
      }

      // Cell to the right.
      d_noFlowBoundaries.cell(r, c) |= Right;

      // Cell to the bottom.
      loc.setRow(r + 1);
      loc.setCol(c);
      if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                 d_flowCondition.value(r + 1, c) == NOFLOW) {
        d_noFlowBoundaries.cell(r, c) |= Bottom;
      }
    }

    // First row.
    r = 0;

    // For every but the first and last col.
    for(c = 1; c < d_noFlowBoundaries.nrCols() - 1; ++c) {

      // Cell to the left.
      loc.setRow(r);
      loc.setCol(c - 1);
      if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r, c - 1) == NOFLOW) {
        d_noFlowBoundaries.cell(r, c) |= Left;
      }

      // Cell to the top.
      d_noFlowBoundaries.cell(r, c) |= Top;

      // Cell to the right.
      loc.setRow(r);
      loc.setCol(c + 1);
      if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r, c + 1) == NOFLOW) {
        d_noFlowBoundaries.cell(r, c) |= Right;
      }

      // Cell to the bottom.
      loc.setRow(r + 1);
      loc.setCol(c);
      if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                 d_flowCondition.value(r + 1, c) == NOFLOW) {
        d_noFlowBoundaries.cell(r, c) |= Bottom;
      }
    }

    // Last row.
    r = d_noFlowBoundaries.nrRows() - 1;

    // For every but the first and last col.
    for(c = 1; c < d_noFlowBoundaries.nrCols() - 1; ++c) {

      // Cell to the left.
      loc.setRow(r);
      loc.setCol(c - 1);
      if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r, c - 1) == NOFLOW) {
        d_noFlowBoundaries.cell(r, c) |= Left;
      }

      // Cell to the top.
      loc.setRow(r - 1);
      loc.setCol(c);
      if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r - 1, c) == NOFLOW) {
        d_noFlowBoundaries.cell(r, c) |= Top;
      }

      // Cell to the right.
      loc.setRow(r);
      loc.setCol(c + 1);
      if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r, c + 1) == NOFLOW) {
        d_noFlowBoundaries.cell(r, c) |= Right;
      }

      // Cell to the bottom.
      d_noFlowBoundaries.cell(r, c) |= Bottom;
    }

    // Upper left corner.
    r = 0;
    c = 0;

    d_noFlowBoundaries.cell(r, c) |= UpperLeft;

    // Cell to the right.
    loc.setRow(r);
    loc.setCol(c + 1);
    if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r, c + 1) == NOFLOW) {
      d_noFlowBoundaries.cell(r, c) |= Right;
    }

    // Cell to the bottom.
    loc.setRow(r + 1);
    loc.setCol(c);
    if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                 d_flowCondition.value(r + 1, c) == NOFLOW) {
      d_noFlowBoundaries.cell(r, c) |= Bottom;
    }

    // Upper right corner.
    r = 0;
    c = d_noFlowBoundaries.nrCols() - 1;

    d_noFlowBoundaries.cell(r, c) |= UpperRight;

    // Cell to the left.
    loc.setRow(r);
    loc.setCol(c - 1);
    if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r, c - 1) == NOFLOW) {
      d_noFlowBoundaries.cell(r, c) |= Left;
    }

    // Cell to the bottom.
    loc.setRow(r + 1);
    loc.setCol(c);
    if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                 d_flowCondition.value(r + 1, c) == NOFLOW) {
      d_noFlowBoundaries.cell(r, c) |= Bottom;
    }

    // Lower right corner.
    r = d_noFlowBoundaries.nrRows() - 1;
    c = d_noFlowBoundaries.nrCols() - 1;

    d_noFlowBoundaries.cell(r, c) |= LowerRight;

    // Cell to the left.
    loc.setRow(r);
    loc.setCol(c - 1);
    if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r, c - 1) == NOFLOW) {
      d_noFlowBoundaries.cell(r, c) |= Left;
    }

    // Cell to the top.
    loc.setRow(r - 1);
    loc.setCol(c);
    if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r - 1, c) == NOFLOW) {
      d_noFlowBoundaries.cell(r, c) |= Top;
    }

    // Lower left corner.
    r = d_noFlowBoundaries.nrRows() - 1;
    c = 0;

    d_noFlowBoundaries.cell(r, c) |= LowerLeft;

    // Cell to the top.
    loc.setRow(r - 1);
    loc.setCol(c);
    if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r - 1, c) == NOFLOW) {
      d_noFlowBoundaries.cell(r, c) |= Top;
    }

    // Cell to the right.
    loc.setRow(r);
    loc.setCol(c + 1);
    if(d_elevation.isMV(loc) || d_flowCondition.isMV(loc) ||
                   d_flowCondition.value(r, c + 1) == NOFLOW) {
      d_noFlowBoundaries.cell(r, c) |= Right;
    }
  }

//! Calculates the new elevation at a cell position.
/*!
  \param     r Row of cell.
  \param     c Column of cell.
  \return    Elevation.
  \warning   All input variables and d_resultElevation must have a valid value
             at \a r, \a c. This is forced by checking d_resultElevation.
*/
  double calcElevation(size_t r, size_t c)
  {
    PRECOND(!d_resultElevation.isMV(geo::CellLoc(r, c)));

    if(d_noFlowBoundaries.cell(r, c) != NoDirection) {
      return calcElevationWithNoFlowBoundary(r, c, d_noFlowBoundaries.cell(r, c));
    }
    else {
      return calcElevationWithoutNoFlowBoundary(r, c);
    }
  }

};

} // namespace calc.

//!
/*!
  \param     .
  \return    .
  \exception .
  \warning   .
  \sa        .

  Elevation                  scalar [ L       ]
  Recharge                   scalar [ L , T-1 ]
  Transmissivity             scalar [ L2, T-1 ]
  Storage coefficient        scalar [ L3, L-3 ]
  Interval                   scalar [ T       ]       > 0.0

  Issues: cell length, error messages and handling of return values
*/
extern "C" int  Transient(void** out, const void** in, int nrArgs)
{
  using namespace calc; // hack hack
  PRECOND(nrArgs == 7);
  (void)nrArgs; // shut up compiler

  ReadWriteReal8_ref(resultElevation, static_cast<MAP_REAL8*>(out[0]));
  ReadOnlyReal8_ref(elevation, static_cast<const MAP_REAL8*>(in[0]));
  ReadOnlyReal8_ref(recharge, static_cast<const MAP_REAL8*>(in[1]));
  ReadOnlyReal8_ref(transmissivity, static_cast<const MAP_REAL8*>(in[2]));
  ReadOnlyInt4_ref(flowCondition, static_cast<const MAP_INT4*>(in[3]));
  ReadOnlyReal8_ref(storageCoefficient, static_cast<const MAP_REAL8*>(in[4]));
  ReadOnlyReal8_ref(intervalInterface, static_cast<const MAP_REAL8*>(in[5]));
  ReadOnlyReal8_ref(toleranceInterface, static_cast<const MAP_REAL8*>(in[6]));

  PRECOND(!intervalInterface.spatial());
  PRECOND(!toleranceInterface.spatial());

  std::vector<const fieldapi::Common*> inputs;
  inputs.push_back(&elevation);
  inputs.push_back(&recharge);
  inputs.push_back(&transmissivity);
  inputs.push_back(&flowCondition);
  inputs.push_back(&storageCoefficient);
  inputs.push_back(&intervalInterface);
  inputs.push_back(&toleranceInterface);
  PRECOND(inputs.size() == static_cast<size_t>(nrArgs));

  size_t nrRows = elevation.nrRows();
  size_t nrCols = elevation.nrCols();
  size_t r,c;
  PRECOND(nrRows >= 2 && nrCols >= 2);

  // Domain checks...
  std::vector<fieldapi::ScalarDomainCheck> scalarDomains, nsDomains;
  scalarDomains.push_back(fieldapi::ScalarDomainCheck(transmissivity,
                   "transmissivity", com::GreaterThan<double>(0)));
  scalarDomains.push_back(fieldapi::ScalarDomainCheck(storageCoefficient,
                   "storage coefficient", com::GreaterThan<double>(0)));
  nsDomains.push_back(fieldapi::ScalarDomainCheck(intervalInterface,
                   "interval", com::GreaterThan<double>(0)));
  nsDomains.push_back(fieldapi::ScalarDomainCheck(toleranceInterface,
                   "tolerance", com::GreaterThan<double>(0)));

  // Checks...
  int check;

  // Check non spatials.
  check = fieldapi::checkScalarDomains(nsDomains, geo::CellLoc(0, 0));
  if(check != -1) {
    return RetError(1, nsDomains[check].msg().c_str());
  }

  // Check spatials.
  for(geo::CellLocVisitor loc(elevation); loc.valid(); ++loc) {
    check = fieldapi::checkScalarDomains(scalarDomains, *loc);
    if(check != -1) {
      return RetError(1, scalarDomains[check].msg().c_str());
    }
  }

  // Inititalize...
  double oldValue;
  double tolerance = toleranceInterface.value(0, 0);
  double difference, maxDifference;    // Current and max difference.
  size_t nrIterations = 0;

  // yepyep: fieldapi::Common::cellLength();
  MAP_REAL8* map = static_cast<MAP_REAL8*>(out[0]);
  double cellLength = map->CellLength(map);
  PRECOND(cellLength > 0.0);

  // Result elevation is missing value if any of the inputs is MV.
  for(geo::CellLocVisitor visitor(elevation); visitor.valid(); ++visitor) {

    if(fieldapi::nonMV(inputs, *visitor)) {
      resultElevation.copy(elevation, *visitor);
    }
    else {
      resultElevation.putMV(*visitor);
    }
  }

  DiffuseAlgorithm algorithm(resultElevation, elevation, recharge,
                   transmissivity, flowCondition, storageCoefficient,
                   cellLength * cellLength, 0.5, intervalInterface.value(0, 0));
  // std::cout << std::endl;

  // Algorithm...
  // Loop over all cells in the raster untill the maximum difference between
  // the current elevation and the new elevation is smaller than the tolerance.
  do {
    maxDifference = 0.0;

    for( r = 0; r < nrRows; ++r) {
      for( c = 0; c < nrCols; ++c) {

        // Skip missing values.
        if(resultElevation.isMV(geo::CellLoc(r, c))) {
          continue;
        }

        // Remember current elevation.
        oldValue = resultElevation.value(r, c);

        // Select calculation method for current cell, based on flow condition.
        if(flowCondition.value(r, c) == DiffuseAlgorithm::CALCULATE) {
          resultElevation.put(algorithm.calcElevation(r, c), r, c);
        }
        // (else FIXED, NOFLOW)

        // Determine difference.
        difference = ABS(resultElevation.value(r, c) - oldValue);
        if(difference > maxDifference) {
          maxDifference = difference;
        }
      }
    }

    ++nrIterations;

    // yepyep: test op aantal iteraties?
    // PRECOND(nrIterations < 30);
    // std::cout << '.' << std::flush;

  } while (maxDifference > tolerance);

  // std::cout << ": " << nrIterations << std::endl;

  return 0;
}


#include "StarKinematics.h"
#include "StarGenerator/EVENT/StarGenEvent.h"
#include "StarGenerator/EVENT/StarGenParticle.h"
#include "StarGenerator/UTIL/StarParticleData.h"
#include "StarGenerator/UTIL/StarRandom.h"

#include <boost/algorithm/string.hpp>
#include <vector>
#include <string>
#include <algorithm>
#include <random>

#define random myrandom

// ----------------------------------------------------------------------------

StarParticleData &data = StarParticleData::instance();
StarRandom       &random = StarRandom::Instance();

static std::random_device _stl_rd;
static std::mt19937 _mt19937( _stl_rd() );

StarGenEvent *gEvent = 0;
StarGenEvent *gUser  = 0;

// ----------------------------------------------------------------------------
StarKinematics::StarKinematics( const Char_t *name ) : StarGenerator(name)
{
  mEvent = new StarGenEvent("kine"); gEvent = mEvent;
  mUser  = new StarGenEvent("user"); gUser  = mUser;
}
// ----------------------------------------------------------------------------
Int_t StarKinematics::PreGenerate()
{
  return kStOK;
}
// ----------------------------------------------------------------------------
Int_t StarKinematics::Generate()
{

  // Copy user event into mEvent
  for ( Int_t i=0;i<mUser->GetNumberOfParticles(); i++ )
    {
      mEvent -> AddParticle( (*mUser)[i] );
    }

  // And clear the user event
  mUser->Clear();

  return kStOK;
}
// ----------------------------------------------------------------------------
Int_t StarKinematics::PostGenerate()
{
  return kStOK;
}
// ----------------------------------------------------------------------------
StarGenParticle *StarKinematics::AddParticle()
{
  StarGenParticle *p = mUser->AddParticle();
  p->SetStatus( StarGenParticle::kFinal );
  return p;
}
// ----------------------------------------------------------------------------
StarGenParticle *StarKinematics::AddParticle( const Char_t *type )
{
  TParticlePDG *pdg = data(type); assert(pdg);
  Int_t id = pdg->PdgCode();
  StarGenParticle *p = AddParticle();
  p->SetStatus( StarGenParticle::kFinal );
  p->SetMass( pdg->Mass() );
  p->SetId( id );
  return p;
}
// ----------------------------------------------------------------------------
void StarKinematics::Kine(Int_t ntrack, const Char_t *_type, Double_t ptlow, Double_t pthigh,
			  Double_t ylow, Double_t yhigh,
			  Double_t philow, Double_t phihigh )
{

  std::string type = _type;
  std::vector<std::string> types;
  boost::split( types, type,  [](char c){ return (c==' ' || c== ',');} );

  for ( Int_t i=0;i<ntrack;i++ )
    {

      std::shuffle( types.begin(), types.end(), _mt19937 );
      type = types[0];

      StarGenParticle *p = AddParticle(type.c_str());

      // Sample pt, eta, phi
      if ( 0 == IAttr("energy") ) { // uniform in pT
	double pt = random(ptlow,  pthigh);
	double y  = random(ylow,   yhigh );
	double phi= random(philow, phihigh );
	double m  = p->GetMass();

	double mt = pt;
	if ( IAttr("rapidity" ) ) {
	  // switch from pseudorapidity to plain vanilla rapidity
	  mt = TMath::Sqrt( pt*pt + m*m );
	}

	double px = pt * TMath::Cos( phi );
	double py = pt * TMath::Sin( phi );
	double pz = mt * TMath::SinH( y );
	double E  = mt * TMath::CosH( y );

	p->SetPx( px );
	p->SetPy( py );
	p->SetPz( pz );
	p->SetEnergy( E );

	p->SetVx( 0. ); // put vertex at 0,0,0,0
	p->SetVy( 0. );
	p->SetVz( 0. );
	p->SetTof( 0. );
      }
      if ( IAttr("energy") ) { // uniform in energy.

	assert( 0==IAttr("rapidity") ); // only flat in eta, not rapidity

	double E  = random(ptlow, pthigh);
	double y  = random(ylow,   yhigh );   // y is eta here, not rapidity
	double phi= random(philow, phihigh );
	double m  = p->GetMass();
	double pmom  = TMath::Sqrt(E*E - m*m);
	double pt = 2.0 * pmom * TMath::Exp( -y ) / ( 1 + TMath::Exp( -2.0*y ) );

	double mt = pt;
	if ( IAttr("rapidity" ) ) {
	  // switch from pseudorapidity to plain vanilla rapidity
	  mt = TMath::Sqrt( pt*pt + m*m );
	}

	double px = pt * TMath::Cos( phi );
	double py = pt * TMath::Sin( phi );
	double pz = mt * TMath::SinH( y );
	//double E  = mt * TMath::CosH( y );

	p->SetPx( px );
	p->SetPy( py );
	p->SetPz( pz );
	p->SetEnergy( E );

	p->SetVx( 1e-11); // put vertex at 0,0,0,0
	p->SetVy( 1e-11);
	p->SetVz( 1e-11);
	p->SetTof( 0. );
      }

    }

}
// ----------------------------------------------------------------------------
void StarKinematics::Dist( Int_t ntrack, const Char_t *_type, TF1 *ptFunc, TF1 *etaFunc, TF1 *phiFunc )
{

  std::string type = _type;
  std::vector<std::string> types;
  boost::split( types, type,  [](char c){ return (c==' ' || c== ',');} );

  for ( Int_t i=0; i<ntrack; i++ )
    {

      std::shuffle( types.begin(), types.end(), _mt19937 );
      type = types[0];

      StarGenParticle *p = AddParticle(type.c_str());

      Double_t pt  = ptFunc  -> GetRandom();
      Double_t y   = etaFunc -> GetRandom();
      Double_t phi = (phiFunc) ? phiFunc->GetRandom() : random( 0., TMath::TwoPi() );
      Double_t m   = p->GetMass();

      double mt = pt;
      if ( IAttr("rapidity" ) ) {
	// switch from pseudorapidity to plain vanilla rapidity
	mt = TMath::Sqrt( pt*pt + m*m );
      }

      double px = pt * TMath::Cos( phi );
      double py = pt * TMath::Sin( phi );
      double pz = mt * TMath::SinH( y );
      double E  = mt * TMath::CosH( y );

      p->SetPx( px );
      p->SetPy( py );
      p->SetPz( pz );
      p->SetEnergy( E );

      p->SetVx( 0. ); // put vertex at 0,0,0,0
      p->SetVy( 0. );
      p->SetVz( 0. );
      p->SetTof( 0. );

    }
}
// ----------------------------------------------------------------------------
void StarKinematics::Dist( Int_t ntrack, const Char_t *_type, TH1 *ptFunc, TH1 *etaFunc, TH1 *phiFunc )
{
  std::string type = _type;
  std::vector<std::string> types;
  boost::split( types, type,  [](char c){ return (c==' ' || c== ',');} );

  for ( Int_t i=0; i<ntrack; i++ )
    {

      std::shuffle( types.begin(), types.end(), _mt19937 );
      type = types[0];

      StarGenParticle *p = AddParticle(type.c_str());

      Double_t pt  = ptFunc  -> GetRandom();
      Double_t y   = etaFunc -> GetRandom();
      Double_t phi = (phiFunc) ? phiFunc->GetRandom() : random( 0., TMath::TwoPi() );
      Double_t m   = p->GetMass();

      double mt = pt;
      if ( IAttr("rapidity" ) ) {
	// switch from pseudorapidity to plain vanilla rapidity
	mt = TMath::Sqrt( pt*pt + m*m );
      }

      double px = pt * TMath::Cos( phi );
      double py = pt * TMath::Sin( phi );
      double pz = mt * TMath::SinH( y );
      double E  = mt * TMath::CosH( y );

      p->SetPx( px );
      p->SetPy( py );
      p->SetPz( pz );
      p->SetEnergy( E );

      p->SetVx( 0. ); // put vertex at 0,0,0,0
      p->SetVy( 0. );
      p->SetVz( 0. );
      p->SetTof( 0. );

    }
}
// ----------------------------------------------------------------------------
const double deg2rad = TMath::DegToRad();

void StarKinematics::Cosmic( int ntrack, const char* _type, double plow, double phigh, double radius, double zmin, double zmax, double dphi )
{
  std::string type = _type;
  std::vector<std::string> types;
  boost::split( types, type,  [](char c){ return (c==' ' || c== ',');} );

  for ( Int_t i=0; i<ntrack; i++ )
    {

      std::shuffle( types.begin(), types.end(), _mt19937 );
      type = types[0];

      StarGenParticle *p = AddParticle(type.c_str());

      // Generate a random vertex
      double zvertex = random( zmin, zmax );
      double phi     = random( 0.0, TMath::TwoPi() );
      double xvertex = radius * TMath::Cos(phi);
      double yvertex = radius * TMath::Sin(phi);

      xvertex *= 10; // cm --> mm per HEPEVT standard
      yvertex *= 10; // cm --> mm
      zvertex *= 10; // cm --> mm

      // Initialize vertex X,Y ... to get unit vector pointing to beam line
      TVector3 vertex(xvertex,yvertex,0);

      // Unit vector pointing away from beamline
      TVector3 direct = vertex.Unit();

      // Sample momentum distribution
      double pmag    = random(plow,  phigh);

      // Momentum vector pointing in towards the beamline
      _momentum = -pmag * direct;

      // Now, randomize phi and theta by +/- dphi degrees about the momentum axis
             phi   = _momentum.Phi()   + deg2rad * random( -dphi, +dphi );
      double theta = _momentum.Theta() + deg2rad * random( -dphi, +dphi );

      _momentum.SetPhi(phi);
      _momentum.SetTheta(theta);

      Double_t m   = p->GetMass();

      Double_t E2 = _momentum.Mag2() + m*m;
      Double_t E  = sqrt(E2);

      p->SetPx( _momentum.Px() );
      p->SetPy( _momentum.Py() );
      p->SetPz( _momentum.Pz() );
      p->SetEnergy( E );

      p->SetVx( xvertex );
      p->SetVy( yvertex );
      p->SetVz( zvertex );
      p->SetTof( 0. );

    }
}
// ----------------------------------------------------------------------------


// Copyright 2014 Jonathan Graehl - http://graehl.org/
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef GRAEHL_SHARED__POWER_OF_10_HPP
#define GRAEHL_SHARED__POWER_OF_10_HPP

//FIXME: according to docs, push/pop should work.  but in 4.3 it has no effect.
#include <graehl/shared/warning_push.h>

#pragma GCC diagnostic ignored "-Woverflow"
//anon namespace - so no need to define in just one linkage

/* usage: powers_of_10<Numeric>::max_exact - no rounding error, can be represented
   powers_of_10<Numeric>::max_any - largest index that can be given to below table (22).  for int types, higher powers are modulo 2^n as usual (overflow)
   powers_of_10<Numeric>::pow10[i] - 10^i for nonnegative i.  for floating point types, use pow10 fn if you want more

   defined for Numeric float,double,(u)int {8,16,32,64}_t
*/
#include <boost/cstdint.hpp>

// these two lists could be generated via a combinator loop thingy: FOLDC22C(F,C,Z) =F(C,... F(C,F(C,Z))...)

#define TIMES0TIMES(x) 1
#define TIMES1TIMES(x) x
#define TIMES2TIMES(x) TIMES1TIMES(x)*x
#define TIMES3TIMES(x) TIMES2TIMES(x)*x
#define TIMES4TIMES(x) TIMES3TIMES(x)*x
#define TIMES5TIMES(x) TIMES4TIMES(x)*x
#define TIMES6TIMES(x) TIMES5TIMES(x)*x
#define TIMES7TIMES(x) TIMES6TIMES(x)*x
#define TIMES8TIMES(x) TIMES7TIMES(x)*x
#define TIMES9TIMES(x) TIMES8TIMES(x)*x
#define TIMES10TIMES(x) TIMES9TIMES(x)*x
#define TIMES11TIMES(x) TIMES10TIMES(x)*x
#define TIMES12TIMES(x) TIMES11TIMES(x)*x
#define TIMES13TIMES(x) TIMES12TIMES(x)*x
#define TIMES14TIMES(x) TIMES13TIMES(x)*x
#define TIMES15TIMES(x) TIMES14TIMES(x)*x
#define TIMES16TIMES(x) TIMES15TIMES(x)*x
#define TIMES17TIMES(x) TIMES16TIMES(x)*x
#define TIMES18TIMES(x) TIMES17TIMES(x)*x
#define TIMES19TIMES(x) TIMES18TIMES(x)*x
#define TIMES20TIMES(x) TIMES19TIMES(x)*x
#define TIMES21TIMES(x) TIMES20TIMES(x)*x
#define TIMES22TIMES(x) TIMES21TIMES(x)*x

#define LIST_POWERS_0_BIG(x)                    \
    TIMES0TIMES(x) ,                            \
      TIMES1TIMES(x) ,                          \
      TIMES2TIMES(x) ,                          \
      TIMES3TIMES(x) ,                          \
      TIMES4TIMES(x) ,                          \
      TIMES5TIMES(x) ,                          \
      TIMES6TIMES(x) ,                          \
      TIMES7TIMES(x) ,                          \
      TIMES8TIMES(x) ,                          \
      TIMES9TIMES(x) ,                          \
      TIMES10TIMES(x) ,                         \
      TIMES11TIMES(x) ,                         \
      TIMES12TIMES(x) ,                         \
      TIMES13TIMES(x) ,                         \
      TIMES14TIMES(x) ,                         \
      TIMES15TIMES(x) ,                         \
      TIMES16TIMES(x) ,                         \
      TIMES17TIMES(x) ,                         \
      TIMES18TIMES(x) ,                         \
      TIMES19TIMES(x) ,                         \
      TIMES20TIMES(x) ,                         \
      TIMES21TIMES(x) ,                         \
      TIMES22TIMES(x)

#define LIST_POWERS_BIGEXP 22

// these powers
#define DECLARE_POWERS_OF_10(F, M)                  \
  template <>                                       \
  struct power_of_10<F> {                           \
    static const int max_exact = M;                 \
    static const int n_exact = max_exact+1;         \
    static const int max_any = LIST_POWERS_BIGEXP;  \
    static const int n_any = max_any+1;             \
    static const F ten = 10;                        \
    static const F pow10[n_any];                    \
  };

#define DEFINE_POWERS_OF_10(F, M)                                       \
  const F power_of_10<F>::pow10[power_of_10<F>::n_any]= { LIST_POWERS_0_BIG(power_of_10<F>::ten) };

#define MAKE_POWERS_OF_10(F, M)                 \
  DECLARE_POWERS_OF_10(F, M)                    \
  DEFINE_POWERS_OF_10(F, M)

namespace {

template <class Float> struct power_of_10 {};

MAKE_POWERS_OF_10(int8_t, 2)
MAKE_POWERS_OF_10(uint8_t, 2)
MAKE_POWERS_OF_10(int16_t, 4)
MAKE_POWERS_OF_10(uint16_t, 4)
MAKE_POWERS_OF_10(int32_t, 9)
MAKE_POWERS_OF_10(uint32_t, 9)
MAKE_POWERS_OF_10(int64_t, 18)
MAKE_POWERS_OF_10(uint64_t, 18)
MAKE_POWERS_OF_10(float, 11) //FIXME: just a guess - haven't tested.
MAKE_POWERS_OF_10(double, 22) // 10^22 = 0x1.0f0cf064dd592p73 is the largest exactly representable power of 10 in the IEEE binary64 format

}//ns

#include <graehl/shared/warning_pop.h>

#endif


/*=============================================================================
    Copyright (C) 1999-2003 Jaakko Jarvi
    Copyright (c) 2001-2011 Joel de Guzman

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
==============================================================================*/
#include <boost/detail/lightweight_test.hpp>
#include <boost/fusion/sequence/intrinsic.hpp>
#include <boost/fusion/support/is_sequence.hpp>
#include <boost/fusion/mpl.hpp>
#include <boost/mpl/find.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/is_sequence.hpp>
#include <boost/type_traits/is_same.hpp>
#include <string>

#if !defined(FUSION_AT)
#define FUSION_AT at_c
#endif

#if !defined(FUSION_SIZE)
#define FUSION_SIZE boost::fusion::result_of::size
#endif

template <typename S1, typename S2>
struct is_same
{
};

struct test_intrinsics1
{
    // test at, begin, end, next, prior, advance, size, deref, etc.

    typedef boost::fusion::FUSION_SEQUENCE<int, float, bool, char> sequence;
    typedef boost::mpl::begin<sequence>::type first;
    typedef boost::mpl::next<first>::type second;
    typedef boost::mpl::next<second>::type third;
    typedef boost::mpl::next<third>::type fourth;
    typedef boost::mpl::end<sequence>::type last;

    BOOST_STATIC_ASSERT((boost::is_same<
        boost::mpl::deref<first>::type, int>::value));

    BOOST_STATIC_ASSERT((boost::is_same<
        boost::mpl::deref<second>::type, float>::value));

    BOOST_STATIC_ASSERT((boost::is_same<
        boost::mpl::deref<third>::type, bool>::value));

    BOOST_STATIC_ASSERT((boost::is_same<
        boost::mpl::deref<fourth>::type, char>::value));

    BOOST_STATIC_ASSERT((boost::is_same<
        boost::mpl::at_c<sequence, 2>::type, bool>::value));

    BOOST_STATIC_ASSERT((boost::is_same<
        boost::mpl::front<sequence>::type, int>::value));

    BOOST_STATIC_ASSERT((boost::is_same<
        boost::mpl::deref<
            boost::mpl::advance_c<second, 2>::type>::type, char>::value));

    BOOST_STATIC_ASSERT((boost::mpl::size<sequence>::value == 4));
    BOOST_STATIC_ASSERT(!(boost::mpl::empty<sequence>::value));
    BOOST_STATIC_ASSERT((boost::mpl::distance<second, fourth>::value == 2));

#if !defined(FUSION_FORWARD_ONLY) // list has no back/prev

    typedef boost::mpl::prior<last>::type fourth_;
    typedef boost::mpl::prior<fourth_>::type third_;
    typedef boost::mpl::prior<third_>::type second_;
    typedef boost::mpl::prior<second_>::type first_;

    BOOST_STATIC_ASSERT((boost::is_same<
        boost::mpl::deref<first_>::type, int>::value));

    BOOST_STATIC_ASSERT((boost::is_same<
        boost::mpl::deref<second_>::type, float>::value));

    BOOST_STATIC_ASSERT((boost::is_same<
        boost::mpl::deref<third_>::type, bool>::value));

    BOOST_STATIC_ASSERT((boost::is_same<
        boost::mpl::deref<fourth_>::type, char>::value));

    BOOST_STATIC_ASSERT((boost::is_same<
        boost::mpl::back<sequence>::type, char>::value));

#endif
};

struct test_intrinsics2
{
    typedef boost::fusion::FUSION_SEQUENCE<> seq0;

#if !defined(BOOST_FUSION_SEQUENCE_CONVERSION_IS_NOT_SEQUENCE__TYPE_PRESERVING)
#if !defined(FUSION_FORWARD_ONLY) // list has no back/prev

    typedef boost::fusion::FUSION_SEQUENCE<int> target1;
    typedef boost::mpl::push_back<seq0, int>::type seq1;
    BOOST_STATIC_ASSERT((boost::mpl::equal<seq1, target1>::value));

    typedef boost::fusion::FUSION_SEQUENCE<int, double> target2;
    typedef boost::mpl::push_back<seq1, double>::type seq2;
    BOOST_STATIC_ASSERT((boost::mpl::equal<seq2, target2>::value));

#endif

    typedef boost::fusion::FUSION_SEQUENCE<int> target3;
    typedef boost::mpl::push_front<seq0, int>::type seq3;
    BOOST_STATIC_ASSERT((boost::mpl::equal<seq3, target3>::value));

    typedef boost::fusion::FUSION_SEQUENCE<double, int> target4;
    typedef boost::mpl::push_front<seq3, double>::type seq4;
    BOOST_STATIC_ASSERT((boost::mpl::equal<seq4, target4>::value));

#endif
};

void
test()
{
    using namespace boost::fusion;

    {   // testing const sequences

        const FUSION_SEQUENCE<int, float> t1(5, 3.3f);
        BOOST_TEST(FUSION_AT<0>(t1) == 5);
        BOOST_TEST(FUSION_AT<1>(t1) == 3.3f);
    }

    {   // testing at<N> works with MPL integral constants
        const FUSION_SEQUENCE<int, char> t1(101, 'z');
        BOOST_TEST(boost::fusion::at<boost::mpl::int_<0> >(t1) == 101);
        BOOST_TEST(boost::fusion::at<boost::mpl::int_<1> >(t1) == 'z');
        // explicitly try something other than mpl::int_
        BOOST_TEST((boost::fusion::at<boost::mpl::integral_c<long, 0> >(t1) == 101));
        BOOST_TEST((boost::fusion::at<boost::mpl::integral_c<long, 1> >(t1) == 'z'));
    }

    {   // testing size & empty

        typedef FUSION_SEQUENCE<int, float, double> t1;
        typedef FUSION_SEQUENCE<> t2;

        BOOST_STATIC_ASSERT(FUSION_SIZE<t1>::value == 3);
        BOOST_STATIC_ASSERT(FUSION_SIZE<t2>::value == 0);
        BOOST_STATIC_ASSERT(!boost::fusion::result_of::empty<t1>::value);
        BOOST_STATIC_ASSERT(boost::fusion::result_of::empty<t2>::value);
    }

    {   // testing front & back

        typedef FUSION_SEQUENCE<int, float, std::string> tup;
        tup t(1, 2.2f, "Kimpo");

        BOOST_TEST(front(t) == 1);
#if !defined(FUSION_FORWARD_ONLY) // list has no back
        BOOST_TEST(back(t) == "Kimpo");
#endif
    }

    {   // testing is_sequence

        typedef FUSION_SEQUENCE<int, float, double> t1;
        typedef FUSION_SEQUENCE<> t2;
        typedef FUSION_SEQUENCE<char> t3;

        BOOST_STATIC_ASSERT(traits::is_sequence<t1>::value);
        BOOST_STATIC_ASSERT(traits::is_sequence<t2>::value);
        BOOST_STATIC_ASSERT(traits::is_sequence<t3>::value);
        BOOST_STATIC_ASSERT(!traits::is_sequence<int>::value);
        BOOST_STATIC_ASSERT(!traits::is_sequence<char>::value);
    }

    {   // testing mpl::is_sequence

        typedef FUSION_SEQUENCE<int, float, double> t1;
        typedef FUSION_SEQUENCE<> t2;
        typedef FUSION_SEQUENCE<char> t3;

        BOOST_STATIC_ASSERT(boost::mpl::is_sequence<t1>::value);
        BOOST_STATIC_ASSERT(boost::mpl::is_sequence<t2>::value);
        BOOST_STATIC_ASSERT(boost::mpl::is_sequence<t3>::value);
    }

    {   // testing mpl compatibility

        // test begin, end, next, prior, advance, size, deref, etc.
        //~ typedef FUSION_SEQUENCE<int, float, bool, char> tuple_type;
        //~ test_intrinsics1<tuple_type> test1;
        //~ (void)test1; // prevent unused variable warning

        // test an algorithm
        typedef FUSION_SEQUENCE<int, float, double> t1;
        typedef boost::mpl::find<t1, float>::type iter;
        typedef boost::mpl::deref<iter>::type type;
        BOOST_STATIC_ASSERT((boost::is_same<type, float>::value));

    }
}


#include <iostream>
#include <fstream>
#include <armadillo>
#include <math.h>
#include <omp.h>
#include <mpi.h>
#include <unordered_set>

#include "class_partition.hpp"
#include "constants.hpp"

/////////////////// Below are private member functions /////////////////

/*
Creats knots in the current region
Input:
    xMin, xMax, yMin, yMax: the boudary of the current region
    nKnotsX, nKnotsY: the number of knots in the current region in each direction
Output:
    currentKnotsX: the array of the coordinates in longitude for each knot
    currentKnotsY: the array of the coordinates in latitude for each knot
*/
void Partition::create_knots(double *&currentKnotsX, double *&currentKnotsY, const double &xMin, const double &xMax, const int &nKnotsX, const double &yMin, const double &yMax, const int &nKnotsY)
{
    double offsetX = (xMax-xMin)*OFFSET;
    double offsetY = (yMax-yMin)*OFFSET;

    double xStart = xMin+offsetX;
    double xEnd = xMax-offsetX;
    double yStart = yMin+offsetY;
    double yEnd = yMax-offsetY;

    double xIncrement = ( nKnotsX==1 ? 0 : (xEnd-xStart)/(nKnotsX-1));
    double yIncrement = ( nKnotsY==1 ? 0 : (yEnd-yStart)/(nKnotsY-1));

    #pragma omp parallel for
    for(int iKnotX = 0; iKnotX < nKnotsX; iKnotX++)
        for(int jKnotY = 0; jKnotY < nKnotsY; jKnotY++)
        {
            currentKnotsX[iKnotX*nKnotsY+jKnotY] = xEnd-(nKnotsX-iKnotX-1)*xIncrement;//xStart+iKnotX*xIncrement;
            currentKnotsY[iKnotX*nKnotsY+jKnotY] = yEnd-(nKnotsY-jKnotY-1)*yIncrement;//yStart+jKnotY*yIncrement;
        }
}

/*
Creats partitions in the current region
*/
void Partition::create_partitions(unsigned long region, double *partitionXMin, double *partitionXMax, double *partitionYMin, double *partitionYMax)
{
    double xMin=partitionXMin[region];
    double xMax=partitionXMax[region];
    double yMin=partitionYMin[region];
    double yMax=partitionYMax[region];

    if(NUM_PARTITIONS_J==2)
    {
        if((xMax-xMin)>=(yMax-yMin))
        {
            double xMid = (xMax+xMin)/2;
            partitionXMin[region*2+1]=xMin; partitionXMin[region*2+2]=xMid;
            partitionXMax[region*2+1]=xMid; partitionXMax[region*2+2]=xMax;

            partitionYMin[region*2+1]=yMin; partitionYMin[region*2+2]=yMin;
            partitionYMax[region*2+1]=yMax; partitionYMax[region*2+2]=yMax;
        }else
        {
            double yMid = (yMax+yMin)/2;
            partitionXMin[region*2+1]=xMin; partitionXMin[region*2+2]=xMin;
            partitionXMax[region*2+1]=xMax; partitionXMax[region*2+2]=xMax;

            partitionYMin[region*2+1]=yMin; partitionYMin[region*2+2]=yMid;
            partitionYMax[region*2+1]=yMid; partitionYMax[region*2+2]=yMax;
        }
    }else
    {
        double xMid = (xMax+xMin)/2;
        double yMid = (yMax+yMin)/2;

        partitionXMin[region*4+1]=xMin; partitionXMin[region*4+2]=xMid; partitionXMin[region*4+3]=xMin; partitionXMin[region*4+4]=xMid;

        partitionXMax[region*4+1]=xMid; partitionXMax[region*4+2]=xMax; partitionXMax[region*4+3]=xMid; partitionXMax[region*4+4]=xMax;

        partitionYMin[region*4+1]=yMin; partitionYMin[region*4+2]=yMin; partitionYMin[region*4+3]=yMid; partitionYMin[region*4+4]=yMid;

        partitionYMax[region*4+1]=yMid; partitionYMax[region*4+2]=yMid; partitionYMax[region*4+3]=yMax; partitionYMax[region*4+4]=yMax;
    }
}

/////////////////// Above are private member functions /////////////////

/////////////////// Below are public member functions /////////////////

/*Implement the constructor of Partition. Assign values to nRegionsAtEachLevel for the array of the number of regions in each level, nRegionsInTotal for the total number of regions, and MPI quantities.
*/
Partition::Partition()
{
    //If user did not predetermine the number of levels, find the number of levels such that the average number of observations per region similar to the number of knots
    if(NUM_LEVELS_M == -99)//-99 stands for the default method to automatically determine NUM_LEVELS_M
    {
        //Find the number of regions required at finest level, nRegionsAtFinestLevel
        unsigned long nRegionsAtFinestLevel = ceil((double)NUM_OBSERVATIONS/(double)NUM_KNOTS_r);

        //Find the number of levels, NUM_LEVELS_M, which satisfies NUM_PARTITIONS_J^(NUM_LEVELS_M-1)>=nRegionsAtFinestLevel
        NUM_LEVELS_M = ceil(log(nRegionsAtFinestLevel)/log(NUM_PARTITIONS_J))+1;

        if(NUM_LEVELS_M < 2)
	    {
            if(WORKER == 0) cout<<"Program exits with an error: the NUM_LEVELS_M calculated by default is "<<NUM_LEVELS_M<<", which is required to be larger than 1. Please decrease NUM_KNOTS_r to get a larger NUM_LEVELS_M.\n";
            MPI_Barrier(MPI_COMM_WORLD);
            MPI_Finalize();
            exit(EXIT_FAILURE);
	    }
    }

    //Find the array of the number of regions in each level, nRgnsVec, and the total number of regions, nRegionsInTotal
    nRegionsAtEachLevel = new unsigned long [NUM_LEVELS_M];
    nRegionsAtEachLevel[0] = 1;
    nRegionsInTotal = 1;
    for(int iLevel = 1; iLevel < NUM_LEVELS_M; iLevel++)
    {
        nRegionsAtEachLevel[iLevel] = nRegionsAtEachLevel[iLevel-1]*NUM_PARTITIONS_J;
        nRegionsInTotal += nRegionsAtEachLevel[iLevel];
    }

    //Calculate MPI information
        WORKING_REGION_FLAG = new bool [nRegionsInTotal]();

        unsigned long indexRegionAtFinestLevelStartThisWorker, indexRegionAtFinestLevelEndThisWorker;
        WORKERS_FOR_EACH_REGION = new std::set<unsigned long>[nRegionsInTotal - nRegionsAtEachLevel[NUM_LEVELS_M-1]];

        //Assign regions in the finest level to the worker
        unsigned long nRegionsPerWorker = nRegionsAtEachLevel[NUM_LEVELS_M-1] / MPI_SIZE;
        unsigned long nWorkersWithAdditionalRegion = nRegionsAtEachLevel[NUM_LEVELS_M-1] - nRegionsPerWorker * MPI_SIZE;

        if(WORKER < nWorkersWithAdditionalRegion)
        {
            indexRegionAtFinestLevelStartThisWorker =  WORKER * (nRegionsPerWorker+1);
            indexRegionAtFinestLevelEndThisWorker =  indexRegionAtFinestLevelStartThisWorker + nRegionsPerWorker;
        }
        else
        {
            indexRegionAtFinestLevelStartThisWorker =  nWorkersWithAdditionalRegion * (nRegionsPerWorker+1) + (WORKER -  nWorkersWithAdditionalRegion)* nRegionsPerWorker;
            indexRegionAtFinestLevelEndThisWorker =  indexRegionAtFinestLevelStartThisWorker + nRegionsPerWorker - 1;
        }

        indexRegionAtFinestLevelStartThisWorker += nRegionsInTotal - nRegionsAtEachLevel[NUM_LEVELS_M-1];
        indexRegionAtFinestLevelEndThisWorker += nRegionsInTotal - nRegionsAtEachLevel[NUM_LEVELS_M-1];

        for(unsigned long i = indexRegionAtFinestLevelStartThisWorker; i < indexRegionAtFinestLevelEndThisWorker+1; i++)
            WORKING_REGION_FLAG[i] = true;

        //Find the regions dealt with by this worker at the second finest level
        for(unsigned long index = indexRegionAtFinestLevelStartThisWorker; index <= indexRegionAtFinestLevelEndThisWorker; index++)
        {
            unsigned long ancestor = index;
            while(ancestor > 0)
            {
                ancestor = (ancestor-1)/NUM_PARTITIONS_J;
                //INDICES_REGIONS.insert(ancestor);
                WORKING_REGION_FLAG[ancestor] = true;
            }
            INDICES_REGIONS_AT_CURRENT_LEVEL.insert((index-1)/NUM_PARTITIONS_J);
        }

        //Assign WORKERS_FOR_EACH_REGION at the second finest level
        unsigned long indexStartFinestLevel = nRegionsInTotal - nRegionsAtEachLevel[NUM_LEVELS_M-1];
        unsigned long tmpIndexStart = indexStartFinestLevel, tmpIndexEnd;
        if(DYNAMIC_SCHEDULE_FLAG) origninalWorker = new int [nRegionsInTotal];

        for(int iWorker = 0; iWorker < nWorkersWithAdditionalRegion; iWorker++)
        {
            tmpIndexEnd =  tmpIndexStart + nRegionsPerWorker + 1;
            for(unsigned long index = tmpIndexStart; index < tmpIndexEnd; index++)
            {
                unsigned long ancestor = (index-1)/NUM_PARTITIONS_J;
                WORKERS_FOR_EACH_REGION[ancestor].insert(iWorker);
                if(DYNAMIC_SCHEDULE_FLAG)
                {
                    origninalWorker[index] = iWorker;
                    origninalWorker[ancestor] = iWorker;
                    while(ancestor > 0)
                    {
                        ancestor = (ancestor-1)/NUM_PARTITIONS_J;
                        origninalWorker[ancestor] = iWorker;
                    }
                }
            }
            tmpIndexStart += nRegionsPerWorker+1;
            tmpIndexEnd += nRegionsPerWorker+1;
        }
        for(int iWorker = nWorkersWithAdditionalRegion; iWorker < MPI_SIZE; iWorker++)
        {
            tmpIndexEnd =  tmpIndexStart + nRegionsPerWorker;
            for(unsigned long index = tmpIndexStart; index < tmpIndexEnd; index++)
            {
                unsigned long ancestor = (index-1)/NUM_PARTITIONS_J;
                WORKERS_FOR_EACH_REGION[ancestor].insert(iWorker);
                if(DYNAMIC_SCHEDULE_FLAG)
                {
                    origninalWorker[index] = iWorker;
                    origninalWorker[ancestor] = iWorker;
                    while(ancestor > 0)
                    {
                        ancestor = (ancestor-1)/NUM_PARTITIONS_J;
                        origninalWorker[ancestor] = iWorker;
                    }
                }
            }
            tmpIndexStart += nRegionsPerWorker;
            tmpIndexEnd += nRegionsPerWorker;
        }

        //Assign regionStart and regionEnd for each level
        REGION_START = new unsigned long [NUM_LEVELS_M];
        REGION_END = new unsigned long [NUM_LEVELS_M];
        REGION_START[NUM_LEVELS_M-1]=indexRegionAtFinestLevelStartThisWorker;
        REGION_END[NUM_LEVELS_M-1]=indexRegionAtFinestLevelEndThisWorker;
        for(int iLevel = NUM_LEVELS_M-2; iLevel > -1; iLevel--)
        {
            REGION_START[iLevel] = (REGION_START[iLevel+1]-1)/NUM_PARTITIONS_J;
            REGION_END[iLevel] = (REGION_END[iLevel+1]-1)/NUM_PARTITIONS_J;
        }
}

/*
Implement the member function of Partition, build_partition, which builds the hierarchical partition
*/

void Partition::build_partition()
{
    if(WORKER == 0)
	{
		gettimeofday(&timeNow, NULL);
		cout<<"===========================> Processor 1: building hierarchical grid starts. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n";
	}

    //The number of knots in each direction
    int nKnotsX = ceil(sqrt(NUM_KNOTS_r));
    int nKnotsY = (int)(NUM_KNOTS_r/nKnotsX);
    nKnots = nKnotsX*nKnotsY;

    //Allocate memory for the coordinates of knots
    knotsX = new double* [nRegionsInTotal];
    knotsY = new double* [nRegionsInTotal];

    //Array of coordinates for the partition in each region with dimension [nRegionsInTotal]
    double *partitionXMin, *partitionYMin, *partitionXMax, *partitionYMax;
    partitionXMin = new double [nRegionsInTotal];
    partitionXMax = new double [nRegionsInTotal];
    partitionYMin = new double [nRegionsInTotal];
    partitionYMax = new double [nRegionsInTotal];

    partitionXMin[0] = data->domainBoundaries[0];
    partitionXMax[0] = data->domainBoundaries[1];
    partitionYMin[0] = data->domainBoundaries[2];
    partitionYMax[0] = data->domainBoundaries[3];

    for(int iLevel = 0; iLevel < NUM_LEVELS_M-1; iLevel++)
    {
        #pragma omp parallel for
        for(unsigned long jRegion = REGION_START[iLevel]; jRegion < REGION_END[iLevel] + 1; jRegion++)
            create_partitions(jRegion, partitionXMin, partitionXMax, partitionYMin, partitionYMax);
    }

    #pragma omp parallel for schedule(dynamic,1)
    for(unsigned long iRegion = 0; iRegion < nRegionsInTotal - nRegionsAtEachLevel[NUM_LEVELS_M-1]; iRegion++)
    {
        if(WORKING_REGION_FLAG[iRegion])
        {
            knotsX[iRegion] = new double [nKnots];
            knotsY[iRegion] = new double [nKnots];
            create_knots(knotsX[iRegion], knotsY[iRegion], partitionXMin[iRegion], partitionXMax[iRegion], nKnotsX, partitionYMin[iRegion], partitionYMax[iRegion], nKnotsY);
        }
    }

    //Allocate memory for nKnotsAtFinestLevel, nPredictionsAtFinestLevel
    nKnotsAtFinestLevel = new unsigned long [nRegionsAtEachLevel[NUM_LEVELS_M-1]];
    nPredictionsAtFinestLevel = new unsigned long [nRegionsAtEachLevel[NUM_LEVELS_M-1]];

    //Allocate memory for nKnotsAtFinestLevel, nPredictionsAtFinestLevel
    knotsResidual = new double* [nRegionsAtEachLevel[NUM_LEVELS_M-1]];

    //Allocate memory for the coordinates of predictions
    predictionX = new double* [nRegionsAtEachLevel[NUM_LEVELS_M-1]];
    predictionY = new double* [nRegionsAtEachLevel[NUM_LEVELS_M-1]];

    //Get coordinates of the knots and predictions at the regions in the finest level
    int maxOpenMPThreads=omp_get_max_threads();

    #pragma omp parallel for num_threads(maxOpenMPThreads) schedule(dynamic,1)
    for(unsigned long iRegion = nRegionsInTotal - nRegionsAtEachLevel[NUM_LEVELS_M-1]; iRegion < nRegionsInTotal; iRegion++)
    {
        std::set<unsigned long> *indexObservationsInThisRegion = new std::set<unsigned long>;
        std::set<unsigned long> *indexPredictionsInThisRegion = new std::set<unsigned long>;

        unsigned long indexStartThisLevel = iRegion+nRegionsAtEachLevel[NUM_LEVELS_M-1]-nRegionsInTotal;
        if(WORKING_REGION_FLAG[iRegion])
        {
            double xMin=partitionXMin[iRegion];
            double xMax=partitionXMax[iRegion];
            double yMin=partitionYMin[iRegion];
            double yMax=partitionYMax[iRegion];

            indexObservationsInThisRegion->clear();

            for(unsigned long jObservation = 0; jObservation < NUM_OBSERVATIONS; jObservation++)
            {
                if(data->observationLon[jObservation] >= xMin && data->observationLon[jObservation] < xMax && data->observationLat[jObservation] >= yMin && data->observationLat[jObservation] < yMax)
                {

                    bool skip = false;
                    //Skip if the observation location coincides with knots in levels above
                    unsigned long ancestor = iRegion;
                    for(int kLevel = 0; kLevel < NUM_LEVELS_M-1; kLevel++)
                    {
                        ancestor = (ancestor-1)/NUM_PARTITIONS_J;
                        for(int pKnot = 0; pKnot < nKnots; pKnot++)
                        {
                            if(knotsX[ancestor][pKnot] == data->observationLon[jObservation] && knotsY[ancestor][pKnot] == data->observationLat[jObservation])
                            {
                                skip = true;
                                break;
                            }
                        }
                        if(skip) break;
                    }

                    //Add this observation
                    if(!skip) indexObservationsInThisRegion->insert(jObservation);
                }
            }

            nKnotsAtFinestLevel[indexStartThisLevel] = indexObservationsInThisRegion->size();
            knotsX[iRegion] = new double [nKnotsAtFinestLevel[indexStartThisLevel]];
            knotsY[iRegion] = new double [nKnotsAtFinestLevel[indexStartThisLevel]];
            knotsResidual[indexStartThisLevel] = new double [nKnotsAtFinestLevel[indexStartThisLevel]];

            unsigned long tmpIndex=0;
            for(std::set<unsigned long>::iterator jObservation = indexObservationsInThisRegion->begin(); jObservation != indexObservationsInThisRegion->end(); jObservation++)
            {
                knotsX[iRegion][tmpIndex] = data->observationLon[*jObservation];
                knotsY[iRegion][tmpIndex] = data->observationLat[*jObservation];
                knotsResidual[indexStartThisLevel][tmpIndex++] = data->observationResiduals[*jObservation];
            }

            if(CALCULATION_MODE=="prediction")
            {
                indexPredictionsInThisRegion->clear();
                for(unsigned long jPrediction = 0; jPrediction < NUM_PREDICTIONS; jPrediction++)
                {
                    if(data->predictionLon[jPrediction] >= xMin && data->predictionLon[jPrediction] < xMax && data->predictionLat[jPrediction] >= yMin && data->predictionLat[jPrediction] < yMax)
                        indexPredictionsInThisRegion->insert(jPrediction);
                }

                nPredictionsAtFinestLevel[indexStartThisLevel] = indexPredictionsInThisRegion->size();
                predictionX[indexStartThisLevel] = new double [nPredictionsAtFinestLevel[indexStartThisLevel]];
                predictionY[indexStartThisLevel] = new double [nPredictionsAtFinestLevel[indexStartThisLevel]];

                unsigned long tmpIndex=0;
                for(std::set<unsigned long>::iterator jPrediction = indexPredictionsInThisRegion->begin(); jPrediction != indexPredictionsInThisRegion->end(); jPrediction++)
                {
                    predictionX[indexStartThisLevel][tmpIndex] = data->predictionLon[*jPrediction];
                    predictionY[indexStartThisLevel][tmpIndex++] = data->predictionLat[*jPrediction];
                }
            }
        }
        indexObservationsInThisRegion->clear();
        indexPredictionsInThisRegion->clear();
    }

    //Release the memory for the temporary arrays
    delete[] partitionXMin;
    delete[] partitionYMin;
    delete[] partitionXMax;
    delete[] partitionYMax;

    //Delete variables that are not used anymore
    delete[] data->observationLon;
    delete[] data->observationLat;
    delete[] data->observationResiduals;

    if(DYNAMIC_SCHEDULE_FLAG)
		dynamic_schedule();
	else
		WORLD = MPI_COMM_WORLD;

    if(WORKER == 0)
	{
		gettimeofday(&timeNow, NULL);
		cout<<"===========================> Processor 1: building hierarchical grid is complete. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n\n";
		if(PRINT_DETAIL_FLAG) print_partition_summary();
	}
}

//Implement the member function of Partition, dynamic_schedule, which assign the work load for each worker by dynamic scheduling
void Partition::dynamic_schedule()
{
    //Synchronize the knots of regions before the finest level
    for(unsigned long iRegion = 0; iRegion < nRegionsInTotal - nRegionsAtEachLevel[NUM_LEVELS_M-1]; iRegion++)
    {
        if(!WORKING_REGION_FLAG[iRegion])
        {
            knotsX[iRegion] = new double [nKnots];
            knotsY[iRegion] = new double [nKnots];
        }
    }

    for(unsigned long iRegion = 0; iRegion < nRegionsInTotal - nRegionsAtEachLevel[NUM_LEVELS_M-1]; iRegion++)
    {
        MPI_Bcast(knotsX[iRegion],nKnots,MPI_DOUBLE,origninalWorker[iRegion],MPI_COMM_WORLD);
        MPI_Bcast(knotsY[iRegion],nKnots,MPI_DOUBLE,origninalWorker[iRegion],MPI_COMM_WORLD);
    }

    //Synchronize the knots of regions at the finest level and predictions if predicting
    unsigned long indexStartFinestLevel = nRegionsInTotal - nRegionsAtEachLevel[NUM_LEVELS_M-1];
    for(unsigned long iRegion = indexStartFinestLevel; iRegion < nRegionsInTotal; iRegion++)
    {

        MPI_Bcast(&nKnotsAtFinestLevel[iRegion-indexStartFinestLevel],1,MPI_UNSIGNED_LONG,origninalWorker[iRegion],MPI_COMM_WORLD);
        if(!WORKING_REGION_FLAG[iRegion])
        {
            knotsX[iRegion] = new double [nKnotsAtFinestLevel[iRegion-indexStartFinestLevel]];
            knotsY[iRegion] = new double [nKnotsAtFinestLevel[iRegion-indexStartFinestLevel]];
            knotsResidual[iRegion-indexStartFinestLevel] = new double [nKnotsAtFinestLevel[iRegion-indexStartFinestLevel]];
        }

        if(CALCULATION_MODE=="prediction")
        {
            MPI_Bcast(&nPredictionsAtFinestLevel[iRegion-indexStartFinestLevel],1,MPI_UNSIGNED_LONG,origninalWorker[iRegion],MPI_COMM_WORLD);
            if(!WORKING_REGION_FLAG[iRegion])
            {
                predictionX[iRegion-indexStartFinestLevel] = new double [nPredictionsAtFinestLevel[iRegion-indexStartFinestLevel]];
                predictionY[iRegion-indexStartFinestLevel] = new double [nPredictionsAtFinestLevel[iRegion-indexStartFinestLevel]];
            }
        }

        MPI_Bcast(knotsX[iRegion],nKnotsAtFinestLevel[iRegion-indexStartFinestLevel],MPI_DOUBLE,origninalWorker[iRegion],MPI_COMM_WORLD);
        MPI_Bcast(knotsY[iRegion],nKnotsAtFinestLevel[iRegion-indexStartFinestLevel],MPI_DOUBLE,origninalWorker[iRegion],MPI_COMM_WORLD);
        MPI_Bcast(knotsResidual[iRegion-indexStartFinestLevel],nKnotsAtFinestLevel[iRegion-indexStartFinestLevel],MPI_DOUBLE,origninalWorker[iRegion],MPI_COMM_WORLD);

        if(CALCULATION_MODE=="prediction")
        {
            MPI_Bcast(predictionX[iRegion-indexStartFinestLevel],nPredictionsAtFinestLevel[iRegion-indexStartFinestLevel],MPI_DOUBLE,origninalWorker[iRegion],MPI_COMM_WORLD);
            MPI_Bcast(predictionY[iRegion-indexStartFinestLevel],nPredictionsAtFinestLevel[iRegion-indexStartFinestLevel],MPI_DOUBLE,origninalWorker[iRegion],MPI_COMM_WORLD);
        }
    }

    //Dynamic schedule work load for each worker

    ///Clear WORKERS_FOR_EACH_REGION at the second finest level
    for(unsigned long iRegion = nRegionsInTotal - nRegionsAtEachLevel[NUM_LEVELS_M-1] - nRegionsAtEachLevel[NUM_LEVELS_M-2]; iRegion < nRegionsInTotal - nRegionsAtEachLevel[NUM_LEVELS_M-1]; iRegion++)
        WORKERS_FOR_EACH_REGION[iRegion].clear();

    ///Clear WORKING_REGION_FLAG for all regions
    memset(WORKING_REGION_FLAG,false,nRegionsInTotal);

    ///Clear INDICES_REGIONS_AT_CURRENT_LEVEL
    INDICES_REGIONS_AT_CURRENT_LEVEL.clear();

    ///Calculate work load
    double totalComplexity = 0;
    for(unsigned long index = 0; index < nRegionsAtEachLevel[NUM_LEVELS_M-1]; index++)
    {
        double tmp = nKnotsAtFinestLevel[index];
        totalComplexity += tmp*tmp;//*tmp;
    }
    double complexityEachWorker = totalComplexity/MPI_SIZE;

    ///Assign working regions for each worker
    REGION_START[NUM_LEVELS_M-1]=nRegionsInTotal;
    REGION_END[NUM_LEVELS_M-1]=0;
    int thisWorker=-1;
    double complexityThisWorker=0;
    unsigned long indexRegionAtFinestLevelStartThisWorker = nRegionsInTotal - nRegionsAtEachLevel[NUM_LEVELS_M-1], indexRegionAtFinestLevelEndThisWorker;
    for(unsigned long index = 0; index < nRegionsAtEachLevel[NUM_LEVELS_M-1]; index++)
    {
        double tmp = nKnotsAtFinestLevel[index];
        double complexityThisRegion = tmp*tmp;//*tmp;
        complexityThisWorker += complexityThisRegion;
        //if(WORKER==0) cout<<complexityThisRegion<<" "<<complexityEachWorker<<endl;
        if(complexityThisWorker >= complexityEachWorker || index == nRegionsAtEachLevel[NUM_LEVELS_M-1]-1)
        {
            thisWorker++;
            if(thisWorker == MPI_SIZE) thisWorker = MPI_SIZE-1;

            indexRegionAtFinestLevelEndThisWorker = indexStartFinestLevel+index;

            //Check whether this region should be handled by this worker or next worker
            if(complexityEachWorker - complexityThisWorker + complexityThisRegion < complexityThisWorker - complexityEachWorker
                && index != nRegionsAtEachLevel[NUM_LEVELS_M-1]-1
                && indexRegionAtFinestLevelEndThisWorker > indexRegionAtFinestLevelStartThisWorker)
            {
                //Next worker handle this region
                indexRegionAtFinestLevelEndThisWorker--;
                complexityThisWorker = complexityThisRegion;
            }
            else
            {
                //This worker handle this region
                complexityThisWorker = 0;
            }

            //if(WORKER==0) cout<<thisWorker<<" "<<indexRegionAtFinestLevelStartThisWorker<<" "<<indexRegionAtFinestLevelEndThisWorker<<endl;

            for(unsigned long jRegion = indexRegionAtFinestLevelStartThisWorker; jRegion < indexRegionAtFinestLevelEndThisWorker+1; jRegion++)
            {
                unsigned long ancestor = (jRegion-1)/NUM_PARTITIONS_J;
                WORKERS_FOR_EACH_REGION[ancestor].insert(thisWorker);
            }

            if(thisWorker == WORKER)
            {
                for(unsigned long jRegion = indexRegionAtFinestLevelStartThisWorker; jRegion < indexRegionAtFinestLevelEndThisWorker+1; jRegion++)
                {
                    unsigned long ancestor = jRegion;
                    WORKING_REGION_FLAG[ancestor] = true;
                    while(ancestor > 0)
                    {
                        ancestor = (ancestor-1)/NUM_PARTITIONS_J;
                        WORKING_REGION_FLAG[ancestor] = true;
                    }
                    INDICES_REGIONS_AT_CURRENT_LEVEL.insert((jRegion-1)/NUM_PARTITIONS_J);
                }
                if(indexRegionAtFinestLevelStartThisWorker<REGION_START[NUM_LEVELS_M-1]) REGION_START[NUM_LEVELS_M-1]=indexRegionAtFinestLevelStartThisWorker;
                if(indexRegionAtFinestLevelEndThisWorker>REGION_END[NUM_LEVELS_M-1]) REGION_END[NUM_LEVELS_M-1]=indexRegionAtFinestLevelEndThisWorker;
            }

            indexRegionAtFinestLevelStartThisWorker = indexRegionAtFinestLevelEndThisWorker+1;
        }
    }

    //Deactivate MPI processes that are not assigned any regions, which rarely happens
    if(thisWorker<MPI_SIZE-1)
    {
        MPI_Group worldGroup;
        MPI_Comm_group(MPI_COMM_WORLD,&worldGroup);

        MPI_Group newWorldGroup;
        int ranges[1][3]={{thisWorker+1,MPI_SIZE-1,1}};
        MPI_Group_range_excl(worldGroup,1,ranges,&newWorldGroup);

        MPI_Comm_create(MPI_COMM_WORLD, newWorldGroup, &WORLD);

        MPI_SIZE=thisWorker+1;

        if(WORKER>thisWorker)
        {
            MPI_Finalize();
            exit(EXIT_SUCCESS);
        }
    }
    else
        WORLD = MPI_COMM_WORLD;

    for(int iLevel = NUM_LEVELS_M-2; iLevel > -1; iLevel--)
    {
        REGION_START[iLevel] = (REGION_START[iLevel+1]-1)/NUM_PARTITIONS_J;
        REGION_END[iLevel] = (REGION_END[iLevel+1]-1)/NUM_PARTITIONS_J;
    }

    //Delete regions that are not handled by this WORKER
    for(unsigned long iRegion = 0; iRegion < nRegionsInTotal; iRegion++)
        if(!WORKING_REGION_FLAG[iRegion])
        {
            delete[] knotsX[iRegion]; knotsX[iRegion]=NULL;
            delete[] knotsY[iRegion]; knotsY[iRegion]=NULL;
        }

    for(unsigned long iRegion = nRegionsInTotal - nRegionsAtEachLevel[NUM_LEVELS_M-1]; iRegion < nRegionsInTotal; iRegion++)
        if(!WORKING_REGION_FLAG[iRegion])
        {
            delete[] knotsResidual[iRegion-indexStartFinestLevel]; knotsResidual[iRegion-indexStartFinestLevel]=NULL;

            if(CALCULATION_MODE=="prediction")
            {
                delete[] predictionX[iRegion-indexStartFinestLevel]; predictionX[iRegion-indexStartFinestLevel]=NULL;
                delete[] predictionY[iRegion-indexStartFinestLevel]; predictionY[iRegion-indexStartFinestLevel]=NULL;
            }
        }

}

//Implement the member function of Partition, print_partition_summary, which shows a brief summary of the partition
void Partition::print_partition_summary()
{
    cout<<">>The number of levels: "<<NUM_LEVELS_M<<endl<<endl;
    cout<<">>The total number of regions: "<<nRegionsInTotal<<endl<<endl;
    cout<<">>The number of regions in each level: \n    ";
    for(int iLevel = 0; iLevel < NUM_LEVELS_M; iLevel++) cout<<nRegionsAtEachLevel[iLevel]<<" ";
    cout<<endl<<endl;

    cout<<">>Knot coordinates in the coarsest region, up to ten knots are shown: \n    longitude: ";
    for(int iKnot = 0; iKnot < std::min(nKnots,10); iKnot++)
        printf("%8.2lf ",knotsX[0][iKnot]);
    cout<<"\n    latitude:  ";
    for(int iKnot = 0; iKnot < std::min(nKnots,10); iKnot++)
        printf("%8.2lf ",knotsY[0][iKnot]);
    cout<<endl<<endl;

    if(CALCULATION_MODE=="predict")
        cout<<">>The number of predictions after eliminating duplicates: "<<NUM_PREDICTIONS<<endl<<endl;

}

void Partition::dump_structure_information()
{
    //Open the file "structure_information.txt" to store structure information
    std::ofstream file;
    file.open("structure_information.txt");

    //Dump the number of levels
    file<<"The number of levels: "<<NUM_LEVELS_M<<endl<<endl;

    //Dump the total number of regions
    file<<"The total number of regions: "<<nRegionsInTotal<<endl<<endl;

    //Dump the number of knots in regions before the finest level
    file<<"The actual number of knots in regions before the finest level: "<<nKnots<<endl<<endl;

    //Dump some statistics of the number of knots in each region at the finest level
    unsigned long nKnotsMin = nKnotsAtFinestLevel[0];
    unsigned long nKnotsMax = nKnotsAtFinestLevel[0];
    unsigned long nRegionsHaveZeroKnots = 0;

    for(unsigned long iRegion = 0; iRegion < nRegionsAtEachLevel[NUM_LEVELS_M-1]; iRegion++)
    {
        if(nKnotsMin>nKnotsAtFinestLevel[iRegion]) nKnotsMin=nKnotsAtFinestLevel[iRegion];
        if(nKnotsMax<nKnotsAtFinestLevel[iRegion]) nKnotsMax=nKnotsAtFinestLevel[iRegion];
        if(nKnotsAtFinestLevel[iRegion] == 0) nRegionsHaveZeroKnots++;
    }

    file<<"The minimal number of knots in regions at the finest level: "<<nKnotsMin<<endl<<endl;
    file<<"The maximal number of knots in regions at the finest level: "<<nKnotsMax<<endl<<endl;
    file<<"The number of in regions at the finest level that have zero knots: "<<nRegionsHaveZeroKnots<<endl<<endl;

    unsigned long thresholdLeft, thresholdRight;
    unsigned long nRegionsBetweenThresholds;
    double interval = (nKnotsMax-nKnotsMin)/10;

    thresholdLeft=nKnotsMin;
    file<<"The number of regions at the finest level with number of knots in the following intervals:"<<endl;

    for(int i = 0; i < 10; i++)
    {
        nRegionsBetweenThresholds = 0;
        thresholdRight=thresholdLeft+interval;
        if(i == 9) thresholdRight = nKnotsMax;
        for(unsigned long iRegion = 0; iRegion < nRegionsAtEachLevel[NUM_LEVELS_M-1]; iRegion++)
           if(thresholdLeft<=nKnotsAtFinestLevel[iRegion] && nKnotsAtFinestLevel[iRegion]<=thresholdRight) nRegionsBetweenThresholds++;
        file<<"[ "<<thresholdLeft<<" , "<<thresholdRight<<" ]: "<<nRegionsBetweenThresholds<<endl;
        thresholdLeft = thresholdRight+1;
    }
    file<<endl;

    //Dump the number of regions in each level
    file<<"The number of regions in each level: \n";
    for(int iLevel = 0; iLevel < NUM_LEVELS_M; iLevel++)
        file<<"Level "<<iLevel+1<<": "<<nRegionsAtEachLevel[iLevel]<<endl;
    file<<endl;

    //Dump the number of knots in each region at the finest level
    file<<"The number of knots in each region at the finest level: \n";
    for(unsigned long iRegion = 0; iRegion < nRegionsAtEachLevel[NUM_LEVELS_M-1]; iRegion++)
        file<<"Region "<<nRegionsInTotal-nRegionsAtEachLevel[NUM_LEVELS_M-1]+iRegion<<": "<<nKnotsAtFinestLevel[iRegion]<<endl;

    //Close the file
    file.close();
}

Partition::~Partition()
{
    delete[] nRegionsAtEachLevel;

    delete[] knotsX;
    delete[] knotsY;
    delete[] knotsResidual;

    delete[] predictionX,
    delete[] predictionY;

    delete[] nKnotsAtFinestLevel,
    delete[] nPredictionsAtFinestLevel;

    if(DYNAMIC_SCHEDULE_FLAG) delete[] origninalWorker;
    //cout<<"Partition is deleted\n";
}


/******************************************************************************
 * $Id$
 *
 * Project:  libLAS - http://liblas.org - A BSD library for LAS format data.
 * Purpose:  Point Partitioning/blocking for OPC
 * Author:   Andrew Bell andrew.bell.ia at gmail.com
 *
 ******************************************************************************
 * Copyright (c) 2010, Andrew Bell
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following
 * conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in
 *       the documentation and/or other materials provided
 *       with the distribution.
 *     * Neither the name of the Andrew Bell or libLAS nor the names of
 *       its contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
 * OF SUCH DAMAGE.
 ****************************************************************************/

#include <liblas/chipper.hpp>
// boost
#include <boost/cstdint.hpp>
// std
#include <cmath>

using namespace std;

/**
The objective is to split the region into non-overlapping blocks, each
containing approximately the same number of points, as specified by the
user.

First, the points are read into arrays - one for the x direction, and one for
the y direction.  The arrays are sorted and are initialized with indices into
the other array of the location of the other coordinate of the same point.

Partitions are created that place the maximum number of points in a
block, subject to the user-defined threshold, using a cumulate and round
procedure.

The distance of the point-space is checked in each direction and the
wider dimension is chosen for splitting at an appropriate partition point.
The points in the narrower direction are copied to locations in the spare
array at one side or the other of the chosen partition, and that portion
of the spare array then becomes the active array for the narrow direction.
This avoids resorting of the arrays, which are already sorted.

This procedure is then recursively applied to the created blocks until
they contains only one or two partitions.  In the case of one partition,
we are done, and we simply store away the contents of the block.  If there are
two partitions in a block, we avoid the recopying the narrow array to the
spare since the wide array already contains the desired points partitioned
into two blocks.  We simply need to locate the maximum and minimum values
from the narrow array so that the approriate extrema of the block can
be stored.
**/

namespace liblas { namespace chipper {

vector<boost::uint32_t> Block::GetIDs() const
{
    vector<boost::uint32_t> ids;

    for (boost::uint32_t i = m_left; i <= m_right; ++i)
        ids.push_back((*m_list_p)[i].m_ptindex);
    return ids;
}

void Chipper::Chip()
{
    Load(m_xvec, m_yvec, m_spare);
    Partition(m_xvec.size());
    DecideSplit(m_xvec, m_yvec, m_spare, 0, m_partitions.size() - 1);
}

void Chipper::Load(RefList& xvec, RefList& yvec, RefList& spare )
{
    PtRef ref;
    boost::uint32_t idx;
    boost::uint32_t count;
    vector<PtRef>::iterator it;

    count = m_reader->GetHeader().GetPointRecordsCount();
    xvec.reserve(count);
    yvec.reserve(count);
    spare.resize(count);

    count = 0;
    while (m_reader->ReadNextPoint()) {
        const liblas::Point& pt = m_reader->GetPoint();

        ref.m_pos = pt.GetX();
        ref.m_ptindex = count;
        xvec.push_back(ref);

        ref.m_pos = pt.GetY();
        yvec.push_back(ref);
        count++;
    }
    // Sort xvec and assign other index in yvec to sorted indices in xvec.
    sort(xvec.begin(), xvec.end());
    for (boost::uint32_t i = 0; i < xvec.size(); ++i) {
        idx = xvec[i].m_ptindex;
        yvec[idx].m_oindex = i;
    }

    // Sort yvec.
    sort(yvec.begin(), yvec.end());

    //Iterate through the yvector, setting the xvector appropriately.
    for (boost::uint32_t i = 0; i < yvec.size(); ++i)
        xvec[yvec[i].m_oindex].m_oindex = i;
}

void Chipper::Partition(boost::uint32_t size)
{
    boost::uint32_t num_partitions;

    num_partitions = size / m_threshold;
    if ( size % m_threshold )
        num_partitions++;
    double total = 0;
    double partition_size = static_cast<double>(size) / num_partitions;
    m_partitions.push_back(0);
    for (boost::uint32_t i = 0; i < num_partitions; ++i) {
        total += partition_size;
        m_partitions.push_back(static_cast<boost::uint32_t>(detail::sround(total)));
    }
}

void Chipper::DecideSplit(RefList& v1, RefList& v2, RefList& spare,
    boost::uint32_t pleft, boost::uint32_t pright)
{
    double v1range;
    double v2range;
    boost::uint32_t left = m_partitions[pleft];
    boost::uint32_t right = m_partitions[pright] - 1;

    // Decide the wider direction of the block, and split in that direction
    // to maintain squareness.
    v1range = v1[right].m_pos - v1[left].m_pos;
    v2range = v2[right].m_pos - v2[left].m_pos;
    if (v1range > v2range)
        Split(v1, v2, spare, pleft, pright);
    else
        Split(v2, v1, spare, pleft, pright);
}

void Chipper::Split(RefList& wide, RefList& narrow, RefList& spare,
    boost::uint32_t pleft, boost::uint32_t pright)
{
    boost::uint32_t lstart;
    boost::uint32_t rstart;
    boost::uint32_t pcenter;
    boost::uint32_t left;
    boost::uint32_t right;
    boost::uint32_t center;

    left = m_partitions[pleft];
    right = m_partitions[pright] - 1;

    // There are two cases in which we are done.
    // 1) We have a distance of two between left and right.
    // 2) We have a distance of three between left and right.

    if (pright - pleft == 1)
        Emit(wide, left, right, narrow, left, right);
    else if (pright - pleft == 2)
        FinalSplit(wide, narrow, pleft, pright);
    else
    {
        pcenter = (pleft + pright) / 2;
        center = m_partitions[pcenter];

        // We are splitting in the wide direction - split elements in the
        // narrow array by copying them to the spare array in the correct
        // partition.  The spare array then becomes the active narrow array
        // for the [left,right] partition.
        lstart = left;
        rstart = center;
        for (boost::uint32_t i = left; i <= right; ++i)
        {
            if (narrow[i].m_oindex < center)
            {
                spare[lstart] = narrow[i];
                wide[narrow[i].m_oindex].m_oindex = lstart;
                lstart++;
            }
            else
            {
                spare[rstart] = narrow[i];
                wide[narrow[i].m_oindex].m_oindex = rstart;
                rstart++;
            }
        }

        // Save away the direction so we know which array is X and which is Y
        // so that when we emit, we can properly label the max/min points.
        Direction dir = narrow.m_dir;
        spare.m_dir = dir;
        DecideSplit(wide, spare, narrow, pleft, pcenter);
        DecideSplit(wide, spare, narrow, pcenter, pright);
        narrow.m_dir = dir;
    }
}

// In this case the wide array is like we want it.  The narrow array is
// ordered, but not for our split, so we have to find the max/min entries
// for each partition in the final split.
void Chipper::FinalSplit(RefList& wide, RefList& narrow,
    boost::uint32_t pleft, boost::uint32_t pright)
{

    boost::int64_t left1 = -1;
    boost::int64_t left2 = -1;
    boost::int64_t right1 = -1;
    boost::int64_t right2 = -1;

    // It appears we're using int64_t here because we're using -1 as
    // an indicator.  I'm not 100% sure that i ends up <0, but I don't
    // think so.  These casts will at least shut up the compiler, but
    // I think this code should be revisited to use std::vector<boost::uint32_t>::const_iterator
    // or std::vector<boost::uint32_t>::size_type instead of this int64_t stuff -- hobu 11/15/10
    boost::int64_t left = static_cast<boost::int64_t>(m_partitions[pleft]);
    boost::int64_t right = static_cast<boost::int64_t>(m_partitions[pright] - 1);
    boost::int64_t center = static_cast<boost::int64_t>(m_partitions[pright - 1]);

    // Find left values for the partitions.
    for (boost::int64_t i = left; i <= right; ++i)
    {
        boost::int64_t idx = static_cast<boost::int64_t>(narrow[static_cast<boost::uint32_t>(i)].m_oindex);
        if (left1 < 0 && (idx < center))
        {
            left1 = i;
            if (left2 >= 0)
                break;
        }
        else if (left2 < 0 && (idx >= center))
        {
            left2 = i;
            if (left1 >= 0)
                break;
        }
    }
    // Find right values for the partitions.
    for (boost::int64_t i = right; i >= left; --i)
    {
        boost::int64_t idx = static_cast<boost::int64_t>(narrow[static_cast<boost::uint32_t>(i)].m_oindex);
        if (right1 < 0 && (idx < center))
        {
            right1 = i;
            if (right2 >= 0)
                break;
        }
        else if (right2 < 0 && (idx >= center))
        {
            right2 = i;
            if (right1 >= 0)
                break;
        }
    }

    // Emit results.
    Emit(wide,
         static_cast<boost::uint32_t>(left),
         static_cast<boost::uint32_t>(center - 1),
         narrow,
         static_cast<boost::uint32_t>(left1),
         static_cast<boost::uint32_t>(right1) );
    Emit(wide,
         static_cast<boost::uint32_t>(center),
         static_cast<boost::uint32_t>(right),
         narrow,
         static_cast<boost::uint32_t>(left2),
         static_cast<boost::uint32_t>(right2) );
}

void Chipper::Emit(RefList& wide, boost::uint32_t widemin, boost::uint32_t widemax,
    RefList& narrow, boost::uint32_t narrowmin, boost::uint32_t narrowmax )
{
    Block b;

    b.m_list_p = &wide;
    if (wide.m_dir == DIR_X) {

        // minx, miny, maxx, maxy
        liblas::Bounds<double> bnd(wide[widemin].m_pos, narrow[narrowmin].m_pos, wide[widemax].m_pos,  narrow[narrowmax].m_pos);
        b.SetBounds(bnd);

        // b.m_xmin = wide[widemin].m_pos;
        // b.m_xmax = wide[widemax].m_pos;
        // b.m_ymin = narrow[narrowmin].m_pos;
        // b.m_ymax = narrow[narrowmax].m_pos;
    }
    else {
        liblas::Bounds<double> bnd(narrow[narrowmin].m_pos, wide[widemin].m_pos, narrow[narrowmax].m_pos, wide[widemax].m_pos);
        b.SetBounds(bnd);

        // b.m_xmin = narrow[narrowmin].m_pos;
        // b.m_xmax = narrow[narrowmax].m_pos;
        // b.m_ymin = wide[widemin].m_pos;
        // b.m_ymax = wide[widemax].m_pos;
    }
    b.m_left = widemin;
    b.m_right = widemax;
    m_blocks.push_back(b);
}

}} // namespace liblas::chipper


#ifndef LATTICE_CHAIN_OPENBOUNDARY_HPP
#define LATTICE_CHAIN_OPENBOUNDARY_HPP

#include <string>
#include <vector>
#include <boost/lexical_cast.hpp>

namespace lattice {

class ChainOpenBoundary{
public:
  static std::string name() { return "1DChain";}
  static int set_num_particles(int Ns){return Ns;}
  ChainOpenBoundary(unsigned int LatticeSize) : dim_(LatticeSize) {}
  void create_table(std::vector<std::vector < int > >& table){
  //for bulk
    for (int i = 1; i < dim_-1; ++i){
      int l = numberize(i);
      int temp = 0;
      for(int d = -1; d < 2; d =d+2){
      table[l][temp] = numberize(i+d);
      temp += 1;
      }
    }
    int l = numberize(0);
    table[l][0] = numberize(0 + 1);
    table[l][1] = numberize(0);
    l = numberize(dim_-1);
    table[l][0] = numberize(dim_ -1 - 1);
    table[l][1] = numberize(dim_ -1);

  }

  int numberize(const int i){
    int i_t = (i + dim_);
    i_t %= dim_ ;
    return i_t ;
  }

  int latticize(int d, const int l){ //d is direction(0 ~ ) ex. 0 denotes x, 1 denotes y,~
    int i = l;
    return i;
  }

  int number_adjacent() {return 2;}

private:
  unsigned int dim_;
};

}//end namespace

#endif //LATTICE_CHAIN_OPENBOUNDARY_HPP


// ****************************************************************************
// test/non_entity_performance.cpp
//
// Part of the test harness for entity.
//
// Copyright Chris Glover 2014-2016
//
// Distributed under 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
//
// ****************************************************************************
#define DAILY_ENABLE_INSTRUMENTATION 0
#include <iostream>
#include <vector>
#include <algorithm>
#include <daily/timer/instrument.h>
#include "performance_common.hpp"

static const int kNumEntities = TEST_SIZE;
static const float kTestLength = 10.0f;
static const float kFrameTime = 0.016f;

struct PhysicsData
{
	unsigned int id;
	float position;
	float velocity;
	float accel;
#if EXTRA_PADDING
	char pad[EXTRA_PADDING];
#endif
};

template<typename I, typename Fn>
void for_each(I i, I e, Fn f)
{
	#pragma loop( no_vector )
	for(; i != e; ++i)
	{
		f(*i);
	}
}

#define BOOST_TEST_MODULE Performance
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE( non_entity )
{
	std::vector<PhysicsData> entities;

	std::clog << "Created Pools\n";

	{	 DAILY_AUTO_INSTRUMENT_NODE(ComponentCreation);

		unsigned int ids = 0;
		for(int i = 0; i < kNumEntities; ++i)
		{
			PhysicsData p;
			p.id = ids++;
			p.accel = 9.8f;
			p.velocity = 0;
			p.position = 0;
			entities.push_back(p);
		}
	}

	std::clog << "Created Components, simulating...";

	// Simulate over some seconds using a fixed step.
	{
		daily::cpu_timer& simulation_node = daily::timer_map::get_default().create_node("Simulation");
		daily::cpu_timer_scope simulation_scope(simulation_node);

		float time_remaining = kTestLength;
		while(time_remaining > 0)
		{
			::for_each(entities.begin(), entities.end(), [](PhysicsData& p)
			{
				// Add a little to accel each frame.
				p.accel += 0.001f;
			});

			::for_each(entities.begin(), entities.end(), [](PhysicsData& p)
			{
				// Compute new velocity.
				p.velocity += (p.accel/2.f) * (kFrameTime * kFrameTime);
			});

			::for_each(entities.begin(), entities.end(), [](PhysicsData& p)
			{
				// Compute new position.
				p.position += p.velocity * kFrameTime;
			});

			time_remaining -= kFrameTime;
		}
	}

	std::clog << "done.\n";

    std::clog << "Positions: " << entities[0].position << std::endl;
    std::clog << "Velocities: " << entities[0].velocity << std::endl;

	if(!daily::timer_map::get_default().empty())
	{
		std::cout << "---------- Report -----------\n";
		daily::timer_map::get_default().gather_report(
			std::ostream_iterator<daily::timer_map::result_type>(std::cout, "\n")
		);
	}

	std::cout.flush();
}


//==============================================================================
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

#include <boost/simd/sdk/simd/pack.hpp>
#include <boost/simd/include/functions/max.hpp>
#include <boost/simd/include/functions/min.hpp>
#include <boost/simd/include/functions/minus.hpp>
#include <boost/simd/include/functions/selinc.hpp>
#include <boost/simd/include/functions/seldec.hpp>
#include <boost/simd/include/functions/if_zero_else_one.hpp>
#include <boost/simd/include/functions/adds.hpp>
#include <boost/simd/include/functions/is_less.hpp>
#include <boost/simd/include/functions/is_greater.hpp>
#include <boost/simd/include/functions/aligned_store.hpp>
#include <boost/simd/memory/allocator.hpp>
#include <boost/fusion/include/at.hpp>
#include <iostream>

#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/protocol/max_duration.hpp>
#include <nt2/sdk/bench/metric/fps.hpp>
#include <nt2/sdk/bench/metric/cycles_per_element.hpp>
#include <nt2/sdk/bench/stats/median.hpp>
#include <nt2/sdk/bench/setup/geometric.hpp>
#include <nt2/sdk/bench/setup/combination.hpp>
#include <nt2/sdk/bench/setup/constant.hpp>

using namespace nt2::bench;
using namespace nt2;

template<class Pixel>
BOOST_FORCEINLINE Pixel do_work(Pixel &bkg, const Pixel &fr, Pixel &var)
{
  Pixel diff_img, mul_img, zero=0;
  bkg = boost::simd::selinc( bkg < fr, boost::simd::seldec( bkg > fr
                                                          , bkg
                                                          )
                           );
  diff_img = boost::simd::max(bkg, fr) - boost::simd::min(bkg, fr);

  mul_img = boost::simd::adds(boost::simd::adds(diff_img,diff_img),diff_img);

  var = boost::simd::if_else( diff_img != zero, boost::simd::selinc( var < mul_img
                                                                   , boost::simd::seldec( var > mul_img
                                                                                , var
                                                                                )
                                                                   )
                            , var
                            );
  return boost::simd::if_zero_else_one( diff_img < var );
}

template<typename T>
struct sigmadelta_simd
{
  template<typename Setup>
  sigmadelta_simd(Setup const& s)
                    :  height(boost::fusion::at_c<1>(s))
                    ,  width(boost::fusion::at_c<2>(s))
                    ,  size_(height*width)
                    ,  nb_frames(boost::fusion::at_c<0>(s))
  {
    image_variance.resize(size_);
    background_img.resize(size_);
    binary_label.resize(size_);
    frames.resize(nb_frames);

    for(std::size_t k=0; k<nb_frames; k++)
      frames[k].resize(size_);
    for(std::size_t k=0; k<nb_frames; k++)
    {
      for(std::size_t j=0; j<width; j++)
      {
        for(std::size_t i=0; i<height;i++)
        {
          if(i>(height/4) && i<(height/2) && j>((width/4)+k%10) && j<((width/2)+k%10))
            frames[k][i*width+j] = 255;
          else
            frames[k][i*width+j] = 0;
        }
      }
    }
    std::fill(image_variance.begin(), image_variance.end(), 1);
    background_img = frames[0];
    std::fill(binary_label.begin(), binary_label.end(), 0);
  }

  void operator()()
  {
    using boost::simd::pack;
    using boost::simd::aligned_load;
    using boost::simd::aligned_store;

    typedef pack<T> type;

    step_size = boost::simd::meta::cardinal_of<type>::value;
    aligned_sz = size_ & ~(step_size-1);
    for(std::size_t k=1; k<nb_frames; k++)
    {
      it         = 0;
      for(std::size_t m=aligned_sz; it != m; it += step_size)
      {
        type bkg(&background_img[it]);
        type fr(&frames[k][it]);
        type var(&image_variance[it]);
        aligned_store(do_work(bkg,fr,var),&binary_label[it]);
        aligned_store(var,&image_variance[it]);
      }
      for(std::size_t m=size_; it != m; it++)
      {
        binary_label[it] = do_work(background_img[it],frames[k][it], image_variance[it]);
      }
    }
  }

  friend std::ostream& operator<<(std::ostream& os, sigmadelta_simd<T> const& p)
  {
    return os << "(" << p.height << " x " << p.width << " @" << p.nb_frames << ")";
  }

  std::size_t size()         const { return size_ * nb_frames; }
  std::size_t frame_count()  const { return nb_frames; }

  private:
  std::size_t height;
  std::size_t width;
  std::size_t size_, aligned_sz, it, step_size;
  std::vector< std::vector<T, boost::simd::allocator<T> > > frames;
  std::vector<T, boost::simd::allocator<T> > image_variance, background_img, binary_label;
  std::size_t nb_frames;
};

NT2_REGISTER_BENCHMARK( sigmadelta_simd )
{
  std::size_t frame = args("frame",  5);
  std::size_t hmin  = args("hmin",  32);
  std::size_t hmax  = args("hmax", 128);
  std::size_t hstep = args("hstep",  2);
  std::size_t wmin  = args("wmin",  32);
  std::size_t wmax  = args("wmax", 128);
  std::size_t wstep = args("wstep",  2);

  run_during_with< sigmadelta_simd<nt2::uint8_t> > ( 1.
                                                   , and_( constant( frame )
                                                         , geometric(hmin,hmax,hstep)
                                                         , geometric(wmin,wmax,wstep)
                                                         )
                                                   , cycles_per_element<stats::median_>()
                                                   , fps<stats::median_>()
                                                   );
}