#include <boost/numeric/bindings/ublas/matrix.hpp>

#include "SpaceFilter.hpp"
#include "SpaceFilter_impl.hpp"

#include "Circle.hpp"
#include "Disk.hpp"
#include "DiskDiskR.hpp"
#include "CircleCircleR.hpp"
#include "DiskPlanR.hpp"
#include "DiskMovingPlanR.hpp"
#include "SphereLDS.hpp"
#include "SphereLDSSphereLDSR.hpp"
#include "SphereNEDSSphereNEDSR.hpp"
#include "SphereLDSPlanR.hpp"
#include "SphereNEDS.hpp"
#include "SphereNEDSPlanR.hpp"
#include "ExternalBody.hpp"

#include <Model.hpp>
#include <Simulation.hpp>
#include <NonSmoothDynamicalSystem.hpp>
#include <SimulationTypeDef.hpp>
#include <NonSmoothLaw.hpp>

#include <cmath>
//#define DEBUG_MESSAGES 1
#include "debug.h"

/* hash is done with encapsulation */

/* needed by boost hash */
bool operator ==(SP::Hashed const& a, SP::Hashed const& b);
bool operator ==(SP::Hashed const& a, SP::Hashed const& b)
{
  return (a->i == b->i &&
          a->j == b->j &&
          a->k == b->k);
}

std::size_t hash_value(SP::Hashed const& h);
std::size_t hash_value(SP::Hashed const& h)
{
  std::size_t seed = 0;
  boost::hash_combine(seed, h->i);
  boost::hash_combine(seed, h->j);
  boost::hash_combine(seed, h->k);
  return seed;
}

SpaceFilter::SpaceFilter(unsigned int bboxfactor,
              unsigned int cellsize,
              SP::Model model,
              SP::SiconosMatrix plans,
              SP::FMatrix moving_plans) :
    _bboxfactor(bboxfactor),
    _cellsize(cellsize),
    _interID(0),
    _model(model),
    _nslaws(new NSLawMatrix()),
    _plans(plans),
    _moving_plans(moving_plans),
    _osnsinit(false),
    _hash_table(new space_hash()),
    diskdisk_relations(new DiskDiskRDeclaredPool()),
    diskplan_relations(new DiskPlanRDeclaredPool()),
  circlecircle_relations(new CircleCircleRDeclaredPool())
{};

SpaceFilter::SpaceFilter(unsigned int bboxfactor,
              unsigned int cellsize,
              SP::Model model,
              SP::SiconosMatrix plans) :
    _bboxfactor(bboxfactor),
    _cellsize(cellsize),
    _interID(0),
    _model(model),
    _nslaws(new NSLawMatrix()),
    _plans(plans),
    _osnsinit(false),
    _hash_table(new space_hash()),
    diskdisk_relations(new DiskDiskRDeclaredPool()),
    diskplan_relations(new DiskPlanRDeclaredPool()),
    circlecircle_relations(new CircleCircleRDeclaredPool())
{};

SpaceFilter::SpaceFilter(SP::Model model) :
  _model(model),
  _nslaws(new NSLawMatrix()),
  _osnsinit(false),
  _hash_table(new space_hash()),
  diskdisk_relations(new DiskDiskRDeclaredPool()),
  diskplan_relations(new DiskPlanRDeclaredPool()),
  circlecircle_relations(new CircleCircleRDeclaredPool())
{};

SpaceFilter::SpaceFilter() :
  _osnsinit(false),
  _hash_table(new space_hash()),
  diskdisk_relations(new DiskDiskRDeclaredPool()),
  diskplan_relations(new DiskPlanRDeclaredPool()),
  circlecircle_relations(new CircleCircleRDeclaredPool())
{};

/* the hashing is done with a visitor */
struct SpaceFilter::_BodyHash : public SiconosVisitor
{
public:
  SpaceFilter& parent;
  _BodyHash(SpaceFilter& p) : parent(p) {};

  using SiconosVisitor::visit;

  void visit(SP::Disk pds)
  {
    int i, j, imin, imax, jmin, jmax;
    unsigned int _bboxfactor = parent.bboxfactor();
    unsigned int _cellsize = parent.cellsize();

    imin = (int) floor((pds->getQ(0) - _bboxfactor * pds->getRadius()) / _cellsize);
    imax = (int) floor((pds->getQ(0) + _bboxfactor * pds->getRadius()) / _cellsize);

    jmin = (int) floor((pds->getQ(1) - _bboxfactor * pds->getRadius()) / _cellsize);
    jmax = (int) floor((pds->getQ(1) + _bboxfactor * pds->getRadius()) / _cellsize);

    for (i = imin; i <= imax; ++i)
    {
      for (j = jmin; j <= jmax; ++j)
      {
        parent.insert(pds, i, j, 0);
      }
    }
  };

  void visit(SP::Circle pds)
  {
    int i, j, imin, imax, jmin, jmax;

    unsigned int _bboxfactor = parent.bboxfactor();
    unsigned int _cellsize = parent.cellsize();

    imin = (int) floor((pds->getQ(0) - _bboxfactor * pds->getRadius()) / _cellsize);
    imax = (int) floor((pds->getQ(0) + _bboxfactor * pds->getRadius()) / _cellsize);

    jmin = (int) floor((pds->getQ(1) - _bboxfactor * pds->getRadius()) / _cellsize);
    jmax = (int) floor((pds->getQ(1) + _bboxfactor * pds->getRadius()) / _cellsize);

    for (i = imin; i <= imax; ++i)
    {
      for (j = jmin; j <= jmax; ++j)
      {
        parent.insert(pds, i, j, 0);
      }
    }
  }

  void visit(SP::SphereLDS pds)
  {
    int i, j, k, imin, imax, jmin, jmax, kmin, kmax;

    unsigned int _bboxfactor = parent.bboxfactor();
    unsigned int _cellsize = parent.cellsize();

    imin = (int) floor((pds->getQ(0) - _bboxfactor * pds->getRadius()) / _cellsize);
    imax = (int) floor((pds->getQ(0) + _bboxfactor * pds->getRadius()) / _cellsize);

    jmin = (int) floor((pds->getQ(1) - _bboxfactor * pds->getRadius()) / _cellsize);
    jmax = (int) floor((pds->getQ(1) + _bboxfactor * pds->getRadius()) / _cellsize);

    kmin = (int) floor((pds->getQ(2) - _bboxfactor * pds->getRadius()) / _cellsize);
    kmax = (int) floor((pds->getQ(2) + _bboxfactor * pds->getRadius()) / _cellsize);

    for (i = imin; i <= imax; ++i)
    {
      for (j = jmin; j <= jmax; ++j)
      {
        for (k = kmin; k <= kmax; ++k)
        {
          parent.insert(pds, i, j, k);
        }
      }
    }

  }

