#include "Tiling.h"
#include "Stage.h"

#include <Eigen/Core>
using namespace Eigen;

namespace mylib
{
#include <image.h>
}

#include <iostream>
#include <functional>

//#define DEBUG__WRITE_IMAGES
//#define DEBUG__SHOW

#ifdef DEBUG__SHOW
#define SHOW(e) std::cout << "---" << std::endl << #e << " is " << std::endl << (e) << std::endl << "~~~"  << std::endl;
#else
#define SHOW(e)
#endif

#define PANIC(e) do{if(!(e)) error("%s(%d)"ENDL "\tExpression evaluated to false."ENDL "\t%s"ENDL,__FILE__,__LINE__,#e);}while(0)

/// Simple mutex wrapper for scoped locking/unlocking
class AutoLock
{ Mutex *l_;
public:
  AutoLock(Mutex* l):l_(l) {Mutex_Lock(l_);}
  ~AutoLock() {Mutex_Unlock(l_);}
};
// AutoLock lock(lock_);

namespace fetch {
namespace device {

  typedef uint8_t  uint8;
  typedef uint32_t uint32;

  //////////////////////////////////////////////////////////////////////
  //  StageTiling  /////////////////////////////////////////////////////
  //////////////////////////////////////////////////////////////////////

  //  Constructors  ////////////////////////////////////////////////////
  StageTiling::StageTiling(const device::StageTravel &travel,
                           const FieldOfViewGeometry &fov,
                           const Mode                 alignment,
						   bool useTwoDimensionalTiling)
    :
      attr_(NULL),
      cursor_(0),
      current_plane_offset_(0),
      sz_plane_nelem_(0),
      latticeToStage_(),
      fov_(fov),
      z_offset_um_(0.0),
      travel_(travel),
      lock_(0),
      mode_(alignment),
	  useTwoDimensionalTiling_(useTwoDimensionalTiling) //DGA: Added initialization of useTwoDimensionalTiling_
  {
    PANIC(lock_=Mutex_Alloc());
    computeLatticeToStageTransform_(fov,alignment);
    initAttr_(computeLatticeExtents_(travel_));
    //markAddressable_(&travel_);
  }

  //  Destructor  /////////////////////////////////////////////////////
  StageTiling::~StageTiling()
  {
    if(attr_) Free_Array(attr_);
    if(lock_) Mutex_Free(lock_);
  }

  //  computeLatticeToStageTransform_  /////////////////////////////////
  //
  //  For an index vector i, compute T, such that T*i are the nodes of
  //  the lattice in stage space.
  //
  //  FOV angle should be between 0 and pi/2 (180 degrees).
  void StageTiling::computeLatticeToStageTransform_
                          (const FieldOfViewGeometry &fov,
                           const Mode                 alignment)
  { latticeToStage_ = TTransform::Identity();
    Vector3f sc = fov.field_size_um_ - fov.overlap_um_;
    switch(alignment)
    {
// should probably do the zoffset before rotate/shear but
// they're orthogonal to z so it doesn't matter.
    case Mode::Stage_TilingMode_PixelAligned:
        // Rotate the lattice
        latticeToStage_
          .rotate( AngleAxis<float>(fov.rotation_radians_,Vector3f::UnitZ()) )
          .translate(Vector3f(0,0,z_offset_um_))
          .scale(sc)
          ;
        SHOW(latticeToStage_.matrix());
        return;
    case Mode::Stage_TilingMode_StageAligned:
        // Shear the lattice
        float th = fov.rotation_radians_;
        latticeToStage_.linear() =
          (Matrix3f() <<
            1.0f/cos(th), -sin(th), 0,
                       0,  cos(th), 0,
                       0,        0, 1).finished();
        latticeToStage_.translate(Vector3f(0,0,z_offset_um_))
                       .scale(sc)
                       ;
        return;
    }
  }

 void StageTiling::set_z_offset_um(f64 z_um)
 { z_offset_um_=z_um;
   computeLatticeToStageTransform_(fov_,mode_);
 }
 void StageTiling::inc_z_offset_um(f64 z_um)
 { z_offset_um_+=z_um;
   computeLatticeToStageTransform_(fov_,mode_);
 }
 f64 StageTiling::z_offset_um()
 { return z_offset_um_;
 }

  //  computeLatticeExtents_  //////////////////////////////////////////
  //
  //  Find the range of indexes that cover the stage.
  //  This is done by applying the inverse latticeToStage_ transform to the
  //    rectangle described by the stage extents, and then finding extreema.
  //  The latticeToStage_ transfrom is adjusted so the minimal extrema is the
  //    origin.  That is, [min extremal] = T(0,0,0).

  mylib::Coordinate* StageTiling::computeLatticeExtents_(const device::StageTravel& travel)
  {
    Matrix<float,8,3> sabox; // vertices of the cube, stage aligned
    sabox << // travel is in mm
         travel.x.min,   travel.y.min,   travel.z.min,
         travel.x.min,   travel.y.max,   travel.z.min,
         travel.x.max,   travel.y.max,   travel.z.min,
         travel.x.max,   travel.y.min,   travel.z.min,

         travel.x.min,   travel.y.min,   travel.z.max,
         travel.x.min,   travel.y.max,   travel.z.max,
         travel.x.max,   travel.y.max,   travel.z.max,
         travel.x.max,   travel.y.min,   travel.z.max;
    sabox *= 1000.0; //mm to um

    Matrix<float,3,8> labox; // vertices of the cube, lattice aligned
    labox.noalias() = latticeToStage_.inverse() * sabox.transpose();

    Vector3f maxs,mins;
    maxs.noalias() = labox.rowwise().maxCoeff();
    mins.noalias() = labox.rowwise().minCoeff();

    latticeToStage_.translate(mins);
    Vector3z c((maxs-mins).unaryExpr(std::ptr_fun<float,float>(ceil)).cast<size_t>());
    SHOW(sabox);
    SHOW(labox);
    SHOW(mins);
    SHOW(maxs);
    SHOW(c);

    mylib::Coordinate* out;
	useTwoDimensionalTiling_ ? out = mylib::Coord3(1, c(1)+1, c(0)+1) : out = mylib::Coord3(c(2)+1,c(1)+1,c(0)+1); //shape of the lattice. DGA: If useTwoDimensionalTiling_ = true, then out will be set to 1 by c(1)+1 by c(0)+1 (where the first coordinate is z). Else, out will be what it normally is (c(2)+1, c(1)+1,c(0)+1)
    return out;
  }

  //  initAttr_  ///////////////////////////////////////////////////////
  //

  void StageTiling::initAttr_(mylib::Coordinate *shape)
  { AutoLock lock(lock_);
    attr_ = mylib::Make_Array_With_Shape(
      mylib::PLAIN_KIND,
      mylib::UINT32_TYPE,
      shape); // shape gets free'd here

    memset(attr_->data,0,sizeof(uint32_t)*attr_->size); //unset
    sz_plane_nelem_ = attr_->dims[0] * attr_->dims[1];
  }

  //  resetCursor  /////////////////////////////////////////////////////
  //

#define ON_PLANE(e)    (((e)>=current_plane_offset_) && ((e)<(current_plane_offset_ + sz_plane_nelem_)))
#define ON_LATTICE(e)  ((e) < (attr_->size))

  typedef mylib::Dimn_Type Dimn_Type;
  void StageTiling::resetCursor()
  { AutoLock lock(lock_);
    cursor_ = 0;

    uint32_t* mask     = AUINT32(attr_);
    uint32_t  attrmask = Addressable | Safe | Active | Done,
              attr     = Addressable | Safe | Active;

    while( (mask[cursor_] & attrmask) != attr
        && ON_LATTICE(cursor_) ) //DGA: Starting at beginning of lattice, will stop when it finds the first tile that is addressable, safe, active and NOT done OR it will stop at the end of the lattice
    {++cursor_;}

    if(ON_LATTICE(cursor_) &&  (mask[cursor_] & attrmask) == attr)
    { mylib::Coordinate *c = mylib::Idx2CoordA(attr_,cursor_);
      current_plane_offset_ = ADIMN(c)[2] * sz_plane_nelem_;
      mylib::Free_Array(c);
      cursor_ -= 1; // so next*Cursor will be the first tile
    } else
      cursor_=0;
  }

  //  setCursorToPlane  ////////////////////////////////////////////////
  //

  /// \todo bounds checking
  void StageTiling::setCursorToPlane(size_t iplane)
  { AutoLock lock(lock_);
    cursor_=current_plane_offset_=iplane*sz_plane_nelem_;
    cursor_--; // always incremented before query, so subtracting one here means first tile will not be skipped
  }

  //  nextInPlanePosition  /////////////////////////////////////////////
  //

  bool StageTiling::nextInPlanePosition(Vector3f &pos)
  { lock();
    uint32_t* mask = AUINT32(attr_);
    uint32_t attrmask = Addressable | Safe | Active | Done,
             attr     = Addressable | Safe | Active;

    do{++cursor_;}
    while( (mask[cursor_] & attrmask) != attr
        && ON_PLANE(cursor_) );

    if(ON_PLANE(cursor_) &&  (mask[cursor_] & attrmask) == attr)
    { pos = computeCursorPos();
      unlock();
      notifyNext(cursor_);
      return true;
    } else
    { unlock();
      return false;
    }
  }

  //  nextInPlane                    /////////////////////////////////////////////
  //
  bool StageTiling::nextInPlaneQuery(Vector3f &pos,uint32_t attrmask,uint32_t attr)
  { lock();
    uint32_t* mask = AUINT32(attr_);
    do{++cursor_;}
    while( (mask[cursor_] & attrmask) != attr
        && ON_PLANE(cursor_) );

    if(ON_PLANE(cursor_) &&  (mask[cursor_] & attrmask) == attr)
    { pos = computeCursorPos();
      unlock();
      notifyNext(cursor_);
      return true;
    } else
    { unlock();
      return false;
    }
  }

  //  nextInPlaneExplorablePosition  /////////////////////////////////////////////
  //

  /** Return the next tile-position to explore in the current tile-plane.

      Exclude tiles that are not Addressable, or that have already been
      marked Active or Done.
  */

  bool StageTiling::nextInPlaneExplorablePosition(Vector3f &pos)
  { lock();
    uint32_t* mask = AUINT32(attr_);
    uint32_t attrmask = Explorable | Safe | Explored | Addressable | Active | Done,
             attr     = Explorable | Safe | Addressable;

    do{++cursor_;}
    while( (mask[cursor_] & attrmask) != attr
        && ON_PLANE(cursor_) );

    if(ON_PLANE(cursor_) &&  (mask[cursor_] & attrmask) == attr)
    { pos = computeCursorPos();
      unlock();
      notifyNext(cursor_);
      return true;
    } else
    { unlock();
      return false;
    }
  }

  //  nextPosition  ////////////////////////////////////////////////////
  //
  bool StageTiling::nextPosition(Vector3f &pos)
  { lock();
    uint32_t* mask    = AUINT32(attr_);
    uint32_t attrmask = Addressable | Safe | Active | Done,
             attr     = Addressable | Safe | Active;

    do {++cursor_;}
    while( (mask[cursor_] & attrmask) != attr
        && ON_LATTICE(cursor_) );

    if(ON_LATTICE(cursor_))
    { pos = computeCursorPos();
      unlock();
      notifyNext(cursor_);
      return true;
    } else
    { unlock();
      return false;
    }
  }

  //  nextSearchPosition  //////////////////////////////////////////////
  //
  // *ctx should be NULL on the first call.  It will be internally managed.

  struct tile_search_parent_t
  { uint32_t *c,dir,n;
  };

