#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)
    :
      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)
  {
    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 = mylib::Coord3(c(2)+1,c(1)+1,c(0)+1); //shape of the lattice
    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_) )
    {++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_,pos);
      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_,pos);
      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_,pos);
      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_,pos);
      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);
      lock();
      ctx->set_outline_mode(false);
    }
    cursor_=beg-AUINT32(attr_); // reset to start of plane
    // Copy explorable to next plane
    if( (current_plane_offset_/sz_plane_nelem_)<(attr_->dims[2]-1)) // if not last plane
    { uint32_t *c,*n;
      for(c=(uint32_t*)beg,n=(uint32_t*)end;c<end;++c,++n)
        *n|=(*c&Explorable);
    }
    unlock();
  }

#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_,pos);
    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_,computeCursorPos(),*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_,computeCursorPos(),*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_,computeCursorPos(),*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_,computeCursorPos(),*m);
  }

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

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

  //  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 and the previous 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_,
             *prev = beg-sz_plane_nelem_;
    #define DETECTED(e) ((*(e)&Detected)==Detected)
    for(c=beg,p=prev;c<end;c++,p++)
    { if( DETECTED(c) || ((iplane>0)?DETECTED(p):0) )
      { *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,Active,1 /*restrict to explorable */);
  }

  // dilate         //////////////////////////////////////////////////////
  //
  // Does not dilate into Done tiles.
  // Optionally restricts dilation to explorable tiles.
  //
  //
  void StageTiling::dilate(size_t iplane, int n, StageTiling::Flags query_flag, StageTiling::Flags write_flag, int explorable_only)
  {
    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)*Reserved;  // this way only the originally labelled tiles will be dilated from
    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
              { *c|=(write_flag|Reserved2); break;
              }
        }
      }
      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;
    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);
    }
    void free_elem(e_t e) { /* release element -- no op */ }
    e_t bind(uint32_t* v,int dist) {
      e_t e=elem();
      if(!e) return 0;
      e->tile=v; e->next=NULL; e->dist=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;
      if(!v) return this; // no-op -- to allow implicit filtering of inputs
      e=bind(v,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;}
      return this;
    }
    uint32_t* pop(int *dist) {
      uint32_t* v=0;
      int d=-1;
      e_t e=head;
      if(head) { head=head->next; v=e->tile; d=e->dist; free_elem(e);}
      if(dist) *dist=d;
      return v;
    }
  };

  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_,
             *beg = AUINT32(attr_)+current_plane_offset_,
             *end = beg+sz_plane_nelem_;
    q_t q(w*h);

    for(uint32_t *t=(uint32_t*)beg;t<end;++t) // Reset Reserved
      *t = t[0]&~Reserved;

    #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)
    try
    { q.push(maybe(c),0);
      while(c=q.pop(&dist))
      { *c |= Reserved; // mark it as looked at
        if((*c&query_mask)==query_flags) {goto Finalize;}
        for(int i=0;i<countof(offsets);++i)
          q.push(maybe(c+offsets[i]),dist+1);
      }
      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
  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, const Vector3f& pos, uint32_t sts)
  { TListeners::iterator i;
    for(i=listeners_.begin();i!=listeners_.end();++i)
      (*i)->tile_done(index,pos,sts);
  }

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

  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;
  }

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

}} // end namespace fetch::device


/*
 * Copyright (c) 2018 Eliane Briand
 *
 * This file is part of SmolDock.
 *
 * SmolDock is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * SmolDock is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with SmolDock.  If not, see <https://www.gnu.org/licenses/>.
 *
 */

#include "VinaLikeRigid.h"

#undef BOOST_LOG

#include <boost/log/trivial.hpp>

#include <cmath>
#include <boost/assert.hpp>
#include <iomanip>
#include <Structures/Atom.h>

#include <Engines/Internals/InternalsUtilityFunctions.h>

#include "VinaLikeCommon.h"

namespace SmolDock {
    namespace Score {

        const std::array<std::string, VinaLikeRigid::numCoefficients>
                VinaLikeRigid::coefficientsNames =  {"Gauss1", "Gauss2", "RepulsionExceptCovalent", "Hydrophobic","Hydrogen"};

