//
// OpenTissue Template Library Demo
// - A specific demonstration of the flexibility of OTTL.
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
//
// OTTL and OTTL Demos are licensed under zlib.
//
#include <OpenTissue/configuration.h>

#define DEFINE_GLUT_MAIN
#include <OpenTissue/utility/glut/glut_perspective_view_application.h>
#undef DEFINE_GLUT_MAIN

#define SPHSH
//#define SPHSH_PARALLEL

#include <OpenTissue/dynamics/sph/sph.h>
#include <OpenTissue/core/math/math_constants.h>
#include <OpenTissue/core/math/math_vector3.h>
#include <OpenTissue/core/containers/grid/grid.h>
#include <OpenTissue/core/containers/grid/util/grid_idx2coord.h>
#include <OpenTissue/core/containers/mesh/mesh.h>
#include <OpenTissue/core/geometry/geometry_capsule.h>
#include <OpenTissue/core/geometry/geometry_sphere.h>
#include <OpenTissue/core/geometry/geometry_obb.h>
#include <OpenTissue/collision/spatial_hashing/spatial_hashing.h>
#include <OpenTissue/utility/utility_fps_counter.h>
#include <OpenTissue/utility/utility_runtime_type.h>

#include <vector>
#include <list>
#include <cmath>
#include <string>

#include <boost/cast.hpp> // we need boost::numeric_cast<>()

using namespace OpenTissue::math;
using namespace OpenTissue::math::detail;

/**
*
* 2007-05-21 kenny:
*
*    The compiler do not like it if these typedef's are moved inside the class
*    definition. As far as I can tell it boils down to using a pointer as a
*    template argument.  This should be re-factored!!!
*/

  template<typename Point_Type>
  class T4NodeTraits
  {
  public:
    typedef Point_Type  point_type;
    point_type const & vertex() const {return *m_vertex;}
    point_type const *  m_vertex;
  };

  typedef OpenTissue::utility::RuntimeType<double>  RTreal;
  RTreal Radius;
  typedef BasicMathTypes<double,int> math_types;

  typedef math_types::vector3_type           point;
  typedef math_types::vector3_type           vector3_type;
  typedef math_types::real_type              real_type;
  typedef std::vector<point>                 point_container;
  typedef OpenTissue::sph::Particle<real_type, Vector3, &Radius>  Particle;

  typedef OpenTissue::geometry::VolumeShape<math_types> Volume;
  typedef OpenTissue::geometry::Capsule<math_types>     CapsuleObj;
  typedef OpenTissue::geometry::Sphere<math_types>      SphereObj;
  typedef OpenTissue::geometry::OBB<math_types>         BoxObj;

  typedef OpenTissue::sph::ImplicitSpherePrimitive<real_type, vector3_type, SphereObj> ImplicitSphere;
  typedef OpenTissue::sph::ImplicitCapsulePrimitive<real_type, vector3_type, CapsuleObj> ImplicitCapsule;
  typedef OpenTissue::sph::ImplicitPlanePrimitive<real_type, vector3_type> ImplicitPlane;
  typedef OpenTissue::sph::ImplicitBoxPrimitive<real_type, vector3_type, BoxObj> ImplicitBox;

  //typedef OpenTissue::CollisionTypeBinder<real_type, OpenTissue::vector3> CollisionTypes;
  //typedef OpenTissue::ParticleWrapper<point, Particle> Particle_Wrapper;
  //typedef OpenTissue::TetrahedronWrapper<real_type, point, tetrahedra_mesh::tetrahedron_type> Tetrahedron_Wrapper;
  //typedef OpenTissue::TetrahedraPointsCollisionDetectionPolicy<CollisionTypes, Tetrahedron_Wrapper, Particle_Wrapper> CollisionDetection;
  //typedef OpenTissue::TetrahedraPointsCollisionDetectionPolicy<real_type, vector3_type, tetrahedra_mesh::tetrahedron_type, Particle> T4CollisionDetection;
  typedef OpenTissue::sph::ImplicitPrimitivesCollisionDetectionPolicy<real_type, vector3_type, Particle> IPCollisionDetection;

  typedef OpenTissue::sph::Types<
      real_type
    , Vector3
    , Particle
    , IPCollisionDetection
    , OpenTissue::spatial_hashing::PrimeNumberHashFunction
    , OpenTissue::spatial_hashing::Grid
    , OpenTissue::spatial_hashing::PointDataQuery
  > SPHTypes;

  typedef OpenTissue::sph::WFixedGaussian<SPHTypes, &Radius, false> KernelGaussian;
  typedef OpenTissue::sph::WPoly6<SPHTypes, &Radius, false> KernelDefault;
  typedef OpenTissue::sph::WSpiky<SPHTypes, &Radius, false> KernelPressure;
  typedef OpenTissue::sph::WViscosity<SPHTypes, &Radius, false> KernelViscosity;

  typedef OpenTissue::sph::Density<SPHTypes, KernelDefault> DensitySolver;
  typedef OpenTissue::sph::SurfaceNormal<SPHTypes, KernelDefault> NormalSolver;
  typedef OpenTissue::sph::PressureForce<SPHTypes, KernelPressure> PressureForce;
  typedef OpenTissue::sph::ViscosityForce<SPHTypes, KernelViscosity> ViscosityForce;
  typedef OpenTissue::sph::SurfaceForce<SPHTypes, KernelDefault> SurfaceForce;
  typedef OpenTissue::sph::ColorField<SPHTypes, KernelDefault> ColorField;

  typedef OpenTissue::sph::Pressure<SPHTypes> PressureSolver;
  typedef OpenTissue::sph::Gravity<SPHTypes> GravityForce;
  typedef OpenTissue::sph::Buoyancy<SPHTypes> BuoyancyForce;

  typedef OpenTissue::sph::Verlet<SPHTypes> VerletIntegrator;
  typedef OpenTissue::sph::Euler<SPHTypes> EulerIntegrator;
  typedef OpenTissue::sph::LeapFrog<SPHTypes> LeapFrogIntegrator;

  typedef OpenTissue::sph::Water<SPHTypes> WaterMaterial;
  typedef OpenTissue::sph::Mucus<SPHTypes> MucusMaterial;
  typedef OpenTissue::sph::Steam<SPHTypes> SteamMaterial;

  typedef OpenTissue::sph::PointEmitter<SPHTypes> PointEmitter;
  typedef OpenTissue::sph::CircleEmitter<SPHTypes> CircleEmitter;

  typedef OpenTissue::sph::System
  <   SPHTypes
    , DensitySolver
    , PressureSolver
    , NormalSolver
    , GravityForce
    , BuoyancyForce
    , PressureForce
    , ViscosityForce
    , SurfaceForce
    , LeapFrogIntegrator
    , ColorField
  > DefaultSystem;

/**
*
* 2007-05-21: kenny: outstanding issues:
*                const-correctness,
*                OT-naming conventions is not used (ie. m_ on members, _type on types,
*                prober placement of braces etc..),
*                messy usage of math types and ad-hoc typedefs.
*                Contains redundant code like Screen2World and putText.
*                documentation is missing too.
*                avoid if-directives in code, in particular ``if 0'' stuff
*/
class Application : public OpenTissue::glut::PerspectiveViewApplication
{
protected:

  bool draw_normals;
  bool draw_surface;
  bool draw_inside;
  bool draw_velocity;
  bool osd;
  bool render;
  int obstacles;
  bool use_emitter;
  bool waves;

  int yScale;
  bool bPanObstacle;
  bool bScaleObstacle;
  double xPan, yPan, zPan;

protected:

  DefaultSystem* sph;
  OpenTissue::sph::Material<SPHTypes>* material;
  OpenTissue::grid::Grid<double,math_types>  phi;
  OpenTissue::polymesh::PolyMesh<> surface;

  OpenTissue::utility::FPSCounter<double> fps;
  point_container points;
  SphereObj sphere;
  SphereObj sphere2;
  BoxObj box;
  ImplicitSphere isphere;
  ImplicitSphere isphere2;
  CapsuleObj capsule;
  ImplicitCapsule icapsule;
  ImplicitPlane iplane;
  ImplicitBox ibox;
  PointEmitter emitter1;
  CircleEmitter emitter2;
  CircleEmitter emitter3;

  BoxObj DBb1;
  BoxObj DBb2;
  BoxObj DBb3;
  ImplicitBox iDBb1;
  ImplicitBox iDBb2;

  OpenTissue::sph::Emitter<SPHTypes>* emitter;
  Volume* object;

public:

  Application()
    : draw_normals(false)
    , draw_surface(false)
    , draw_inside(false)
    , draw_velocity(false)
    , osd(true)
    , render(false)
    , obstacles(0)
    , use_emitter(false)
    , waves(false)
    , bPanObstacle(false)
    , bScaleObstacle(false)
    , sph(NULL)
    , material(NULL)
    , sphere(vector3_type(0,0,0.1), 0.25)
    , sphere2(vector3_type(0,0,-0.2), 0.25)
    , box(vector3_type(0.0,0.0,0.0), diag(1.), vector3_type(0.1875,0.1875,0.1875))
    , isphere(sphere)
    , isphere2(sphere2)
    , capsule(vector3_type(0.0,0.0,0.0), vector3_type(0.0,0.0,0.6), 0.2)
    , icapsule(capsule)
    , iplane(vector3_type(0,0,-2.0), vector3_type(0,0,1.0))
    , ibox(box)
    , emitter1(vector3_type(0,0,0), vector3_type(-1.5,0,.5))
    , emitter2( vector3_type(0.0), 0.015, vector3_type(0.0,0.0,1.5))
    , emitter3(vector3_type(0,0,0.7), 0.02, vector3_type(0,0,-1))
    , DBb1(vector3_type(0.0,0.0,0.0), diag(1.), vector3_type(0.1875,0.1,0.4))
    , DBb2(vector3_type(0.3125,0.0,0.0), diag(1.), vector3_type(0.5,0.1,0.4))
    , DBb3(vector3_type(0.0,0.0,0.0), diag(1.), vector3_type(0.5,0.1,0.4))
    , iDBb1(DBb1)
    , iDBb2(DBb2)
    , emitter(&emitter2)
    , object(&capsule)
  { }

protected:

  /**
  * Put text onto screen (OSD)
  *
  * 2007-05-21 kenny: what is wrong with OpenTissue::gl::DrawString()?
  */
  void putText(const double &x, const double &y, const double &r, const double &g, const double &b, const std::string &text)
  {
    // setup orthogonal projection to use 2D text
    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();
    glViewport(0, 0, this->width(), this->height());
    glOrtho(0, this->width(), 0, this->height(), 0, 1);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

    glDisable(GL_TEXTURE_2D);

    glColor4d(r, g, b, 1);
    glRasterPos2i( static_cast<int>( x ), static_cast<int>( y ) );
    for (const char *c = text.c_str(); *c != '\0'; ++c)
      glutBitmapCharacter(GLUT_BITMAP_HELVETICA_12, *c);//GLUT_BITMAP_8_BY_13
    glColor4d(r+.5, g+.5, b+.5, 1);
    glRasterPos2i( static_cast<int>( x+1 ), static_cast<int>( y-1 ) );
    for (const char *c = text.c_str(); *c != '\0'; ++c)
      glutBitmapCharacter(GLUT_BITMAP_HELVETICA_12, *c);//GLUT_BITMAP_8_BY_13

    glMatrixMode(GL_PROJECTION);
    glPopMatrix();
  }

  void renderParticles(const DefaultSystem::fluid_material* mat)
  {
    GLUquadric * qobj = gluNewQuadric();
    gluQuadricDrawStyle(qobj, GLU_FILL);
    glEnable(GL_COLOR_MATERIAL);
    glDisable(GL_LIGHTING);

    //    unsigned long cnt = 0;
    const SPHTypes::particle_container &pars = sph->particles();
    for (SPHTypes::particle_container::const_iterator p = pars.begin(); p != pars.end(); p++) {
      if (p->fixed()) continue;
      const vector3_type &pos = p->position();

      //
      // V = 4/3 pi r^3  =>  m/rho = 4/3 pi r^3  =>  r^3 = (3 m)/(4 pi rho)  =>  r = ((3 m)/(4 pi rho))^1/3
      //
      const SPHTypes::real_type d = 0.9*pow((3.*p->mass())/(4.*pi<real_type>()*p->density()), 1./3.);
      const vector3_type& n = p->normal();
      const SPHTypes::real_type l = n*n;

      bool draw_sphere = false;
      if (l >= mat->threshold() || l < 0.0025)
      {
        // draw surface normals
        if (draw_normals && l >= mat->threshold())
        {
          const vector3_type sn = -0.05*unit(n);
          glColor3d(0.5, 0.5, 0.5);
          glBegin(GL_LINES);
          glVertex3d(pos[0], pos[1], pos[2]);
          glVertex3d(pos[0]+sn[0], pos[1]+sn[1], pos[2]+sn[2]);
          glEnd();
        }
        glColor3d(mat->red(), mat->green(), mat->blue());
        draw_sphere = draw_surface;
      }
      else
      {
        glColor3d(.5*mat->red(), .5*mat->green(), .5*mat->blue());
        draw_sphere = draw_inside;
      }

      // default behaviour
      if (!(draw_surface || draw_inside))
      {
        glColor3d(mat->red(), mat->green(), mat->blue());
        draw_sphere = true;
      }

      if (draw_velocity)
      {
        glEnable(GL_LIGHTING);
        const vector3_type vel = 0.05*p->velocity();
        OpenTissue::gl::DrawVector(pos, vel, 1, false);
      }
      else if (draw_sphere)
      {
        glPushMatrix();
        glTranslated(pos[0], pos[1], pos[2]);
        glEnable(GL_LIGHTING);
        gluSphere(qobj, d, 8, 8);
        glDisable(GL_LIGHTING);
        glPopMatrix();
      }

    }
    gluDeleteQuadric(qobj);
  }