    tile_search_history_t::tile_search_history_t():history(0),i(0),n(0){}
    tile_search_history_t::~tile_search_history_t() {if (history) {free(history);}}
    /** Returns 0 on error (memory) */
    int tile_search_history_t::push(uint32_t *c,int dir,int n)
    { if(!request(i+1)) return 0;
      ++i;
      history[i].c  =c;
      history[i].dir=(uint32_t)dir;
      history[i].n  =(uint32_t)n;
      return 1;
    }
    /** Returns 0 on underflow */
    int tile_search_history_t::pop(uint32_t **pc, uint32_t *pdir, uint32_t* pn)
    { if(i<=0) return 0; // zero is never written to, it's reserved for signalling empty
      *pc  =history[i].c;
      *pdir=history[i].dir;
      *pn  =history[i].n;
      --i;
      return 1;
    }
    int tile_search_history_t::pop(uint32_t **pc, int *pdir, int *pn)
    { uint32_t d,n;
      int ret=pop(pc,&d,&n);
      *pdir=d;
      *pn=n;
      return ret;
    }
    /** returns 0 on failure, otherwise 1 */
    int tile_search_history_t::request(size_t  i_)
    { if(i_>=n)
      { size_t oldn=n;
        n=1.2*i_+50;
        history=(tile_search_parent_t*)realloc(history,sizeof(tile_search_parent_t)*n);
        memset(history+oldn,0,sizeof(tile_search_parent_t)*(n-oldn));
      }
      return (history!=NULL);
    }
    void tile_search_history_t::flush() {i=0;}

    TileSearchContext::TileSearchContext(StageTiling *t,int radius/*=0*/) : tiling(t), c(0),n(0),dir(0),mode(0),radius(radius) {}
    TileSearchContext::~TileSearchContext()
    { if(tiling) tiling->tileSearchCleanup(this);
    }
    void TileSearchContext::sync()
    {  c    =AUINT32(tiling->attr_)+tiling->cursor_;
       line =tiling->attr_->dims[0];
       plane=tiling->sz_plane_nelem_;
       n=0;
    }
    void TileSearchContext::set_outline_mode(bool tf/*=true*/) { mode=tf; if(!tf) flush();}
    bool TileSearchContext::is_outline_mode() {return mode;}
    bool TileSearchContext::is_hunt_mode()    {return !mode;}
    uint32_t *TileSearchContext::next_neighbor()
    { int steps[]={-1,-line,1,line};
      uint32_t *p=c+steps[ (dir+n)%4 ];
      ++n;
      if(n<4 && tiling->on_plane(p))
        return p;
      return 0;
    }
    void TileSearchContext::flush() {n=dir=0;history.flush();}
    /** returns 0 on failure, otherwise 1 */
    int TileSearchContext::push(uint32_t* p)
    { if(!history.push(c,dir,n))
        return 0;
      c=p;
      tiling->cursor_=c-AUINT32(tiling->attr_);
      dir=(dir+n-2)%4; // remmber n is the next neighbor, subtract 1 for current, subtract another 1 to start with left turn
      n=0;
      return 1;
    }
    int TileSearchContext::pop()
    { if(!history.pop(&c,&dir,&n))
        return false;
      tiling->cursor_=c-AUINT32(tiling->attr_);
      return true;
    }
#define CHECK(e,flag) ((*(e)&(flag))==(flag))
#define MARK(e,flag)  (*(e)|=(flag))
    bool TileSearchContext::detected() {return CHECK(c,StageTiling::Detected); }
    void TileSearchContext::reserve()  { MARK(c,StageTiling::Reserved); }

  bool StageTiling::on_plane(uint32_t *p)
  { return ON_PLANE(p-AUINT32(attr_));
  }

  void StageTiling::tileSearchCleanup(TileSearchContext *ctx)
  { const uint32_t *beg = AUINT32(attr_)+current_plane_offset_,
                   *end = beg+sz_plane_nelem_;
    int iplane=current_plane_offset_/sz_plane_nelem_;
    lock();
    for(uint32_t *t=(uint32_t*)beg;t<end;++t) // Reset Reserved
      *t = t[0]&~Reserved;
    if(ctx->is_outline_mode())
    { unlock(); // FIXME: possible race condition
      fillHoles(iplane,Detected);
      if(ctx->radius)
        dilate(iplane,ctx->radius,Detected,Explorable,0,0,Safe); //DGA: Added Safe to make sure that the tiles aren't dilated into nonsafe regions. Just for aesthetics
      lock();
      ctx->set_outline_mode(false);
    }
    cursor_=beg-AUINT32(attr_); // reset to start of plane
								//DGA: Removed copying of explorable tiles from one frame to another since that means the explorable region always grows
    unlock();
  }

  //DGA: Uses done tiles from current plane as the explorable tiles for the next plane
  void StageTiling::useCurrentDoneTilesAsNextExplorableTiles()
  { const uint32_t *beg = AUINT32(attr_) + current_plane_offset_,
	*end = beg + sz_plane_nelem_; //DGA: Beginning and end of plane
    uint32_t *c, *n; //DGA: Pointers to tiles in current (c) and next (n) planes
	{ AutoLock lock(lock_); //DGA: Scoped locking/unlocking since the destructor calls the unlock. This means that this section of code can only be accessed by one thread at a time.
	  if ((current_plane_offset_ / sz_plane_nelem_) < (attr_->dims[2] - 1)) // if not last plane
	  { for (c = (uint32_t*)beg, n = (uint32_t*)end; c < end; ++c, ++n) //DGA: Loop through current (c) and next (n) plane tiles
	    { *n&=~Explorable; //DGA: Reset explorable, since you don't want to use what was initially defined as explorable
		  if ((*c&Done) == Done) *n |= Explorable ; //DGA: If c was done, then make n (corresponding tile in next plane) explorable
	    }
	  }
	}
  }

  //DGA: Use current done tiles as explorable tiles when two dimensional tiling is being used
  void StageTiling::useDoneTilesAsExplorableTilesForTwoDimensionalTiling()
  { const uint32_t *beg = AUINT32(attr_),
    *end = beg + sz_plane_nelem_; //DGA: Beginning and end of plane
    uint32_t *c; //DGA: Pointers to tiles in current plane
	{ AutoLock lock(lock_); //DGA: Scoped locking/unlocking since the destructor calls the unlock. This means that this section of code can only be accessed by one thread at a time.
	  for (c = (uint32_t*)beg; c < end; ++c) //DGA: Loop through current (c) plane tiles
	  { *c&=(Addressable | Safe | Done); //DGA: Reset everything except if the tile is Addressable, Safe and Done
		if ((*c&Done) == Done) //DGA: If tile c is done
		{
	      *c |= Explorable ; //DGA: If c was done, then make it explorable and then mark it as not done
		  *c &= ~Done;
	    }
	  }
	}
  }

#define ELIGABLE(e)          ((*(e)&eligable_mask)==eligable)
#define ALREADY_DETECTED(e)  ((*(e)&(Addressable|Explorable|Detected|Reserved))==(Addressable|Explorable|Detected))
//#define IMPLY_DETECTED(e)    ((*(e)&(Done))==(Done)) | ((*(e)&(Active))==(Active))

  /* Sorry for the goto's.  don't hate. */
  bool StageTiling::nextSearchPosition(int iplane, int radius, Vector3f &pos,TileSearchContext **pctx)
  { const uint32_t eligable_mask = Addressable | Safe | Explorable | Explored /*| Active | Done*/, // Active/Done do not imply detection and we don't want them to
                   eligable      = Addressable | Safe | Explorable,
                  *beg = AUINT32(attr_)+current_plane_offset_,
                  *end = beg+sz_plane_nelem_;
    if(!pctx) return 0;
    TileSearchContext *ctx=*pctx;
    lock();
    if(!ctx)
    { *pctx=ctx=new TileSearchContext(this,radius);
    }
    ctx->sync();
Start:
    if(ctx->detected())
    { ctx->reserve();
      ctx->set_outline_mode();
    }
    if(ctx->is_outline_mode())
    { uint32_t *n;
      if(ctx->detected())
      { while(n=ctx->next_neighbor())
        { if(CHECK(n,Reserved)) // BOOM DONE - the loop is closed
          { goto DoneOutlining;
          }
#if 0 // Actually don't want to infer detected from any other flags
          if(IMPLY_DETECTED(n)) //infer_detected - some flags imply tile is detected
            *n|=Detected;
#endif
          if(ALREADY_DETECTED(n))
          { ctx->push(n); // treat previously detected as ok part of the perimeter
            goto Start;   // don't need to detect again
          }
          if(ELIGABLE(n))
          { ctx->push(n);
            goto Yield;
          }
        }
      }
      if(ctx->pop())      // Back up.  Also part of handling dead end - no neighbors eligable or reserved
        goto Start;
      goto DoneOutlining; // underflow...The found region is just a strip, not a closed loop.
    }
Hunt:
    if(ctx->is_hunt_mode())
    { unlock(); // FIXME: racy
      return nextInPlaneQuery(pos,
        Addressable|Safe|Explorable|Explored|Detected|Active|Done,  // skip explored,detected,active or done tiles.
        Addressable|Safe|Explorable                              );
      lock();
    }
    // ALL DONE
    delete ctx;
    *pctx=0;
    unlock();
    return false;
Yield:
    pos=computeCursorPos();
    unlock();
    notifyNext(cursor_);
    return true;
DoneOutlining:
    unlock(); // FIXME - racy
    tileSearchCleanup(ctx); // this cleans up the outlining and resets to hunt mode
    lock();
    goto Hunt;
  }
#undef CHECK
#undef MARK
#undef ELIGABLE

  //  markDone  ////////////////////////////////////////////////////////
  //
  void StageTiling::markDone(bool success)
  { uint32_t *m=0;
    { AutoLock lock(lock_);
      m = AUINT32(attr_) + cursor_;
      *m |= Done;
      if(!success)
        *m |= TileError;
    }
    notifyDone(cursor_,*m);
  }

  //  markSafe  ////////////////////////////////////////////////////////
  //
  void StageTiling::markSafe(bool success)
  { uint32_t *m=0;
    { AutoLock lock(lock_);
      m = AUINT32(attr_) + cursor_;
      *m |= Safe;
      if(!success)
        *m |= TileError;
    }
    notifyDone(cursor_,*m);
  }

  //  markExplored  //////////////////////////////////////////////////////
  //
  void StageTiling::markExplored(bool tf)
  { uint32_t *m=0;
    { AutoLock lock(lock_);
      m = AUINT32(attr_) + cursor_;
      if(tf)
        *m |= Explored;
      else
        *m &= ~Explored;
    }
    notifyDone(cursor_,*m);
  }

  //  markDetected  //////////////////////////////////////////////////////
  //
  void StageTiling::markDetected(bool tf)
  { uint32_t *m=0;
    { AutoLock lock(lock_);
      m = AUINT32(attr_) + cursor_;
      if(tf)
        *m |= Detected;
      else
        *m &= ~Detected;
    }
    notifyDone(cursor_,*m);
  }

  //  markActive  ////////////////////////////////////////////////////////
  //
  void StageTiling::markActive()
  { uint32_t *m=0;
    { AutoLock lock(lock_);
      m = AUINT32(attr_) + cursor_;
      *m |= Active;
    }
    notifyDone(cursor_,*m);
  }

  //  markOffsetMeasured  ////////////////////////////////////////////////////////
  //
  void StageTiling::markOffsetMeasured(bool tf)
  { uint32_t *m=0; //DGA: pointer to tile address
    { AutoLock lock(lock_);
      m = AUINT32(attr_) + cursor_;
      if(tf)
        *m |= OffsetMeasured; //DGA: Mark OffsetMeasured
      else
        *m &= ~OffsetMeasured; //DGA: Unmark OffsetMeasured
    }
    notifyDone(cursor_,*m); //DGA: Used to notify that the tile has been changed
  }

  //  markUserReset  /////////////////////////////////////////////////////
  //