        double vina_like_rigid_inter_scoring_func(const iConformer &ligand, iTransform &transform,
                                                  const iProtein &protein) {

            BOOST_ASSERT(!ligand.x.empty());
            BOOST_ASSERT(!protein.x.empty());

            if(std::abs(transform.rota.norm() - 1) > 0.1) {
                transform.rota.normalize();
            }

            double score_raw = 0;

            Eigen::Vector3d ProtCenterPosition = {protein.center_x, protein.center_y, protein.center_z};

            for (unsigned int idxLig = 0; idxLig < ligand.x.size(); idxLig++) {
                for (unsigned int idxProt = 0; idxProt < protein.x.size(); idxProt++) {

                    Eigen::Vector3d LigPosition = {ligand.x[idxLig], ligand.y[idxLig], ligand.z[idxLig]};
                    applyRigidTransformInPlace(LigPosition, transform);

                    Eigen::Vector3d ProtPosition = {protein.x[idxProt], protein.y[idxProt], protein.z[idxProt]};

                    Eigen::Vector3d distToCenterVector = LigPosition - ProtCenterPosition;

                    double distanceToProteinCenter = distToCenterVector.norm();

                    if (distanceToProteinCenter > (protein.radius - 1)) {
                        score_raw += std::pow((distanceToProteinCenter - protein.radius), 4) + 10;
                        continue;
                    }

                    Eigen::Vector3d distVect = ProtPosition - LigPosition;

                    double atomicRadiusLig = ligand.atomicRadius[idxLig];
                    double atomicRadiusProt = protein.atomicRadius[idxProt];
                    double rawDist = distVect.norm();

                    const double cutoff = 8.0;
                    if (rawDist >= cutoff)
                        continue;

                    double radToRemove = (atomicRadiusLig + atomicRadiusProt);
                    double distance = rawDist - radToRemove;

                    score_raw += scoreForAtomCouple(distance, ligand.type[idxLig], ligand.variant[idxLig],
                                                    protein.type[idxProt], protein.variant[idxProt]);
/*
                    // Special rubber band term not in Vina scoring func
                    double x_diff_center = std::pow(LigPosition.x - protein.center_x, 2);
                    double y_diff_center = std::pow(LigPosition.y - protein.center_y, 2);
                    double z_diff_center = std::pow(LigPosition.z - protein.center_z, 2);
                    double rawDistCenter = std::sqrt(x_diff_center + y_diff_center + z_diff_center);

                    score += 0.00001 * rawDistCenter;
*/

                } // for
            } // for
            /*
            BOOST_LOG_TRIVIAL(debug) << "Intermolecular scoring contribution without weighting :";
            BOOST_LOG_TRIVIAL(debug) << "Gauss 1      : " << std::fixed<< std::setprecision(5) << score_gauss1;
            BOOST_LOG_TRIVIAL(debug) << "Gauss 2      : " << std::fixed<< std::setprecision(5) << score_gauss2;
            BOOST_LOG_TRIVIAL(debug) << "Repulsion    : " << std::fixed<< std::setprecision(5) << score_repulsion;
            BOOST_LOG_TRIVIAL(debug) << "Hydrophobic  : " << std::fixed<< std::setprecision(5) << score_hydrophobic;
            BOOST_LOG_TRIVIAL(debug) << "Hydrogen     : " << std::fixed<< std::setprecision(5) << score_hydrogen;
            BOOST_LOG_TRIVIAL(debug) << "------------------------------------------";
            BOOST_LOG_TRIVIAL(debug) << "Raw Score    : " << score_raw;
            BOOST_LOG_TRIVIAL(debug) << "Nrotatable   : " << ligand.num_rotatable_bond;
            //*/
            double final_score = score_raw / (1 + (0.058459999999999998 * ligand.num_rotatable_bond));
            /*
            BOOST_LOG_TRIVIAL(debug) << "------------------------------------------";
            BOOST_LOG_TRIVIAL(debug) << "Final Score  : " << final_score;
            //*/

            return final_score;
        }

        double VinaLikeRigid::Evaluate(const arma::mat &x) {
            BOOST_ASSERT(x.n_rows == 7);

            iTransform tr = this->internalToExternalRepr(x);
            tr.doHousekeeping();

            double score_ = vina_like_rigid_inter_scoring_func(this->startingConformation, tr, this->prot);

            return score_;
        }

        double VinaLikeRigid::EvaluateWithGradient(const arma::mat &x, arma::mat &grad) {

            BOOST_ASSERT(!x.has_nan());
            BOOST_ASSERT(!grad.has_nan());
            BOOST_ASSERT(x.n_rows == 7);
            BOOST_ASSERT(grad.n_rows == 7);

            iTransform tr = this->internalToExternalRepr(x);
            tr.doHousekeeping();

            double score_ = vina_like_rigid_inter_scoring_func(this->startingConformation, tr, this->prot);

            // Translation
            {
                iTransform transform_dx = tr;
                transform_dx.transl.x() += this->differential_epsilon;
                grad[0] = vina_like_rigid_inter_scoring_func(this->startingConformation, transform_dx, this->prot) -
                          score_;
            }

            {
                iTransform transform_dy = tr;
                transform_dy.transl.y() += this->differential_epsilon;
                grad[1] = vina_like_rigid_inter_scoring_func(this->startingConformation, transform_dy, this->prot) -
                          score_;
            }

            {
                iTransform transform_dz = tr;
                transform_dz.transl.z() += this->differential_epsilon;
                grad[2] = vina_like_rigid_inter_scoring_func(this->startingConformation, transform_dz, this->prot) -
                          score_;
            }

            // Rotation

            {
                iTransform transform_ds = tr;
                transform_ds.rota.w() += this->differential_epsilon;
                transform_ds.doHousekeeping();
                grad[3] = vina_like_rigid_inter_scoring_func(this->startingConformation, transform_ds, this->prot) -
                          score_;
            }

            {
                iTransform transform_du = tr;
                transform_du.rota.x() += this->differential_epsilon;
                transform_du.doHousekeeping();
                grad[4] = vina_like_rigid_inter_scoring_func(this->startingConformation, transform_du, this->prot) -
                          score_;
            }

            {
                iTransform transform_dv = tr;
                transform_dv.rota.y() += this->differential_epsilon;
                transform_dv.doHousekeeping();
                grad[5] = vina_like_rigid_inter_scoring_func(this->startingConformation, transform_dv, this->prot) -
                          score_;
            }

            {
                iTransform transform_dt = tr;
                transform_dt.rota.z() += this->differential_epsilon;
                transform_dt.doHousekeeping();
                grad[6] = vina_like_rigid_inter_scoring_func(this->startingConformation, transform_dt, this->prot) -
                          score_;
            }
            /*
            BOOST_LOG_TRIVIAL(debug) << "Transform: " << x.t();
            BOOST_LOG_TRIVIAL(debug) << "Score: " << score_;
            BOOST_LOG_TRIVIAL(debug) << "Gradient";
            BOOST_LOG_TRIVIAL(debug) << "     ds: " << grad[3];
            BOOST_LOG_TRIVIAL(debug) << "     du: " << grad[4] << "   dx: " << grad[0];
            BOOST_LOG_TRIVIAL(debug) << "     dv: " << grad[5] << "   dy: " << grad[1];
            BOOST_LOG_TRIVIAL(debug) << "     dt: " << grad[6] << "   dx: " << grad[2];
            //*/

            BOOST_ASSERT(score_ == score_); // catches NaN
            return score_;
        }

        VinaLikeRigid::VinaLikeRigid(const iConformer &startingConformation_,
                                                                   const iProtein &p,
                                                                   const iTransform &initialTransform_,
                                                                   double differential_epsilon_) :

                startingConformation(startingConformation_),
                prot(p),
                initialTransform(initialTransform_),
                differential_epsilon(differential_epsilon_) {}

        double VinaLikeRigid::getDifferentialEpsilon() const {
            return this->differential_epsilon;
        }

        arma::mat VinaLikeRigid::getStartingConditions() const {
            return this->externalToInternalRepr(this->initialTransform);
        }