  void visit(SP::SphereNEDS pds)
  {
    int i, j, k, imin, imax, jmin, jmax, kmin, kmax;

    unsigned int _bboxfactor = parent.bboxfactor();
    unsigned int _cellsize = parent.cellsize();

    imin = (int) floor((pds->getQ(0) - _bboxfactor * pds->getRadius()) / _cellsize);
    imax = (int) floor((pds->getQ(0) + _bboxfactor * pds->getRadius()) / _cellsize);

    jmin = (int) floor((pds->getQ(1) - _bboxfactor * pds->getRadius()) / _cellsize);
    jmax = (int) floor((pds->getQ(1) + _bboxfactor * pds->getRadius()) / _cellsize);

    kmin = (int) floor((pds->getQ(2) - _bboxfactor * pds->getRadius()) / _cellsize);
    kmax = (int) floor((pds->getQ(2) + _bboxfactor * pds->getRadius()) / _cellsize);

    for (i = imin; i <= imax; ++i)
    {
      for (j = jmin; j <= jmax; ++j)
      {
        for (k = kmin; k <= kmax; ++k)
        {
          parent.insert(pds, i, j, k);
        }
      }
    }

  }

  void visit(SP::ExternalBody d)
  {
    d->selfHash(parent);
  }

  /* ... visit other objects */

};

/* proximity detection for circular object */
struct SpaceFilter::_CircularFilter : public SiconosVisitor
{
  using SiconosVisitor::visit;

  SP::SpaceFilter parent;
  SP::CircularDS ds1;

  _CircularFilter(SP::SpaceFilter parent, SP::CircularDS ds1) : parent(parent), ds1(ds1) {};

  void visit_circular(SP::CircularDS ds2)
  {

    SP::CircularR rel;
    SP::DynamicalSystemsGraph DSG0 = parent->model()->nonSmoothDynamicalSystem()->topology()->dSG(0);

    assert(ds1 != ds2);
    assert(DSG0->bundle(DSG0->descriptor(ds1)) == ds1);
    assert(DSG0->bundle(DSG0->descriptor(ds2)) == ds2);

    double r1 = ds1->getRadius();
    double r2 = ds2->getRadius();
    double tol = r1 + r2;

    double x1 = ds1->getQ(0);
    double y1 = ds1->getQ(1);
    double x2 = ds2->getQ(0);
    double y2 = ds2->getQ(1);
    double rmax = fmax(r1, r2);
    double rmin = fmin(r1, r2);

    double d = hypot(x1 - x2, y1 - y2);

    if (d < rmax)
    {
      // one inside the other : CircleCircle relation
      if (rmax - (d + rmin) < tol)
      {
        CircleCircleRDeclaredPool::iterator rcandid =
          parent->circlecircle_relations->find(CircleCircleRDeclared(r1, r2));
        if (rcandid == parent->circlecircle_relations->end())
        {
          // a new relation
          rel.reset(new CircleCircleR(r1, r2));
          (*(parent->circlecircle_relations))[CircleCircleRDeclared(r1, r2)] = rel;
        }
        else
        {
          // get relation from pool
          rel = (*rcandid).second;
        }
      }
    }
    else
    {
      // a DiskDisk relation
      if (d - (r1 + r2) < tol)
      {
        DiskDiskRDeclaredPool::iterator rcandid =
          parent->diskdisk_relations->find(DiskDiskRDeclared(r1, r2));
        if (rcandid == parent->diskdisk_relations->end())
        {
          // a new relation
          rel.reset(new DiskDiskR(r1, r2));

          // FIX : this does not work!!
          // parent->diskdisk_relations[DiskDiskRDeclared(r1,r2)] = rel;
        }
        else
        {
          // get relation from pool
          rel = (*rcandid).second;
        }
      }
    }

    if (rel)
    {
      bool found = false;
      DynamicalSystemsGraph::OEIterator oei, oeiend;
      for (std11::tie(oei, oeiend) = DSG0->out_edges(DSG0->descriptor(ds1));
           oei != oeiend; ++oei)
      {
        if (DSG0->bundle(DSG0->target(*oei)) == ds2)
        {
          found = true;
          break;
        }
      }

      if (!found)
      {
        SP::NonSmoothLaw nslaw = (*parent->_nslaws)(DSG0->groupId[DSG0->descriptor(ds1)],
                                                    DSG0->groupId[DSG0->descriptor(ds2)]);

        SP::Interaction inter(new Interaction(2,
                                              nslaw,
                                              rel, parent->_interID++));
        parent->link(inter, ds1, ds2);
      }
    }
    else
    {
      // is interaction in graph ?
      bool found = false;
      DynamicalSystemsGraph::OEIterator oei, oeiend;
      for (std11::tie(oei, oeiend) = DSG0->out_edges(DSG0->descriptor(ds1));
           oei != oeiend; ++oei)
      {
        if (DSG0->bundle(DSG0->target(*oei)) == ds2)
        {
          found = true;
          break;
        }
      }

      if (found)
      {

        DEBUG_PRINTF("remove interaction : %d\n", DSG0->bundle(*oei)->number());
        parent->model()->nonSmoothDynamicalSystem()->topology()->
        removeInteraction(DSG0->bundle(*oei));
      }
    }
  }

  void visit(SP::Disk disk)
  {
    visit_circular(disk);
  }

  void visit(SP::Circle circle)
  {
    visit_circular(circle);
  }

  // do nothing (everything must be done in ExternalBody.findInteractions)
  void visit(SP::ExternalBody)
  {}

};

/* proximity detection for sphere objects */
struct SpaceFilter::_SphereLDSFilter : public SiconosVisitor
{
  using SiconosVisitor::visit;

  SP::SpaceFilter parent;
  SP::SphereLDS ds1;

  _SphereLDSFilter(SP::SpaceFilter p, SP::SphereLDS s) : parent(p), ds1(s) {};