  void StageTiling::markUserReset()
  { uint32_t *m=0;
    { AutoLock lock(lock_);
      m = AUINT32(attr_) + cursor_;
	  *m &= ~( Active|Detected|Explored|Explorable|Safe|Done|TileError|OffsetMeasured|Dilated); //DGA: Added TileError, OffsetMeasured, Dilated
    }
    notifyDone(cursor_,*m);
  }

    //  markResetGivenAttributes  /////////////////////////////////////////////////////
  //
  void StageTiling::markResetGivenAttributeCombinationForTilesInCurrentPlane(uint32_t query_mask)
  {
	{
	  uint32_t *beg = AUINT32(attr_) + current_plane_offset_, // DGA: First tile in current plane
			   *end = beg + sz_plane_nelem_; // DGA: Last tile in current plane, sz_plane_nelem_ is number of tiles in plane
	  for (uint32_t *t = beg; t < end; ++t)
	  {
	    if (((*t)&query_mask) == query_mask) //DGA: If the tile has the attributes of the query mask
		{ AutoLock lock(lock_); //DGA: Scoped locking
		  (*t)&=~query_mask; //DGA: Reset the
		   notifyDone(cursor_,*t); //DGA: Used to notify that the tile has been changed
		}
	  }
	}
  }

  //  markAddressable  ///////////////////////////////////////////////////
  //
  static inline unsigned in(const Vector3f &a,const Vector3f &b,const Vector3f &v)
  { return (a[0]<v[0]) && (v[0]<b[0])
         &&(a[1]<v[1]) && (v[1]<b[1])
         &&(a[2]<v[2]) && (v[2]<b[2]);
  }
  /** Marks the indicated plane as addressable according to the travel.
  */
  void StageTiling::markAddressable(size_t iplane)
  {
    size_t old = cursor_;
    setCursorToPlane(iplane); // FIXME: chance for another thread to change the cursor...recursive locks not allowed
    AutoLock lock(lock_);
    Vector3f a = Vector3f(travel_.x.min,travel_.y.min,travel_.z.min),
             b = Vector3f(travel_.x.max,travel_.y.max,travel_.z.max);
    uint32_t* v = AUINT32(attr_);
    while(ON_PLANE(++cursor_))
      v[cursor_]|=Addressable*in(a,b,computeCursorPos()*0.001);
    cursor_=old; // restore cursor
  }

  //  anyExplored  ///////////////////////////////////////////////////////////////
  //
  int StageTiling::anyExplored(int iplane)
  {
    setCursorToPlane(iplane);// FIXME: chance for another thread to change the cursor...recursive locks not allowed
    AutoLock lock(lock_);
    uint32_t* mask = AUINT32(attr_);
    uint32_t attrmask = Explorable|Addressable,
             attr     = Explorable|Addressable;

    do{++cursor_;}
    while( (mask[cursor_] & attrmask) != attr
        && ON_PLANE(cursor_) );
    return ON_PLANE(cursor_);
  }

  //  fillHolesInActive  /////////////////////////////////////////////////
  //

  /// Private stack implementation for non-recursive flood fill.
  typedef struct _stack_t
  { size_t     n,cap;
    uint32_t **data;
  } stack_t;
  static void grow(stack_t *s)
  { if(s->n==s->cap-1)
    { size_t newsize = s->cap*1.2+50;
      Guarded_Realloc((void**)&s->data,newsize,"grow stack");
      memset(s->data+s->cap,0,(newsize-s->cap)*sizeof(uint32_t*));
      s->cap=newsize;
    }
  }
  static void push(stack_t *s, uint32_t* v)
  { grow(s);
    s->data[s->n++] = v;
  }
  static uint32_t* pop(stack_t *s)
  { return s->data[--s->n];
  }
  static stack_t make_stack(size_t reserve)
  { stack_t s;
    s.n=0;
    s.data=(uint32_t**)Guarded_Calloc(s.cap=reserve,sizeof(uint32_t*),"make stack");
    return s;
  }
  static void destroy_stack(stack_t *s)
  { if(s && s->data) free(s->data);
  }

  /**
    Looks at tiles in this plane for detection events in
    order to determine whether tiles in the plane should be marked Active

    \returns 1 if any tiles were marked active, otherwise 0.
  */
  int StageTiling::updateActive(size_t iplane)
  { int any=0;
    setCursorToPlane(iplane);// FIXME: chance for another thread to change the cursor...recursive locks not allowed
    AutoLock lock(lock_);
    uint32_t *c,*p,
             *beg  = AUINT32(attr_)+current_plane_offset_,
             *end  = beg+sz_plane_nelem_;
    #define DETECTED(e) ((*(e)&Detected)==Detected)
    for(c=beg;c<end;c++)
    { if( DETECTED(c)) //DGA: Removed condition to also check previous plane for detection events, which would be used to mark the current plane tiles active. Now only currently detected tiles are set to active.
      { *c |= Active;
        any=1;
      }
    }
    #undef DETECTED
    return any;
  }

  /** Fill holes in regions marked as active

    Filled regions are 4-connected.
  */
  void StageTiling::fillHolesInActive(size_t iplane)
  { fillHoles(iplane,Active);}

  void StageTiling::fillHoles(size_t iplane, StageTiling::Flags flag)
  {
    setCursorToPlane(iplane);// FIXME: chance for another thread to change the cursor...recursive locks not allowed
    AutoLock lock(lock_);
    uint32_t *c,
             *beg = AUINT32(attr_)+current_plane_offset_,
             *end = beg+sz_plane_nelem_;
    int is_open=0;
    const unsigned w=attr_->dims[0],
                   h=attr_->dims[1];
    stack_t stack = make_stack(sz_plane_nelem_);
    for(c=beg;c<end;)
    { uint32 *n,*next;
      uint32 mask = flag | Reserved;
      // mark connected and do bounds test for region
      is_open=0;
      push(&stack,0); // push 0 to detect underflow when the fill is done
      push(&stack,c);
      while(n=pop(&stack))
      {   unsigned x = (n-beg)%w,
                   y = (n-beg)/w;
#if 0
          debug("%3d %3d %15s %15s %15s %15s\n",x,y,
                (n[0]&Reserved)?"Reserved":".",
                (n[0]&flag)?"flag":".",
                (n[0]&Explorable)?"Explorable":".",
                (n[0]&Addressable)?"Addressable":".");
#endif
          *n |= Reserved;
          is_open |= ((x==0)||(x==w-1)||(y==0)||(y==h-1)); // is the region connected to the plane bounds?
          if(!(y>=(h-1) || *(next=(n+w))&mask )) {*next|=Reserved; push(&stack,next);}  // down
          if(!(y<=0     || *(next=(n-w))&mask )) {*next|=Reserved; push(&stack,next);}  // up
          if(!(x<=0     || *(next=(n-1))&mask )) {*next|=Reserved; push(&stack,next);}  // left
          if(!(x>=(w-1) || *(next=(n+1))&mask )) {*next|=Reserved; push(&stack,next);}  // right
      } // end first fill
      if(!is_open)                             // second fill to mark interior as flag
      { mask = flag;                         // edges and self are labeled flag
        push(&stack,0);
        push(&stack,c);
        while(n=pop(&stack))
        {   unsigned x = (n-beg)%w,
                     y = (n-beg)/w;
#if 0
            debug("%3d %3d %15s %15s %15s %15s\n",x,y,
                (n[0]&Reserved)?"Reserved":".",
                (n[0]&flag)?"flag":".",
                (n[0]&Explorable)?"Explorable":".",
                (n[0]&Addressable)?"Addressable":".");
#endif
            *n |= flag;
            if(!(y>=(h-1) || *(next=(n+w))&mask )) {*next|=flag; push(&stack,next);}  // down
            if(!(y<=0     || *(next=(n-w))&mask )) {*next|=flag; push(&stack,next);}  // up
            if(!(x<=0     || *(next=(n-1))&mask )) {*next|=flag; push(&stack,next);}  // left
            if(!(x>=(w-1) || *(next=(n+1))&mask )) {*next|=flag; push(&stack,next);}  // right
        } // end second fill
      }

      while(c++<end && *c&(Reserved|flag));  // move to next unreserved
    } // done searching for regions
    destroy_stack(&stack);
    for(c=beg;c<end;++c)                       // mark all unreserved
      *c = c[0]&~Reserved;
  }

  //  dilateActive  //////////////////////////////////////////////////////
  //
  #define countof(e) (sizeof(e)/sizeof(*e))
  /** Mark tiles as Active if they are 8-connected to an Active tile. */
  void StageTiling::dilateActive(size_t iplane)
  { dilate(iplane,1,Active|Safe,(Active|Dilated),1 /*restrict to explorable */,Dilated /*DGA: exclude tiles that have alread been dilated*/, Safe /*DGA: make sure to only dilate into safe tiles*/);
  }

  // dilate         //////////////////////////////////////////////////////
  //
  // Does not dilate into Done tiles.
  // Optionally restricts dilation to explorable tiles.
  //
  //
  void StageTiling::dilate(size_t iplane, int n,  uint16_t query_flag, uint16_t write_flag, int explorable_only, uint16_t exclude_flag, uint16_t valid_flag)
  {
    setCursorToPlane(iplane); // FIXME: chance for another thread to change the cursor...recursive locks not allowed
    AutoLock lock(lock_);
    uint32_t *c,
             *beg = AUINT32(attr_)+current_plane_offset_,
             *end = beg+sz_plane_nelem_;
    const unsigned w=attr_->dims[0],
                   h=attr_->dims[1];
    unsigned       x,y,j;

    const int offsets[] = {-w-1,-w,-w+1,-1,1,w-1,w,w+1};
    const unsigned top=1,left=2,bot=4,right=8; // bit flags
#define MAYBE_EXPLORABLE ((explorable_only)?Explorable:0)
    const unsigned masks[]   = {top|left,top,top|right,left,right,bot|left,bot,bot|right};
    const unsigned attrmask = Reserved|Safe|Addressable|Done|MAYBE_EXPLORABLE,            // attr is the neighbor query
                   attr     = Reserved|Safe|Addressable     |MAYBE_EXPLORABLE,
                   lblmask  = Addressable|MAYBE_EXPLORABLE;                          // lblmask selects for valid write points
#undef MAYBE_EXPLORABLE
    for(c=beg;c<end;++c)                             // mark original active tiles as reserved
      *c |= ((*c&query_flag)==query_flag && (*c&exclude_flag)==0)*Reserved;  // this way only the originally labelled tiles will be dilated from, DGA: and it excludes any that match the exclude_flag
    while(n-->0)
    { for(y=0;y<h;++y)
      { unsigned rowmask = ((y==0)*top)|((y==h-1)*bot);
        for(x=0;x<w;++x)
        { const unsigned colmask = ((x==0)*left)|((x==w-1)*right),
                            mask = rowmask|colmask;
          c=beg+y*w+x;
          if((*c&lblmask)==lblmask)                // mark only tile that are addressable and explorable
            for(j=0;j<countof(offsets);++j)
              if(  (mask&masks[j])==0              // is neighbor in bounds
                && (c[offsets[j]]&attrmask)==attr) // query neighbor attribute for match
			  { if ((*c&valid_flag)==valid_flag) { *c|=(write_flag|Reserved2); break; } //DGA: Ensure that tiles that are going to be dilated are valid
              }
        }
      }
      for(c=beg;c<end;++c)                     // make query transitive over dilation radius
        if(*c&Reserved2)
        { *c|=Reserved;
          *c = c[0]&~Reserved2;
        }
    }
    for(c=beg;c<end;++c)                       // mark all unreserved
      *c = c[0]&~Reserved;
  }

  // minDistTo ///////////////////////////////////////////////////////////////
  //
  // returns -1 on error, otherwise
  //         the lattice distance from the current tile to the nearest tile
  //         in plane satisfying the query.
  //
  // Algorithm is breadth first search.  So it requires a FIFO queue.
  //
  // NOT THREAD SAFE
  // - right now this is only called by AdaptiveTilingTask
  // - to make it thread safe
  //   - make estore and friends non-static
  //   or
  //   - protext estore and friends
  //   - deallocation has to track which elements are used and which are free
  //   - occasionally repack estore
  //