        iConformer VinaLikeRigid::getConformerForParamMatrix(const arma::mat &x) {
            BOOST_ASSERT(x.n_rows == 7);

            iTransform tr = this->internalToExternalRepr(x);
            normalizeQuaternionInPlace(
                    tr.rota); //!< Note that the internal representation arma::mat is not by itself normalized
            //! (because we always normalize it in the scoring function, so no constraint)

            iConformer ret = this->startingConformation;
            applyRigidTransformInPlace(ret, tr);

            return ret;
        }

        unsigned int VinaLikeRigid::getParamVectorDimension() const {
            return 7;
        }

        std::vector<std::tuple<std::string, double>> VinaLikeRigid::EvaluateSubcomponents(const arma::mat &x) {
            std::vector<std::tuple<std::string, double>> ret;

            BOOST_ASSERT(x.n_rows == 7);

            iTransform tr = this->internalToExternalRepr(x);
            normalizeQuaternionInPlace(tr.rota);

            BOOST_ASSERT(!this->startingConformation.x.empty());
            BOOST_ASSERT(!this->prot.x.empty());

            if(std::abs(tr.rota.norm() - 1) > 0.1) {
                tr.rota.normalize();
            }

            BOOST_ASSERT(tr.bondRotationsAngles.size() == this->startingConformation.num_rotatable_bond);

            double gauss1_total = 0.0;
            double gauss2_total = 0.0;
            double repulsion_total = 0.0;
            double hydrogen_total = 0.0;
            double hydrophobic_total = 0.0;
            double score_raw = 0.0;

            iConformer ligand = this->startingConformation;
            applyBondRotationInPlace(ligand, tr);

            Eigen::Vector3d ProtCenterPosition = {this->prot.center_x, this->prot.center_y, this->prot.center_z};

            for (unsigned int idxLig = 0; idxLig < ligand.x.size(); idxLig++) {
                for (unsigned int idxProt = 0; idxProt < this->prot.x.size(); idxProt++) {

                    Eigen::Vector3d LigPosition = {ligand.x[idxLig], ligand.y[idxLig], ligand.z[idxLig]};
                    applyRigidTransformInPlace(LigPosition, tr);

                    Eigen::Vector3d ProtPosition = {this->prot.x[idxProt], this->prot.y[idxProt], this->prot.z[idxProt]};
                    Eigen::Vector3d distToCenterVector = LigPosition - ProtCenterPosition;

                    double distanceToProteinCenter = distToCenterVector.norm();

                    if (distanceToProteinCenter > (this->prot.radius - 1)) {
                        //FIXME : temporary disable
                        //score_raw += std::pow((distanceToProteinCenter - this->prot.radius), 4) + 10;
                        continue;
                    }

                    Eigen::Vector3d distVect = ProtPosition - LigPosition;

                    double rawDist = distVect.norm();

                    const double cutoff = 8.0;
                    if (rawDist >= cutoff)
                        continue;

                    double distance = distanceFromRawDistance(rawDist,  ligand.atomicRadius[idxLig], this->prot.atomicRadius[idxProt]);

                    unsigned char atom1AtomicNumber = ligand.type[idxLig];
                    unsigned int atom1AtomVariant = ligand.variant[idxLig];
                    unsigned char atom2AtomicNumber = this->prot.type[idxProt];
                    unsigned int atom2AtomVariant = this->prot.variant[idxProt];

                    gauss1_total += vinaGaussComponent(distance, 0.0, 0.5);
                    gauss2_total += vinaGaussComponent(distance, 3.0, 2.0);
                    repulsion_total += vinaRepulsionComponent(distance, 0.0);
                    hydrogen_total += vinaHydrophobicComponent(distance,
                                                               atom1AtomicNumber, atom1AtomVariant,
                                                               atom2AtomicNumber, atom2AtomVariant);
                    hydrophobic_total += vinaHydrogenComponent(distance,
                                                               atom1AtomicNumber, atom1AtomVariant,
                                                               atom2AtomicNumber, atom2AtomVariant);
                    score_raw += scoreForAtomCouple(distance,
                                                    atom1AtomicNumber, atom1AtomVariant,
                                                    atom2AtomicNumber, atom2AtomVariant);

                } // for
            } // for

            double final_score = score_raw / (1 + (0.058459999999999998 * ligand.num_rotatable_bond));

            ret.emplace_back(std::make_tuple("Gauss1",gauss1_total));
            ret.emplace_back(std::make_tuple("Gauss2",gauss2_total));
            ret.emplace_back(std::make_tuple("Repulsion",repulsion_total));
            ret.emplace_back(std::make_tuple("Hydrophobic",hydrogen_total));
            ret.emplace_back(std::make_tuple("Hydrogen",hydrophobic_total));
            ret.emplace_back(std::make_tuple("ScoreRaw",score_raw));
            ret.emplace_back(std::make_tuple("Score",final_score));
            return ret;
        }

        double VinaLikeRigid::EvaluateOnlyIntermolecular(const arma::mat &x) {
            return this->Evaluate(x);
        }

        unsigned int VinaLikeRigid::getCoefficientsVectorWidth() {
            return this->numCoefficients;
        }

        std::vector<std::string> VinaLikeRigid::getCoefficientsNames() {
            return std::vector<std::string>(this->coefficientsNames.begin(),this->coefficientsNames.end());
        }

        std::vector<double> VinaLikeRigid::getCurrentCoefficients() {
            return {VinaClassic::coeff_gauss1, VinaClassic::coeff_gauss2,
                    VinaClassic::coeff_repulsion, VinaClassic::coeff_hydrophobic,
                    VinaClassic::coeff_hydrogen };
        }

        bool VinaLikeRigid::setNonDefaultCoefficients(std::vector<double> coeffs) {
            BOOST_LOG_TRIVIAL(debug) << "Non default coefficients not supported yet on Vina rigid scoring function.";
            return false;
        }

    }

}


//  Copyright (c) 2006-2009, Bernhard Reiter
//
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

/**
 * @file treap.hpp
 * Treap balancer and metadata
 */
//TODO: maybe have treap really model binary_heap?