  void renderSurface(const DefaultSystem::fluid_material* mat, const SPHTypes::real_type& size)
  {
    using std::max;
    using std::min;

    typedef SPHTypes::real_type   real_type;
    typedef vector3_type vector3_type;
    // find BB of fluid (for high res rendering)
    vector3_type  m( highest<real_type>() ), M( lowest<real_type>() );

    const SPHTypes::particle_container &pars = sph->particles();
    SPHTypes::particle_container::const_iterator pars_end = pars.end();
    for (SPHTypes::particle_container::const_iterator p = pars.begin(); p != pars_end; ++p)
    {
      vector3_type const & pos = p->position();
      M = max(M, pos);
      m = min(m, pos);
    }
    real_type const d = 1.35*pow((3.*mat->particle_mass())/(4.*pi<real_type>()*mat->density()), 1./3.);
    m -= vector3_type(d);
    M += vector3_type(d);

    vector3_type v((M-m)/size);
    v = ceil(v);

    if(phi.I()<v(0) ||  phi.J()<v(1)  || phi.K()<v(2))
      phi.create( m ,M
      , static_cast<math_types::index_type>(v[0])
      , static_cast<math_types::index_type>(v[1])
      , static_cast<math_types::index_type>(v[2])
      ); // TODO 2005-08-28 KE: hmmm, this should be pre-allocated!!!

    OpenTissue::grid::Grid<double,math_types>::index_iterator phi_begin = phi.begin();
    OpenTissue::grid::Grid<double,math_types>::index_iterator phi_end = phi.end();
    for(OpenTissue::grid::Grid<double,math_types>::index_iterator phi_ = phi_begin; phi_ != phi_end; ++phi_)
      *phi_ = 0.;

    /*
    SPHTypes::particle_container::const_iterator par = pars.begin();
    SPHTypes::particle_container::const_iterator par_end = pars.end();
    for (;par!=par_end;++par)
    {
    const vector3_type& n = par->normal();
    const SPHTypes::real_type l = n*n;
    if (l >= mat->threshold() || l < 0.0025)
    {
    const vector3_type &r = par->position();
    unsigned int i = static_cast<unsigned int>(std::floor( (r(0) - phi.min_coord(0)) / phi.dx() ));
    unsigned int j = static_cast<unsigned int>(std::floor( (r(1) - phi.min_coord(1)) / phi.dy() ));
    unsigned int k = static_cast<unsigned int>(std::floor( (r(2) - phi.min_coord(2)) / phi.dz() ));
    phi(  i,  j,k) = 100;
    phi(  i,j+1,k) = 100;
    phi(i+1,  j,k) = 100;
    phi(i+1,j+1,k) = 100;
    phi(  i,  j,k+1) = 100;
    phi(  i,j+1,k+1) = 100;
    phi(i+1,  j,k+1) = 100;
    phi(i+1,j+1,k+1) = 100;
    }
    }
    */

    vector3_type coord;
    for(OpenTissue::grid::Grid<double,math_types>::index_iterator phi_ = phi_begin; phi_ != phi_end; ++phi_)
    {
      //    if(*phi_>1.)
      {
        OpenTissue::grid::idx2coord(phi_,coord);
        *phi_ = sph->isoValue(coord);//0.35 - sph->isoValue(pos);
      }
    }

    //OpenTissue::mesh::isosurface(phi,0.0,surface);
    OpenTissue::mesh::smooth_isosurface(phi,0.6,surface,1,true);

    //  OpenTissue::polymesh::PolyMesh<>::vertex_iterator end = surface.vertex_end();
    //  for (OpenTissue::polymesh::PolyMesh<>::vertex_iterator vtx = surface.vertex_begin(); vtx != end; ++vtx)
    //    vtx->m_color = OpenTissue::polymesh::PolyMesh<>::vector3_type(mat->red(), mat->green(), mat->blue());

    glColor3d(mat->red(), mat->green(), mat->blue());
    //  glEnableClientState(GL_VERTEX_ARRAY);
    //  glEnableClientState(GL_NORMAL_ARRAY);

    glEnable(GL_LIGHTING);
    glEnable(GL_COLOR_MATERIAL);
    glEnable(GL_CULL_FACE);
    glCullFace(GL_BACK);

    OpenTissue::gl::DrawMesh(surface, GL_POLYGON, false, true, false);
    //  OpenTissue::MeshDrawArray<PolyMesh<> > dm(surface);
    //  dm();
    //OpenTissue::gl::DrawMesh(surface);

    //char *vertexArray = (char *) surface.getVertexArray();
    //glVertexPointer(3, GL_FLOAT, sizeof(ISvertex), vertexArray);
    //glNormalPointer(   GL_FLOAT, sizeof(ISvertex), vertexArray + sizeof(ISvec3));
    //glDrawElements(GL_TRIANGLES, surface.getIndexCount(), GL_UNSIGNED_INT, surface.getIndexArray());

    //  glDisableClientState(GL_NORMAL_ARRAY);
    //  glDisableClientState(GL_VERTEX_ARRAY);

    // draw bb
#if 0
    glDisable(GL_LIGHTING);
    glColor3d(0,0,0);
    glBegin(GL_LINES);
    glVertex3d(m[0], m[1], m[2]);
    glVertex3d(M[0], m[1], m[2]);
    glVertex3d(M[0], m[1], m[2]);
    glVertex3d(M[0], M[1], m[2]);
    glVertex3d(M[0], M[1], m[2]);
    glVertex3d(m[0], M[1], m[2]);
    glVertex3d(m[0], M[1], m[2]);
    glVertex3d(m[0], m[1], m[2]);
    glVertex3d(m[0], m[1], M[2]);
    glVertex3d(M[0], m[1], M[2]);
    glVertex3d(M[0], m[1], M[2]);
    glVertex3d(M[0], M[1], M[2]);
    glVertex3d(M[0], M[1], M[2]);
    glVertex3d(m[0], M[1], M[2]);
    glVertex3d(m[0], M[1], M[2]);
    glVertex3d(m[0], m[1], M[2]);
    glVertex3d(m[0], m[1], m[2]);
    glVertex3d(m[0], m[1], M[2]);
    glVertex3d(M[0], m[1], m[2]);
    glVertex3d(M[0], m[1], M[2]);
    glVertex3d(M[0], M[1], m[2]);
    glVertex3d(M[0], M[1], M[2]);
    glVertex3d(m[0], M[1], m[2]);
    glVertex3d(m[0], M[1], M[2]);
    glEnd();
#endif
    /*
    glEnable(GL_LIGHTING);
    for (SPHTypes::real_type k = m[2]; k < M[2]; k += size)
    for (SPHTypes::real_type j = m[1]; j <= M[1]; j += size)
    for (SPHTypes::real_type i = m[0]; i <= M[0]; i += size) {
    vector3_type test(i+.5*size, j+.5*size, k+.5*size);
    if ((0.35-sph->isoValue(test)) <= 0) {
    glBegin(GL_QUADS);
    glColor3d(1,0,0);
    glNormal3d(1, 0, 0);
    glVertex3d(i, j, k);
    glVertex3d(i, j+size, k);
    glVertex3d(i, j+size, k+size);
    glVertex3d(i, j, k+size);
    glColor3d(0,1,0);
    glNormal3d(0, 1, 0);
    glVertex3d(i, j, k);
    glVertex3d(i+size, j, k);
    glVertex3d(i+size, j, k+size);
    glVertex3d(i, j, k+size);
    glColor3d(0,0,1);
    glNormal3d(0, 0, 1);
    glVertex3d(i, j, k);
    glVertex3d(i+size, j, k);
    glVertex3d(i+size, j+size, k);
    glVertex3d(i, j+size, k);
    glEnd();
    }
    }
    */
    glDisable(GL_COLOR_MATERIAL);
    glDisable(GL_CULL_FACE);
  }

  void addCollisionObject(Volume* obj)
  {
    object = obj;
    if (sph) {
      if (object == &capsule)
        sph->collisionSystem().addContainer(icapsule);
      else if (object == &sphere)
        sph->collisionSystem().addContainer(isphere);
      else if (object == &box)
        sph->collisionSystem().addContainer(ibox);
      else if (object == &DBb1)
        sph->collisionSystem().addContainer(iDBb1);
      else if (object == &DBb2)
        sph->collisionSystem().addContainer(iDBb2);
    }
  }

  template<typename MaterialPolicy>
  bool createFluid(size_t const & particles)
  {
    delete material;
    delete sph;

    material = new MaterialPolicy;
    material->particles() = particles;
    material->particle_mass(material->particle_mass());

    const SPHTypes::real_type  x = material->kernel_particles();         //--- Average number of particles inside kernel...

    material->threshold() = material->density()/x;

    Radius = material->radius(x);
    vector3_type gravity(0,0,-9.82);

    sph = new DefaultSystem;

    if (!sph->create(
      *static_cast< MaterialPolicy* >(material), gravity)
      )  // material, gravity
      return false;

    if (!sph->initHashing(2u*particles, Radius))  // hash table size, cell spacing (AABBs)
      return false;

    addCollisionObject(object);
    //  sph->collisionSystem().addContainer(isphere); object = &sphere;
    //  sph->collisionSystem().addObstacle(isphere2);
    //  sph->collisionSystem().addContainer(icapsule); object = &capsule;
    //  sph->collisionSystem().addObstacle(iplane);
    //  sph->collisionSystem().addObstacle(ibox); object = &box;
    //  sph->collisionSystem().addContainer(ibox); object = &box;
    //  sph->collisionSystem().addContainer(iDBb1); object = &DBb1;
    //  sph->collisionSystem().addContainer(iDBb2); object = &DBb2;

    if (use_emitter)
    {
      emitter = &emitter2;
      emitter->batch() = 5;//7
      emitter->rate() = 2;

      if (!sph->init(emitter2, particles))
        return false;
    }
    else
    {
      emitter = NULL;
      typedef std::vector<vector3_type> vectors;
      vectors positions, velocities;

      const SPHTypes::real_type dist = 1./45.;
      const SPHTypes::real_type off = -.14;
      size_t k = 1, j = 1, i = 1, p = particles;
      while (p--)
      {
        positions.push_back(vector3_type(off+i*dist, off+j*dist, off+k*dist));
        velocities.push_back(vector3_type(0.,0.,0.));
        if (++i > 12)
        {
          i = 1;
          if (++j > 12)
          {
            j = 1;
            ++k;
          }
        }
      }
      if (!sph->init( positions.begin()
        , positions.end()
        , velocities.begin()
        , velocities.end())
        )
        return false;
    }
    return true;
  }

  /**
  * 2007-05-21 kenny: what is wrong with OpenTissue::gl::screen2object()?
  */
  void ScreenToWorld(double& xw, double& yw, double& zw, const int xs, const int ys)
  {
    GLdouble projMatrix[16];
    GLdouble modelViewMatrix[16];
    GLint viewPort[4];

    //  glMatrixMode(GL_MODELVIEW);
    //  glLoadIdentity();
    //  gluLookAt( eyex, eyey, eyez, centerx, centery, centerz, upx, upy, upz );
    //  glMultMatrixd( trackball.get_gl_current_rotation() );
    //  glRotatef(-90,1,0,0);

    glGetDoublev(GL_MODELVIEW_MATRIX, modelViewMatrix);
    glGetDoublev(GL_PROJECTION_MATRIX, projMatrix);
    glGetIntegerv(GL_VIEWPORT, viewPort);

    gluUnProject(xs, viewPort[3]-ys, 1, modelViewMatrix, projMatrix, viewPort, &xw, &yw, &zw);
  }

public:

  char const * do_get_title() const { return "Smoothed Particle Hydrodynamics Demo Application"; }