  class q_t
  { typedef struct e_t_ {int dist; uint32_t *tile; struct e_t_* next;}* e_t; //DGA: e_t can now be used such that eg. e_t head is equivalent to e_t_ * head (a pointer to e_t_)
    e_t head,tail;

    // block allocate elements
    // - have to do this or lock's during free (called by ~q_t()) get too time consuming
    e_t estore;
    size_t estore_sz;
    size_t estore_i;
    e_t elem() { // alloc new element
      estore_i++;
      if(estore_i>estore_sz)
        return 0;
      return estore+(estore_i-1); // DGA: go to next address (a_pointer+a_number is actually a_pointer + (a_number*sizeof(*a_pointer))
    }
    void free_elem(e_t e) { /* release element -- no op */ }
    e_t bind(uint32_t* v,int dist) {
      e_t e=elem(); // DGA: e is a pointer to e_t_ structure, whose value is the next memory address
      if(!e) return 0; // DGA: if estore_i exceeded estore_sz, then something went wrong and return 0
      e->tile=v; e->next=NULL; e->dist=dist; // DGA: set the tile to the input tile, set next to null and dist to dist
      return e;
    }
  public:
    q_t(size_t n):head(0),tail(0),estore(0),estore_sz(n),estore_i(0) {estore=(e_t)calloc(n,sizeof(struct e_t_));}
    ~q_t() { free(estore); } //e_t e=head; while(e) {e_t t=e->next; free_elem(e); e=t; } estore_i=0;}
    q_t* push(uint32_t* v, int dist) throw(const char*) {
      e_t e=0; // DGA: Creates e, which is a pointer to an e_t_ struct, initialized to NULL
      if(!v) return this; // no-op -- to allow implicit filtering of inputs
      e=bind(v,dist); // DGA: Makes e at the next address, with tile v and dist=dist
      if(!e) throw "allocation error";    // memory allocation error
      if(!head) { head=tail=e; }          // first elem. must test head bc of how I pop
      else      { tail->next=e; tail=e;}  // DGA: sets tail->next to e; then sets tail to e. Thus, each e points to next one in stack
      return this; // DGA: Returns the current object (the whole stack)
    }
    uint32_t* pop(int *dist) {
      uint32_t* v=0; // DGA: creates null tile pointer
      int d=-1; // DGA: Initialize d to -1
      e_t e=head; // DGA: e is now the head of the stack
      if(head) { head=head->next; v=e->tile; d=e->dist; free_elem(e);} // DGA: If head is not NULL pointer, then head points to the next element in stack, v is the tile corresponding to the initial head, d is the initial head's distance
      if(dist) *dist=d; // DGA: If dist is not NULL, then distance equals d (ie. the distance from the reference to the tile being popped)
      return v; // DGA: Returns the tile
    }
  };

  int StageTiling::numberOfTilesWithGivenAttributes(uint32_t query_mask){ // DGA: This is the definition of the function to count the number of tiles with attributes defined by query mask
	  int numberOfTilesWithGivenAttributes=0;
	  uint32_t *beg = AUINT32(attr_) + current_plane_offset_, // DGA: First tile in current plane
			   *end = beg + sz_plane_nelem_; // DGA: Last tile in current plane, sz_plane_nelem_ is number of tiles in plane
	  for (uint32_t *t = beg; t < end; ++t){
		  if (((*t)&query_mask) == query_mask) // DGA: Can have more attributes than query_mask, but must at least have all of the ones in query_mask
			  numberOfTilesWithGivenAttributes++;
	  }
	  return numberOfTilesWithGivenAttributes;
  }

  int StageTiling::minDistTo(
    uint32_t search_mask,uint32_t search_flags, // area to search
    uint32_t query_mask,uint32_t query_flags)  // tile to find
  {
    int dist=0;
    const unsigned w=attr_->dims[0],
                   h=attr_->dims[1];
    const int offsets[] = {-w-1,-w,-w+1,-1,1,w-1,w,w+1}; // moves
	uint32_t *c = AUINT32(attr_) + cursor_, // DGA: Current tile address?
		   	 *beg = AUINT32(attr_) + current_plane_offset_, // DGA: First tile in current plane
		     *end = beg + sz_plane_nelem_; // DGA: Last tile in current plane, sz_plane_nelem_ is number of tiles in plane
    q_t q(w*h); // DGA: q is of class q_t with input size n = width * height (number of frames?). class variable estore_i set to 0

    for(uint32_t *t=(uint32_t*)beg;t<end;++t) // Reset Reserved
      *t = t[0]&~Reserved; // DGA: Mark all the tiles as not reserved (Reserved = 512, eg 1000000000, so ~Reserved = 0111111111, resets 10th bit to 0?)

    #define isvalid(p)    ((*(p)&(search_mask|Reserved)) == search_flags)
    #define isinbounds(p) (beg<=(p) && (p)<end)
    #define maybe(p)      ( (isvalid(p)&&isinbounds(p)) ?(p):NULL)

	// DGA: maybeAndReserve will check if the tile is in bounds and valid. If it is, then it will mark the tile as reserved and return it, otherwise it will return NULL
	// Before, tiles were reserved only after popped leading to them being pushed to the stack many times, leading to the allocation error being thrown
	#define maybeAndReserve(p) ( (isvalid(p)&&isinbounds(p)) ? &(*(p) |= Reserved ):NULL)

	try
	{ q.push(maybeAndReserve(c),0); // DGA: maybeAndReserve(c) will mark c as reserved and return it if it is valid and in bounds, NULL otherwise. c is then pushed as next tile in stack of e_t structs
	  while(c=q.pop(&dist)) // DGA: Pops off the head, setting c to the pointer to the former head's tile, continuing in loop if it is not the NULL pointer
      {// *c |= Reserved; // mark it as looked at. DGA: sets 10th? bit to 1, so you don't recheck them when searching
        if((*c&query_mask)==query_flags) {goto Finalize;} // DGA: If condition met, then will return distance
		for (int i = 0; i < countof(offsets); ++i){
			q.push(maybeAndReserve(c + offsets[i]), dist + 1); // DGA: Push all the neighbors of c to the stack, with a distance of c's distance+1 (only if they are valid, inbounds); they are also marked as reserved
		}
      } // DGA: Stops when no more tiles found that are inbounds and valid
      dist=0; // - no tiles found or started in a bad spot
    }
    catch(const char* msg)
    { debug("%s(%d): %s()\r\n\t%s\r\n",__FILE__,__LINE__,__FUNCTION__,msg);
      dist = -1;
    }
    #undef isvalid
    #undef isinbounds
    #undef maybe
	#undef maybeAndReserve
  Finalize:
    for(uint32_t *t=(uint32_t*)beg;t<end;++t) // Reset Reserved
      *t = t[0]&~Reserved;
    return dist;
  }

  void StageTiling::notifyDone(size_t index, uint32_t sts)
  { TListeners::iterator i;
    for(i=listeners_.begin();i!=listeners_.end();++i)
      (*i)->tile_done(index,sts);
  }

  void StageTiling::notifyNext(size_t index)
  { TListeners::iterator i;
    for(i=listeners_.begin();i!=listeners_.end();++i)
      (*i)->tile_next(index);
  }

  const Vector3f StageTiling::computeCursorPos()
  {
    mylib::Coordinate *c = mylib::Idx2CoordA(attr_,cursor_);
    mylib::Dimn_Type *d = (mylib::Dimn_Type*)ADIMN(c);
    Vector3z r;
    r << d[0],d[1],d[2];
    Vector3f pos = latticeToStage_ * r.transpose().cast<float>();
    Free_Array(c);
    return pos;
  }

  void StageTiling::getCursorLatticePosition(int *x,int *y,int *z)
  {
      mylib::Coordinate *c=mylib::Idx2CoordA(attr_,cursor_);
      mylib::Dimn_Type *d=(mylib::Dimn_Type*)ADIMN(c);
      if(x) *x=d[0];
      if(y) *y=d[1];
      if(z) *z=d[2];
      Free_Array(c);
  }

  float StageTiling::plane_mm()
  {
    Vector3f r(0,0,(float)plane());
    r = latticeToStageTransform() * r * 0.001;
    return r(2);
  }

}} // end namespace fetch::device


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <qle/termstructures/datedstrippedoptionlet.hpp>

#include <ql/utilities/dataformatters.hpp>

#include <boost/algorithm/cxx11/is_sorted.hpp>

using boost::algorithm::is_strictly_increasing;

namespace QuantExt {

DatedStrippedOptionlet::DatedStrippedOptionlet(const Date& referenceDate,
                                               const boost::shared_ptr<StrippedOptionletBase>& s)
    : referenceDate_(referenceDate), calendar_(s->calendar()), businessDayConvention_(s->businessDayConvention()),
      optionletDates_(s->optionletFixingDates()), nOptionletDates_(s->optionletMaturities()),
      optionletTimes_(s->optionletFixingTimes()), optionletStrikes_(nOptionletDates_, vector<Rate>()),
      optionletVolatilities_(nOptionletDates_, vector<Volatility>()), optionletAtmRates_(s->atmOptionletRates()),
      dayCounter_(s->dayCounter()), type_(s->volatilityType()), displacement_(s->displacement()) {

    // Populate the optionlet strikes and volatilities
    for (Size i = 0; i < nOptionletDates_; ++i) {
        optionletStrikes_[i] = s->optionletStrikes(i);
        optionletVolatilities_[i] = s->optionletVolatilities(i);
    }
}

DatedStrippedOptionlet::DatedStrippedOptionlet(const Date& referenceDate, const Calendar& calendar,
                                               BusinessDayConvention bdc, const vector<Date>& optionletDates,
                                               const vector<vector<Rate> >& strikes,
                                               const vector<vector<Volatility> >& volatilities,
                                               const vector<Rate>& optionletAtmRates, const DayCounter& dayCounter,
                                               VolatilityType type, Real displacement)
    : referenceDate_(referenceDate), calendar_(calendar), businessDayConvention_(bdc), optionletDates_(optionletDates),
      nOptionletDates_(optionletDates.size()), optionletTimes_(nOptionletDates_), optionletStrikes_(strikes),
      optionletVolatilities_(volatilities), optionletAtmRates_(optionletAtmRates), dayCounter_(dayCounter), type_(type),
      displacement_(displacement) {

    checkInputs();
    // Populate the optionlet times
    for (Size i = 0; i < nOptionletDates_; ++i)
        optionletTimes_[i] = dayCounter_.yearFraction(referenceDate_, optionletDates_[i]);
}

void DatedStrippedOptionlet::checkInputs() const {

    QL_REQUIRE(!optionletDates_.empty(), "Need at least one optionlet to create optionlet surface");
    QL_REQUIRE(nOptionletDates_ == optionletVolatilities_.size(),
               "Mismatch between number of option tenors (" << nOptionletDates_ << ") and number of volatility rows ("
                                                            << optionletVolatilities_.size() << ")");
    QL_REQUIRE(nOptionletDates_ == optionletStrikes_.size(), "Mismatch between number of option tenors ("
                                                                 << nOptionletDates_ << ") and number of strike rows ("
                                                                 << optionletStrikes_.size() << ")");
    QL_REQUIRE(nOptionletDates_ == optionletAtmRates_.size(), "Mismatch between number of option tenors ("
                                                                  << nOptionletDates_ << ") and number of ATM rates ("
                                                                  << optionletAtmRates_.size() << ")");
    QL_REQUIRE(optionletDates_[0] > referenceDate_,
               "First option date (" << optionletDates_[0] << ") must be greater than the reference date");
    QL_REQUIRE(is_strictly_increasing(optionletDates_.begin(), optionletDates_.end()),
               "Optionlet dates must be sorted in ascending order");

    for (Size i = 0; i < nOptionletDates_; ++i) {
        QL_REQUIRE(!optionletStrikes_[i].empty(), "The " << io::ordinal(i) << " row of strikes is empty");
        QL_REQUIRE(optionletStrikes_[i].size() == optionletVolatilities_[i].size(),
                   "Size of " << io::ordinal(i) << " row of strikes and volatilities are not equal");
        QL_REQUIRE(is_strictly_increasing(optionletStrikes_[i].begin(), optionletStrikes_[i].end()),
                   "The " << io::ordinal(i) << " row of strikes is not sorted in ascending order");
    }
}

const vector<Rate>& DatedStrippedOptionlet::optionletStrikes(Size i) const {
    QL_REQUIRE(i < optionletStrikes_.size(),
               "index (" << i << ") must be less than optionletStrikes size (" << optionletStrikes_.size() << ")");
    return optionletStrikes_[i];
}

const vector<Volatility>& DatedStrippedOptionlet::optionletVolatilities(Size i) const {
    QL_REQUIRE(i < optionletVolatilities_.size(), "index (" << i << ") must be less than optionletVolatilities size ("
                                                            << optionletVolatilities_.size() << ")");
    return optionletVolatilities_[i];
}

const vector<Date>& DatedStrippedOptionlet::optionletFixingDates() const { return optionletDates_; }

const vector<Time>& DatedStrippedOptionlet::optionletFixingTimes() const { return optionletTimes_; }

Size DatedStrippedOptionlet::optionletMaturities() const { return nOptionletDates_; }

const vector<Time>& DatedStrippedOptionlet::atmOptionletRates() const { return optionletAtmRates_; }

const DayCounter& DatedStrippedOptionlet::dayCounter() const { return dayCounter_; }

const Calendar& DatedStrippedOptionlet::calendar() const { return calendar_; }

const Date& DatedStrippedOptionlet::referenceDate() const { return referenceDate_; }

BusinessDayConvention DatedStrippedOptionlet::businessDayConvention() const { return businessDayConvention_; }

VolatilityType DatedStrippedOptionlet::volatilityType() const { return type_; }

Real DatedStrippedOptionlet::displacement() const { return displacement_; }
} // namespace QuantExt