  void visit(SP::SphereLDS ds2)
  {
    SP::SphereLDSSphereLDSR rel;
    SP::DynamicalSystemsGraph DSG0 = parent->model()->nonSmoothDynamicalSystem()->topology()->dSG(0);

    assert(ds1 != ds2);
    assert(DSG0->bundle(DSG0->descriptor(ds1)) == ds1);
    assert(DSG0->bundle(DSG0->descriptor(ds2)) == ds2);

    double r1 = ds1->getRadius();
    double r2 = ds2->getRadius();
    double tol = r1 + r2;

    double x1 = ds1->getQ(0);
    double y1 = ds1->getQ(1);
    double z1 = ds1->getQ(2);
    double x2 = ds2->getQ(0);
    double y2 = ds2->getQ(1);
    double z2 = ds2->getQ(2);

    double dx = x1 - x2;
    double dy = y1 - y2;
    double dz = z1 - z2;

    double d = sqrt(dx * dx + dy * dy + dz * dz);

    if (d < 2 * tol)
    {
      rel.reset(new SphereLDSSphereLDSR(r1, r2));

      bool found = false;
      DynamicalSystemsGraph::OEIterator oei, oeiend;
      for (std11::tie(oei, oeiend) = DSG0->out_edges(DSG0->descriptor(ds1));
           oei != oeiend; ++oei)
      {
        if (DSG0->bundle(DSG0->target(*oei)) == ds2)
        {
          found = true;
          break;
        }
      }

      if (!found)
      {
        SP::NonSmoothLaw nslaw = (*parent->_nslaws)(DSG0->groupId[DSG0->descriptor(ds1)],
                                                    DSG0->groupId[DSG0->descriptor(ds2)]);

        SP::Interaction inter(new Interaction(3,
                                              nslaw,
                                              rel, parent->_interID++));

        parent->link(inter, ds1, ds2);
      }
    }
    else
    {
      // is interaction in graph ?
      bool found = false;
      DynamicalSystemsGraph::OEIterator oei, oeiend;
      for (std11::tie(oei, oeiend) = DSG0->out_edges(DSG0->descriptor(ds1));
           oei != oeiend; ++oei)
      {
        if (DSG0->bundle(DSG0->target(*oei)) == ds2)
        {
          found = true;
          break;
        }
      }

      if (found)
      {
        parent->model()->nonSmoothDynamicalSystem()->topology()->
        removeInteraction(DSG0->bundle(*oei));
      }

    }
  }
};

struct SpaceFilter::_SphereNEDSFilter : public SiconosVisitor
{
  using SiconosVisitor::visit;

  SP::SpaceFilter parent;
  SP::SphereNEDS ds1;

  _SphereNEDSFilter(SP::SpaceFilter p, SP::SphereNEDS s) : parent(p), ds1(s) {};

  void visit(SP::SphereNEDS ds2)
  {
    SP::SphereNEDSSphereNEDSR rel;
    SP::DynamicalSystemsGraph DSG0 = parent->model()->nonSmoothDynamicalSystem()->topology()->dSG(0);

    assert(ds1 != ds2);
    assert(DSG0->bundle(DSG0->descriptor(ds1)) == ds1);
    assert(DSG0->bundle(DSG0->descriptor(ds2)) == ds2);

    double r1 = ds1->getRadius();
    double r2 = ds2->getRadius();
    double tol = r1 + r2;

    double x1 = ds1->getQ(0);
    double y1 = ds1->getQ(1);
    double z1 = ds1->getQ(2);
    double x2 = ds2->getQ(0);
    double y2 = ds2->getQ(1);
    double z2 = ds2->getQ(2);

    double dx = x1 - x2;
    double dy = y1 - y2;
    double dz = z1 - z2;

    double d = sqrt(dx * dx + dy * dy + dz * dz);

    if (d < 2 * tol)
    {
      rel.reset(new SphereNEDSSphereNEDSR(r1, r2));

      bool found = false;
      DynamicalSystemsGraph::OEIterator oei, oeiend;
      for (std11::tie(oei, oeiend) = DSG0->out_edges(DSG0->descriptor(ds1));
           oei != oeiend; ++oei)
      {
        if (DSG0->bundle(DSG0->target(*oei)) == ds2)
        {
          found = true;
          break;
        }
      }

      if (!found)
      {
        SP::NonSmoothLaw nslaw = (*parent->_nslaws)(DSG0->groupId[DSG0->descriptor(ds1)],
                                                    DSG0->groupId[DSG0->descriptor(ds2)]);

        SP::Interaction inter(new Interaction(3,
                                              nslaw,
                                              rel, parent->_interID++));

        parent->link(inter, ds1, ds2);
      }
    }
    else
    {
      // is interaction in graph ?
      bool found = false;
      DynamicalSystemsGraph::OEIterator oei, oeiend;
      for (std11::tie(oei, oeiend) = DSG0->out_edges(DSG0->descriptor(ds1));
           oei != oeiend; ++oei)
      {
        if (DSG0->bundle(DSG0->target(*oei)) == ds2)
        {
          found = true;
          break;
        }
      }

      if (found)
      {
        parent->model()->nonSmoothDynamicalSystem()->topology()->
        removeInteraction(DSG0->bundle(*oei));
      }

    }
  }
};

/* disk plan relation comparison */
struct SpaceFilter::_IsSameDiskPlanR : public SiconosVisitor
{

  using SiconosVisitor::visit;

  SP::SpaceFilter parent;
  double A, B, C, r, xCenter, yCenter, width;
  bool flag;
  _IsSameDiskPlanR(SP::SpaceFilter p, double A, double B, double C, double r,
                   double xCenter, double yCenter, double width) :
    parent(p), A(A), B(B), C(C), r(r), xCenter(xCenter), yCenter(yCenter), width(width), flag(false) {};

  void visit(const DiskDiskR&)
  {
    flag = false;
  };

  void visit(const CircleCircleR&)
  {
    flag = false;
  };

  void visit(const DiskMovingPlanR&)
  {
    flag = false;
  };

  void visit(const LagrangianScleronomousR&)
  {
    flag = false;
  };

  void visit(const DiskPlanR& rel)
  {
    flag = rel.equal(A, B, C, r, xCenter, yCenter, width);
  };

};

struct SpaceFilter::_IsSameDiskMovingPlanR : public SiconosVisitor
{

  using SiconosVisitor::visit;

  SP::SpaceFilter parent;
  FTime AF, BF, CF;
  double r;
  bool flag;
  _IsSameDiskMovingPlanR(SP::SpaceFilter p, FTime AF, FTime BF, FTime CF, double r) :
    parent(p), AF(AF), BF(BF), CF(CF), r(r), flag(false) {};

  void visit(const DiskDiskR&)
  {
    flag = false;
  };

  void visit(const CircleCircleR&)
  {
    flag = false;
  };

  void visit(const DiskPlanR&)
  {
    flag = false;
  };

  void visit(const LagrangianScleronomousR&)
  {
    flag = false;
  }

  void visit(const DiskMovingPlanR& rel)
  {
    flag = rel.equal(AF, BF, CF, r);
  };

};

/* sphere plan relation comparison */
struct SpaceFilter::_IsSameSpherePlanR : public SiconosVisitor
{

  using SiconosVisitor::visit;

  SP::SpaceFilter parent;
  double A, B, C, D, r;
  bool flag;
  _IsSameSpherePlanR(SP::SpaceFilter p, double A, double B, double C, double D, double r):
    parent(p), A(A), B(B), C(C), D(D), r(r), flag(false) {};

  void visit(const SphereLDSSphereLDSR&)
  {
    flag = false;
  };

  void visit(const SphereNEDSSphereNEDSR&)
  {
    flag = false;
  };

  void visit(const SphereLDSPlanR& rel)
  {
    flag = rel.equal(A, B, C, D, r);
  };

  void visit(const SphereNEDSPlanR& rel)
  {
    flag = rel.equal(A, B, C, D, r);
  };
};