  void do_display()
  {

    if (obstacles)
    {
      glEnable(GL_COLOR_MATERIAL);
      glEnable(GL_LIGHTING);
      glColor3d(0.5, 0.0, 0.3);
      bool const wireframe = 1 == obstacles;
      if (CapsuleObj::id() == object->class_id())
        OpenTissue::gl::DrawCapsule(*static_cast<CapsuleObj*>(object), wireframe);
      else if (SphereObj::id() == object->class_id())
        OpenTissue::gl::DrawSphere(*static_cast<SphereObj*>(object), wireframe);
      else if (BoxObj::id() == object->class_id())
        OpenTissue::gl::DrawOBB(*static_cast<BoxObj*>(object), wireframe);
      glDisable(GL_LIGHTING);
    }

    if (use_emitter && emitter)
    {
      glDisable(GL_COLOR_MATERIAL);
      glDisable(GL_LIGHTING);
      if (emitter->active())
      {
        if (emitter->running())
          glColor3d(0.1, 0.9, 0.1);
        else
          glColor3d(0.9, 0.1, 0.1);
      }
      else
        glColor3d(0.2, 0.2, 0.2);
      const vector3_type& c = emitter->center();
      const SPHTypes::real_type x = .03;

      glBegin(GL_LINES);
      glVertex3d(c(0)-x,c(1)-x,c(2)-x);
      glVertex3d(c(0)+x,c(1)-x,c(2)-x);

      glVertex3d(c(0)+x,c(1)-x,c(2)-x);
      glVertex3d(c(0)+x,c(1)+x,c(2)-x);

      glVertex3d(c(0)+x,c(1)+x,c(2)-x);
      glVertex3d(c(0)-x,c(1)+x,c(2)-x);

      glVertex3d(c(0)-x,c(1)+x,c(2)-x);
      glVertex3d(c(0)-x,c(1)-x,c(2)-x);

      glVertex3d(c(0)-x,c(1)-x,c(2)+x);
      glVertex3d(c(0)+x,c(1)-x,c(2)+x);

      glVertex3d(c(0)+x,c(1)-x,c(2)+x);
      glVertex3d(c(0)+x,c(1)+x,c(2)+x);

      glVertex3d(c(0)+x,c(1)+x,c(2)+x);
      glVertex3d(c(0)-x,c(1)+x,c(2)+x);

      glVertex3d(c(0)-x,c(1)+x,c(2)+x);
      glVertex3d(c(0)-x,c(1)-x,c(2)+x);

      glVertex3d(c(0)-x,c(1)-x,c(2)-x);
      glVertex3d(c(0)-x,c(1)-x,c(2)+x);

      glVertex3d(c(0)+x,c(1)-x,c(2)-x);
      glVertex3d(c(0)+x,c(1)-x,c(2)+x);

      glVertex3d(c(0)+x,c(1)+x,c(2)-x);
      glVertex3d(c(0)+x,c(1)+x,c(2)+x);

      glVertex3d(c(0)-x,c(1)+x,c(2)-x);
      glVertex3d(c(0)-x,c(1)+x,c(2)+x);
      glEnd();
    }

    const DefaultSystem::fluid_material* mat = sph?sph->material():NULL;
    if (sph)
    {

      if (render)
        renderSurface(mat, 0.02);
      else
        renderParticles(mat);

      fps.frame(); // probe both sim + vis
      if (osd)
      {
        glDisable(GL_LIGHTING);
        std::stringstream ss;
        ss << "FPS: " /*<< setw(4)*/ << fps();
        putText(this->width(), this->height()-30, 0.15, 0.0, 0.05, ss.str());
        std::ostringstream ost;
        ost << "Material: " << (mat?mat->name():"N/A");
        putText(16, this->height()-30, 0.15, 0.0, 0.05, ost.str());
        ost.str("");
        ost << "Particle Mass [kg]: " << (mat?mat->particle_mass():0.);
        putText(16, this->height()-50, 0.15, 0.0, 0.05, ost.str());
        ost.str("");
        ost << "Volume [m3]: " << (mat?mat->volume():0.);
        putText(16, this->height()-70, 0.15, 0.0, 0.05, ost.str());
        ost.str("");
        ost << "Particles: " << (mat?mat->particles():0.);
        putText(16, this->height()-90, 0.15, 0.0, 0.05, ost.str());
      }
    }
  }

  void do_action(unsigned char choice)
  {
    using std::cos;
    using std::sin;

    switch (choice)
    {
    case 'n':
    case 'N':
      draw_normals = !draw_normals;
      break;
    case 'v':
    case 'V':
      draw_velocity = !draw_velocity;
      break;
    case 'w':
    case 'W':
      waves = !waves;
      break;
    case 's':
    case 'S':
      draw_surface = !draw_surface;
      break;
    case 'i':
    case 'I':
      draw_inside = !draw_inside;
      break;
    case 'o':
    case 'O':
      osd = !osd;
      break;
    case 'r':
    case 'R':
      render = !render;
      break;
    case 'c':
      if (sph) sph->collisionSystem().clear();
      if (object == &capsule)
        addCollisionObject(&sphere);
      else if (object == &sphere)
        addCollisionObject(&box);
      else
        addCollisionObject(&capsule);
      break;
    case 'C':
      if (++obstacles > 2)
        obstacles = 0;
      break;
    case 'E':
      use_emitter = !use_emitter;
      break;
    case 'e':
      if (!emitter) break;
      if (emitter->running())
        emitter->stop();
      else
        emitter->start();
      break;
    case '1':
      // create small water
      createFluid<WaterMaterial>(500);
      break;
    case '2':
      // create medium water
      createFluid<WaterMaterial>(1250);//1500
      break;
    case '3':
      // create medium water
      createFluid<WaterMaterial>(2250);
      break;
    case '4':
      // create large water
      createFluid<WaterMaterial>(4400);
      break;
    case '5':
      // create small mucus
      createFluid<MucusMaterial>(500);
      break;
    case '6':
      // create medium mucus
      createFluid<MucusMaterial>(1250);//1500
      break;
    case '7':
      // create medium mucus
      createFluid<MucusMaterial>(2250);
      break;
    case '8':
      // create large mucus
      createFluid<MucusMaterial>(4400);
      break;
    case '0':
      // create test steam
      createFluid<SteamMaterial>(2000);
      break;
    case '.':
      {
        const double a = to_radians<double>(45.), b = to_radians<double>(0.), c = to_radians<double>(0.);
        object->rotate(Volume::matrix3x3_type(1,0,0, 0,cos(a),sin(a), 0,-sin(a),cos(a)));
        object->rotate(Volume::matrix3x3_type(cos(b),0,-sin(b), 0,1,0, sin(b),0,cos(b)));
        object->rotate(Volume::matrix3x3_type(cos(c),sin(c),0, -sin(c),cos(c),0, 0,0,1));
        break;
      }
    case ',':
      {
        const double a = to_radians<double>(-2.), b = to_radians<double>(-2.), c = -2*to_radians<double>(-2.);
        object->rotate(Volume::matrix3x3_type(1,0,0, 0,cos(a),sin(a), 0,-sin(a),cos(a)));
        object->rotate(Volume::matrix3x3_type(cos(b),0,-sin(b), 0,1,0, sin(b),0,cos(b)));
        object->rotate(Volume::matrix3x3_type(cos(c),sin(c),0, -sin(c),cos(c),0, 0,0,1));
        break;
      }
    case '+':
      if (sph)
      {
        sph->collisionSystem().clear();
        sph->collisionSystem().addContainer(iDBb2); object = &DBb2;
      }
      break;
    case '-':
      if (sph)
      {
        sph->collisionSystem().clear();
        sph->collisionSystem().addContainer(iDBb1); object = &DBb1;
      }
      break;
    default:
      break;
    }
  }

  void do_init_right_click_menu(int main_menu, void menu(int entry))
  {
    int toggles = glutCreateMenu(menu);
    glutAddMenuEntry("On Screen Display [h]", 'h');
    glutAddMenuEntry("Render Surface [r]", 'r');
    glutAddMenuEntry("Surface Particles [s]", 's');
    glutAddMenuEntry("Intrinsic Particles [i]", 'i');
    glutAddMenuEntry("Surface Normals [n]", 'n');
    glutAddMenuEntry("Velocities [v]", 'v');
    glutAddMenuEntry("View Collision Objects [C]", 'C');
    glutAddMenuEntry("Change Collision Objects [c]", 'c');
    glutAddMenuEntry("Emitter [E]", 'E');
    glutAddMenuEntry("Start/Stop Emitter [e]", 'e');
    glutAddMenuEntry("Dam-Break: Init Dam [-]", '-');
    glutAddMenuEntry("Dam-Break: Break Dam [+]", '+');
    glutAddMenuEntry("Start/Stop Box Waves [w]", 'w');

    int materials = glutCreateMenu(menu);
    glutAddMenuEntry("Water  (500) [1]", '1');
    glutAddMenuEntry("Water (1250) [2]", '2');
    glutAddMenuEntry("Water (2250) [3]", '3');
    glutAddMenuEntry("Water (4400) [4]", '4');
    glutAddMenuEntry("Mucus  (500) [5]", '5');
    glutAddMenuEntry("Mucus (1250) [6]", '6');
    glutAddMenuEntry("Mucus (2250) [7]", '7');
    glutAddMenuEntry("Mucus (4400) [8]", '8');
    glutAddMenuEntry("Steam (test) [0]", '0');

    glutSetMenu(main_menu);
    glutAddSubMenu("toggles", toggles);
    glutAddSubMenu("materials", materials);
  }

  void do_init()
  {
    this->camera().move(95);
    this->zoom_sensitivity() = 0.02;
    this->pan_sensitivity() = 0.01;
  }

  void do_run()
  {
    if (emitter)
    {
      emitter->execute();
    }
    if (waves)
    {
      vector3_type ext = DBb2.ext();
      vector3_type cen = DBb2.center();
      static double t = 0;
      using std::fabs;
      const double w = 0.1*fabs(sin(t));
      ext(0) = 0.5+w; t += 0.05;
      cen(0) = 0.3125+w;
      DBb2.set(cen, DBb2.orientation(), ext);
    }
    //  for (int n = 0; n < 10; ++n) sph->simulate();
    sph->simulate();
  }

  void do_shutdown(){}

  void mouse_down(double cur_x,double cur_y,bool shift,bool ctrl, bool alt,bool left,bool middle,bool right)
  {
    if( !bScaleObstacle && middle && ctrl)
    {
      yScale = boost::numeric_cast<int>(cur_y);
      bScaleObstacle = true;
    }
    else if( !bPanObstacle && left && shift && ctrl)
    {
      bPanObstacle = true;
      ScreenToWorld(xPan, yPan, zPan, boost::numeric_cast<int>(cur_x), boost::numeric_cast<int>(cur_y));
    }
    else
    {
      OpenTissue::glut::PerspectiveViewApplication::mouse_down(cur_x,cur_y,shift, ctrl, alt, left, middle, right);
    }
  }

  void mouse_move(double cur_x,double cur_y)
  {

    if (bPanObstacle)
    {
      double x, y, z;
      ScreenToWorld(x, y, z, boost::numeric_cast<int>(cur_x), boost::numeric_cast<int>(cur_y));
      object->translate(this->pan_sensitivity()*vector3_type(x-xPan,y-yPan,z-zPan));
      xPan = x;
      yPan = y;
      zPan = z;
    }
    else if (bScaleObstacle)
    {
      const GLdouble scale = cur_y - yScale;
      if (scale > 0)
        object->scale(1.02);
      else if (scale < 0)
        object->scale(0.98);
      yScale = boost::numeric_cast<int>(cur_y);
    }
    else
    {
      OpenTissue::glut::PerspectiveViewApplication::mouse_move(cur_x,cur_y);
    }
  }

  void mouse_up(double cur_x,double cur_y,bool shift,bool ctrl, bool alt, bool left,bool middle,bool right)
  {
    if( bScaleObstacle)
    {
      yScale = boost::numeric_cast<int>(cur_y);
      bScaleObstacle = false;
    }
    else if(  bPanObstacle )
    {
      ScreenToWorld(xPan, yPan, zPan, boost::numeric_cast<int>(cur_x), boost::numeric_cast<int>(cur_y));
      bPanObstacle = false;
    }
    else
    {
      OpenTissue::glut::PerspectiveViewApplication::mouse_up(cur_x,cur_y,shift, ctrl, alt, left, middle, right);
    }
  }

};

OpenTissue::glut::instance_pointer init_glut_application(int argc, char **argv)
{
  OpenTissue::glut::instance_pointer instance;
  instance.reset( new Application() );
  return instance;
}


#include "apps/detect_duplicates/outliers_action.h"
#include "utils/math_utils.h"
#include "utils/missing_values.hpp"
#include <algorithm>
#include <boost/iterator/transform_iterator.hpp>
#include <map>
//#include <iostream>

namespace
{
    using algo_data = std::vector<double>;

    algo_data detect_outliers(const algo_data& inside);

    algo_data detect_outliers(const algo_data& inside)
    {
        static const double alpha_val = 0.1;
        return utils::grubbstest(inside, alpha_val);
    }
}   // end of local namespace

outlier_predicate::outlier_predicate(iterator s, iterator l) :
    to_ignore(s, l)
{
    std::sort(std::begin(to_ignore), std::end(to_ignore));
}

outlier_predicate::result_type outlier_predicate::apply_action(input_data d) const
{
    return std::make_pair(
            std::binary_search(std::begin(to_ignore), std::end(to_ignore), d.row),
            d);
}

outlier_predicate::result_type apply_action(outlier_predicate& da, input_data arg)
{
    return da.apply_action(arg);
}

outliers_detector::outliers_detector(policy_action op) :
   action(std::move(op))
{
}

outlier_predicate::result_type outliers_detector::apply_action(input_data data)
{
    if (missing_value(data.value)) {
        missings.push_back(data.row);
        return std::make_pair(true, data);     // ignore missing values
    }
    auto i = ::apply_action(action, data);
    if (!i.first) {
        legal_input.push_back(data);
    }
    return i;
}