#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/console/print.h>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <flann/flann.h>
#include <flann/io/hdf5.h>
#include <boost/filesystem.hpp>

typedef std::pair<
    std::string,
    std::vector <float>
> VfhModel;

bool
loadHist (const boost::filesystem::path &path, VfhModel &vfh)
{
  int vfh_idx;
  // Load the file as a PCD
  try
  {
    pcl::PCLPointCloud2 cloud;
    int version;
    Eigen::Vector4f origin;
    Eigen::Quaternionf orientation;
    pcl::PCDReader r;
    int type; unsigned int idx;
    r.readHeader (path.string (), cloud, origin, orientation, version, type, idx);

    vfh_idx = pcl::getFieldIndex (cloud, "vfh");
    if (vfh_idx == -1)
      return (false);
    if ((int)cloud.width * cloud.height != 1)
      return (false);
  }
  catch (const pcl::InvalidConversionException&)
  {
    return (false);
  }

  // Treat the VFH signature as a single Point Cloud
  pcl::PointCloud <pcl::VFHSignature308> point;
  pcl::io::loadPCDFile (path.string (), point);
  vfh.second.resize (308);

  std::vector <pcl::PCLPointField> fields;
  pcl::getFieldIndex<pcl::VFHSignature308> ("vfh", fields);

  for (std::size_t i = 0; i < fields[vfh_idx].count; ++i)
  {
    vfh.second[i] = point[0].histogram[i];
  }
  vfh.first = path.string ();
  return (true);
}

inline void
nearestKSearch (flann::Index<flann::ChiSquareDistance<float> > &index, const VfhModel &model,
                int k, flann::Matrix<int> &indices, flann::Matrix<float> &distances)
{
  // Query point
  flann::Matrix<float> p = flann::Matrix<float>(new float[model.second.size ()], 1, model.second.size ());
  memcpy (&p.ptr ()[0], &model.second[0], p.cols * p.rows * sizeof (float));

  indices = flann::Matrix<int>(new int[k], 1, k);
  distances = flann::Matrix<float>(new float[k], 1, k);
  index.knnSearch (p, indices, distances, k, flann::SearchParams (512));
  delete[] p.ptr ();
}

int main(int argc, char** argv)
{   int k = 16;
    double thresh = DBL_MAX;

    std::string extension(".pcd");
    transform(extension.begin(), extension.end(), extension.begin(), (int(*)(int))tolower);

    //load the test histogram
    std::vector<int> pcdIndices = pcl::console::parse_file_extension_argument(argc, argv, ".pcd");
    VfhModel histogram;
    if(!loadHist(argv[pcdIndices.at(0)], histogram))
    {
        pcl::console::print_error("Cannot load test file %s\n", argv[pcdIndices.at(0)]);
        return -1;
    }

    std::string idxFile = "kdtree.idx";
    std::string h5File = "training_data.h5";
    std::string listFile = "training_data.list";

    std::vector<VfhModel> models;
    flann::Matrix<int> kIndices;
    flann::Matrix<float> kDistances;
    flann::Matrix<float> data;

    if(!boost::filesystem::exists(h5File) || !boost::filesystem::exists(listFile))
    {
        pcl::console::print_error("could not find kd-tree index in file %s!", idxFile.c_str());
        return -1;
    }else{
        flann::Index<flann::ChiSquareDistance<float>> index (data, flann::SavedIndexParams("kdtree.idx"));
        index.buildIndex();
        nearestKSearch(index, histogram, k, kIndices, kDistances);
    }

    //output the result on screen
    pcl::console::print_highlight("The closest %d neighbors for %s are : \n", k, argv[pcdIndices[0]]);
    for(int i = 0; i < k; i++)
    {
        pcl::console::print_info("%d - $s (%d) with a distance of : %f\n",
            i, models.at(kIndices[0][i]).first.c_str(), kIndices[0][i], kDistances[0][i]);
    }

    //load the result
    pcl::visualization::PCLVisualizer p (argc, argv, "VFH Cluster Classifier");
    int ys = (int )std::floor(sqrt((double)k));
    int xs = ys + (int) std::ceil((k/(double)ys) - ys);
    double xstep = (double)(1/(double)xs);
    double ystep = (double)(1/(double)ys);
    pcl::console::print_highlight ("Preparing to load ");
    pcl::console::print_value ("%d", k);
    pcl::console::print_info (" files (");
    pcl::console::print_value ("%d", xs);
    pcl::console::print_info ("x");
    pcl::console::print_value ("%d", ys);
    pcl::console::print_info (" / ");
    pcl::console::print_value ("%f", xstep);
    pcl::console::print_info ("x");
    pcl::console::print_value ("%f", ystep);
    pcl::console::print_info (")\n");

    int viewport = 0, l=0, m=0;
    for(int i =0; i<k; ++i)
    {
        std::string cloud_name = models.at(kIndices[0][i]).first;
        boost::replace_last(cloud_name, "_vfh", "");

        p.createViewPort(l*xstep, m*ystep, (l+1) * xstep, (m+1)* ystep, viewport);
        l++;
        if(l >= xs)
        {
            l=0;
            m++;
        }

        pcl::PCLPointCloud2 cloud;
        pcl::console::print_highlight(stderr, "Loading ");
        pcl::console::print_value(stderr, "%s ", cloud_name.c_str());
        if(pcl::io::loadPCDFile(cloud_name, cloud) == -1)
        {
            break;
        }

        //convert from blob to pointlcloud
        pcl::PointCloud<pcl::PointXYZ> cloud_xyz;
        pcl::fromPCLPointCloud2(cloud, cloud_xyz);

        if(cloud_xyz.size() == 0)
        {
            break;
        }

        pcl::console::print_info ("[done, ");
        pcl::console::print_value ("%zu", static_cast<std::size_t>(cloud_xyz.size ()));
        pcl::console::print_info (" points]\n");
        pcl::console::print_info ("Available dimensions: ");
        pcl::console::print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());

    // Demean the cloud
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid (cloud_xyz, centroid);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_demean (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::demeanPointCloud<pcl::PointXYZ> (cloud_xyz, centroid, *cloud_xyz_demean);
    // Add to renderer*
    p.addPointCloud (cloud_xyz_demean, cloud_name, viewport);

    // Check if the model found is within our inlier tolerance
    std::stringstream ss;
    ss << kDistances[0][i];
    if (kDistances[0][i] > thresh)
    {
      p.addText (ss.str (), 20, 30, 1, 0, 0, ss.str (), viewport);  // display the text with red

      // Create a red line
      pcl::PointXYZ min_p, max_p;
      pcl::getMinMax3D (*cloud_xyz_demean, min_p, max_p);
      std::stringstream line_name;
      line_name << "line_" << i;
      p.addLine (min_p, max_p, 1, 0, 0, line_name.str (), viewport);
      p.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 5, line_name.str (), viewport);
    }
    else
      p.addText (ss.str (), 20, 30, 0, 1, 0, ss.str (), viewport);

    // Increase the font size for the score*
    p.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_FONT_SIZE, 18, ss.str (), viewport);

    // Add the cluster name
    p.addText (cloud_name, 20, 10, cloud_name, viewport);
  }
  // Add coordianate systems to all viewports
  p.addCoordinateSystem (0.1, "global", 0);

  p.spin ();
  return (0);
}


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

/* Author: Omid Heidari */

#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>

#include <moveit_msgs/MotionPlanRequest.h>

#include <ros/ros.h>

#include <limits>
#include <vector>
#include <Eigen/Geometry>
#include <unordered_map>

#include "lerp_interface/lerp_interface.h"

namespace lerp_interface
{
LERPInterface::LERPInterface(const ros::NodeHandle& nh) : nh_(nh), name_("LERPInterface")
{
}

bool LERPInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                          const planning_interface::MotionPlanRequest& req,
                          moveit_msgs::MotionPlanDetailedResponse& res)
{
  // Load the planner-specific parameters
  nh_.param("num_steps", num_steps_, 10);

  ros::WallTime start_time = ros::WallTime::now();
  moveit::core::RobotModelConstPtr robot_model = planning_scene->getRobotModel();
  moveit::core::RobotStatePtr start_state(new moveit::core::RobotState(robot_model));
  *start_state = planning_scene->getCurrentState();
  const moveit::core::JointModelGroup* joint_model_group = start_state->getJointModelGroup(req.group_name);
  std::vector<std::string> joint_names = joint_model_group->getVariableNames();
  dof_ = joint_names.size();
  std::vector<double> start_joint_values;
  start_state->copyJointGroupPositions(joint_model_group, start_joint_values);

  // This planner only supports one goal constraint in the request
  const std::vector<moveit_msgs::Constraints>& goal_constraints = req.goal_constraints;
  const std::vector<moveit_msgs::JointConstraint>& goal_joint_constraint = goal_constraints[0].joint_constraints;

  std::vector<double> goal_joint_values;
  goal_joint_values.reserve(goal_joint_constraint.size());

  for (const auto& constraint : goal_joint_constraint)
  {
    goal_joint_values.push_back(constraint.position);
  }

  // ==================== Interpolation
  trajectory_msgs::JointTrajectory joint_trajectory;
  interpolate(joint_names, start_state, joint_model_group, start_joint_values, goal_joint_values, joint_trajectory);

  // ==================== feed the response
  res.trajectory.resize(1);
  res.trajectory[0].joint_trajectory.joint_names = joint_names;
  res.trajectory[0].joint_trajectory.header = req.start_state.joint_state.header;
  res.trajectory[0].joint_trajectory = joint_trajectory;

  res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
  res.processing_time.push_back((ros::WallTime::now() - start_time).toSec());

  res.group_name = req.group_name;
  res.trajectory_start.joint_state.name = joint_names;
  res.trajectory_start.joint_state.position = start_joint_values;

  return true;
}