#ifndef BOOST_TREE_BALANCERS_TREAP_HPP
#define BOOST_TREE_BALANCERS_TREAP_HPP

#include <boost/tree/detail/nary_cursor.hpp>

#include <limits.h>
#include <stdlib.h>

namespace boost {
namespace tree {
namespace balancers {

class treap_metadata {
public:
    treap_metadata()
    : m_priority((lrand48() >> 1) + 1) {}

    int const get_priority() const
    {
        return m_priority;
    }

    void set_priority(int priority)
    {
        m_priority = priority;
    }
private:
    int m_priority;
};

class treap {
 public:
    typedef treap_metadata metadata_type;

    template <class Tree>
    static void add(Tree& t, typename Tree::cursor& x)
    {
        int priority = x->metadata().get_priority();

        x.to_parent();
        while ((x != t.root()) && (priority > x->metadata().get_priority())) {
            t.rotate(x);
            priority = x->metadata().get_priority();
        }
        x.to_begin();
    }

    template <class Tree>
    static typename Tree::cursor remove(Tree& t, typename Tree::cursor& p)
    {
        typename Tree::cursor q;
        while((q = ((p.begin()->metadata().get_priority()
                     > p.end()->metadata().get_priority())
                     ? p.begin() : p.end())).is_leaf())
            t.rotate(q);
        p = q;
        return p;
    }

    template <class Tree>
    static void touch(Tree&, typename Tree::cursor&)
    { }
};

} // namespace balancers
} // namespace tree
} // namespace boost

#endif // BOOST_TREE_BALANCERS_TREAP_HPP


/****************************************************************************
**
** Copyright (C) 2017 TU Wien, ACIN, Vision 4 Robotics (V4R) group
** Contact: v4r.acin.tuwien.ac.at
**
** This file is part of V4R
**
** V4R is distributed under dual licenses - GPLv3 or closed source.
**
** GNU General Public License Usage
** V4R is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published
** by the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** V4R is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** Please review the following information to ensure the GNU General Public
** License requirements will be met: https://www.gnu.org/licenses/gpl-3.0.html.
**
**
** Commercial License Usage
** If GPL is not suitable for your project, you must purchase a commercial
** license to use V4R. Licensees holding valid commercial V4R licenses may
** use this file in accordance with the commercial license agreement
** provided with the Software or, alternatively, in accordance with the
** terms contained in a written agreement between you and TU Wien, ACIN, V4R.
** For licensing terms and conditions please contact office<at>acin.tuwien.ac.at.
**
**
** The copyright holder additionally grants the author(s) of the file the right
** to use, copy, modify, merge, publish, distribute, sublicense, and/or
** sell copies of their contributions without any restrictions.
**
****************************************************************************/

/**
 * @file main.cpp
 * @author Johann Prankl (prankl@acin.tuwien.ac.at)
 * @date 2017
 * @brief
 *
 */

#ifndef KP_GLOBAL_CLOUD_FILTERING_HH
#define KP_GLOBAL_CLOUD_FILTERING_HH

#include <float.h>
#include <pcl/io/io.h>
#include <pcl/octree/octree.h>
#include <pcl/octree/octree_impl.h>
#include <pcl/octree/octree_pointcloud.h>
#include <pcl/octree/octree_pointcloud_voxelcentroid.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <v4r/camera_tracking_and_mapping/TSFData.h>
#include <v4r/core/macros.h>
#include <Eigen/Dense>
#include <boost/smart_ptr.hpp>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <fstream>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <v4r/camera_tracking_and_mapping/OctreeVoxelCentroidContainerXYZRGBNormal.hpp>
#include <v4r/camera_tracking_and_mapping/TSFFrame.hh>
#include <v4r/common/impl/DataMatrix2D.hpp>
#include <v4r/keypoints/impl/triple.hpp>
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/video/tracking.hpp"

namespace v4r {

/**
 * TSFGlobalCloudFiltering
 */
class V4R_EXPORTS TSFGlobalCloudFiltering {
 public:
  /**
   * Parameter
   */
  class Parameter {
   public:
    float thr_angle;
    float max_weight;
    float sigma_pts;
    float sigma_depth;
    float z_cut_off_integration;
    float voxel_size;
    float max_dist_integration;
    Parameter()
    : thr_angle(80), max_weight(20), sigma_pts(10), sigma_depth(0.008), z_cut_off_integration(0.02), voxel_size(0.001),
      max_dist_integration(7.) {}
  };

 private:
  Parameter param;

  float cos_thr_angle;
  float neg_inv_sqr_sigma_pts;

  cv::Mat_<double> intrinsic;

  std::vector<cv::Vec4i> npat;
  std::vector<float> exp_error_lookup;

  std::vector<cv::Mat_<float>> reliability;

  pcl::octree::OctreePointCloudVoxelCentroid<
      pcl::PointXYZRGBNormal, pcl::octree::OctreeVoxelCentroidContainerXYZRGBNormal<pcl::PointXYZRGBNormal>>::Ptr
      octree;
  typedef pcl::octree::OctreePointCloudVoxelCentroid<
      pcl::PointXYZRGBNormal,
      pcl::octree::OctreeVoxelCentroidContainerXYZRGBNormal<pcl::PointXYZRGBNormal>>::AlignedPointTVector
      AlignedPointXYZRGBNormalVector;
  std::shared_ptr<AlignedPointXYZRGBNormalVector> oc_cloud;

  //  void integrateData(const pcl::PointCloud<pcl::PointXYZRGB> &cloud, const Eigen::Matrix4f &pose, const
  //  Eigen::Matrix4f &filt_pose, v4r::DataMatrix2D<TSFData::Surfel> &filt_cloud);
  void computeReliability(const std::vector<TSFFrame::Ptr> &frames);
  void maxReliabilityIndexing(const std::vector<TSFFrame::Ptr> &frames);
  void getMaxPoints(const std::vector<TSFFrame::Ptr> &frames, pcl::PointCloud<pcl::PointXYZRGBNormal> &cloud);
  void computeNormals(v4r::DataMatrix2D<v4r::Surfel> &sf_cloud);