/* proximity detection between circular object and plans */
void SpaceFilter::_PlanCircularFilter(double A, double B, double C,
                                      double xCenter, double yCenter,
                                      double width, SP::CircularDS ds)
{
  double r = ds->getRadius();

  /* tolerance */
  double tol = r;

  SP::DynamicalSystemsGraph DSG0 = model()->nonSmoothDynamicalSystem()->topology()->dSG(0);

  _IsSameDiskPlanR
  isSameDiskPlanR = _IsSameDiskPlanR(shared_from_this(), A, B, C, r,
                                     xCenter, yCenter, width);

  // all DS must be in DS graph
  assert(DSG0->bundle(DSG0->descriptor(ds)) == ds);
  SP::DiskPlanR relp(new DiskPlanR(r, A, B, C, xCenter, yCenter, width));

  if (relp->distance(ds->getQ(0),
                     ds->getQ(1),
                     ds->getRadius()) < tol)

  {
    // is interaction in graph ?
    bool found = false;
    DynamicalSystemsGraph::OEIterator oei, oeiend;
    for (std11::tie(oei, oeiend) = DSG0->out_edges(DSG0->descriptor(ds));
         oei != oeiend; ++oei)
    {
      DSG0->bundle(*oei)
      ->relation()->accept(isSameDiskPlanR);
      if (DSG0->bundle(DSG0->target(*oei)) == ds
          && isSameDiskPlanR.flag)
      {
        found = true;
        break;
      }
    }

    if (!found)
      // no
    {
      SP::NonSmoothLaw nslaw = (*_nslaws)(DSG0->groupId[DSG0->descriptor(ds)],
                                          DSG0->groupId[DSG0->descriptor(ds)]);

      SP::Interaction inter(new Interaction(2,
                                            nslaw,
                                            relp, _interID++));
      DEBUG_PRINTF("insert interaction : %d\n", inter->number());
      link(inter, ds);
    }
  }
  else
  {
    // is interaction in graph ?
    DynamicalSystemsGraph::OEIterator oei, oeiend;
    for (std11::tie(oei, oeiend) = DSG0->out_edges(DSG0->descriptor(ds));
         oei != oeiend; ++oei)
    {
      DSG0->bundle(*oei)
      ->relation()->accept(isSameDiskPlanR);

      if (DSG0->bundle(DSG0->target(*oei)) == ds
          && isSameDiskPlanR.flag)
      {

        DEBUG_PRINTF("remove interaction : %d\n", DSG0->bundle(*oei)->number());

        model()->nonSmoothDynamicalSystem()->topology()->
        removeInteraction(DSG0->bundle(*oei));
        break;
      }
    }
  }
}

/* proximity detection between circular object and plans */
void SpaceFilter::_MovingPlanCircularFilter(unsigned int i, SP::CircularDS ds, double time)
{
  double r = ds->getRadius();

  /* tolerance */
  double tol = r;

  SP::DynamicalSystemsGraph DSG0 = model()->nonSmoothDynamicalSystem()->topology()->dSG(0);

  _IsSameDiskMovingPlanR
  isSameDiskMovingPlanR = _IsSameDiskMovingPlanR(shared_from_this(),
                          (*_moving_plans)(i, 0),
                          (*_moving_plans)(i, 1),
                          (*_moving_plans)(i, 2),
                          r);

  // all DS must be in DS graph
  assert(DSG0->bundle(DSG0->descriptor(ds)) == ds);
  SP::DiskMovingPlanR relp(new DiskMovingPlanR((*_moving_plans)(i, 0), (*_moving_plans)(i, 1), (*_moving_plans)(i, 2),
                           (*_moving_plans)(i, 3), (*_moving_plans)(i, 4), (*_moving_plans)(i, 5), r));

  relp->init(time);

  if (relp->distance(ds->getQ(0),
                     ds->getQ(1),
                     ds->getRadius()) < tol)

  {
    // is interaction in graph ?
    bool found = false;
    DynamicalSystemsGraph::OEIterator oei, oeiend;
    for (std11::tie(oei, oeiend) = DSG0->out_edges(DSG0->descriptor(ds));
         oei != oeiend; ++oei)
    {
      DSG0->bundle(*oei)
      ->relation()->accept(isSameDiskMovingPlanR);
      if (DSG0->bundle(DSG0->target(*oei)) == ds
          && isSameDiskMovingPlanR.flag)
      {
        found = true;
        break;
      }
    }
    if (!found)
      // no
    {
      SP::NonSmoothLaw nslaw = (*_nslaws)(DSG0->groupId[DSG0->descriptor(ds)],
                                          DSG0->groupId[DSG0->descriptor(ds)]);

      SP::Interaction inter(new Interaction(2,
                                            nslaw,
                                            relp, _interID++));
      link(inter, ds);
    }
  }
  else
  {
    // is interaction in graph ?
    DynamicalSystemsGraph::OEIterator oei, oeiend;
    for (std11::tie(oei, oeiend) = DSG0->out_edges(DSG0->descriptor(ds));
         oei != oeiend; ++oei)
    {
      DSG0->bundle(*oei)
      ->relation()->accept(isSameDiskMovingPlanR);

      if (DSG0->bundle(DSG0->target(*oei)) == ds
          && isSameDiskMovingPlanR.flag)
      {
        model()->nonSmoothDynamicalSystem()->topology()->
        removeInteraction(DSG0->bundle(*oei));
        break;
      }
    }
  }
}

/* proximity detection between circular object and plans */
void SpaceFilter::_PlanSphereLDSFilter(double A, double B, double C, double D, SP::SphereLDS ds)
{
  double r = ds->getRadius();

  /* tolerance */
  double tol = r;

  SP::DynamicalSystemsGraph DSG0 = model()->nonSmoothDynamicalSystem()->topology()->dSG(0);

  _IsSameSpherePlanR
  IsSameSpherePlanR =
    _IsSameSpherePlanR(shared_from_this(), A, B, C, D, r);

  // all DS must be in DS graph
  assert(DSG0->bundle(DSG0->descriptor(ds)) == ds);
  SP::SphereLDSPlanR relp(new SphereLDSPlanR(r, A, B, C, D));
  if (relp->distance(ds->getQ(0),
                     ds->getQ(1),
                     ds->getQ(2),
                     ds->getRadius()) < tol)

  {
    // is interaction in graph ?
    bool found = false;
    DynamicalSystemsGraph::OEIterator oei, oeiend;
    for (std11::tie(oei, oeiend) = DSG0->out_edges(DSG0->descriptor(ds));
         oei != oeiend; ++oei)
    {
      DSG0->bundle(*oei)
      ->relation()->accept(IsSameSpherePlanR);
      if (DSG0->bundle(DSG0->target(*oei)) == ds
          && IsSameSpherePlanR.flag)
      {
        found = true;
        break;
      }
    }
    if (!found)
      // no
    {
      SP::NonSmoothLaw nslaw = (*_nslaws)(DSG0->groupId[DSG0->descriptor(ds)],
                                                  DSG0->groupId[DSG0->descriptor(ds)]);

      SP::Interaction inter(new Interaction(3,
                                            nslaw,
                                            relp, _interID++));
      link(inter, ds);
    }
  }
  else
  {
    // is interaction in graph ?
    DynamicalSystemsGraph::OEIterator oei, oeiend;
    for (std11::tie(oei, oeiend) = DSG0->out_edges(DSG0->descriptor(ds));
         oei != oeiend; ++oei)
    {
      DSG0->bundle(*oei)
      ->relation()->accept(IsSameSpherePlanR);

      if (DSG0->bundle(DSG0->target(*oei)) == ds
          && IsSameSpherePlanR.flag)
      {
        model()->nonSmoothDynamicalSystem()->topology()->
        removeInteraction(DSG0->bundle(*oei));
        break;
      }
    }
  }
}