void LERPInterface::interpolate(const std::vector<std::string>& joint_names, moveit::core::RobotStatePtr& rob_state,
                                const moveit::core::JointModelGroup* joint_model_group,
                                const std::vector<double>& start_joint_vals, const std::vector<double>& goal_joint_vals,
                                trajectory_msgs::JointTrajectory& joint_trajectory)
{
  joint_trajectory.points.resize(num_steps_ + 1);

  std::vector<double> dt_vector;
  for (int joint_index = 0; joint_index < dof_; ++joint_index)
  {
    double dt = (goal_joint_vals[joint_index] - start_joint_vals[joint_index]) / num_steps_;
    dt_vector.push_back(dt);
  }

  for (int step = 0; step <= num_steps_; ++step)
  {
    std::vector<double> joint_values;
    for (int k = 0; k < dof_; ++k)
    {
      double joint_value = start_joint_vals[k] + step * dt_vector[k];
      joint_values.push_back(joint_value);
    }
    rob_state->setJointGroupPositions(joint_model_group, joint_values);
    rob_state->update();

    joint_trajectory.joint_names = joint_names;
    joint_trajectory.points[step].positions = joint_values;
  }
}

}  // namespace lerp_interface


// Boost.Geometry Index
//
// Spatial query predicates
//
// Copyright (c) 2011-2015 Adam Wulkiewicz, Lodz, Poland.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_INDEX_PREDICATES_HPP
#define BOOST_GEOMETRY_INDEX_PREDICATES_HPP

#include <boost/geometry/index/detail/predicates.hpp>
#include <boost/geometry/index/detail/tuples.hpp>

/*!
\defgroup predicates Predicates (boost::geometry::index::)
*/

namespace boost { namespace geometry { namespace index {

/*!
\brief Generate \c contains() predicate.

Generate a predicate defining Value and Geometry relationship.
Value will be returned by the query if <tt>bg::within(Geometry, Indexable)</tt>
returns true.

\par Example
\verbatim
bgi::query(spatial_index, bgi::contains(box), std::back_inserter(result));
\endverbatim

\ingroup predicates

\tparam Geometry    The Geometry type.

\param g            The Geometry object.
*/
template <typename Geometry> inline
detail::predicates::spatial_predicate<Geometry, detail::predicates::contains_tag, false>
contains(Geometry const& g)
{
    return detail::predicates::spatial_predicate
                <
                    Geometry,
                    detail::predicates::contains_tag,
                    false
                >(g);
}

/*!
\brief Generate \c covered_by() predicate.

Generate a predicate defining Value and Geometry relationship.
Value will be returned by the query if <tt>bg::covered_by(Indexable, Geometry)</tt>
returns true.

\par Example
\verbatim
bgi::query(spatial_index, bgi::covered_by(box), std::back_inserter(result));
\endverbatim

\ingroup predicates

\tparam Geometry    The Geometry type.

\param g            The Geometry object.
*/
template <typename Geometry> inline
detail::predicates::spatial_predicate<Geometry, detail::predicates::covered_by_tag, false>
covered_by(Geometry const& g)
{
    return detail::predicates::spatial_predicate
                <
                    Geometry,
                    detail::predicates::covered_by_tag,
                    false
                >(g);
}

/*!
\brief Generate \c covers() predicate.

Generate a predicate defining Value and Geometry relationship.
Value will be returned by the query if <tt>bg::covered_by(Geometry, Indexable)</tt>
returns true.

\par Example
\verbatim
bgi::query(spatial_index, bgi::covers(box), std::back_inserter(result));
\endverbatim

\ingroup predicates

\tparam Geometry    The Geometry type.

\param g            The Geometry object.
*/
template <typename Geometry> inline
detail::predicates::spatial_predicate<Geometry, detail::predicates::covers_tag, false>
covers(Geometry const& g)
{
    return detail::predicates::spatial_predicate
                <
                    Geometry,
                    detail::predicates::covers_tag,
                    false
                >(g);
}

/*!
\brief Generate \c disjoint() predicate.

Generate a predicate defining Value and Geometry relationship.
Value will be returned by the query if <tt>bg::disjoint(Indexable, Geometry)</tt>
returns true.

\par Example
\verbatim
bgi::query(spatial_index, bgi::disjoint(box), std::back_inserter(result));
\endverbatim

\ingroup predicates

\tparam Geometry    The Geometry type.

\param g            The Geometry object.
*/
template <typename Geometry> inline
detail::predicates::spatial_predicate<Geometry, detail::predicates::disjoint_tag, false>
disjoint(Geometry const& g)
{
    return detail::predicates::spatial_predicate
                <
                    Geometry,
                    detail::predicates::disjoint_tag,
                    false
                >(g);
}

/*!
\brief Generate \c intersects() predicate.

Generate a predicate defining Value and Geometry relationship.
Value will be returned by the query if <tt>bg::intersects(Indexable, Geometry)</tt>
returns true.

\par Example
\verbatim
bgi::query(spatial_index, bgi::intersects(box), std::back_inserter(result));
bgi::query(spatial_index, bgi::intersects(ring), std::back_inserter(result));
bgi::query(spatial_index, bgi::intersects(polygon), std::back_inserter(result));
\endverbatim

\ingroup predicates

\tparam Geometry    The Geometry type.

\param g            The Geometry object.
*/
template <typename Geometry> inline
detail::predicates::spatial_predicate<Geometry, detail::predicates::intersects_tag, false>
intersects(Geometry const& g)
{
    return detail::predicates::spatial_predicate
                <
                    Geometry,
                    detail::predicates::intersects_tag,
                    false
                >(g);
}

/*!
\brief Generate \c overlaps() predicate.

Generate a predicate defining Value and Geometry relationship.
Value will be returned by the query if <tt>bg::overlaps(Indexable, Geometry)</tt>
returns true.

\par Example
\verbatim
bgi::query(spatial_index, bgi::overlaps(box), std::back_inserter(result));
\endverbatim

\ingroup predicates

\tparam Geometry    The Geometry type.

\param g            The Geometry object.
*/
template <typename Geometry> inline
detail::predicates::spatial_predicate<Geometry, detail::predicates::overlaps_tag, false>
overlaps(Geometry const& g)
{
    return detail::predicates::spatial_predicate
                <
                    Geometry,
                    detail::predicates::overlaps_tag,
                    false
                >(g);
}

#ifdef BOOST_GEOMETRY_INDEX_DETAIL_EXPERIMENTAL

/*!
\brief Generate \c touches() predicate.

Generate a predicate defining Value and Geometry relationship.
Value will be returned by the query if <tt>bg::touches(Indexable, Geometry)</tt>
returns true.

\ingroup predicates

\tparam Geometry    The Geometry type.

\param g            The Geometry object.
*/
template <typename Geometry> inline
detail::predicates::spatial_predicate<Geometry, detail::predicates::touches_tag, false>
touches(Geometry const& g)
{
    return detail::predicates::spatial_predicate
                <
                    Geometry,
                    detail::predicates::touches_tag,
                    false
                >(g);
}

#endif // BOOST_GEOMETRY_INDEX_DETAIL_EXPERIMENTAL

/*!
\brief Generate \c within() predicate.

Generate a predicate defining Value and Geometry relationship.
Value will be returned by the query if <tt>bg::within(Indexable, Geometry)</tt>
returns true.

\par Example
\verbatim
bgi::query(spatial_index, bgi::within(box), std::back_inserter(result));
\endverbatim

\ingroup predicates

\tparam Geometry    The Geometry type.

\param g            The Geometry object.
*/
template <typename Geometry> inline
detail::predicates::spatial_predicate<Geometry, detail::predicates::within_tag, false>
within(Geometry const& g)
{
    return detail::predicates::spatial_predicate
                <
                    Geometry,
                    detail::predicates::within_tag,
                    false
                >(g);
}

/*!
\brief Generate satisfies() predicate.

A wrapper around user-defined UnaryPredicate checking if Value should be returned by spatial query.

\par Example
\verbatim
bool is_red(Value const& v) { return v.is_red(); }

struct is_red_o {
template <typename Value> bool operator()(Value const& v) { return v.is_red(); }
}

// ...

rt.query(index::intersects(box) && index::satisfies(is_red),
std::back_inserter(result));

rt.query(index::intersects(box) && index::satisfies(is_red_o()),
std::back_inserter(result));

#ifndef BOOST_NO_CXX11_LAMBDAS
rt.query(index::intersects(box) && index::satisfies([](Value const& v) { return v.is_red(); }),
std::back_inserter(result));
#endif
\endverbatim

\ingroup predicates

\tparam UnaryPredicate  A type of unary predicate function or function object.

\param pred             The unary predicate function or function object.
*/
template <typename UnaryPredicate> inline
detail::predicates::satisfies<UnaryPredicate, false>
satisfies(UnaryPredicate const& pred)
{
    return detail::predicates::satisfies<UnaryPredicate, false>(pred);
}

/*!
\brief Generate nearest() predicate.

When nearest predicate is passed to the query, k-nearest neighbour search will be performed.
\c nearest() predicate takes a \c Geometry from which distances to \c Values are calculated
and the maximum number of \c Values that should be returned. Internally
boost::geometry::comparable_distance() is used to perform the calculation.

\par Example
\verbatim
bgi::query(spatial_index, bgi::nearest(pt, 5), std::back_inserter(result));
bgi::query(spatial_index, bgi::nearest(pt, 5) && bgi::intersects(box), std::back_inserter(result));
bgi::query(spatial_index, bgi::nearest(box, 5), std::back_inserter(result));
\endverbatim

\warning
Only one \c nearest() predicate may be used in a query.

\ingroup predicates

\param geometry     The geometry from which distance is calculated.
\param k            The maximum number of values to return.
*/
template <typename Geometry> inline
detail::predicates::nearest<Geometry>
nearest(Geometry const& geometry, unsigned k)
{
    return detail::predicates::nearest<Geometry>(geometry, k);
}

#ifdef BOOST_GEOMETRY_INDEX_DETAIL_EXPERIMENTAL

/*!
\brief Generate path() predicate.

When path predicate is passed to the query, the returned values are k values along the path closest to
its begin. \c path() predicate takes a \c Segment or a \c Linestring defining the path and the maximum
number of \c Values that should be returned.

\par Example
\verbatim
bgi::query(spatial_index, bgi::path(segment, 5), std::back_inserter(result));
bgi::query(spatial_index, bgi::path(linestring, 5) && bgi::intersects(box), std::back_inserter(result));
\endverbatim

\warning
Only one distance predicate (\c nearest() or \c path()) may be used in a query.

\ingroup predicates

\param linestring   The path along which distance is calculated.
\param k            The maximum number of values to return.
*/
template <typename SegmentOrLinestring> inline
detail::predicates::path<SegmentOrLinestring>
path(SegmentOrLinestring const& linestring, unsigned k)
{
    return detail::predicates::path<SegmentOrLinestring>(linestring, k);
}

#endif // BOOST_GEOMETRY_INDEX_DETAIL_EXPERIMENTAL

namespace detail { namespace predicates {

// operator! generators

template <typename Fun, bool Negated> inline
satisfies<Fun, !Negated>
operator!(satisfies<Fun, Negated> const& p)
{
    return satisfies<Fun, !Negated>(p);
}

template <typename Geometry, typename Tag, bool Negated> inline
spatial_predicate<Geometry, Tag, !Negated>
operator!(spatial_predicate<Geometry, Tag, Negated> const& p)
{
    return spatial_predicate<Geometry, Tag, !Negated>(p.geometry);
}

// operator&& generators

template <typename Pred1, typename Pred2> inline
boost::tuples::cons<
    Pred1,
    boost::tuples::cons<Pred2, boost::tuples::null_type>
>
operator&&(Pred1 const& p1, Pred2 const& p2)
{
    /*typedef typename boost::mpl::if_c<is_predicate<Pred1>::value, Pred1, Pred1 const&>::type stored1;
    typedef typename boost::mpl::if_c<is_predicate<Pred2>::value, Pred2, Pred2 const&>::type stored2;*/
    namespace bt = boost::tuples;

    return
    bt::cons< Pred1, bt::cons<Pred2, bt::null_type> >
        ( p1, bt::cons<Pred2, bt::null_type>(p2, bt::null_type()) );
}

template <typename Head, typename Tail, typename Pred> inline
typename tuples::push_back<
    boost::tuples::cons<Head, Tail>, Pred
>::type
operator&&(boost::tuples::cons<Head, Tail> const& t, Pred const& p)
{
    //typedef typename boost::mpl::if_c<is_predicate<Pred>::value, Pred, Pred const&>::type stored;
    namespace bt = boost::tuples;

    return
    tuples::push_back<
        bt::cons<Head, Tail>, Pred
    >::apply(t, p);
}

}} // namespace detail::predicates

}}} // namespace boost::geometry::index