  inline float sqr(const float &d) {
    return d * d;
  }

 public:
  cv::Mat dbg;

  TSFGlobalCloudFiltering(const Parameter &p = Parameter());
  ~TSFGlobalCloudFiltering();

  void start();
  void stop();

  void computeRadius(v4r::DataMatrix2D<v4r::Surfel> &sf_cloud);

  void filter(const std::vector<TSFFrame::Ptr> &frames, pcl::PointCloud<pcl::PointXYZRGBNormal> &cloud);

  void setCameraParameter(const cv::Mat &_intrinsic);
  void setParameter(const Parameter &p);

  typedef std::shared_ptr<::v4r::TSFGlobalCloudFiltering> Ptr;
  typedef std::shared_ptr<::v4r::TSFGlobalCloudFiltering const> ConstPtr;
};

/*************************** INLINE METHODES **************************/

}  // namespace v4r

#endif


#include "CompositeTECPetal.h"

#include "FWCore/MessageLogger/interface/MessageLogger.h"

#include "ForwardDiskSectorBuilderFromWedges.h"
#include "LayerCrossingSide.h"
#include "DetGroupMerger.h"
#include "CompatibleDetToGroupAdder.h"

#include "TrackingTools/DetLayers/interface/DetLayerException.h"
#include "TrackingTools/DetLayers/interface/MeasurementEstimator.h"
#include "TrackingTools/GeomPropagators/interface/HelixForwardPlaneCrossing.h"

#include "TkDetUtil.h"
#include "DataFormats/GeometryVector/interface/VectorUtil.h"

#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <algorithm>
#include <numeric>
#include <iterator>

using namespace std;

typedef GeometricSearchDet::DetWithState DetWithState;

namespace {
  namespace details {

    struct Mean {
      float operator()(const GeometricSearchDet* a, const GeometricSearchDet* b) const {
        return 0.5 * (b->position().perp() + a->position().perp());
      }
      float operator()(float a, float b) const { return 0.5 * (b + a); }
    };

    void fillBoundaries(std::vector<const TECWedge*> const& dets, std::vector<float>& boundaries) {
      boundaries.resize(dets.size());
      std::transform(dets.begin(),
                     dets.end(),
                     boundaries.begin(),
                     boost::bind(&GlobalPoint::perp, boost::bind(&GeometricSearchDet::position, _1)));
      std::adjacent_difference(boundaries.begin(), boundaries.end(), boundaries.begin(), Mean());
    }

    inline int findBin(std::vector<float> const& boundaries, float r) {
      return std::lower_bound(boundaries.begin() + 1, boundaries.end(), r) - boundaries.begin() - 1;
    }

    void fillPars(std::vector<const TECWedge*> const& dets, std::vector<CompositeTECPetal::WedgePar>& pars) {
      for (auto gsdet : dets) {
        const BoundDiskSector& wedgeSector = static_cast<const BoundDiskSector&>(gsdet->surface());
        float wedgeMinZ = std::abs(wedgeSector.position().z()) - 0.5 * wedgeSector.bounds().thickness();
        float wedgeMaxZ = std::abs(wedgeSector.position().z()) + 0.5 * wedgeSector.bounds().thickness();
        float thetaWedgeMin = wedgeSector.innerRadius() / wedgeMaxZ;
        float thetaWedgeMax = wedgeSector.outerRadius() / wedgeMinZ;
        CompositeTECPetal::WedgePar apar = {gsdet->position().perp(), thetaWedgeMin, thetaWedgeMax};
        pars.push_back(apar);
      }
    }

    inline bool overlap(const GlobalPoint& gpos, const CompositeTECPetal::WedgePar& wpar, float ymax) {
      // this method is just a duplication of overlapInR
      // adapeted for groupedCompatibleDets() needs

      // assume "fixed theta window", i.e. margin in local y = r is changing linearly with z
      auto tsRadius = gpos.perp();
      auto rmin = std::max(0.f, tsRadius - ymax);
      auto zmax = std::abs(gpos.z()) + 10.f;  // add 10 cm contingency
      auto rmax = (tsRadius + ymax);
      auto zmin = std::abs(gpos.z()) - 10.f;

      // do the theta regions overlap ?

      return !((rmin > zmax * wpar.thetaMax) | (zmin * wpar.thetaMin > rmax));
    }

  }  // namespace details
}  // namespace

CompositeTECPetal::CompositeTECPetal(vector<const TECWedge*>& innerWedges, vector<const TECWedge*>& outerWedges)
    : GeometricSearchDet(true), theFrontComps(innerWedges), theBackComps(outerWedges) {
  theComps.assign(theFrontComps.begin(), theFrontComps.end());
  theComps.insert(theComps.end(), theBackComps.begin(), theBackComps.end());

  details::fillBoundaries(theFrontComps, theFrontBoundaries);
  details::fillBoundaries(theBackComps, theBackBoundaries);
  details::fillPars(theFrontComps, theFrontPars);
  details::fillPars(theBackComps, theBackPars);

  for (vector<const GeometricSearchDet*>::const_iterator it = theComps.begin(); it != theComps.end(); it++) {
    theBasicComps.insert(theBasicComps.end(), (**it).basicComponents().begin(), (**it).basicComponents().end());
  }

  //the Wedge are already R ordered
  //sort( theWedges.begin(), theWedges.end(), DetLessR());
  //sort( theFrontWedges.begin(), theFrontWedges.end(), DetLessR() );
  //sort( theBackWedges.begin(), theBackWedges.end(), DetLessR() );
  vector<const TECWedge*> allWedges;
  allWedges.assign(innerWedges.begin(), innerWedges.end());
  allWedges.insert(allWedges.end(), outerWedges.begin(), outerWedges.end());

  theDiskSector = ForwardDiskSectorBuilderFromWedges()(allWedges);
  theFrontSector = ForwardDiskSectorBuilderFromWedges()(innerWedges);
  theBackSector = ForwardDiskSectorBuilderFromWedges()(outerWedges);

  //--------- DEBUG INFO --------------
  LogDebug("TkDetLayers") << "DEBUG INFO for CompositeTECPetal";

  for (auto it = theFrontComps.begin(); it != theFrontComps.end(); it++) {
    LogDebug("TkDetLayers") << "frontWedge phi,z,r: " << (*it)->surface().position().phi() << " , "
                            << (*it)->surface().position().z() << " , " << (*it)->surface().position().perp();
  }

  for (auto it = theBackComps.begin(); it != theBackComps.end(); it++) {
    LogDebug("TkDetLayers") << "backWedge phi,z,r: " << (*it)->surface().position().phi() << " , "
                            << (*it)->surface().position().z() << " , " << (*it)->surface().position().perp();
  }
  //-----------------------------------
}