void outliers_detector::finding(row_numbers& ol, row_numbers& missing) const
{
    auto tranformer = [](auto it) { return it.value; };
    using tranformer_type = decltype(tranformer);
    using iterator_type = boost::transform_iterator<tranformer_type, data_list::const_iterator>;
    using lookup_table = std::map<algo_data::value_type, std::size_t>;

    algo_data data{iterator_type{std::begin(legal_input), tranformer},
        iterator_type{std::end(legal_input), tranformer}
    };

    algo_data outliers{detect_outliers(data)};
    //auto lib = std::begin(legal_input);
    //auto lie = std::end(legal_input);
    // match row number with outlier that we found, this is done
    // since the actual outlier detection cannot accept row number
    // and what we are returning is outliers
    lookup_table tbl;
    std::for_each(std::begin(outliers), std::end(outliers),
            [this, &ol, &tbl] (auto outlier) {
                auto off = tbl[outlier];
                auto lib = std::begin(legal_input);
                auto i = std::next(lib, off);
                lib = std::find_if(i, std::end(legal_input), [outlier] (auto rd) {
                        return rd.value == outlier;
                    }
                );
                if (lib != std::end(legal_input)) {   // an error
                    ol.push_back(lib->row);
                    tbl[outlier] = std::distance(std::begin(legal_input), lib) + 1u;
                    return false;
                } else {
                    return true;
                }
            }
    );
    if (missing.empty()) {  // we assume that if this is not missing then we fill it up in duplications
        missing.insert(std::end(missing), std::begin(missings), std::end(missings));
    }
}

outlier_predicate::result_type apply_action(outliers_detector& da, input_data arg)
{
    return da.apply_action(arg);
}

void finding(const outliers_detector& od, row_numbers& outlier, row_numbers& mis)
{
    od.finding(outlier, mis);
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   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/plus.hpp>
#include <boost/simd/include/functions/multiplies.hpp>
#include <nt2/include/functions/aligned_store.hpp>
#include <boost/simd/memory/allocator.hpp>
#include <boost/fusion/include/at.hpp>
#include <vector>

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

using namespace nt2::bench;
using namespace nt2;

template<typename T> struct axpy_simd
{
  template<typename Setup>
  axpy_simd(Setup const& s)
              :  alpha(boost::fusion::at_c<1>(s))
              ,  size_(boost::fusion::at_c<0>(s))
  {
    X.resize(size_); Y.resize(size_);
    for(std::size_t i = 0; i<size_; ++i)
      X[i] = Y[i] = T(i);
  }

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

    typedef pack<T> type;
    std::size_t step_size_ = boost::simd::meta::cardinal_of<type>::value;
    for (std::size_t i = 0; i<size_; i+=step_size_)
    {
      type X_pack(&X[i]);
      type Y_pack(&Y[i]);
      aligned_store( alpha * X_pack + Y_pack, &Y[i] );
    }
  }

  friend std::ostream& operator<<(std::ostream& os, axpy_simd<T> const& p)
  {
    return os << "(" << p.size() << ")";
  }

  std::size_t size() const { return size_; }
  double flops() const { return 2.*size_; }

private:
  T alpha;
  std::size_t size_;
  typename std::vector<T, boost::simd::allocator<T> > X, Y;
};

NT2_REGISTER_BENCHMARK_TPL( axpy_simd, NT2_SIMD_REAL_TYPES )
{
  std::size_t size_min  = args("size_min",   16);
  std::size_t size_max  = args("size_max", 4096);
  std::size_t size_step = args("size_step",   2);
  T alpha = args("alpha", 1.);

  run_during_with< axpy_simd<T> > ( 1.
                                  , and_( geometric(size_min,size_max,size_step)
                                        , constant(alpha)
                                        )
                                  , gflops<stats::median_>()
                                  );
}


#ifndef MAIA_GAMEENGINE_LOCALROTATION_H_INCLUDED
#define MAIA_GAMEENGINE_LOCALROTATION_H_INCLUDED

#include <Eigen/Geometry>

namespace Maia::GameEngine::Components
{
	struct Local_rotation
	{
		Eigen::Quaternionf value{ 1.0f, 0.0f, 0.0f, 0.0f };
	};
}

#endif


//
// Copyright Jesse Manning 2007
//
// 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 "convenience.h"
#include <boost/numeric/bindings/lapack/driver/gelsd.hpp>

// set to 1 to write test output to file, otherwise outputs to console
#define OUTPUT_TO_FILE 0

// determines which tests to run
#define TEST_SQUARE 1
#define TEST_UNDERDETERMINED 1
#define TEST_OVERDETERMINED 1
#define TEST_MULTIPLE_SOLUTION_VECTORS 1

// determines if optimal, minimal, or both workspaces are used for testing
#define USE_OPTIMAL_WORKSPACE 1
#define USE_MINIMAL_WORKSPACE 1

namespace lapack = boost::numeric::bindings::lapack;
namespace bindings = boost::numeric::bindings;

// test function declarations
template <typename StreamType, typename MatrType, typename VecType>
int test_square_gelsd(StreamType& oss);

template <typename StreamType, typename MatType, typename VecType>
int test_under_gelsd(StreamType& oss);

template <typename StreamType, typename MatType, typename VecType>
int test_over_gelsd(StreamType& oss);

template <typename StreamType, typename MatType, typename VecType>
int test_multiple_gelsd(StreamType& oss);

template <typename StreamType, typename MatType, typename VecType>
int test_transpose_gel(StreamType& oss, const char& trans);

int main()
{
  // stream for test output
  typedef std::ostringstream stream_t;
  stream_t oss;

#if TEST_SQUARE
  oss << "Start Square Matrix Least Squares Tests" << std::endl;
  oss << "Testing sgelsd" << std::endl;
  if(test_square_gelsd<stream_t, fmat_t, fvec_t>(oss) == 0)
  {
    oss << "sgelsd passed." << std::endl;
  }
  else return 255;
  oss << "End sgelsd tests" << std::endl;
  oss << std::endl;
  oss << "Testing dgelsd" << std::endl;
  if(test_square_gelsd<stream_t, dmat_t, dvec_t>(oss) == 0)
  {
    oss << "dgelsd passed." << std::endl;
  }
  else return 255;
  oss << "End dgelsd tests" << std::endl;
  oss << std::endl;
  oss << "Testing cgelsd" << std::endl;
  if(test_square_gelsd<stream_t, fcmat_t, fcvec_t>(oss) == 0)
  {
    oss << "cgelsd passed." << std::endl;
  }
  else return 255;
  oss << "End cgelsd tests" << std::endl;
  oss << std::endl;
  oss << "Testing zgelsd" << std::endl;
  if(test_square_gelsd<stream_t, fcmat_t, fcvec_t>(oss) == 0)
  {
    oss << "zgelsd passed." << std::endl;
  }
  else return 255;
  oss << "End zgelsd tests" << std::endl;
  oss << std::endl;
  oss << "End Square Matrix Least Squares Tests" << std::endl;
#endif

#if TEST_UNDERDETERMINED
  oss << std::endl;
  oss << "Start Under-determined Matrix Least Squares Test" << std::endl;
  oss << "Testing sgelsd" << std::endl;
  if(test_under_gelsd<stream_t, fmat_t, fvec_t>(oss) == 0)
  {
    oss << "sgelsd passed." << std::endl;
  }
  else return 255;
  oss << "End sgelsd tests" << std::endl;
  oss << std::endl;
  oss << "Testing dgelsd" << std::endl;
  if(test_under_gelsd<stream_t, dmat_t, dvec_t>(oss) == 0)
  {
    oss << "dgelsd passed." << std::endl;
  }
  else return 255;
  oss << "End dgelsd tests" << std::endl;
  oss << std::endl;
  oss << "Testing cgelsd" << std::endl;
  if(test_under_gelsd<stream_t, fcmat_t, fcvec_t>(oss) == 0)
  {
    oss << "cgelsd passed." << std::endl;
  }
  else return 255;
  oss << "End cgelsd tests" << std::endl;
  oss << std::endl;
  oss << "Testing zgelsd" << std::endl;
  if(test_under_gelsd<stream_t, fcmat_t, fcvec_t>(oss) == 0)
  {
    oss << "zgelsd passed." << std::endl;
  }
  else return 255;
  oss << "End zgelsd tests" << std::endl;
  oss << std::endl;
  oss << "End Underdetermined Matrix Least Squares Tests" << std::endl;
#endif

#if TEST_OVERDETERMINED
  oss << std::endl;
  oss << "Start Overdetermined Matrix Least Squares Test" << std::endl;
  oss << "Testing sgelsd" << std::endl;
  if(test_over_gelsd<stream_t, fmat_t, fvec_t>(oss) == 0)
  {
    oss << "sgelsd passed." << std::endl;
  }
  else return 255;
  oss << "End sgelsd tests" << std::endl;
  oss << std::endl;
  oss << "Testing dgelsd" << std::endl;
  if(test_over_gelsd<stream_t, dmat_t, dvec_t>(oss) == 0)
  {
    oss << "dgelsd passed." << std::endl;
  }
  else return 255;
  oss << "End dgelsd tests" << std::endl;
  oss << std::endl;
  oss << "Testing cgelsd" << std::endl;
  if(test_over_gelsd<stream_t, fcmat_t, fcvec_t>(oss) == 0)
  {
    oss << "cgelsd passed." << std::endl;
  }
  else return 255;
  oss << "End cgelsd tests" << std::endl;
  oss << std::endl;
  oss << "Testing zgelsd" << std::endl;
  if(test_over_gelsd<stream_t, fcmat_t, fcvec_t>(oss) == 0)
  {
    oss << "zgelsd passed." << std::endl;
  }
  else return 255;
  oss << "End zgelsd tests" << std::endl;
  oss << std::endl;
  oss << "End Overdetermined Matrix Least Squares Test" << std::endl;
#endif

#if TEST_MULTIPLE_SOLUTION_VECTORS
  oss << std::endl;
  oss << "Start Multiple Solution Vectors Least Squares Test" << std::endl;
  oss << "Testing sgelsd" << std::endl;
  if(test_multiple_gelsd<stream_t, fmat_t, fvec_t>(oss) == 0)
  {
    oss << "sgelsd passed." << std::endl;
  }
  else return 255;
  oss << "End sgelsd tests" << std::endl;
  oss << std::endl;
  oss << "Testing dgelsd" << std::endl;
  if(test_multiple_gelsd<stream_t, dmat_t, dvec_t>(oss) == 0)
  {
    oss << "dgelsd passed." << std::endl;
  }
  else return 255;
  oss << "End dgelsd tests" << std::endl;
  oss << std::endl;
  oss << "Testing cgelsd" << std::endl;
  if(test_multiple_gelsd<stream_t, fcmat_t, fcvec_t>(oss) == 0)
  {
    oss << "cgelsd passed." << std::endl;
  }
  else return 255;
  oss << "End cgelsd tests" << std::endl;
  oss << std::endl;
  oss << "Testing zgelsd" << std::endl;
  if(test_multiple_gelsd<stream_t, fcmat_t, fcvec_t>(oss) == 0)
  {
    oss << "zgelsd passed." << std::endl;
  }
  else return 255;
  oss << "End zgelsd tests" << std::endl;
  oss << std::endl;
  oss << "End Multiple Solution Vectors Least Squares Test" << std::endl;
#endif

#if OUTPUT_TO_FILE
  // Finished testing
  std::cout << std::endl;
  std::cout << "Tests Completed." << std::endl;
  std::cout << std::endl;

  std::string filename;
  std::cout << "Enter filename to write test results: ";
  std::getline(std::cin, filename);

  std::ofstream testFile(filename.c_str());

  if(testFile)
  {
    testFile << oss.str();
    testFile.close();
  }
#else
  std::cout << oss.str() << std::endl;

  // Finished testing
  std::cout << std::endl;
  std::cout << "Tests Completed." << std::endl;
  std::cout << std::endl;
#endif

  // wait for user to finish
//	std::string done;
//	std::cout << "Press Enter to exit";
//	std::getline(std::cin, done);

}

// tests square system (m-by-n where m == n)
template <typename StreamType, typename MatType, typename VecType>
int test_square_gelsd(StreamType& oss)
{
  typedef typename bindings::value_type<MatType>::type val_t;
  typedef typename bindings::remove_imaginary<val_t>::type real_t;
  const real_t rcond = -1;    // use machine precision
  fortran_int_t rank;

  // return value
  int err = 0;

  // square matrix test
  MatType mat(MatrixGenerator<MatType>()(row_size, col_size));
  VecType vec(VectorGenerator<VecType>()(row_size));

  //const int m = bindings::size_row(mat);
  const int n = bindings::size_column(mat);
  bindings::detail::array<real_t> s(n);

#if USE_OPTIMAL_WORKSPACE
  MatType optimalmat(mat);
  VecType optimalvec(vec);
  err += lapack::gelsd(optimalmat, optimalvec, s, rcond, rank, lapack::optimal_workspace());
  VecType optimalanswer(ublas::project(optimalvec, ublas::range(0, n)));
  VecType optimal_check = ublas::prod(mat, optimalanswer);
#endif
#if USE_MINIMAL_WORKSPACE
  MatType minimalmat(mat);
  VecType minimalvec(vec);
  err += lapack::gelsd(minimalmat, minimalvec, s, rcond, rank, lapack::minimal_workspace());
  VecType minimalanswer(ublas::project(minimalvec, ublas::range(0, n)));
  VecType minimal_check = ublas::prod(mat, minimalanswer);
#endif

  matrix_print(oss, "A", mat);
  oss << std::endl;
  vector_print(oss, "B", vec);
  oss << std::endl;

#if USE_OPTIMAL_WORKSPACE
  vector_print(oss, "optimal workspace x", optimalanswer);
  oss << std::endl;
#endif
#if USE_MINIMAL_WORKSPACE
  vector_print(oss, "minimal workspace x", minimalanswer);
  oss << std::endl;
#endif
#if USE_OPTIMAL_WORKSPACE
  // check A*x=B
  vector_print(oss, "optimal A*x=B", optimal_check);
  oss << std::endl;
#endif
#if USE_MINIMAL_WORKSPACE
  vector_print(oss, "minimal A*x=B", minimal_check);
  oss << std::endl;
#endif

  return err;
}