#endif // BOOST_GEOMETRY_INDEX_PREDICATES_HPP


#pragma once

#include <boost/preprocessor/cat.hpp>

namespace TableCesdl
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// PACK
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
    template <typename...>
    struct pack {};

    template <typename Pack>
    struct pack_size;

    template <typename... List>
    struct pack_size <pack <List...>> {
        enum {
            value = sizeof...(List)
        };
    };

    template <std::size_t Index, typename Pack>
    struct pack_element;

    template <typename Head, typename... Tail>
    struct pack_element <0, pack <Head, Tail...>> {
        using type = Head;
    };

    template <std::size_t Index, typename Head, typename... Tail>
    struct pack_element <Index, pack <Head, Tail...>>
        : pack_element <Index-1, pack <Tail...>>
    {
    };

    template <typename, typename>
    struct concat
    { };

    template <typename ... Ts, typename ... Us>
    struct concat <pack <Ts...>, pack <Us...>> {
        using type = pack <Ts..., Us...>;
    };

    template <typename T, typename U>
    using concat_t = typename concat <T, U>::type;

    template <typename Pack>
    struct pop_back
    { };

    template <>
    struct pop_back <pack <>> {
        using type = pack <>;
    };

    template <typename T>
    struct pop_back <pack <T>> {
        using type = pack <>;
    };

    template <typename T, typename ... List>
    struct pop_back <pack <T, List...>> {
        using type = concat_t <pack <T>, typename pop_back <pack <List...>>::type>;
    };

    template <typename Pack>
    using pop_back_t = typename pop_back <Pack>::type;

    template <typename Pack, template <typename...> class Function>
    struct apply {    };

    template <template <typename...> class Function, typename... List>
    struct apply <pack <List...>, Function> {
        using type = Function <List...>;
    };

    template <typename Pack, template <typename...> class Function>
    using apply_t = typename apply <Pack, Function>::type;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// TRIM
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
    template <typename Pack, template <typename> class Predicate, bool Abort = false>
    struct trim_right {
        // Abort == false
        using type = typename std::conditional <Predicate<typename pack_element <pack_size <Pack>::value - 1, Pack>::type>::value,
                                                typename trim_right <pop_back_t<Pack>, Predicate>::type,
                                                Pack>::type;
    };

    template <typename Pack, template <typename> class Predicate>
    struct trim_right <Pack, Predicate, true> {
        using type = Pack;
    };

    template <template <typename> class Predicate>
    struct trim_right <pack <>, Predicate, false> {
        using type = pack <>;
    };

    template <typename Pack, template <typename> class Predicate>
    using trim_right_t = typename trim_right <Pack, Predicate>::type;

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// CEXPR_STRING
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
    template <char C>
    using char_ = std::integral_constant <char, C>;

    template <typename... List>
    struct cexpr_string
    {
        using size_type = std::make_signed <std::size_t>::type;

        using type = pack <List...>;
        constexpr static const size_type size = sizeof...(List);
        constexpr static const size_type length = size;

        template <size_type N>
        struct at {
            static_assert(N < size, "Index out of bounds");

            constexpr static const char value = pack_element <N, cexpr_string::type>::type::value;
            using type = typename pack_element <N, cexpr_string::type>::type;
        };

        template <size_type N>
        using at_t = typename at <N>::type;

        constexpr static const char c_str[] = {
            List::value..., '\0'
        };

        constexpr static const size_type npos = -1;
    };

    template <typename... List>
    constexpr const char cexpr_string <List...>::c_str[];

    template <typename Char>
    struct is_zero {
        constexpr static const bool value = (Char::value == 0);
    };

    #define TCEDSL_SPLIT_1(s, x)    TableCesdl::char_<( x < sizeof(s) ? s[x] : '\0' )>
    #define TCEDSL_SPLIT_4(s, x)    TCEDSL_SPLIT_1  (s, x), TCEDSL_SPLIT_1  (s, x+1)  , TCEDSL_SPLIT_1  (s, x+2)  , TCEDSL_SPLIT_1  (s, x+3)
    #define TCEDSL_SPLIT_16(s, x)   TCEDSL_SPLIT_4  (s, x), TCEDSL_SPLIT_4  (s, x+4)  , TCEDSL_SPLIT_4  (s, x+8)  , TCEDSL_SPLIT_4  (s, x+12)
    #define TCEDSL_SPLIT_32(s, x)   TCEDSL_SPLIT_16 (s, x), TCEDSL_SPLIT_16 (s, x+16)
    #define TCEDSL_SPLIT_64(s, x)   TCEDSL_SPLIT_16 (s, x), TCEDSL_SPLIT_16 (s, x+16) , TCEDSL_SPLIT_16 (s, x+32) , TCEDSL_SPLIT_16 (s, x+48)
    #define TCEDSL_SPLIT_256(s, x)  TCEDSL_SPLIT_64 (s, x), TCEDSL_SPLIT_64 (s, x+64) , TCEDSL_SPLIT_64 (s, x+128) , TCEDSL_SPLIT_64 (s, x+194)
    #define TCEDSL_SPLIT_1024(s, x) TCEDSL_SPLIT_256(s, x), TCEDSL_SPLIT_256(s, x+256), TCEDSL_SPLIT_256(s, x+512), TCEDSL_SPLIT_256(s, x+768)

    #define TCEDSL_STRING_IMPL(str, n) TableCesdl::apply_t < \
        TableCesdl::trim_right_t < \
            TableCesdl::pack <BOOST_PP_CAT(TCEDSL_SPLIT_,n)(str, 0)>, TableCesdl::is_zero \
        >, TableCesdl::cexpr_string \
    >

    #define TCEDSL_SHORT_STRING(str) TCEDSL_STRING_IMPL(str, 16)
    #define TCEDSL_MEDIUM_STRING(str) TCEDSL_STRING_IMPL(str, 32)
    #define TCEDSL_STRING(str) TCEDSL_STRING_IMPL(str, 64)
    #define TCEDSL_LONG_STRING(str) TCEDSL_STRING_IMPL(str, 256) // HEAVY
    #define TCEDSL_LONG_LONG_STRING(str) TCEDSL_STRING_IMPL(str, 1024) // ULTRA HEAVY

    #ifndef TCEDSL_STRING_SIZE
    #   define TCEDSL_STRING_SIZE 32
    #endif
    #define TCEDSL_STRING_CONFIGURED(str) TCEDSL_STRING_IMPL(str, TCEDSL_STRING_SIZE)
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
}