CompositeTECPetal::~CompositeTECPetal() {
  vector<const GeometricSearchDet*>::const_iterator i;
  for (i = theComps.begin(); i != theComps.end(); i++) {
    delete *i;
  }
}

pair<bool, TrajectoryStateOnSurface> CompositeTECPetal::compatible(const TrajectoryStateOnSurface& ts,
                                                                   const Propagator&,
                                                                   const MeasurementEstimator&) const {
  edm::LogError("TkDetLayers") << "temporary dummy implementation of CompositeTECPetal::compatible()!!";
  return pair<bool, TrajectoryStateOnSurface>();
}

void CompositeTECPetal::groupedCompatibleDetsV(const TrajectoryStateOnSurface& tsos,
                                               const Propagator& prop,
                                               const MeasurementEstimator& est,
                                               std::vector<DetGroup>& result) const {
  vector<DetGroup> closestResult;
  SubLayerCrossings crossings;
  crossings = computeCrossings(tsos, prop.propagationDirection());
  if (!crossings.isValid())
    return;

  addClosest(tsos, prop, est, crossings.closest(), closestResult);
  LogDebug("TkDetLayers") << "in TECPetal, closestResult.size(): " << closestResult.size();

  if (closestResult.empty()) {
    vector<DetGroup> nextResult;
    addClosest(tsos, prop, est, crossings.other(), nextResult);
    LogDebug("TkDetLayers") << "in TECPetal, nextResult.size(): " << nextResult.size();
    if (nextResult.empty())
      return;

    DetGroupElement nextGel(nextResult.front().front());
    int crossingSide = LayerCrossingSide::endcapSide(nextGel.trajectoryState(), prop);
    DetGroupMerger::orderAndMergeTwoLevels(
        std::move(closestResult), std::move(nextResult), result, crossings.closestIndex(), crossingSide);
  } else {
    DetGroupElement closestGel(closestResult.front().front());
    float window = computeWindowSize(closestGel.det(), closestGel.trajectoryState(), est);

    searchNeighbors(tsos, prop, est, crossings.closest(), window, closestResult, false);

    vector<DetGroup> nextResult;
    searchNeighbors(tsos, prop, est, crossings.other(), window, nextResult, true);

    int crossingSide = LayerCrossingSide::endcapSide(closestGel.trajectoryState(), prop);
    DetGroupMerger::orderAndMergeTwoLevels(
        std::move(closestResult), std::move(nextResult), result, crossings.closestIndex(), crossingSide);
  }
}

SubLayerCrossings CompositeTECPetal::computeCrossings(const TrajectoryStateOnSurface& startingState,
                                                      PropagationDirection propDir) const {
  HelixPlaneCrossing::PositionType startPos(startingState.globalPosition());
  HelixPlaneCrossing::DirectionType startDir(startingState.globalMomentum());

  auto rho = startingState.transverseCurvature();

  HelixForwardPlaneCrossing crossing(startPos, startDir, rho, propDir);

  pair<bool, double> frontPath = crossing.pathLength(*theFrontSector);
  if (!frontPath.first)
    return SubLayerCrossings();

  pair<bool, double> backPath = crossing.pathLength(*theBackSector);
  if (!backPath.first)
    return SubLayerCrossings();

  GlobalPoint gFrontPoint(crossing.position(frontPath.second));
  GlobalPoint gBackPoint(crossing.position(backPath.second));

  LogDebug("TkDetLayers") << "in TECPetal,front crossing : r,z,phi: (" << gFrontPoint.perp() << "," << gFrontPoint.z()
                          << "," << gFrontPoint.phi() << ")";

  LogDebug("TkDetLayers") << "in TECPetal,back crossing r,z,phi: (" << gBackPoint.perp() << "," << gBackPoint.z() << ","
                          << gBackPoint.phi() << ")";

  int frontIndex = findBin(gFrontPoint.perp(), 0);
  SubLayerCrossing frontSLC(0, frontIndex, gFrontPoint);

  int backIndex = findBin(gBackPoint.perp(), 1);
  SubLayerCrossing backSLC(1, backIndex, gBackPoint);

  auto frontDist = std::abs(theFrontPars[frontIndex].theR - gFrontPoint.perp());
  auto backDist = std::abs(theBackPars[backIndex].theR - gBackPoint.perp());

  // 0ss: frontDisk has index=0, backDisk has index=1
  if (frontDist < backDist) {
    return SubLayerCrossings(frontSLC, backSLC, 0);
  } else {
    return SubLayerCrossings(backSLC, frontSLC, 1);
  }
}

bool CompositeTECPetal::addClosest(const TrajectoryStateOnSurface& tsos,
                                   const Propagator& prop,
                                   const MeasurementEstimator& est,
                                   const SubLayerCrossing& crossing,
                                   vector<DetGroup>& result) const {
  auto det = subLayer(crossing.subLayerIndex())[crossing.closestDetIndex()];

  LogDebug("TkDetLayers") << "in TECPetal, adding Wedge at r,z,phi: (" << det->position().perp() << ","
                          << det->position().z() << "," << det->position().phi() << ")";
  LogDebug("TkDetLayers") << "wedge comps size: " << det->basicComponents().size();

  return CompatibleDetToGroupAdder::add(*det, tsos, prop, est, result);
}