// tests overdetermined system (m-by-n where m < n)
template <typename StreamType, typename MatType, typename VecType>
int test_under_gelsd(StreamType& oss)
{
  typedef typename bindings::value_type<MatType>::type val_t;
  typedef typename bindings::remove_imaginary<val_t>::type real_t;
  const real_t rcond = -1;    // use machine precision
  fortran_int_t rank;

  // return value
  int err = 0;

  // under-determined matrix test
  MatType mat(MatrixGenerator<MatType>()(row_range, col_size));
  VecType vec(VectorGenerator<VecType>()(row_size));

  //const int m = bindings::size_row(mat);
  const int n = bindings::size_column(mat);
  bindings::detail::array<real_t> s(n);

#if USE_OPTIMAL_WORKSPACE
  MatType optimalmat(mat);
  VecType optimalvec(vec);
  err += lapack::gelsd(optimalmat, optimalvec, s, rcond, rank, lapack::optimal_workspace());
  VecType optimalanswer(ublas::project(optimalvec, ublas::range(0, n)));
  VecType optimal_check = ublas::prod(mat, optimalanswer);
#endif
#if USE_MINIMAL_WORKSPACE
  MatType minimalmat(mat);
  VecType minimalvec(vec);
  err += lapack::gelsd(minimalmat, minimalvec, s, rcond, rank, lapack::minimal_workspace());
  VecType minimalanswer(ublas::project(minimalvec, ublas::range(0, n)));
  VecType minimal_check = ublas::prod(mat, minimalanswer);
#endif

  matrix_print(oss, "A", mat);
  oss << std::endl;
  vector_print(oss, "B", vec);
  oss << std::endl;

#if USE_OPTIMAL_WORKSPACE
  vector_print(oss, "optimal workspace x", optimalanswer);
  oss << std::endl;
#endif
#if USE_MINIMAL_WORKSPACE
  vector_print(oss, "minimal workspace x", minimalanswer);
  oss << std::endl;
#endif
#if USE_OPTIMAL_WORKSPACE
  // check A*x=B
  vector_print(oss, "optimal A*x=B", optimal_check);
  oss << std::endl;
#endif
#if USE_MINIMAL_WORKSPACE
  vector_print(oss, "minimal A*x=B", minimal_check);
  oss << std::endl;
#endif

  return err;
}

// tests overdetermined system (m-by-n where m > n)
template <typename StreamType, typename MatType, typename VecType>
int test_over_gelsd(StreamType& oss)
{
  typedef typename bindings::value_type<MatType>::type val_t;
  typedef typename bindings::remove_imaginary<val_t>::type real_t;
  const real_t rcond = -1;    // use machine precision
  fortran_int_t rank;

  // return value
  int err = 0;

  // overdetermined matrix test
  MatType mat(MatrixGenerator<MatType>()(row_size, col_range));
  VecType vec(VectorGenerator<VecType>()(row_size));

  //const int m = bindings::size_row(mat);
  const int n = bindings::size_column(mat);
  bindings::detail::array<real_t> s(n);

#if USE_OPTIMAL_WORKSPACE
  MatType optimalmat(mat);
  VecType optimalvec(vec);
  err += lapack::gelsd(optimalmat, optimalvec, s, rcond, rank, lapack::optimal_workspace());
  VecType optimalanswer(ublas::project(optimalvec, ublas::range(0, n)));
  VecType optimal_check = ublas::prod(mat, optimalanswer);
#endif
#if USE_MINIMAL_WORKSPACE
  MatType minimalmat(mat);
  VecType minimalvec(vec);
  err += lapack::gelsd(minimalmat, minimalvec, s, rcond, rank, lapack::minimal_workspace());
  VecType minimalanswer(ublas::project(minimalvec, ublas::range(0, n)));
  VecType minimal_check = ublas::prod(mat, minimalanswer);
#endif

  matrix_print(oss, "A", mat);
  oss << std::endl;
  vector_print(oss, "B", vec);
  oss << std::endl;

#if USE_OPTIMAL_WORKSPACE
  vector_print(oss, "optimal workspace x", optimalanswer);
  oss << std::endl;
#endif
#if USE_MINIMAL_WORKSPACE
  vector_print(oss, "minimal workspace x", minimalanswer);
  oss << std::endl;
#endif
#if USE_OPTIMAL_WORKSPACE
  // check A*x=B
  vector_print(oss, "optimal A*x=B", optimal_check);
  oss << std::endl;
#endif
#if USE_MINIMAL_WORKSPACE
  vector_print(oss, "minimal A*x=B", minimal_check);
  oss << std::endl;
#endif

  return err;
}

// tests multiple solution vectors stored column-wise in B for equation A*x=B
template <typename StreamType, typename MatType, typename VecType>
int test_multiple_gelsd(StreamType& oss)
{
  typedef typename bindings::value_type<MatType>::type val_t;
  typedef typename bindings::remove_imaginary<val_t>::type real_t;
  const real_t rcond = -1;    // use machine precision
  fortran_int_t rank;

  // return value
  int err = 0;

  // multiple solutions vectors test
  MatType mat(MatrixGenerator<MatType>()(row_size, col_size));
  MatType vec(mat.size1(), 2);
  ublas::column(vec, 0) = VectorGenerator<VecType>()(mat.size1());
  ublas::column(vec, 1) = VectorGenerator<VecType>()(mat.size1());

  //const int m = bindings::size_row(mat);
  const int n = bindings::size_column(mat);
  const int nrhs = bindings::size_column(vec);
  bindings::detail::array<real_t> s(n);

#if USE_OPTIMAL_WORKSPACE
  MatType optimalmat(mat);
  MatType optimalvec(vec);
  err += lapack::gelsd(optimalmat, optimalvec, s, rcond, rank, lapack::optimal_workspace());
  MatType optimalanswer(ublas::project(optimalvec, ublas::range(0, n), ublas::range(0, nrhs)));
  MatType optimal_check = ublas::prod(mat, optimalanswer);
#endif
#if USE_MINIMAL_WORKSPACE
  MatType minimalmat(mat);
  MatType minimalvec(vec);
  err += lapack::gelsd(minimalmat, minimalvec, s, rcond, rank, lapack::minimal_workspace());
  MatType minimalanswer(ublas::project(minimalvec, ublas::range(0, n), ublas::range(0, nrhs)));
  MatType minimal_check = ublas::prod(mat, minimalanswer);
#endif

  matrix_print(oss, "A", mat);
  oss << std::endl;
  matrix_print(oss, "B", vec);
  oss << std::endl;

#if USE_OPTIMAL_WORKSPACE
  matrix_print(oss, "optimal workspace x", optimalanswer);
  oss << std::endl;
#endif
#if USE_MINIMAL_WORKSPACE
  matrix_print(oss, "minimal workspace x", minimalanswer);
  oss << std::endl;
#endif
#if USE_OPTIMAL_WORKSPACE
  // check A*x=B
  matrix_print(oss, "optimal A*x=B", optimal_check);
  oss << std::endl;
#endif
#if USE_MINIMAL_WORKSPACE
  matrix_print(oss, "minimal A*x=B", minimal_check);
  oss << std::endl;
#endif

  return err;
}


/**
 * Copyright (C) Omar Thor <omarthoro@gmail.com> - All Rights Reserved
 * Unauthorized copying of this file, via any medium is strictly prohibited
 * Proprietary and confidential
 *
 * Written by Omar Thor <omarthoro@gmail.com>, 2017
 */

#ifndef SP_ALGO_NN_NORMALIZE_HPP
#define SP_ALGO_NN_NORMALIZE_HPP

#include <boost/assert.hpp>
#include "sp/config.hpp"
#include "matrix.hpp"
#include "types.hpp"
#include "layer/detail/layers.hpp"

SP_ALGO_NN_NAMESPACE_BEGIN

/**
 * \file Utility to normalize values for the network specified.
 */

/**
 * \brief Normalize and scale all values and translate to output maximum
 *
 * Note: considers all element at once for min/max of samples
 */
template<typename Network>
void normalize_and_scale(Network& network, sample_vector_type& vec) {
    auto [a, b] = network.out_range();
    float_t min = b;
    float_t max = a;
    for(const auto& v : vec) {
        //find min, max
        std::for_each(v.data(), v.data() + v.size(), [&](auto& x) {
            max = std::max(x, max);
            min = std::min(x, min);
        });
    }

    /**
     * Perform translation and scaling at the same time: First, scale value v_i
     * to [min, max], then rescale to [a, b].
     */
    for(auto& v : vec) {
        v = ((b - a) * (v - min) / ( max - min)) + a;
    }
}

/**
 * \brief Normalize input_type given the network parameters into an input_type
 *
 * @param network
 * @return
 */
template<typename Network>
auto prepare_batch(Network& network, const size_t& batch_size, sample_vector_type& samples) {
    BOOST_ASSERT(batch_size >= 1);
    BOOST_ASSERT(!samples.empty());

    const size_t input_count = samples.size();
    const size_t batch_count = input_count / batch_size;

    const auto& sample_dims = samples[0].dimensions();

    std::vector<tensor_4> vec(
        batch_count,
        tensor_4(
            batch_size,
            sample_dims[0],
            sample_dims[1],
            sample_dims[2]
        )
    );

    /**
     * Merge input sample_type into input_type batch
     */
    for(size_t b = 0; b < batch_count; ++b) {
        for(size_t s = 0; s < batch_size; ++s) {
            BOOST_ASSERT_MSG(
                detail::validate_dimensions<typename Network::input_dims>(samples[b*batch_size+s]),
                "Dimensions of sample matches network input dimension"
            );
            vec[b].chip(s, 0) = samples[b*batch_size + s];
        }
    }

    return vec;
}

/**
 * \brief Prepare samples into batch format of input_type given the network parameters into an input_type
 *
 * @param network
 * @return
 */
template<typename Network>
auto prepare_batch(Network& network, sample_vector_type& samples) {
    return prepare_labels(network, 1, samples);
}

/**
 * \brief Prepare a batch of input_type given the network parameters into an input_type
 *
 * @param network
 * @return
 */
template<typename Network>
auto prepare_batch(Network& network, sample_type& sample) {
    sample_vector_type vec{sample};
    return prepare_labels(network, 1, vec)[0];
}

namespace detail {
    /**
     * \brief Convert class vector type to a a vector of output_type
     */
    template<typename Network>
    auto normalize_label_to_vector_helper(Network& network, const size_t& batch_size, class_vector_type& classes) {

        const size_t class_count = classes.size();
        const size_t batch_count = class_count / batch_size;

        /* Output is a vector of output_type */
        std::vector<tensor_4> vec(
            /* of dimensions (1, 1, 1, the size of the output of the last layer) */
            batch_count, tensor_4(batch_size, Network::output_dims::size, 1, 1)
        );

        auto[min, max] = network.out_target_range();

        for(size_t b = 0; b < batch_count; ++b) {
            /* default to minimum value */
            vec[b].setConstant(min);
            for(size_t s = 0; s < batch_size; ++s) {
                const size_t idx = b * batch_size + s;

                BOOST_ASSERT_MSG(
                    classes[idx] < Network::output_dims::size,
                    "Class value can not exceeds network output size"
                );
                /* set the max value to output layer max */
                vec[b](s, classes[idx], 0, 0) = max;
            }
        }

        return vec;
    }
}

/**
 * \brief Normalize a vector of classes
 */
template<typename Network>
std::vector<tensor_4> prepare_labels(     Network& network,
                                        const size_t& batch_size,
                                        class_vector_type& classes) {
    return detail::normalize_label_to_vector_helper(network, batch_size, classes);
}

/**
 * \brief Normalize a vector of classes
 */
template<typename Network>
std::vector<tensor_4> prepare_batch(     Network& network,
                                        class_vector_type& classes) {
    return prepare_labels(network, 1, classes);
}

/**
 * \brief Converters a vector of a vector to a vector of sample_type
 */
sample_vector_type nested_vector_to_sample_vector(std::vector<std::vector<float_t>> vectors) {
    if(vectors.empty()) {
        return {};
    } else {
        const size_t vector_element_count = vectors[0].size();
        sample_vector_type res(vectors.size(), sample_type(1, 1, vector_element_count));
        size_t idx = 0;
        for(auto& v : vectors) {
            auto& r = res[idx];
            float_t* start = r.data();
            BOOST_ASSERT((static_cast<size_t>(std::distance(start, r.data() + r.size())) == vector_element_count));
            BOOST_ASSERT(vector_element_count == v.size());
            std::copy(v.begin(), v.end(), start);
            ++idx;
        }
        return res;
    }
}
SP_ALGO_NN_NAMESPACE_END

#endif	/* SP_ALGO_NN_NORMALIZE_HPP */


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2013 Nikhar Agrawal
//  Copyright 2013 Christopher Kormanyos
//  Copyright 2014 John Maddock
//  Copyright 2013 Paul Bristow
//  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)

#ifndef _BOOST_POLYGAMMA_2013_07_30_HPP_
  #define _BOOST_POLYGAMMA_2013_07_30_HPP_

#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/detail/polygamma.hpp>
#include <boost/math/special_functions/trigamma.hpp>

namespace boost { namespace math {