/**
 * @file kernel_traits_test.cpp
 * @author Ryan Curtin
 *
 * Test the KernelTraits class.  Because all of the values are known at compile
 * time, this test is meant to ensure that uses of KernelTraits still compile
 * okay and react as expected.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::kernel;

BOOST_AUTO_TEST_SUITE(KernelTraitsTest);

BOOST_AUTO_TEST_CASE(IsNormalizedTest)
{
  // Reason number ten billion why macros are bad:
  //
  // The Boost unit test framework is built on macros.  When I write
  // BOOST_REQUIRE_EQUAL(KernelTraits<int>::IsNormalized, false), what actually
  // happens (in gcc at least) is that the 'false' gets implicitly converted to
  // an int; then, the compiler goes looking for an int IsNormalized variable in
  // KernelTraits.  But this doesn't exist, so we get this error at linker time:
  //
  // kernel_traits_test.cpp:(.text+0xb86): undefined reference to
  // `mlpack::kernel::KernelTraits<mlpack::kernel::LinearKernel>::IsNormalized'
  //
  // and this actually tells us nothing about the error.  When you dig deep
  // enough or get frustrated enough, you end up realizing it's a macro problem
  // and now have to *explicitly* cast to bool.  Yes, really; try it if you
  // don't believe me.

  // Test each kernel individually.
  // If the type is not a valid kernel, it should be false (default value).
  BOOST_REQUIRE_EQUAL((bool) KernelTraits<int>::IsNormalized, false);

  // Normalized kernels.
  BOOST_REQUIRE_EQUAL((bool) KernelTraits<CosineDistance>::IsNormalized, true);
  BOOST_REQUIRE_EQUAL((bool) KernelTraits<EpanechnikovKernel>::IsNormalized,
      true);
  BOOST_REQUIRE_EQUAL((bool) KernelTraits<GaussianKernel>::IsNormalized, true);
  BOOST_REQUIRE_EQUAL((bool) KernelTraits<LaplacianKernel>::IsNormalized, true);
  BOOST_REQUIRE_EQUAL((bool) KernelTraits<SphericalKernel>::IsNormalized, true);
  BOOST_REQUIRE_EQUAL((bool) KernelTraits<TriangularKernel>::IsNormalized,
      true);

  // Unnormalized kernels.
  BOOST_REQUIRE_EQUAL((bool) KernelTraits<LinearKernel>::IsNormalized, false);
  BOOST_REQUIRE_EQUAL((bool) KernelTraits<PolynomialKernel>::IsNormalized,
      false);
  BOOST_REQUIRE_EQUAL((bool) KernelTraits<PSpectrumStringKernel>::IsNormalized,
      false);
}

BOOST_AUTO_TEST_SUITE_END();


#include <boost/property_tree/json_parser.hpp>

#include <fstream>

#include <gtest/gtest.h>

#include <paper/lib/interface.h>
#include <paper/node/common.hpp>
#include <paper/node/node.hpp>

#include <ed25519-donna/ed25519.h>

TEST (ed25519, signing)
{
	paper::uint256_union prv (0);
	paper::uint256_union pub;
	ed25519_publickey (prv.bytes.data (), pub.bytes.data ());
	paper::uint256_union message (0);
	paper::uint512_union signature;
	ed25519_sign (message.bytes.data (), sizeof (message.bytes), prv.bytes.data (), pub.bytes.data (), signature.bytes.data ());
	auto valid1 (ed25519_sign_open (message.bytes.data (), sizeof (message.bytes), pub.bytes.data (), signature.bytes.data ()));
	ASSERT_EQ (0, valid1);
	signature.bytes[32] ^= 0x1;
	auto valid2 (ed25519_sign_open (message.bytes.data (), sizeof (message.bytes), pub.bytes.data (), signature.bytes.data ()));
	ASSERT_NE (0, valid2);
}

TEST (transaction_block, empty)
{
	paper::keypair key1;
	paper::send_block block (0, 1, 13, key1.prv, key1.pub, 2);
	paper::uint256_union hash (block.hash ());
	ASSERT_FALSE (paper::validate_message (key1.pub, hash, block.signature));
	block.signature.bytes[32] ^= 0x1;
	ASSERT_TRUE (paper::validate_message (key1.pub, hash, block.signature));
}

TEST (block, send_serialize)
{
	paper::send_block block1 (0, 1, 2, paper::keypair ().prv, 4, 5);
	std::vector<uint8_t> bytes;
	{
		paper::vectorstream stream1 (bytes);
		block1.serialize (stream1);
	}
	auto data (bytes.data ());
	auto size (bytes.size ());
	ASSERT_NE (nullptr, data);
	ASSERT_NE (0, size);
	paper::bufferstream stream2 (data, size);
	bool error;
	paper::send_block block2 (error, stream2);
	ASSERT_FALSE (error);
	ASSERT_EQ (block1, block2);
}

TEST (block, send_serialize_json)
{
	paper::send_block block1 (0, 1, 2, paper::keypair ().prv, 4, 5);
	std::string string1;
	block1.serialize_json (string1);
	ASSERT_NE (0, string1.size ());
	boost::property_tree::ptree tree1;
	std::stringstream istream (string1);
	boost::property_tree::read_json (istream, tree1);
	bool error;
	paper::send_block block2 (error, tree1);
	ASSERT_FALSE (error);
	ASSERT_EQ (block1, block2);
}

TEST (block, receive_serialize)
{
	paper::receive_block block1 (0, 1, paper::keypair ().prv, 3, 4);
	paper::keypair key1;
	std::vector<uint8_t> bytes;
	{
		paper::vectorstream stream1 (bytes);
		block1.serialize (stream1);
	}
	paper::bufferstream stream2 (bytes.data (), bytes.size ());
	bool error;
	paper::receive_block block2 (error, stream2);
	ASSERT_FALSE (error);
	ASSERT_EQ (block1, block2);
}

TEST (block, receive_serialize_json)
{
	paper::receive_block block1 (0, 1, paper::keypair ().prv, 3, 4);
	std::string string1;
	block1.serialize_json (string1);
	ASSERT_NE (0, string1.size ());
	boost::property_tree::ptree tree1;
	std::stringstream istream (string1);
	boost::property_tree::read_json (istream, tree1);
	bool error;
	paper::receive_block block2 (error, tree1);
	ASSERT_FALSE (error);
	ASSERT_EQ (block1, block2);
}

TEST (block, open_serialize_json)
{
	paper::open_block block1 (0, 1, 0, paper::keypair ().prv, 0, 0);
	std::string string1;
	block1.serialize_json (string1);
	ASSERT_NE (0, string1.size ());
	boost::property_tree::ptree tree1;
	std::stringstream istream (string1);
	boost::property_tree::read_json (istream, tree1);
	bool error;
	paper::open_block block2 (error, tree1);
	ASSERT_FALSE (error);
	ASSERT_EQ (block1, block2);
}

TEST (block, change_serialize_json)
{
	paper::change_block block1 (0, 1, paper::keypair ().prv, 3, 4);
	std::string string1;
	block1.serialize_json (string1);
	ASSERT_NE (0, string1.size ());
	boost::property_tree::ptree tree1;
	std::stringstream istream (string1);
	boost::property_tree::read_json (istream, tree1);
	bool error;
	paper::change_block block2 (error, tree1);
	ASSERT_FALSE (error);
	ASSERT_EQ (block1, block2);
}

TEST (uint512_union, parse_zero)
{
	paper::uint512_union input (paper::uint512_t (0));
	std::string text;
	input.encode_hex (text);
	paper::uint512_union output;
	auto error (output.decode_hex (text));
	ASSERT_FALSE (error);
	ASSERT_EQ (input, output);
	ASSERT_TRUE (output.number ().is_zero ());
}

TEST (uint512_union, parse_zero_short)
{
	std::string text ("0");
	paper::uint512_union output;
	auto error (output.decode_hex (text));
	ASSERT_FALSE (error);
	ASSERT_TRUE (output.number ().is_zero ());
}

TEST (uint512_union, parse_one)
{
	paper::uint512_union input (paper::uint512_t (1));
	std::string text;
	input.encode_hex (text);
	paper::uint512_union output;
	auto error (output.decode_hex (text));
	ASSERT_FALSE (error);
	ASSERT_EQ (input, output);
	ASSERT_EQ (1, output.number ());
}

TEST (uint512_union, parse_error_symbol)
{
	paper::uint512_union input (paper::uint512_t (1000));
	std::string text;
	input.encode_hex (text);
	text[5] = '!';
	paper::uint512_union output;
	auto error (output.decode_hex (text));
	ASSERT_TRUE (error);
}

TEST (uint512_union, max)
{
	paper::uint512_union input (std::numeric_limits<paper::uint512_t>::max ());
	std::string text;
	input.encode_hex (text);
	paper::uint512_union output;
	auto error (output.decode_hex (text));
	ASSERT_FALSE (error);
	ASSERT_EQ (input, output);
	ASSERT_EQ (paper::uint512_t ("0xffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff"), output.number ());
}

TEST (uint512_union, parse_error_overflow)
{
	paper::uint512_union input (std::numeric_limits<paper::uint512_t>::max ());
	std::string text;
	input.encode_hex (text);
	text.push_back (0);
	paper::uint512_union output;
	auto error (output.decode_hex (text));
	ASSERT_TRUE (error);
}

TEST (send_block, deserialize)
{
	paper::send_block block1 (0, 1, 2, paper::keypair ().prv, 4, 5);
	ASSERT_EQ (block1.hash (), block1.hash ());
	std::vector<uint8_t> bytes;
	{
		paper::vectorstream stream1 (bytes);
		block1.serialize (stream1);
	}
	ASSERT_EQ (paper::send_block::size, bytes.size ());
	paper::bufferstream stream2 (bytes.data (), bytes.size ());
	bool error;
	paper::send_block block2 (error, stream2);
	ASSERT_FALSE (error);
	ASSERT_EQ (block1, block2);
}

TEST (receive_block, deserialize)
{
	paper::receive_block block1 (0, 1, paper::keypair ().prv, 3, 4);
	ASSERT_EQ (block1.hash (), block1.hash ());
	block1.hashables.previous = 2;
	block1.hashables.source = 4;
	std::vector<uint8_t> bytes;
	{
		paper::vectorstream stream1 (bytes);
		block1.serialize (stream1);
	}
	ASSERT_EQ (paper::receive_block::size, bytes.size ());
	paper::bufferstream stream2 (bytes.data (), bytes.size ());
	bool error;
	paper::receive_block block2 (error, stream2);
	ASSERT_FALSE (error);
	ASSERT_EQ (block1, block2);
}

TEST (open_block, deserialize)
{
	paper::open_block block1 (0, 1, 0, paper::keypair ().prv, 0, 0);
	ASSERT_EQ (block1.hash (), block1.hash ());
	std::vector<uint8_t> bytes;
	{
		paper::vectorstream stream (bytes);
		block1.serialize (stream);
	}
	ASSERT_EQ (paper::open_block::size, bytes.size ());
	paper::bufferstream stream (bytes.data (), bytes.size ());
	bool error;
	paper::open_block block2 (error, stream);
	ASSERT_FALSE (error);
	ASSERT_EQ (block1, block2);
}

TEST (change_block, deserialize)
{
	paper::change_block block1 (1, 2, paper::keypair ().prv, 4, 5);
	ASSERT_EQ (block1.hash (), block1.hash ());
	std::vector<uint8_t> bytes;
	{
		paper::vectorstream stream1 (bytes);
		block1.serialize (stream1);
	}
	ASSERT_EQ (paper::change_block::size, bytes.size ());
	auto data (bytes.data ());
	auto size (bytes.size ());
	ASSERT_NE (nullptr, data);
	ASSERT_NE (0, size);
	paper::bufferstream stream2 (data, size);
	bool error;
	paper::change_block block2 (error, stream2);
	ASSERT_FALSE (error);
	ASSERT_EQ (block1, block2);
}

TEST (frontier_req, serialization)
{
	paper::frontier_req request1;
	request1.start = 1;
	request1.age = 2;
	request1.count = 3;
	std::vector<uint8_t> bytes;
	{
		paper::vectorstream stream (bytes);
		request1.serialize (stream);
	}
	paper::bufferstream buffer (bytes.data (), bytes.size ());
	paper::frontier_req request2;
	ASSERT_FALSE (request2.deserialize (buffer));
	ASSERT_EQ (request1, request2);
}

TEST (block, publish_req_serialization)
{
	paper::keypair key1;
	paper::keypair key2;
	auto block (std::unique_ptr<paper::send_block> (new paper::send_block (0, key2.pub, 200, paper::keypair ().prv, 2, 3)));
	paper::publish req (std::move (block));
	std::vector<uint8_t> bytes;
	{
		paper::vectorstream stream (bytes);
		req.serialize (stream);
	}
	paper::publish req2;
	paper::bufferstream stream2 (bytes.data (), bytes.size ());
	auto error (req2.deserialize (stream2));
	ASSERT_FALSE (error);
	ASSERT_EQ (req, req2);
	ASSERT_EQ (*req.block, *req2.block);
}

TEST (block, confirm_req_serialization)
{
	paper::keypair key1;
	paper::keypair key2;
	auto block (std::unique_ptr<paper::send_block> (new paper::send_block (0, key2.pub, 200, paper::keypair ().prv, 2, 3)));
	paper::confirm_req req (std::move (block));
	std::vector<uint8_t> bytes;
	{
		paper::vectorstream stream (bytes);
		req.serialize (stream);
	}
	paper::confirm_req req2;
	paper::bufferstream stream2 (bytes.data (), bytes.size ());
	auto error (req2.deserialize (stream2));
	ASSERT_FALSE (error);
	ASSERT_EQ (req, req2);
	ASSERT_EQ (*req.block, *req2.block);
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          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
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_SATURATE_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_SATURATE_HPP_INCLUDED

#include <boost/simd/ieee/functions/saturate.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>

namespace boost { namespace simd { namespace ext
{
 //============================================================================
  // floating/xxx is a cast
  //============================================================================
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::saturate_, tag::cpu_
                                    , (A0)(T)
                                    , (scalar_< arithmetic_<A0> >)
                                      (target_< generic_<arithmetic_<T> > >)
                                    )
  {
    typedef A0 result_type;
    typedef typename meta::scalar_of<T>::type target_t;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, T const& ) const
    {
      return static_cast<target_t>(a0);
    }
  };

  //============================================================================
  // Trivial case int/xxx : Just compare over Valmax/Valmin after conversion
  //============================================================================
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::saturate_, tag::cpu_
                                    , (A0)(T)
                                    , (scalar_<int_<A0> >)
                                      (target_< generic_<integer_<T> > >)
                                    )
  {
    typedef A0                                              result_type;
    typedef typename meta::scalar_of<T>::type               target_t;
    typedef typename boost::common_type<A0,target_t>::type  base_t;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, T const& ) const
    {
      base_t a00 = static_cast<base_t>(a0);
      if      (a00 > Valmax<target_t>()) return result_type(Valmax<target_t>());
      else if (a00 < Valmin<target_t>()) return result_type(Valmin<target_t>());
      else                               return a0;
    }
  };

  //============================================================================
  // Trivial case uint/uint : Just compare over Valmax
  //============================================================================
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::saturate_, tag::cpu_
                                    , (A0)(T)
                                    , (scalar_<uint_<A0> >)
                                      (target_< generic_<uint_<T> > >)
                                    )
  {
    typedef A0 result_type;
    typedef typename meta::scalar_of<T>::type target_t;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, T const& ) const
    {
      return (a0 > Valmax<target_t>()) ? result_type(Valmax<target_t>()) : a0;
    }
  };

  //============================================================================
  // For uint/int, do the comparison after casting Valmax<T> to the unsigned
  // type to avoid warnings.
  //============================================================================
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::saturate_, tag::cpu_
                                    , (A0)(T)
                                    , (scalar_<uint_<A0> >)
                                      (target_< generic_<int_<T> > >)
                                    )
  {
    typedef A0                                result_type;
    typedef typename meta::scalar_of<T>::type target_t;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, T const& ) const
    {
      A0 a00 = static_cast<A0>(Valmax<target_t>());
      return (a0 > a00) ? a00 : a0;
    }
  };
} } }

#endif