void CompositeTECPetal::searchNeighbors(const TrajectoryStateOnSurface& tsos,
                                        const Propagator& prop,
                                        const MeasurementEstimator& est,
                                        const SubLayerCrossing& crossing,
                                        float window,
                                        vector<DetGroup>& result,
                                        bool checkClosest) const {
  const GlobalPoint& gCrossingPos = crossing.position();

  int closestIndex = crossing.closestDetIndex();
  int negStartIndex = closestIndex - 1;
  int posStartIndex = closestIndex + 1;

  float detR = findPar(closestIndex, crossing.subLayerIndex()).theR;

  if (checkClosest) {  // must decide if the closest is on the neg or pos side
    if (gCrossingPos.perp2() < detR * detR) {
      posStartIndex = closestIndex;
    } else {
      negStartIndex = closestIndex;
    }
  }

  const std::vector<const TECWedge*>& sLayer = subLayer(crossing.subLayerIndex());

  //const BinFinderType& binFinder = (crossing.subLayerIndex()==0 ? theFrontBinFinder : theBackBinFinder);
  int theSize = crossing.subLayerIndex() == 0 ? theFrontComps.size() : theBackComps.size();

  typedef CompatibleDetToGroupAdder Adder;
  for (int idet = negStartIndex; idet >= 0; idet--) {
    //if(idet<0 || idet>= theSize) {edm::LogInfo(TkDetLayers) << "===== error! gone out vector bounds.idet: " << idet ;exit;}
    const GeometricSearchDet& neighborWedge = *sLayer[idet];
    WedgePar const& wpar = findPar(idet, crossing.subLayerIndex());
    if (!details::overlap(gCrossingPos, wpar, window))
      break;  // --- to check
    if (!Adder::add(neighborWedge, tsos, prop, est, result))
      break;
    // maybe also add shallow crossing angle test here???
  }
  for (int idet = posStartIndex; idet < theSize; idet++) {
    //if(idet<0 || idet>= theSize) {edm::LogInfo(TkDetLayers) << "===== error! gone out vector bounds.idet: " << idet ;exit;}
    const GeometricSearchDet& neighborWedge = *sLayer[idet];
    WedgePar const& wpar = findPar(idet, crossing.subLayerIndex());
    if (!details::overlap(gCrossingPos, wpar, window))
      break;  // ---- to check
    if (!Adder::add(neighborWedge, tsos, prop, est, result))
      break;
    // maybe also add shallow crossing angle test here???
  }
}

float CompositeTECPetal::computeWindowSize(const GeomDet* det,
                                           const TrajectoryStateOnSurface& tsos,
                                           const MeasurementEstimator& est) {
  return est.maximalLocalDisplacement(tsos, det->surface()).y();
}

int CompositeTECPetal::findBin(float R, int diskSectorType) const {
  return details::findBin(diskSectorType == 0 ? theFrontBoundaries : theBackBoundaries, R);
}


#ifdef WITH_CPLEX
#include <boost/python.hpp>
#include <stdexcept>
#include <stddef.h>
#include <string>
#include "nifty_iterator.hxx"
#include "inf_def_visitor.hxx"
#include "lp_def_suite.hxx"

#include <opengm/python/opengmpython.hxx>
#include <opengm/python/converter.hxx>
#include <opengm/python/numpyview.hxx>
#include <opengm/python/pythonfunction.hxx>

#include <opengm/inference/lpcplex.hxx>
#include <param/lpcplex_param.hxx>

// export function
template<class GM, class ACC>
void export_cplex() {
   using namespace boost::python;
   import_array();
   append_subnamespace("solver");

   // setup
   InfSetup setup;
   setup.algType    = "linear-programming";
   setup.guarantees = "global optimal";
   setup.examples   = ">>> parameter = opengm.InfParam(integerConstraint=True)\n\n"
                      ">>> inference = opengm.inference.LpCplex(gm=gm,accumulator='minimizer',parameter=parameter)\n\n"
                      "\n\n";
   setup.dependencies = "This algorithm needs the IBM CPLEX Optimizer, compile OpenGM with CMake-Flag ``WITH_CPLEX`` set to ``ON`` ";
   // export parameter
   typedef typename GM::ValueType ValueType;
   typedef typename GM::IndexType IndexType;
   typedef opengm::LPCplex<GM, ACC> PyLPCplex;
   exportInfParam<PyLPCplex>("_LpCplex");
   // export inference
   class_< PyLPCplex>("_LpCplex",init<const GM & >())
   .def(InfSuite<PyLPCplex,false,true,false>(std::string("LpCplex"),setup))
   .def(LpInferenceSuite<PyLPCplex>())
   ;
   /*
   // more members
   .def("addConstraint", &pycplex::addConstraintPythonNumpy<PyLPCplex,ValueType,IndexType>  )
   .def("addConstraint", &pycplex::addConstraintPythonList<PyLPCplex,ValueType,IndexType>  )
   .def("addConstraints", &pycplex::addConstraintsPythonListListOrListNumpy<PyLPCplex,ValueType,IndexType>  )
   .def("addConstraints", &pycplex::addConstraintsPythonNumpy<PyLPCplex,ValueType,IndexType>  )
   .def("lpNodeVariableIndex",&PyLPCplex::lpNodeVi)
   .def("lpFactorVariableIndex",&pycplex::lpFactorIter<PyLPCplex>)
   .def("lpFactorVariableIndex",&pycplex::lpFactorViScalar<PyLPCplex>)
   ;
   */
}
// explicit template instantiation for the supported semi-rings
template void export_cplex<opengm::python::GmAdder, opengm::Minimizer>();
#endif


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

#include <algorithm>
#include <bit>
#include <cctype>
#include <cstring>
#include <boost/algorithm/string/predicate.hpp>
#include "WString.h"

#if !defined(__APPLE__) || (defined(__GNUG__) && !defined(__clang__))
#    define bit_width std::bit_width
#else
static std::size_t bit_width(unsigned long long value) {
    return std::numeric_limits<unsigned long long>::digits - __builtin_clzll(value);
}
#endif

String::String() noexcept = default;
String::String(const String&) = default;
String::String(String&&) noexcept = default;
String& String::operator=(const String&) = default;
String& String::operator=(String&&) noexcept = default;
String::~String() = default;

String::String(std::string u) : m_u{std::move(u)} {}