// note : all PlanObject should be merged
void SpaceFilter::_PlanSphereNEDSFilter(double A, double B, double C, double D, SP::SphereNEDS ds)
{
  double r = ds->getRadius();

  /* tolerance */
  double tol = r;

  SP::DynamicalSystemsGraph DSG0 = model()->nonSmoothDynamicalSystem()->topology()->dSG(0);

  _IsSameSpherePlanR
  isSameSpherePlanR =
    _IsSameSpherePlanR(shared_from_this(), A, B, C, D, r);

  // all DS must be in DS graph
  assert(DSG0->bundle(DSG0->descriptor(ds)) == ds);
  SP::SphereNEDSPlanR relp(new SphereNEDSPlanR(r, A, B, C, D));
  if (relp->distance(ds->getQ(0),
                     ds->getQ(1),
                     ds->getQ(2),
                     ds->getRadius()) < tol)

  {
    // is interaction in graph ?
    bool found = false;
    DynamicalSystemsGraph::OEIterator oei, oeiend;
    for (std11::tie(oei, oeiend) = DSG0->out_edges(DSG0->descriptor(ds));
         oei != oeiend; ++oei)
    {
      DSG0->bundle(*oei)
      ->relation()->accept(isSameSpherePlanR);
      if (DSG0->bundle(DSG0->target(*oei)) == ds
          && isSameSpherePlanR.flag)
      {
        found = true;
        break;
      }
    }
    if (!found)
      // no
    {
      SP::NonSmoothLaw nslaw = (*_nslaws)(DSG0->groupId[DSG0->descriptor(ds)],
                                          DSG0->groupId[DSG0->descriptor(ds)]);

      SP::Interaction inter(new Interaction(3,
                                            nslaw,
                                            relp, _interID++));
      link(inter, ds);
    }
  }
  else
  {
    // is interaction in graph ?
    DynamicalSystemsGraph::OEIterator oei, oeiend;
    for (std11::tie(oei, oeiend) = DSG0->out_edges(DSG0->descriptor(ds));
         oei != oeiend; ++oei)
    {
      DSG0->bundle(*oei)
      ->relation()->accept(isSameSpherePlanR);

      if (DSG0->bundle(DSG0->target(*oei)) == ds
          && isSameSpherePlanR.flag)
      {
        model()->nonSmoothDynamicalSystem()->topology()->
        removeInteraction(DSG0->bundle(*oei));
        break;
      }
    }
  }
}

/* insertion */
void SpaceFilter::insert(SP::Disk ds, int i, int j, int k)
{

  SP::Hashed hashed(new Hashed(std11::static_pointer_cast<DynamicalSystem>(ds), i, j));
  _hash_table->insert(hashed);
}

void SpaceFilter::insert(SP::Circle ds, int i, int j, int k)
{

  SP::Hashed hashed(new Hashed(std11::static_pointer_cast<DynamicalSystem>(ds), i, j));
  _hash_table->insert(hashed);
}

void SpaceFilter::insert(SP::SphereLDS ds, int i, int j, int k)
{

  SP::Hashed hashed(new Hashed(ds, i, j, k));
  _hash_table->insert(hashed);
}

void SpaceFilter::insert(SP::SphereNEDS ds, int i, int j, int k)
{

  SP::Hashed hashed(new Hashed(ds, i, j, k));
  _hash_table->insert(hashed);
}

void SpaceFilter::insert(SP::Hashed hashed)
{
  _hash_table->insert(hashed);
}

/* insert other objects */

/* dynamical systems proximity detection */
typedef std::pair<int, int> interPair;
bool operator ==(interPair const& a, interPair const& b);
bool operator ==(interPair const& a, interPair const& b)
{
  return ((a.first == b.first) && (a.second == b.second));
}

bool operator ==(std::pair<double, double> const& a,
                 std::pair<double, double> const& b);
bool operator ==(std::pair<double, double> const& a,
                 std::pair<double, double> const& b)
{
  return ((a.first == b.first) && (a.second == b.second));
}

bool operator ==(DiskPlanRDeclared const& a, DiskPlanRDeclared const& b);
bool operator ==(DiskPlanRDeclared const& a, DiskPlanRDeclared const& b)
{
  return ((a[0] == b[0] &&
           a[1] == b[1] &&
           a[2] == b[2] &&
           a[3] == b[3] &&
           a[4] == b[4] &&
           a[5] == b[5]));
}

struct SpaceFilter::_FindInteractions : public SiconosVisitor
{

  using SiconosVisitor::visit;

  typedef boost::unordered_multiset < interPair,
          boost::hash<interPair> > interPairs;

  SP::SpaceFilter parent;
  double time;
  _FindInteractions(SP::SpaceFilter p, double time) : parent(p), time(time) {};

  void visit_circular(SP::CircularDS  ds1)
  {
    assert(parent->_plans->size(0) > 0);

    // interactions with plans

    if (parent->_plans)
    {
      for (unsigned int i = 0; i < parent->_plans->size(0); ++i)
      {
        parent->_PlanCircularFilter((*parent->_plans)(i, 0),
                                    (*parent->_plans)(i, 1),
                                    (*parent->_plans)(i, 2),
                                    (*parent->_plans)(i, 3),
                                    (*parent->_plans)(i, 4),
                                    (*parent->_plans)(i, 5),
                                    ds1);
      }
    }

    if (parent->_moving_plans)
    {
      for (unsigned int i = 0; i < parent->_moving_plans->size1(); ++i)
      {
        parent->_MovingPlanCircularFilter(i, ds1, time);
      }
    }

    SP::SiconosVector Q1 = ds1->q();

    double x1 = Q1->getValue(0);
    double y1 = Q1->getValue(1);
    SP::Hashed hds1(new Hashed(std11::static_pointer_cast<DynamicalSystem>(ds1),
                             (int) floor(x1 / parent->_cellsize),
                               (int) floor(y1 / parent->_cellsize)));

    // find all other systems that are in the same cells
    std::pair<space_hash::iterator, space_hash::iterator>
    neighbours = parent->_hash_table->equal_range(hds1);

    unsigned int j;
    interPairs declaredInteractions;
    std11::shared_ptr<_CircularFilter>
    circularFilter(new _CircularFilter(parent, ds1));

    for (j = 0; neighbours.first != neighbours.second; ++neighbours.first, ++j)
    {
      SP::DynamicalSystem ds2 = (*neighbours.first)->body;
      int ids1 = ds1->number();
      int ids2 = ds2->number();
      int imax = (std::max)(ids1, ids2);
      int imin = (std::min)(ids1, ids2);
      if (ids1 != ids2)
      {
        // is interaction already treated ?
        interPair interpair;
        interpair = std::pair<int, int>(imin, imax);

        if (declaredInteractions.find(interpair)
            == declaredInteractions.end())
        {
          // no, check proximity
          declaredInteractions.insert(interpair);
          ds2->acceptSP(circularFilter);
        }

      }
    }
  };