  template<class T, class Policy>
  inline typename tools::promote_args<T>::type polygamma(const int n, T x, const Policy& pol)
  {
     //
     // Filter off special cases right at the start:
     //
     if(n == 0)
        return boost::math::digamma(x, pol);
     if(n == 1)
        return boost::math::trigamma(x, pol);
     //
     // We've found some standard library functions to misbehave if any FPU exception flags
     // are set prior to their call, this code will clear those flags, then reset them
     // on exit:
     //
     BOOST_FPU_EXCEPTION_GUARD
     //
     // The type of the result - the common type of T and U after
     // any integer types have been promoted to double:
     //
     typedef typename tools::promote_args<T>::type result_type;
     //
     // The type used for the calculation.  This may be a wider type than
     // the result in order to ensure full precision:
     //
     typedef typename policies::evaluation<result_type, Policy>::type value_type;
     //
     // The type of the policy to forward to the actual implementation.
     // We disable promotion of float and double as that's [possibly]
     // happened already in the line above.  Also reset to the default
     // any policies we don't use (reduces code bloat if we're called
     // multiple times with differing policies we don't actually use).
     // Also normalise the type, again to reduce code bloat in case we're
     // called multiple times with functionally identical policies that happen
     // to be different types.
     //
     typedef typename policies::normalise<
        Policy,
        policies::promote_float<false>,
        policies::promote_double<false>,
        policies::discrete_quantile<>,
        policies::assert_undefined<> >::type forwarding_policy;
     //
     // Whew.  Now we can make the actual call to the implementation.
     // Arguments are explicitly cast to the evaluation type, and the result
     // passed through checked_narrowing_cast which handles things like overflow
     // according to the policy passed:
     //
     return policies::checked_narrowing_cast<result_type, forwarding_policy>(
        detail::polygamma_imp(n, static_cast<value_type>(x), forwarding_policy()),
        "boost::math::polygamma<%1%>(int, %1%)");
  }

  template<class T>
  inline typename tools::promote_args<T>::type polygamma(const int n, T x)
  {
      return boost::math::polygamma(n, x, policies::policy<>());
  }

} } // namespace boost::math

#endif // _BOOST_BERNOULLI_2013_05_30_HPP_


/**
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 * For more about this software visit:
 *
 *      http://graphlab.org
 *
 */

/**
 * \file
 *
 * \brief The main file for the BIAS-SGD matrix factorization algorithm.
 *
 * This file contains the main body of the BIAS-SGD matrix factorization
 * algorithm.
 */

#include <graphlab/util/stl_util.hpp>
#include <graphlab.hpp>
#include "eigen_serialization.hpp"
#include <Eigen/Dense>
#include <graphlab/macros_def.hpp>

typedef Eigen::VectorXd vec_type;
typedef Eigen::MatrixXd mat_type;

//when using negative node id range, we are not allowed to use
//0 and 1 so we add 2.
const static int SAFE_NEG_OFFSET=2;
static bool debug;
int iter = 0;
float itmBiasStep = 1e-4;
float itmBiasReg = 1e-4;
float usrBiasStep = 1e-4;
float usrBiasReg = 1e-4;
float usrFctrStep = 1e-4;
float usrFctrReg = 1e-4;
float itmFctrStep = 1e-4;
float itmFctrReg = 1e-4; //gamma7
float itmFctr2Step = 1e-4;
float itmFctr2Reg = 1e-4;
/**
 * \ingroup toolkit_matrix_pvecization
 *
 * \brief the vertex data type which contains the latent pvec.
 *
 * Each row and each column in the matrix corresponds to a different
 * vertex in the BIASSGD graph.  Associated with each vertex is a pvec
 * (vector) of latent parameters that represent that vertex.  The goal
 * of the BIASSGD algorithm is to find the values for these latent
 * parameters such that the non-zero entries in the matrix can be
 * predicted by taking the dot product of the row and column pvecs.
 */
struct vertex_data {
  /**
   * \brief A shared "constant" that specifies the number of latent
   * values to use.
   */
  static size_t NLATENT;
  /** \brief The number of times this vertex has been updated. */
  uint32_t nupdates;
  /** \brief The latent pvec for this vertex */
  vec_type pvec;
  vec_type weight;
  double bias;
  /**
   * \brief Simple default constructor which randomizes the vertex
   *  data
   */
  vertex_data() : nupdates(0) { randomize(); }
  /** \brief Randomizes the latent pvec */
  void randomize() { pvec.resize(NLATENT); pvec.setRandom(); weight.resize(NLATENT); weight.setRandom(); }
  /** \brief Save the vertex data to a binary archive */
  void save(graphlab::oarchive& arc) const {
    arc << nupdates << pvec << weight << bias;
  }
  /** \brief Load the vertex data from a binary archive */
  void load(graphlab::iarchive& arc) {
    arc >> nupdates >> pvec >> weight >> bias;
  }
}; // end of vertex data

/**
 * \brief The edge data stores the entry in the matrix.
 *
 * In addition the edge data svdppo stores the most recent error estimate.
 */
struct edge_data : public graphlab::IS_POD_TYPE {
  /**
   * \brief The type of data on the edge;
   *
   * \li *Train:* the observed value is correct and used in training
   * \li *Validate:* the observed value is correct but not used in training
   * \li *Predict:* The observed value is not correct and should not be
   *        used in training.
   */
  enum data_role_type { TRAIN, VALIDATE, PREDICT  };

  /** \brief the observed value for the edge */
  float obs;

  /** \brief The train/validation/test designation of the edge */
  data_role_type role;

  /** \brief basic initialization */
  edge_data(float obs = 0, data_role_type role = PREDICT) :
    obs(obs), role(role) { }

}; // end of edge data

/**
 * \brief The graph type is defined in terms of the vertex and edge
 * data.
 */
typedef graphlab::distributed_graph<vertex_data, edge_data> graph_type;

#include "implicit.hpp"

double extract_l2_error(const graph_type::edge_type & edge);

/**
 * \brief Given a vertex and an edge return the other vertex in the
 * edge.
 */
inline graph_type::vertex_type
get_other_vertex(graph_type::edge_type& edge,
    const graph_type::vertex_type& vertex) {
  return vertex.id() == edge.source().id()? edge.target() : edge.source();
}; // end of get_other_vertex

/**
 * \brief The gather type used to construct XtX and Xty needed for the BIASSGD
 * update
 *
 * To compute the ALS update we need to compute the sum of
 * \code
 *  sum: XtX = nbr.pvec.transpose() * nbr.pvec
 *  sum: Xy  = nbr.pvec * edge.obs
 * \endcode
 * For each of the neighbors of a vertex.
 *
 * To do this in the Gather-Apply-Scatter model the gather function
 * computes and returns a pair consisting of XtX and Xy which are then
 * added. The gather type represents that tuple and provides the
 * necessary gather_type::operator+= operation.
 *
 */
class gather_type {
  public:
    /**
     * \brief Stores the current sum of nbr.pvec.transpose() *
     * nbr.pvec
     */

    /**
     * \brief Stores the current sum of nbr.pvec * edge.obs
     */
    vec_type pvec;
    vec_type weight;
    double bias;

    /** \brief basic default constructor */
    gather_type() { }

    /**
     * \brief This constructor computes XtX and Xy and stores the result
     * in XtX and Xy
     */
    gather_type(const vec_type& X, const vec_type & _weight, double _bias) {
      pvec = X;
      bias = _bias;
      weight = _weight;
    } // end of constructor for gather type

    /** \brief Save the values to a binary archive */
    void save(graphlab::oarchive& arc) const { arc << pvec << bias << weight; }

    /** \brief Read the values from a binary archive */
    void load(graphlab::iarchive& arc) { arc >> pvec >> bias >> weight; }

    /**
     * \brief Computes XtX += other.XtX and Xy += other.Xy updating this
     * tuples value
     */
    gather_type& operator+=(const gather_type& other) {
      if (pvec.size() == 0){
        pvec = other.pvec;
        bias = other.bias;
        weight = other.weight;
        return *this;
      }
      else if (other.pvec.size() == 0)
        return *this;
      pvec += other.pvec;
      bias += other.bias;
      weight += other.weight;
      return *this;
    } // end of operator+=

}; // end of gather type

//typedef gather_type message_type;

enum{
  PHASE1 = 0, PHASE2 = 1
};

/**
 * BIASSGD vertex program type
 */
class svdpp_vertex_program :
  public graphlab::ivertex_program<graph_type, gather_type,
  gather_type> {
    public:
      /** The convergence tolerance */
      static double TOLERANCE;
      static double LAMBDA;
      static double GAMMA;
      static double MAXVAL;
      static double MINVAL;
      static double STEP_DEC;
      static bool debug;
      static size_t MAX_UPDATES;
      static double GLOBAL_MEAN;
      static size_t NUM_TRAINING_EDGES;
      static uint   USERS;

      gather_type pmsg;
      void save(graphlab::oarchive& arc) const {
        arc << pmsg;
      }
      /** \brief Load the vertex data from a binary archive */
      void load(graphlab::iarchive& arc) {
        arc >> pmsg;
      }

      /** The set of edges to gather along */
      edge_dir_type gather_edges(icontext_type& context,
          const vertex_type& vertex) const {
        return graphlab::ALL_EDGES;
      }; // end of gather_edges

      gather_type gather(icontext_type& context, const vertex_type& vertex,
          edge_type& edge) const {
        vec_type step = vec_type::Zero(vertex_data::NLATENT);
        double bias =0, other_bias = 0;
        vec_type delta, other_delta;

        //user node
        if (vertex.num_in_edges() == 0){
          vertex_type other_vertex(get_other_vertex(edge, vertex));
          vertex_type my_vertex(vertex);

          int phase = my_vertex.data().nupdates % 2;
          if (phase == PHASE1){
            //my_vertex.data().weight += movie.weight;
            context.signal(other_vertex, gather_type(vec_type::Zero(vertex_data::NLATENT), vec_type::Zero(vertex_data::NLATENT), 0));
            return gather_type(vec_type::Zero(vertex_data::NLATENT), other_vertex.data().weight, 0);
          }
          else if (phase == PHASE2){
            //vertex_data & my_data = my_vertex.data();
            double pred = svdpp_vertex_program::GLOBAL_MEAN +
              my_vertex.data().bias + other_vertex.data().bias + my_vertex.data().pvec.dot(other_vertex.data().pvec+other_vertex.data().weight);
            pred = std::min(pred, svdpp_vertex_program::MAXVAL);
            pred = std::max(pred, svdpp_vertex_program::MINVAL);
            const float err = edge.data().obs - pred;
            if (debug)
              std::cout<<"entering edge " << (int)edge.source().id() << ":" << (int)edge.target().id() << " err: " << err << " rmse: " << err*err <<std::endl;
            if (std::isnan(err))
              logstream(LOG_FATAL)<<"Got into numeric errors.. try to tune step size and regularization using command line flags" << std::endl;
            if (edge.data().role == edge_data::TRAIN){
              vec_type itmFctr = other_vertex.data().pvec;
              vec_type usrFctr = my_vertex.data().pvec;

              bias = usrBiasStep*(err - usrBiasReg*bias);
              other_bias = itmBiasStep*(err - itmBiasReg*other_bias);

              delta = usrFctrStep*(err*(itmFctr - usrFctrReg *usrFctr));
              other_delta = itmFctrStep*(err*(usrFctr+my_vertex.data().weight) - itmFctrReg*other_vertex.data().pvec);

              step = err*itmFctr;
              float usrNorm = double(1.0/sqrt(my_vertex.num_out_edges()));
              step *= itmFctr2Step*usrNorm;

              double mult = itmFctr2Step*itmFctr2Reg;
              step -= mult*other_vertex.data().weight;
              //A HACK: update memory cached values to reflect new vals
              /*my_vertex.data().bias += bias;
                other_vertex.data().bias += other_bias;
                my_vertex.data().pvec += delta;
                other_vertex.data().pvec += other_delta;*/

              if (debug)
                std::cout<<"new val:" << (int)edge.source().id() << ":" << (int)edge.target().id() << " U " << my_vertex.data().pvec.transpose() << " V " << other_vertex.data().pvec.transpose() << std::endl;

              if(other_vertex.data().nupdates < MAX_UPDATES)
                context.signal(other_vertex, gather_type(other_delta, step, other_bias));
            }
            return gather_type(delta, step, bias);

          } //end of PHASE2
        }
        return gather_type(delta, step, bias);
      }

      //typedef vec_type message_type;
      void init(icontext_type& context,
          const vertex_type& vertex,
          const message_type& msg) {

        int phase = vertex.data().nupdates % 2;
        //movie node receives updates here
        if (vertex.num_in_edges() > 0){
          if (phase == PHASE1){
            pmsg = msg;
          }
          else if (phase == PHASE2){
            pmsg = msg;
          }
        }
      }

      /** apply collects the sum of XtX and Xy */
      void apply(icontext_type& context, vertex_type& vertex,
          const gather_type& sum) {
        vertex_data& vdata = vertex.data();
        int phase = vdata.nupdates %2;

        if (phase == PHASE1){
          //user node receives the sum of movie weights
          if (vertex.num_out_edges() > 0){
            vertex.data().weight = sum.pvec;
            float usrNorm = double(1.0/sqrt(vertex.num_out_edges()));
            vertex.data().weight *= usrNorm;
          }
          //movie node doe nothing
          else {}
        }
        else if (phase == PHASE2){
          //user node update gradients and bias
          if (vertex.num_in_edges() == 0){
            vdata.pvec += sum.pvec;
            vdata.bias += sum.bias;
            //does not update weight here (since was done in phase1)
          }
          //movie node
          else {
            vdata.weight += pmsg.weight; //step
            vdata.pvec += pmsg.pvec;
            vdata.bias += pmsg.bias;
          }
        }
        ++vdata.nupdates;
      } // end of apply

      /** The edges to scatter along */
      edge_dir_type scatter_edges(icontext_type& context,
          const vertex_type& vertex) const {
        return graphlab::ALL_EDGES;
      }; // end of scatter edges

      /** Scatter reschedules neighbors */
      void scatter(icontext_type& context, const vertex_type& vertex,
          edge_type& edge) const {
        edge_data& edata = edge.data();
        if(edata.role == edge_data::TRAIN) {
          const vertex_type other_vertex = get_other_vertex(edge, vertex);
          // Reschedule neighbors ------------------------------------------------
          if(other_vertex.data().nupdates < MAX_UPDATES)
            context.signal(other_vertex, gather_type(vec_type::Zero(vertex_data::NLATENT),vec_type::Zero(vertex_data::NLATENT),0));
        }
      } // end of scatter function

      /**
       * \brief Signal all vertices on one side of the bipartite graph
       */
      static graphlab::empty signal_left(icontext_type& context,
          vertex_type& vertex) {
        if(vertex.num_out_edges() > 0) context.signal(vertex, gather_type(vec_type::Zero(vertex_data::NLATENT),vec_type::Zero(vertex_data::NLATENT),0));
        return graphlab::empty();
      } // end of signal_left

  }; // end of svdpp vertex program