String::String(const char* cstr) : m_u{cstr} {}
String::String(char c) : m_u(1, c) {}

String::String(InternalTag, const char* ptr, std::size_t len) : m_u{ptr, len} {}

String::String(ConvTag, std::uintmax_t val, SMCE__BIN) {
    if (val == 0) {
        m_u = "0";
        return;
    }
    m_u.resize(bit_width(val));
    std::for_each(m_u.rbegin(), m_u.rend(), [&](char& c) {
        c = static_cast<char>((val & 1) + '0');
        val >>= 1;
    });
}

String::String(ConvTag, std::uintmax_t val, SMCE__HEX) {
    if (val == 0) {
        m_u = "0";
        return;
    }
    const auto bits = bit_width(val);
    m_u.resize(bits / 4 + static_cast<bool>(bits % 4));
    std::for_each(m_u.rbegin(), m_u.rend(), [&](char& c) {
        c = "0123456789ABCDEF"[val & 0xF];
        val >>= 4;
    });
}

[[nodiscard]] const char* String::c_str() const noexcept { return m_u.c_str(); }
[[nodiscard]] std::size_t String::length() const noexcept { return m_u.length(); }
[[nodiscard]] char String::charAt(unsigned idx) const noexcept { return m_u.at(idx); }
[[nodiscard]] char& String::charAt(unsigned idx) noexcept { return m_u.at(idx); }
[[nodiscard]] char String::operator[](unsigned idx) const noexcept { return m_u[idx]; }
[[nodiscard]] char& String::operator[](unsigned idx) noexcept { return m_u[idx]; }

[[nodiscard]] int String::compareTo(const String& s) const noexcept {
    return std::memcmp(m_u.c_str(), s.m_u.c_str(), (std::min)(s.m_u.size(), m_u.size()));
}

[[nodiscard]] bool String::startsWith(const String& s) const noexcept { return m_u.starts_with(s.m_u); }

[[nodiscard]] bool String::endsWith(const String& s) const noexcept { return m_u.ends_with(s.m_u); }

void String::getBytes(std::uint8_t* buffer, unsigned length) const noexcept {
    std::copy(m_u.begin(), (length > m_u.length()) ? m_u.end() : m_u.begin() + length, buffer);
}

[[nodiscard]] int String::indexOf(const char* c) const noexcept { return static_cast<int>(m_u.find(c)); }

[[nodiscard]] int String::indexOf(const char* c, unsigned index) const noexcept {
    return static_cast<int>(m_u.find(c, index));
}

[[nodiscard]] int String::indexOf(const String& str) const noexcept { return static_cast<int>(m_u.find(str.m_u)); }

[[nodiscard]] int String::indexOf(const String& str, unsigned index) const noexcept {
    return static_cast<int>(m_u.find(str.m_u, index));
}

void String::remove(unsigned idx) { m_u.erase(idx); }

void String::remove(unsigned idx, unsigned count) { m_u.erase(idx, idx + count - 1); }

void String::replace(const String& substring1, const String& substring2) {
    size_t position = m_u.find(substring1.m_u);

    while (position != std::string::npos) {
        m_u.replace(position, m_u.size(), substring2.m_u);
        position = m_u.find(substring1.m_u, position + substring2.m_u.size());
    }
}

void String::reserve(unsigned size) { m_u.reserve(size); }

void String::setCharAt(unsigned index, char c) { m_u[index] = c; }

[[nodiscard]] String String::substring(unsigned from) const { return m_u.substr(from); }

[[nodiscard]] String String::substring(unsigned from, unsigned to) const { return m_u.substr(from, to - from); }

void String::toCharArray(char* buffer, unsigned length) noexcept {
    std::memcpy(buffer, m_u.c_str(), (std::min)(static_cast<std::size_t>(length), m_u.length()));
}

[[nodiscard]] long String::toInt() const noexcept try { return std::stoi(m_u); } catch (const std::exception&) {
    return 0;
}

[[nodiscard]] double String::toDouble() const noexcept try { return std::stod(m_u); } catch (const std::exception&) {
    return 0;
}

[[nodiscard]] float String::toFloat() const noexcept try { return std::stof(m_u); } catch (const std::exception&) {
    return 0;
}

void String::toLowerCase() noexcept {
    std::transform(m_u.begin(), m_u.end(), m_u.begin(), [](char c) { return static_cast<char>(std::tolower(+c)); });
}

void String::toUpperCase() noexcept {
    std::transform(m_u.begin(), m_u.end(), m_u.begin(), [](char c) { return static_cast<char>(std::toupper(+c)); });
}

void String::trim() {
    if (const auto spos = m_u.find_first_not_of(' '); spos != std::string::npos && spos != 0)
        m_u.erase(m_u.begin(), m_u.begin() + spos);
    if (const auto lpos = m_u.find_last_not_of(' '); lpos != std::string::npos)
        m_u.erase(m_u.begin() + lpos + 1, m_u.end());
}

[[nodiscard]] bool String::equals(const String& s) const noexcept { return m_u == s.m_u; }

[[nodiscard]] bool String::equalsIgnoreCase(const String& s) const noexcept { return boost::iequals(m_u, s.m_u); }

[[nodiscard]] bool String::operator==(const String& s) const noexcept { return m_u == s.m_u; }
[[nodiscard]] bool String::operator!=(const String& s) const noexcept { return m_u != s.m_u; }
[[nodiscard]] bool String::operator<(const String& s) const noexcept { return m_u < s.m_u; }
[[nodiscard]] bool String::operator<=(const String& s) const noexcept { return m_u <= s.m_u; }
[[nodiscard]] bool String::operator>(const String& s) const noexcept { return m_u > s.m_u; }
[[nodiscard]] bool String::operator>=(const String& s) const noexcept { return m_u >= s.m_u; }

[[nodiscard]] String operator+(const String& lhs, const String& rhs) { return {lhs.m_u + rhs.m_u}; }
[[nodiscard]] String operator+(const String& lhs, const char* rhs) { return {lhs.m_u + rhs}; }
[[nodiscard]] String operator+(const char* lhs, const String& rhs) { return {lhs + rhs.m_u}; }