  void visit(SP::Circle circle)
  {
    visit_circular(circle);
  };

  void visit(SP::Disk disk)
  {
    visit_circular(disk);
  };

  void visit(SP::SphereLDS ds1)
  {
    // interactions with plans
    for (unsigned int i = 0; i < parent->_plans->size(0); ++i)
    {
      parent->_PlanSphereLDSFilter((*parent->_plans)(i, 0),
                                   (*parent->_plans)(i, 1),
                                   (*parent->_plans)(i, 2),
                                   (*parent->_plans)(i, 3), ds1);
    }

    SP::SiconosVector Q1 = ds1->q();

    double x1 = Q1->getValue(0);
    double y1 = Q1->getValue(1);
    double z1 = Q1->getValue(2);
    SP::Hashed hds1(new Hashed(ds1, (int) floor(x1 / parent->_cellsize),
                               (int) floor(y1 / parent->_cellsize),
                               (int) floor(z1 / parent->_cellsize)));

    // find all other systems that are in the same cells
    std::pair<space_hash::iterator, space_hash::iterator>
    neighbours = parent->_hash_table->equal_range(hds1);

    unsigned int j;
    interPairs declaredInteractions;

    std11::shared_ptr<_SphereLDSFilter> sphereFilter(new _SphereLDSFilter(parent, ds1));

    for (j = 0; neighbours.first != neighbours.second; ++neighbours.first, ++j)
    {
      SP::DynamicalSystem ds2 = (*neighbours.first)->body;
      int ids1 = ds1->number();
      int ids2 = ds2->number();
      int imax = (std::max)(ids1, ids2);
      int imin = (std::min)(ids1, ids2);
      if (ids1 != ids2)
      {
        // is interaction already treated ?
        interPair interpair;
        interpair = std::pair<int, int>(imin, imax);

        if (declaredInteractions.find(interpair)
            == declaredInteractions.end())
        {
          // no, check proximity
          declaredInteractions.insert(interpair);
          ds2->acceptSP(sphereFilter);
        }

      }
    }
  }

  void visit(SP::SphereNEDS ds1)
  {
    // interactions with plans
    for (unsigned int i = 0; i < parent->_plans->size(0); ++i)
    {
      parent->_PlanSphereNEDSFilter((*parent->_plans)(i, 0),
                                    (*parent->_plans)(i, 1),
                                    (*parent->_plans)(i, 2),
                                    (*parent->_plans)(i, 3), ds1);
    }

    SP::SiconosVector Q1 = ds1->q();

    double x1 = Q1->getValue(0);
    double y1 = Q1->getValue(1);
    double z1 = Q1->getValue(2);
    SP::Hashed hds1(new Hashed(ds1, (int) floor(x1 / parent->_cellsize),
                               (int) floor(y1 / parent->_cellsize),
                               (int) floor(z1 / parent->_cellsize)));

    // find all other systems that are in the same cells
    std::pair<space_hash::iterator, space_hash::iterator>
    neighbours = parent->_hash_table->equal_range(hds1);

    unsigned int j;
    interPairs declaredInteractions;

    std11::shared_ptr<_SphereNEDSFilter> sphereFilter(new _SphereNEDSFilter(parent, ds1));

    for (j = 0; neighbours.first != neighbours.second; ++neighbours.first, ++j)
    {
      SP::DynamicalSystem ds2 = (*neighbours.first)->body;
      int ids1 = ds1->number();
      int ids2 = ds2->number();
      int imax = (std::max)(ids1, ids2);
      int imin = (std::min)(ids1, ids2);
      if (ids1 != ids2)
      {
        // is interaction already treated ?
        interPair interpair;
        interpair = std::pair<int, int>(imin, imax);

        if (declaredInteractions.find(interpair)
            == declaredInteractions.end())
        {
          // no, check proximity
          declaredInteractions.insert(interpair);
          ds2->acceptSP(sphereFilter);
        }

      }
    }
  }

  void visit(SP::ExternalBody d)
  {
    d->selfFindInteractions(parent);
  }

};

void SpaceFilter::link(SP::Interaction inter, SP::DynamicalSystem ds1,
                       SP::DynamicalSystem ds2)
{
  DEBUG_PRINTF("link interaction : %d\n", inter->number());
  model()->nonSmoothDynamicalSystem()->link(inter, ds1, ds2);
  model()->simulation()->computeLevelsForInputAndOutput(inter);
  // Note FP : ds init should probably be done once and only once for
  // all ds (like in simulation->initialize()) but where/when?
  unsigned int levelMinForInput = inter->lowerLevelForInput();
  unsigned int levelMaxForInput = inter->upperLevelForInput();
  bool has2DS = inter->has2Bodies();
  for (unsigned int k = levelMinForInput ; k < levelMaxForInput + 1; k++)
  {
    ds1->initializeNonSmoothInput(k);
    if(has2DS)
      ds2->initializeNonSmoothInput(k);
  }

  SP::InteractionsGraph indexSet0 = model()->nonSmoothDynamicalSystem()->topology()->indexSet0();
  InteractionsGraph::VDescriptor ui = indexSet0->descriptor(inter);

  inter->initialize(model()->simulation()->nextTime(), indexSet0->properties(ui));
  //inter->initialize(model()->simulation()->nextTime(), ds1, ds2);
}

/* general proximity detection */
void SpaceFilter::buildInteractions(double time)
{
  SP::DynamicalSystemsGraph
  DSG0 = model()->nonSmoothDynamicalSystem()->topology()->dSG(0);

  std11::shared_ptr<_BodyHash>
  hasher(new _BodyHash(*this));

  std11::shared_ptr<_FindInteractions>
  findInteractions(new _FindInteractions(shared_from_this(), time));

  _hash_table->clear();

  // 1: rehash DS
  DynamicalSystemsGraph::VIterator vi, viend;
  for (std11::tie(vi, viend) = DSG0->vertices();
       vi != viend; ++vi)
  {
    // to avoid cast see dual dispatch, visitor pattern
    DSG0->bundle(*vi)->acceptSP(hasher);
  }

  // 2: prox detection
  for (std11::tie(vi, viend) = DSG0->vertices();
       vi != viend; ++vi)
  {
    DSG0->bundle(*vi)->acceptSP(findInteractions);
  }
  model()->simulation()->initOSNS();
}