struct error_aggregator : public graphlab::IS_POD_TYPE {
  typedef svdpp_vertex_program::icontext_type icontext_type;
  typedef graph_type::edge_type edge_type;
  double train_error, validation_error;
  size_t ntrain, nvalidation;
  error_aggregator() :
    train_error(0), validation_error(0), ntrain(0), nvalidation(0) { }
  error_aggregator& operator+=(const error_aggregator& other) {
    train_error += other.train_error;
    assert(!std::isnan(train_error));
    validation_error += other.validation_error;
    ntrain += other.ntrain;
    nvalidation += other.nvalidation;
    return *this;
  }
  static error_aggregator map(icontext_type& context, const graph_type::edge_type& edge) {
    error_aggregator agg;
    if (edge.data().role == edge_data::TRAIN){
      agg.train_error = extract_l2_error(edge); agg.ntrain = 1;
      assert(!std::isnan(agg.train_error));
    }
    else if (edge.data().role == edge_data::VALIDATE){
      agg.validation_error = extract_l2_error(edge); agg.nvalidation = 1;
    }
    return agg;
  }

  static void finalize(icontext_type& context, const error_aggregator& agg) {
    iter++;
    if (iter%2 == 0)
      return;
    ASSERT_GT(agg.ntrain, 0);
    const double train_error = std::sqrt(agg.train_error / agg.ntrain);
    assert(!std::isnan(train_error));
    context.cout() << std::setw(8) << context.elapsed_seconds() << std::setw(8) << train_error;
    if(agg.nvalidation > 0) {
      const double validation_error =
        std::sqrt(agg.validation_error / agg.nvalidation);
      context.cout() << std::setw(8) << validation_error;
    }
    context.cout() << std::endl;
    usrBiasStep *= svdpp_vertex_program::STEP_DEC;
    itmBiasStep *= svdpp_vertex_program::STEP_DEC;
    usrFctrStep  *= svdpp_vertex_program::STEP_DEC;
    itmFctrStep  *= svdpp_vertex_program::STEP_DEC;
    itmFctr2Step *= svdpp_vertex_program::STEP_DEC;

  }
}; // end of error aggregator

/**
 * \brief Given an edge compute the error associated with that edge
 */
double extract_l2_error(const graph_type::edge_type & edge) {
  double pred = svdpp_vertex_program::GLOBAL_MEAN +
    edge.source().data().bias +
    edge.target().data().bias +
    edge.source().data().pvec.dot(edge.target().data().pvec);
  pred = std::min(svdpp_vertex_program::MAXVAL, pred);
  pred = std::max(svdpp_vertex_program::MINVAL, pred);
  double rmse = (edge.data().obs - pred) * (edge.data().obs - pred);
  assert(rmse <= pow(svdpp_vertex_program::MAXVAL-svdpp_vertex_program::MINVAL,2));
  return rmse;
} // end of extract_l2_error

struct prediction_saver {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  std::string save_vertex(const vertex_type& vertex) const {
    return ""; //nop
  }
  std::string save_edge(const edge_type& edge) const {
    if (edge.data().role != edge_data::PREDICT)
      return "";

    std::stringstream strm;
    double pred = svdpp_vertex_program::GLOBAL_MEAN +
      edge.target().data().bias + edge.source().data().bias + edge.source().data().pvec.dot(edge.target().data().pvec+edge.target().data().weight);
      pred = std::min(pred, svdpp_vertex_program::MAXVAL);
      pred = std::max(pred, svdpp_vertex_program::MINVAL);
    strm << edge.source().id() << '\t'
      << -edge.target().id()-SAFE_NEG_OFFSET << '\t'
      << pred << '\n';
    return strm.str();
  }
}; // end of prediction_saver

struct linear_model_saver_U {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  /* save the linear model, using the format:
     nodeid) factor1 factor2 ... factorNLATENT \n
     */
  std::string save_vertex(const vertex_type& vertex) const {
    if (vertex.num_out_edges() > 0){
      std::string ret = boost::lexical_cast<std::string>(vertex.id()) + " ";
      for (uint i=0; i< vertex_data::NLATENT; i++)
        ret += boost::lexical_cast<std::string>(vertex.data().pvec[i]) + " ";
      ret += "\n";
      return ret;
    }
    else return "";
  }
  std::string save_edge(const edge_type& edge) const {
    return "";
  }
};

struct linear_model_saver_V {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  /* save the linear model, using the format:
     nodeid) factor1 factor2 ... factorNLATENT \n
     */
  std::string save_vertex(const vertex_type& vertex) const {
    if (vertex.num_out_edges() == 0){
      std::string ret = boost::lexical_cast<std::string>(-vertex.id()-SAFE_NEG_OFFSET) + " ";
      for (uint i=0; i< vertex_data::NLATENT; i++)
        ret += boost::lexical_cast<std::string>(vertex.data().pvec[i]) + " ";
      ret += "\n";
      return ret;
    }
    else return "";
  }
  std::string save_edge(const edge_type& edge) const {
    return "";
  }
};

struct linear_model_saver_bias_U {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  /* save the linear model, using the format:
     nodeid) factor1 factor2 ... factorNLATENT \n
     */
  std::string save_vertex(const vertex_type& vertex) const {
    if (vertex.num_out_edges() > 0){
      std::string ret = boost::lexical_cast<std::string>(vertex.id()) + " ";
      ret += boost::lexical_cast<std::string>(vertex.data().bias) + "\n";
      return ret;
    }
    else return "";
  }
  std::string save_edge(const edge_type& edge) const {
    return "";
  }
};
struct linear_model_saver_bias_V {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  /* save the linear model, using the format:
     nodeid) factor1 factor2 ... factorNLATENT \n
     */
  std::string save_vertex(const vertex_type& vertex) const {
    if (vertex.num_out_edges() == 0){
      std::string ret = boost::lexical_cast<std::string>(-vertex.id()-SAFE_NEG_OFFSET) + " ";
      ret += boost::lexical_cast<std::string>(vertex.data().bias) + "\n";
      return ret;
    }
    else return "";
  }
  std::string save_edge(const edge_type& edge) const {
    return "";
  }
};

/**
 * \brief The graph loader function is a line parser used for
 * distributed graph construction.
 */
inline bool graph_loader(graph_type& graph,
    const std::string& filename,
    const std::string& line) {
  ASSERT_FALSE(line.empty());
  // Determine the role of the data
  edge_data::data_role_type role = edge_data::TRAIN;
  if(boost::ends_with(filename,".validate")) role = edge_data::VALIDATE;
  else if(boost::ends_with(filename, ".predict")) role = edge_data::PREDICT;
  // Parse the line
  std::stringstream strm(line);
  graph_type::vertex_id_type source_id(-1), target_id(-1);
  float obs(0);
  strm >> source_id >> target_id;

  if(role == edge_data::TRAIN || role == edge_data::VALIDATE){
    strm >> obs;
    if (obs < svdpp_vertex_program::MINVAL || obs > svdpp_vertex_program::MAXVAL)
      logstream(LOG_FATAL)<<"Rating values should be between " << svdpp_vertex_program::MINVAL << " and " << svdpp_vertex_program::MAXVAL << ". Got value: " << obs << " [ user: " << source_id << " to item: " <<target_id << " ] " << std::endl;
  }
  target_id = -(graphlab::vertex_id_type(target_id + SAFE_NEG_OFFSET));
  // Create an edge and add it to the graph
  graph.add_edge(source_id, target_id, edge_data(obs, role));
  return true; // successful load
} // end of graph_loader

size_t vertex_data::NLATENT = 20;
double svdpp_vertex_program::TOLERANCE = 1e-3;
double svdpp_vertex_program::LAMBDA = 0.001;
double svdpp_vertex_program::GAMMA = 0.001;
size_t svdpp_vertex_program::MAX_UPDATES = -1;
double svdpp_vertex_program::MAXVAL = 1e+100;
double svdpp_vertex_program::MINVAL = -1e+100;
double svdpp_vertex_program::STEP_DEC = 0.9;
bool svdpp_vertex_program::debug = false;
double svdpp_vertex_program::GLOBAL_MEAN = 0;
size_t svdpp_vertex_program::NUM_TRAINING_EDGES = 0;

/**
 * \brief The engine type used by the ALS matrix factorization
 * algorithm.
 *
 * The ALS matrix factorization algorithm currently uses the
 * synchronous engine.  However we plan to add support for alternative
 * engines in the future.
 */
typedef graphlab::omni_engine<svdpp_vertex_program> engine_type;

  double calc_global_mean(const graph_type::edge_type & edge){
    if (edge.data().role == edge_data::TRAIN)
      return edge.data().obs;
    else return 0;
  }

  size_t count_edges(const graph_type::edge_type & edge){
    if (edge.data().role == edge_data::TRAIN)
      return 1;
    else return 0;
  }