//std::pair<space_hash::iterator, space_hash::iterator> SpaceFilter::neighbours(SP::Hashed h)
//{
//  return _hash_table.equal_range(h);
//}

bool SpaceFilter::haveNeighbours(SP::Hashed h)
{
  std::pair<space_hash::iterator, space_hash::iterator> neighbours
    = _hash_table->equal_range(h);
  return (neighbours.first != neighbours.second);
}

struct SpaceFilter::_DiskDistance : public SiconosVisitor
{

  using SiconosVisitor::visit;

  double x;
  double y;
  double r;
  double result;

  _DiskDistance(double x, double y, double r)
    : x(x), y(y), r(r)
  {};

  void visit(SP::Disk d)
  {
    double xd = d->q()->getValue(0);
    double yd = d->q()->getValue(1);

    result = (hypot(x - xd, y - yd) - (r + d->getRadius()));
  }
};

/* only for disks at the moment */
double SpaceFilter::minDistance(SP::Hashed h)
{
  std::pair<space_hash::iterator, space_hash::iterator> neighbours
    = _hash_table->equal_range(h);

  SP::SiconosVector q = std11::static_pointer_cast<LagrangianDS>(h->body)->q();

  double dmin = INFINITY;

  {
    SP::Disk disk = std11::static_pointer_cast<Disk>(h->body);

    std11::shared_ptr<_DiskDistance> distance(new _DiskDistance((*q)(0), (*q)(1), disk->getRadius()));

    for (; neighbours.first != neighbours.second; ++neighbours.first)
    {
      (*neighbours.first)->body->acceptSP(distance);

      dmin = (std::min)(dmin, distance->result);
    }
  }

  return dmin;

}

void SpaceFilter::insert(SP::NonSmoothLaw nslaw,
                         long unsigned int id1,
                         long unsigned int id2)
{
  NSLawMatrix& nslaws = *_nslaws;

  // ublas::matrix size type is not the same on 32 bits and 64 bits
  unsigned int id = std::max((unsigned int) id1, (unsigned int) id2);
  nslaws.resize (std::max((unsigned int) nslaws.size1(), (unsigned int) id+1));

  nslaws(id1, id2) = nslaw;
}


#include "stdafx.h"

#include <vector>

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

#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>

#include <iostream>

template<int blockSize>
class SparseBlockSquareMatrix {
public :
	typedef struct {
		float e[blockSize * blockSize];
	} block_t;

	typedef struct {
		int col;
		int blk;
	} offset_t;
public :
	int gridSize;

	// buffers
	std::vector<int>				rowScan;		// row offset
	std::vector<offset_t>			blockInfos;		// column index and block offset for row elements
	std::vector<block_t>			blocks;			// blocks buffer

	// graph of sparse matrix for creating buffers
	std::vector<std::vector<int>>	colLinks;

public :
	void free( void ) {
		gridSize = 0;
		rowScan.clear();
		blockInfos.clear();
		colLinks.clear();
	}

	// gridSize = mat.size / blockSize
	void create( int gridSize ) {
		free();

		this->gridSize = gridSize;

		colLinks.resize( gridSize );

		for ( int i = 0; i < gridSize; i++ ) {
			insertLink( i, i );
		}
	}

	bool isIndexValid( int row, int col ) const {
		return ( row >= 0 && row < gridSize && col >= 0 && col < gridSize );
	}

	int size( void ) const {
		return gridSize * blockSize;
	}

	int calcBlockIndex( int row, int col ) const {
		assert( isIndexValid( row, col ) );

		int index = findLink( row, col );
		if ( index < 0 ) {
			return -1;
		}

		return blockInfos[rowScan[row] + index].blk;
	}

	// FIXME: linear search, improve
	int findLink( int row, int col ) const {
		assert( isIndexValid( row, col ) );
		const std::vector<int> &links = colLinks[row];
		for ( int i = 0; i < links.size(); i++ ) {
			if ( links[i] == col ) {
				return i;
			}
		}
		return -1;
	}

	void insertLink( int row, int col ) {
		assert( isIndexValid( row, col ) );
		int index = findLink( row, col );
		if ( index < 0 ) {
			colLinks[row].push_back( col );
		}
	}

	// only alloc buffer for upper triangle elements
	void alloc( void ) {
		// row scan buffer
		rowScan.resize( gridSize + 1 ); // add additional one for total number
		rowScan[0] = 0;
		for ( int i = 1; i <= gridSize; i++ ) {
			rowScan[i] = rowScan[i-1] + colLinks[i-1].size();
		}

		// block info buffer
		int blockInfosIndex = 0;
		int blockIndex = 0;
		blockInfos.resize( rowScan[gridSize] );
		for ( int i = 0; i < colLinks.size(); i++ ) {
			const std::vector<int> &links = colLinks[i];
			for ( int j = 0; j < links.size(); j++ ) {
				int col = links[j];

				offset_t &index = blockInfos[blockInfosIndex];

				index.col = col;
				if ( col < i ) {
					index.blk = calcBlockIndex( col, i );
				} else {
					index.blk = blockIndex;
					++blockIndex;
				}

				++blockInfosIndex;
			}
		}

		// block buffer
		blocks.resize( blockIndex );
		memset( blocks.data(), 0, blocks.size() * sizeof( block_t ) );

		std::cout << blocks.size() << std::endl;
	}

	Eigen::VectorXf diagonal( void ) const {
		Eigen::VectorXf diag = Eigen::VectorXf::Zero( size() );

		for ( int i = 0; i < gridSize; i++ ) {
			const block_t &block = blocks[calcBlockIndex( i, i )];
			for ( int k = 0; k < blockSize; k++ ) {
				diag[k + i * blockSize] = block.e[k + k * blockSize];
			}
		}

		return diag;
	}

	static void mad_mat_mult_v( int size, const float *B, const float *v, float *r ) {
		for ( int row = 0; row < blockSize; row++ ) {
			float sum = 0.0f;
			for ( int col = 0; col < blockSize; col++ ) {
				sum += B[col + row * blockSize] * v[col];
			}
			r[row] += sum; // FIXME : +=
		}
	}

	static void mad_v_mult_mat( int size, const float *B, const float *v, float *r ) {
		for ( int row = 0; row < blockSize; row++ ) {
			float sum = 0.0f;
			for ( int col = 0; col < blockSize; col++ ) {
				sum += B[row + col * blockSize] * v[col];
			}
			r[row] += sum; // FIXME : +=
		}
	}

	Eigen::VectorXf operator*( const Eigen::VectorXf &v ) const {
		Eigen::VectorXf r = Eigen::VectorXf::Zero( size() );

		for ( int row = 0; row < gridSize; row++ ) { // for each row
			int start = rowScan[row];
			int end = rowScan[row+1]; // column blocks
			for ( int i = start; i < end; i++ ) {
				offset_t info = blockInfos[i];
				int col = info.col;
				int blk = info.blk;

				const float *ptr_B = blocks[blk].e;
				const float *ptr_v = &v[col * blockSize];
				float *ptr_r = &r[row * blockSize];

				if ( col >= row ) {
					mad_mat_mult_v( gridSize, ptr_B, ptr_v, ptr_r );
				} else {
					mad_v_mult_mat( gridSize, ptr_B, ptr_v, ptr_r );
				}
			}
		}

		return r;
	}

	SparseBlockSquareMatrix<blockSize> &operator=( const Eigen::MatrixXf &m ) {
		assert( m.rows() == m.cols() );
		create( m.rows() / blockSize );

		for ( int y = 0; y < gridSize; y++ ) {
		for ( int x = 0; x < gridSize; x++ ) {
			insertLink( y, x );
		}}
		alloc();

		for ( int y = 0; y < gridSize; y++ ) {
		for ( int x = y; x < gridSize; x++ ) {
			block_t &block = blocks[calcBlockIndex( y, x )];

			for ( int ly = 0; ly < blockSize; ly++ ) {
			for ( int lx = 0; lx < blockSize; lx++ ) {
				block.e[lx + ly * blockSize] = m(ly + y * blockSize, lx + x * blockSize);
			}}
		}}
		return *this;
	}
};

Eigen::VectorXf PCG( const SparseBlockSquareMatrix<12> &A, const Eigen::VectorXf &b, int maxIters, float threshold = 1e-6 ) {
	Eigen::VectorXf M = A.diagonal();
	for ( int i = 0; i < M.rows(); i++ ) {
		if ( fabs( M[i] ) > threshold ) {
			M[i] = 1.0f / M[i];
		} else {
			M[i] = 1.0f;
		}
	}

	Eigen::VectorXf x = Eigen::VectorXf::Zero(A.size());
	Eigen::VectorXf p = Eigen::VectorXf::Zero(A.size());
	Eigen::VectorXf r = b; // b - A * x

	for ( int i = 0; i < maxIters; i++ ) {
		float rme = sqrt( r.dot( r ) / A.size() );
		printf( "PCG : %d : %f\n", i, rme );
		if ( rme < 1e-6 ) {
			break;
		}
		Eigen::VectorXf Mr = M.array() * r.array();
		float r_rMr = 1.0f / r.dot( Mr );
		p += Mr * r_rMr;

		Eigen::VectorXf Ap = A * p;
		float r_pAp = 1.0f / p.dot( Ap );
		x +=  p * r_pAp;
		r -= Ap * r_pAp;
	}

	return x;
}

void TestPCG( void ) {
	int dim = 48;

	// A : positive definite matrix
	Eigen::MatrixXf A = Eigen::MatrixXf::Random( dim, dim );
	A = A * A.transpose();
	A += Eigen::MatrixXf::Identity( dim, dim ) * dim;

	//std::cout << A << std::endl;

	// x
	Eigen::VectorXf x = Eigen::VectorXf::Random( dim );

	// b
	Eigen::VectorXf b = A * x;

	{
		Eigen::VectorXf xx = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
		std::cout << ( xx - x ).dot( xx - x ) / xx.rows() << std::endl << std::endl;
	}

	{
		SparseBlockSquareMatrix<12> B;
		B = A;
		Eigen::VectorXf xx = PCG( B, b, 100 );
		std::cout << ( xx - x ).dot( xx - x ) / xx.rows() << std::endl << std::endl;
	}
}

int _tmain(int argc, _TCHAR* argv[])
{
	TestPCG();

	return 0;
}


// This code is licensed under the MIT License (MIT). See LICENCE.txt for details
#pragma once

#include <algorithm>
#include <cstdint>
#include <exception>
#include <iostream>
#include <numeric>
#include <sstream>
#include <string>

#include <boost/lexical_cast.hpp>

namespace collatzOpenCL
{

class ValidateErr : public std::exception {
public:
  ValidateErr(const std::string& val, unsigned int expectedStep, unsigned int calculatedStep)
  {
    std::stringstream ss("Value :");
    ss << val << "\nExpected Step: " << expectedStep << "\nCalculated Step: " << calculatedStep << '\n';
    errMsg_.assign(ss.str());
  }
  const char* what() const override { return errMsg_.c_str(); }
private:
  std::string errMsg_;
};

template<typename MaxStepTy = uint32_t, typename MaxPosTy = uint64_t, typename TotalStepsTy = uint64_t>
struct Result {
  MaxStepTy maxStep = 0;
  MaxPosTy maxPos = 0;
  TotalStepsTy totalSteps = 0;
};

template<typename T, typename U, typename V>
inline decltype(auto) operator << (std::ostream& os, const Result<T,U,V>& result)
{
  return os << result.maxStep << '\n' << result.maxPos << '\n' << result.totalSteps;
}
template<typename T, typename U, typename V>
inline decltype(auto) operator >> (std::istream& is, Result<T,U,V>& result)
{
  return is >> result.maxStep >> result.maxPos >> result.totalSteps;
}

template<typename T, typename U, typename V, typename ValGen, typename CollatzFunc>
inline auto compare_and_validate(const Result<T,U,V>& oldRes, const Result<T,U,V>& newRes, ValGen&& get_val, CollatzFunc&& collatz_step)
{
  //Only validate the results when new champion arrives
  if (newRes.maxStep > oldRes.maxStep) {
    auto val = get_val(newRes); //synthesize the value from the results
    auto expectedStep = collatz_step(val);
    if (newRes.maxStep != expectedStep) {
      throw ValidateErr{ boost::lexical_cast<std::string>(val), expectedStep, newRes.maxStep };
    }
    return Result<T, U, V>{ newRes.maxStep, newRes.maxPos, newRes.totalSteps + oldRes.totalSteps };
  } else {
    return Result<T, U, V>{ oldRes.maxStep, oldRes.maxPos, newRes.totalSteps + oldRes.totalSteps };
  }
}

template <typename MaxStepVec, typename MaxPosVec, typename TotalStepsVec> inline
auto gather_results(MaxStepVec&& maxStep, MaxPosVec&& maxPos, TotalStepsVec&& totalSteps)
{
  using std::cbegin; using std::cend; using std::next;
  auto pos = std::distance(cbegin(maxStep), std::max_element(cbegin(maxStep), cend(maxStep)));
  return Result<decltype(maxStep[0]), decltype(maxPos[0]), decltype(totalSteps[0])> {
    *next(cbegin(maxStep), pos),
    *next(cbegin(maxPos), pos),
    std::accumulate(cbegin(totalSteps), cend(totalSteps), 0)
  };
}

}