int main(int argc, char** argv) {
  global_logger().set_log_level(LOG_INFO);
  global_logger().set_log_to_console(true);

  // Parse command line options -----------------------------------------------
  const std::string description =
    "Compute the ALS factorization of a matrix.";
  graphlab::command_line_options clopts(description);
  std::string input_dir, output_dir;
  std::string predictions;
  size_t interval = 0;
  std::string exec_type = "synchronous";
  clopts.attach_option("matrix", input_dir,
      "The directory containing the matrix file");
  clopts.add_positional("matrix");
  clopts.attach_option("D", vertex_data::NLATENT,
      "Number of latent parameters to use.");
  clopts.attach_option("engine", exec_type,
      "The engine type synchronous or asynchronous");
  clopts.attach_option("max_iter", svdpp_vertex_program::MAX_UPDATES,
      "The maxumum number of udpates allowed for a vertex");
  clopts.attach_option("lambda", svdpp_vertex_program::LAMBDA,
      "SGD regularization weight");
  clopts.attach_option("gamma", svdpp_vertex_program::GAMMA,
      "SGD step size");
  clopts.attach_option("debug", svdpp_vertex_program::debug,
      "debug - additional verbose info");
  clopts.attach_option("tol", svdpp_vertex_program::TOLERANCE,
      "residual termination threshold");
  clopts.attach_option("maxval", svdpp_vertex_program::MAXVAL, "max allowed value");
  clopts.attach_option("minval", svdpp_vertex_program::MINVAL, "min allowed value");
  clopts.attach_option("step_dec", svdpp_vertex_program::STEP_DEC, "multiplicative step decrement");
  clopts.attach_option("user_bias_step", usrBiasStep, "user_bias_step");
  clopts.attach_option("user_bias_reg", usrBiasReg, "user_bias_reg");
  clopts.attach_option("item_bias_step",itmBiasStep, "item_bias_step");
  clopts.attach_option("item_bias_reg", itmBiasReg, "item_bias_reg");
  clopts.attach_option("user_factor_step", usrFctrStep, "user_factor_step");
  clopts.attach_option("user_factor_reg", usrFctrReg, "user_factor_reg");
  clopts.attach_option("item_factor_step", itmFctrStep, "item_factor_step");
  clopts.attach_option("item_factor_reg", itmFctrReg, "item_factor_reg");
  clopts.attach_option("item_factor2_step", itmFctr2Step, "item_factor2_step");
  clopts.attach_option("item_factor2_reg", itmFctr2Reg, "item_factor2_reg");
  clopts.attach_option("interval", interval, "The time in seconds between error reports");
  clopts.attach_option("predictions", predictions,
      "The prefix (folder and filename) to save predictions.");
  clopts.attach_option("output", output_dir, "Output results");

  parse_implicit_command_line(clopts);

  if(!clopts.parse(argc, argv) || input_dir == "") {
    std::cout << "Error in parsing command line arguments." << std::endl;
    clopts.print_description();
    return EXIT_FAILURE;
  }
  debug = svdpp_vertex_program::debug;
  //  omp_set_num_threads(clopts.get_ncpus());
  ///! Initialize control plain using mpi
  graphlab::mpi_tools::init(argc, argv);
  graphlab::distributed_control dc;

  dc.cout() << "Loading graph." << std::endl;
  graphlab::timer timer;
  graph_type graph(dc, clopts);
  graph.load(input_dir, graph_loader);
  dc.cout() << "Loading graph. Finished in "
    << timer.current_time() << std::endl;

  if (dc.procid() == 0)
    add_implicit_edges<edge_data>(implicitratingtype, graph, dc);

  dc.cout() << "Finalizing graph." << std::endl;
  timer.start();
  graph.finalize();
  dc.cout() << "Finalizing graph. Finished in "
    << timer.current_time() << std::endl;

  dc.cout()
    << "========== Graph statistics on proc " << dc.procid()
    << " ==============="
    << "\n Num vertices: " << graph.num_vertices()
    << "\n Num edges: " << graph.num_edges()
    << "\n Num replica: " << graph.num_replicas()
    << "\n Replica to vertex ratio: "
    << float(graph.num_replicas())/graph.num_vertices()
    << "\n --------------------------------------------"
    << "\n Num local own vertices: " << graph.num_local_own_vertices()
    << "\n Num local vertices: " << graph.num_local_vertices()
    << "\n Replica to own ratio: "
    << (float)graph.num_local_vertices()/graph.num_local_own_vertices()
    << "\n Num local edges: " << graph.num_local_edges()
    //<< "\n Begin edge id: " << graph.global_eid(0)
    << "\n Edge balance ratio: "
    << float(graph.num_local_edges())/graph.num_edges()
    << std::endl;

  dc.cout() << "Creating engine" << std::endl;
  engine_type engine(dc, graph, exec_type, clopts);

  // Add error reporting to the engine
  const bool success = engine.add_edge_aggregator<error_aggregator>
    ("error", error_aggregator::map, error_aggregator::finalize) &&
    engine.aggregate_periodic("error", interval);
  ASSERT_TRUE(success);

  svdpp_vertex_program::GLOBAL_MEAN = graph.map_reduce_edges<double>(calc_global_mean);
  svdpp_vertex_program::NUM_TRAINING_EDGES = graph.map_reduce_edges<size_t>(count_edges);
  svdpp_vertex_program::GLOBAL_MEAN /= svdpp_vertex_program::NUM_TRAINING_EDGES;
  dc.cout() << "Global mean is: " <<svdpp_vertex_program::GLOBAL_MEAN << std::endl;

  // Signal all vertices on the vertices on the left (libersgd)
  engine.map_reduce_vertices<graphlab::empty>(svdpp_vertex_program::signal_left);

  // Run the PageRank ---------------------------------------------------------
  dc.cout() << "Running Bias-SGD" << std::endl;
  dc.cout() << "(C) Code by Danny Bickson, CMU " << std::endl;
  dc.cout() << "Please send bug reports to danny.bickson@gmail.com" << std::endl;
  dc.cout() << "Time   Training    Validation" <<std::endl;
  dc.cout() << "       RMSE        RMSE " <<std::endl;
  timer.start();
  engine.start();

  const double runtime = timer.current_time();
  dc.cout() << "----------------------------------------------------------"
    << std::endl
    << "Final Runtime (seconds):   " << runtime
                                        << std::endl
                                        << "Updates executed: " << engine.num_updates() << std::endl
                                        << "Update Rate (updates/second): "
                                          << engine.num_updates() / runtime << std::endl;

  // Compute the final training error -----------------------------------------
  dc.cout() << "Final error: " << std::endl;
  engine.aggregate_now("error");

  // Make predictions ---------------------------------------------------------
  if(!predictions.empty()) {
    std::cout << "Saving predictions" << std::endl;
    const bool gzip_output = false;
    const bool save_vertices = false;
    const bool save_edges = true;
    const size_t threads_per_machine = 1;
    graph.save(predictions, prediction_saver(),
        gzip_output, save_vertices,
        save_edges, threads_per_machine);
    //save the linear model
    graph.save(predictions + ".U", linear_model_saver_U(),
        gzip_output, save_edges, save_vertices, threads_per_machine);
    graph.save(predictions + ".V", linear_model_saver_V(),
        gzip_output, save_edges, save_vertices, threads_per_machine);
    graph.save(predictions + ".bias.U", linear_model_saver_bias_U(),
        gzip_output, save_edges, save_vertices, threads_per_machine);
    graph.save(predictions + ".bias.V", linear_model_saver_bias_V(),
        gzip_output, save_edges, save_vertices, threads_per_machine);

  }

  graphlab::mpi_tools::finalize();
  return EXIT_SUCCESS;
} // end of main


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <iostream>
using namespace std;
using namespace boost;
typedef property<edge_weight_t, int> EdgeWeightProperty;
typedef boost::adjacency_list < listS, vecS, undirectedS, no_property, EdgeWeightProperty> mygraph;

class custom_dfs_visitor : public boost::default_dfs_visitor
{
    public:
    template < typename Vertex, typename Graph >
    void discover_vertex(Vertex u, const Graph & g)
    const { cout << "At " << u << endl; }
    template < typename Edge, typename Graph >
    void examine_edge(Edge e, const Graph& g)
    const { cout << "Examining edges " << e << endl;}
};

int main()
{
    mygraph g;
    add_edge (0, 1, 8, g);
    add_edge (0, 3, 18, g);
    add_edge (1, 2, 20, g);
    add_edge (2, 3, 2, g);
    add_edge (3, 1, 1, g);
    add_edge (1, 3, 7, g);
    custom_dfs_visitor vis;
    depth_first_search(g, visitor(vis));
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <limits>
#include <vector>
#include <algorithm>
#include <string>
#include <sstream>
#include <chrono>

#include <boost/filesystem.hpp>
#include <boost/circular_buffer.hpp>

#include <ScrimpSequVec.hpp>
#include <logging.hpp>
#include <papiwrapper.hpp>

using namespace matrix_profile;

static FactoryRegistration<ScrimpSequVec> s_sequRegistr("scrimp_sequ_vec");
static const int notification_interval_iter = 10000;

PerfCounters cumsum_perf("cumulative_sum");
PerfCounters eval_perf("vectorized evaluation");

tsa_dtype ScrimpSequVec::init_diagonal(const aligned_tsdtype_vec& ASigmaInv, aligned_tsdtype_vec& cumDotproduct, aligned_tsdtype_vec& profile, const int diag, const aligned_tsdtype_vec& A, aligned_int_vec& profileIndex, const int windowSize, const aligned_tsdtype_vec& AMeanScaledSigSqrM)
{
	tsa_dtype corr_score=0;
	tsa_dtype lastz=0;

	//evaluate the fist distance value in the current diagonal
	for (int k = 0; k < windowSize; k++)
	{
		cumDotproduct[k+diag]=lastz;
		lastz += A[k+diag]*A[k];
	}
	//j is the column index, i is the row index of the current distance value in the distance matrix

	return lastz;
}

void ScrimpSequVec::eval_diagonal(idx_dtype* const profileIndex, const tsa_dtype* const A, tsa_dtype& lastz, const idx_dtype windowSize, tsa_dtype* const cumDotproduct, tsa_dtype* const ASigmaInv, const tsa_dtype* const AMeanScaledSigSqrM, const idx_dtype diag, tsa_dtype* const profile, const idx_dtype profileLength)
{
	{
		ScopedPerfAccumulator monitor(cumsum_perf);
		tsa_dtype cumProd = lastz; //TODO: rename lastz. The idea of this line is toavoid unnecessary writes to the input reference....
		for (idx_dtype j=diag; j<profileLength; j++)
		{
			cumDotproduct[j+windowSize]=cumProd;
			cumProd += A[j+windowSize]*A[j+windowSize-diag];
		}
	}
	{
		ScopedPerfAccumulator monitor(eval_perf);
        #pragma omp simd aligned(profile, profileIndex, ASigmaInv, AMeanScaledSigSqrM, cumDotproduct : alignment)
		for (idx_dtype j=diag; j<profileLength; j++)
		{
			idx_dtype i=j-diag;
			tsa_dtype corrScore = ( (cumDotproduct[j+windowSize] - cumDotproduct [j]) * (ASigmaInv[j] * ASigmaInv[i]) - AMeanScaledSigSqrM[j] * AMeanScaledSigSqrM[i]) ;

			bool update_j = (corrScore > profile[j]);
			profile[j] = update_j?corrScore:profile[j];
			profileIndex [j] = update_j?i:profileIndex [j];

			bool update_i = (corrScore > profile[i]);
			profile[i] = update_i?corrScore:profile[i];
			profileIndex [i] = update_i?j:profileIndex[i];
		}
	}
}

void ScrimpSequVec::compute_matrix_profile(const Scrimppp_params& params)
{
	std::chrono::high_resolution_clock::time_point tstart, tend;
	std::chrono::duration<double> time_elapsed;
	aligned_tsdtype_vec A = fetch_time_series<aligned_tsdtype_vec::allocator_type>(params); //load the time series data
	aligned_tsdtype_vec AMeanScaledSqrtM(A.size());
	aligned_tsdtype_vec ASigmaInv(A.size());
	int windowSize = params.query_window_len;
	int exclusionZone = windowSize / 4;
	int timeSeriesLength = A.size();
	int ProfileLength = timeSeriesLength - windowSize + 1;
	//Initialize Matrix Profile and Matrix Profile Index
	aligned_tsdtype_vec profile(ProfileLength, 0.0);
	aligned_int_vec profileIndex(ProfileLength, 0);
	aligned_tsdtype_vec dotproduct(timeSeriesLength); // stores products between two Timeseries values with a distinct offset
	aligned_int_vec idx; // store indices of the diagonals, defining their evaluation order
	idx.reserve(ProfileLength-exclusionZone-1);

	//several monitors for performance measurement
	PerfCounters setup_perf("setup performance");
	PerfCounters init_diag_perf("diagonal initialization");
	//PerfCounters eval_diag_perf("diagonal evaluation");

	//validation of parameters
	if (timeSeriesLength < windowSize) {
		throw std::invalid_argument("ERROR: Time series is shorter than the window length, can not proceed");
	}

	EXEC_INFO( "Sequential SCRIMP matrix profile computation with profile length " << ProfileLength << " and window size " << windowSize);

	{
		ScopedPerfAccumulator monitor(setup_perf);
		//precompute the mean and standard deviations of the sliding windows along the time series
		precompute_window_statistics(windowSize, A, ProfileLength, AMeanScaledSqrtM, ASigmaInv);

		//start time measurment
		tstart = std::chrono::high_resolution_clock::now();

		/******************** SCRIMP ********************/
		//Random shuffle the computation order of the diagonals of the distance matrix
		for (int i = exclusionZone+1; i < ProfileLength; i++) {
			idx.push_back(i);
		}
		std::random_shuffle(idx.begin(), idx.end());
	}

	//iteratively evaluate the diagonals of the distance matrix
	for (int ri = 0; ri < idx.size(); ri++)
	    {
		//select a random diagonal
		int diag = idx[ri];
		tsa_dtype lastz=0; //the dot product of a subsequence

		// compute the first distance value in the diagonal (i.e. compute the distance between the first windows)
		{
			ScopedPerfAccumulator monitor(init_diag_perf);
			lastz = init_diagonal(ASigmaInv, dotproduct, profile, diag, A, profileIndex, windowSize, AMeanScaledSqrtM);
		}

		//evaluate the second to the last distance values along the diagonal in the matrix and update the matrix profile/matrix profile index.
		{
			//ScopedPerfAccumulator monitor(eval_diag_perf);
			// eval_diagonal(profileIndex, A, lastz, windowSize, dotproduct, ASigmaInv, AMeanScaledSqrtM, diag, profile);
			eval_diagonal(profileIndex.data(), A.data(), lastz, windowSize, dotproduct.data(), ASigmaInv.data(), AMeanScaledSqrtM.data(), diag, profile.data(), AMeanScaledSqrtM.size());
		}

		//Show time per 10000 iterations
		if ((ri+1) % notification_interval_iter == 0)
		{
			tend = std::chrono::high_resolution_clock::now();
			time_elapsed = tend - tstart;
			EXEC_INFO ( "finished " << ri+1 << " iterations after " << std::setprecision(std::numeric_limits<tsa_dtype>::digits10 + 2) << time_elapsed.count() << " seconds.");
		}
	}

	// apply a correction of the distance values, as we dropped a factor of 2 to avoid unnecessary computations
	tsa_dtype twice_m = 2.0*static_cast<tsa_dtype>(windowSize);
	for (auto iter=profile.begin(); iter<profile.end(); ++iter) {
		(*iter) = twice_m - 2.0 * (*iter);
	}

	// end timer
	// tend = time(0);
	tend = std::chrono::high_resolution_clock::now();
	time_elapsed = tend - tstart;

	PERF_LOG ( "total computation time: " << std::setprecision(std::numeric_limits<tsa_dtype>::digits10 + 2) << time_elapsed.count() << " seconds." );
	const double triang_len = ProfileLength-exclusionZone;
	PERF_LOG ( "throughput computations: " << triang_len * triang_len / time_elapsed.count() << " matrix entries/second");

	//store the result
	store_matrix_profile(profile, profileIndex, params);

	setup_perf.log_perf();
	init_diag_perf.log_perf();
	cumsum_perf.log_perf();
	eval_perf.log_perf();
	//eval_diag_perf.log_perf();
#ifdef PROFILING
	PERF_LOG ( "number of matrix profile updates: " << _profileUpdateCounter);
#endif

}