#include "bstracker.h"
#include "blurredsegmentproto.h"



const int BSTracker::DEFAULT_FAST_TRACK_MAX_WIDTH = 5;
const int BSTracker::DEFAULT_FINE_TRACK_MAX_WIDTH = 8;
const int BSTracker::DEFAULT_ACCEPTED_LACKS = 5;
const int BSTracker::NO_VICINITY = 10000;
const int BSTracker::DEFAULT_VICINITY_THRESHOLD = 10;
const int BSTracker::MIN_SCAN = 8;
const int BSTracker::DEFAULT_MAX_SCAN = 32;
const int BSTracker::DEFAULT_FITTING_DELAY = 20;

const int BSTracker::DEFAULT_THINNING_DELAY = 20;
const int BSTracker::DEFAULT_THINNING_SPEED = 2;
const int BSTracker::DEFAULT_THINNING_REACH = 50;
const int BSTracker::DEFAULT_THINNING_RESOLUTION = 1000;

const int BSTracker::FAILURE_NO_START = 1;
const int BSTracker::FAILURE_IMAGE_BOUND_ON_RIGHT = 2;
const int BSTracker::FAILURE_IMAGE_BOUND_ON_LEFT = 4;
const int BSTracker::FAILURE_LOST_ORIENTATION = 32;



BSTracker::BSTracker ()
{
  vicinityThreshold = NO_VICINITY + DEFAULT_VICINITY_THRESHOLD;
  imaxWidth = DEFAULT_FAST_TRACK_MAX_WIDTH;
  fmaxWidth = DEFAULT_FINE_TRACK_MAX_WIDTH;
  acceptedLacks = DEFAULT_ACCEPTED_LACKS;
  minRestart = acceptedLacks;

  dynamicScans = true;
  fittingDelay = DEFAULT_FITTING_DELAY;
  recordScans = false;
  orthoScan = false;

  thinningOn = true;
  thinningDelay = DEFAULT_THINNING_DELAY;
  thinningSpeed.set (DEFAULT_THINNING_SPEED, 100);
  thinningReach.set (100 + DEFAULT_THINNING_REACH, 100);
  gMap = NULL;

  maxScan = DEFAULT_MAX_SCAN;

  fail = 0;

  cand = new int[1]; // to avoid systematic tests
  lscan = 0;
  rscan = 0;
}


BSTracker::~BSTracker ()
{
  delete cand;
}


void BSTracker::clear ()
{
  scanBound1.clear ();
  scanBound2.clear ();
  scanLine.clear ();
}


void BSTracker::setGradientMap (VMap *data)
{
  gMap = data;
  scanp.setSize (gMap->getWidth (), gMap->getHeight ());
  delete cand;
  cand = new int[data->getHeightWidthMax ()];
}


BlurredSegment *BSTracker::fastTrack (const Pt2i &p1, const Pt2i &p2,
                                      Pt2i *p0)
{
  // Creates the scanner
  DirectionalScanner *ds = NULL;
  if (p0 != NULL)
    ds = scanp.getScanner (*p0, p1.vectorTo (p2), 4 * imaxWidth, false);
  else ds = scanp.getScanner (p1, p2);
  if (ds == NULL) return NULL;

  // Builds a BS builder around a central point
  vector<Pt2i> pix;
  if (ds->first (pix) < MIN_SCAN)
  {
    delete ds;
    return NULL;
  }

  if (recordScans)
  {
    scanBound1.push_back (pix.front ());
    scanBound2.push_back (pix.back ());
    scanLine.push_back (pix);
  }
  int candide;
  Pt2i pfirst;
  if (p0 != NULL) pfirst.set (p0->x (), p0->y ());
  else
  {
    candide = gMap->largestIn (pix);
    if (candide == -1)
    {
      delete ds;
      return NULL;
    }
    pfirst.set (pix.at (candide));
  }

  BlurredSegmentProto bs (imaxWidth, pfirst);
  Pt2i lastLeft (pfirst);
  Pt2i lastRight (pfirst);
  
  // Extends the segment
  int lstop = 0;
  int rstop = 0;
  int lstart = 0;
  int rstart = 0;
  bool added = false;
  bool scanningRight = true;
  bool scanningLeft = true;
  int fsCount = maxScan;

  while ((scanningRight || scanningLeft) && (fsCount--))
  {
    // Extends on right
    if (scanningRight)
    {
      if (ds->nextOnRight (pix) < MIN_SCAN) scanningRight = false;
      else
      {
        if (recordScans)
        {
          scanBound1.push_back (pix.front ());
          scanBound2.push_back (pix.back ());
          scanLine.push_back (pix);
        }
        added = false;
        candide = gMap->largestIn (pix);
        if (candide != -1)
        {
          if (lastRight.manhattan (pix.at (candide)) <= vicinityThreshold)
            added = bs.addRight (pix.at (candide));
        }
        if (added)
        {
          lastRight = pix.at (candide);
          if (rstop != 0) rstop = 0;
        }
        else if (++rstop > acceptedLacks)
          scanningRight = false;
      }
    }
    // Extends on left
    if (scanningLeft)
    {
      if (ds->nextOnLeft (pix) < MIN_SCAN) scanningLeft = false;
      else
      {
        if (recordScans)
        {
          scanBound1.push_back (pix.front ());
          scanBound2.push_back (pix.back ());
          scanLine.push_back (pix);
        }
        added = false;
        candide = gMap->largestIn (pix);
        if (candide != -1)
        {
          if (lastLeft.manhattan (pix.at (candide)) < vicinityThreshold)
            added = bs.addLeft (pix.at (candide));
        }
        if (added)
        {
          lastLeft = pix.at (candide);
          if (lstop != 0) lstop = 0;
        }
        else if (++lstop > acceptedLacks)
          scanningLeft = false;
      }
    }
  }
  if (rstart) bs.removeRight (rstart);
  if (lstart) bs.removeLeft (lstart);
  delete ds;
  return (bs.endOfBirth ());
}



BlurredSegment *BSTracker::fineTrack (const Pt2i &center, const Vr2i &scandir,
                                      int scanwidth, int bswidth,
                                      const Vr2i &gref)
{
  // Checks scan width minimal size
  if (scanwidth < MIN_SCAN) scanwidth = MIN_SCAN;

  // Gets detected segment normal vector
  Vr2i normal = scandir.orthog ();
  if (! normal.orientedAs (gref)) normal.invert ();

  fail = 0;

  // Creation of the directional scanner and the array of candidates
  DirectionalScanner *ds = scanp.getScanner (center, normal,
                                             scanwidth, dynamicScans);
  if (ds == NULL)
  {
    fail = FAILURE_NO_START;
    return NULL;
  }

  // Looking for a central point
  vector<Pt2i> pix;
  if (ds->first (pix) < MIN_SCAN)
  {
    delete ds;
    fail = FAILURE_NO_START;
    return NULL;
  }
  if (recordScans)
  {
    scanBound1.push_back (pix.front ());
    scanBound2.push_back (pix.back ());
    scanLine.push_back (pix);
  }
  int nbc = gMap->localMax (cand, pix, normal);
  if (nbc == 0)
  {
    delete ds;
    fail = FAILURE_NO_START;
    return NULL;
  }

  BlurredSegmentProto bs (bswidth, pix[cand[0]]);

  // Handles thinning
  int count = 0;
  AbsRat maxw (bswidth * DEFAULT_THINNING_RESOLUTION,
               DEFAULT_THINNING_RESOLUTION);

  // Extends the segment
  lscan = 0;
  rscan = 0;
  int lstop = 0;
  int rstop = 0;
  int lstart = 0;
  int rstart = 0;
  bool added = false;
  bool scanningRight = true;
  bool scanningLeft = true;
  bool thon = thinningOn;

  while (scanningRight || scanningLeft)
  {
    count ++;

    // Handles thinning
    if (thon)
    {
      if (count > thinningDelay)
      {
        AbsRat sw = bs.segmentRationalWidth ();
        AbsRat oldmaxw (maxw);
        maxw.attractsTo (sw, thinningSpeed);
        sw.mult (thinningReach);
        if (maxw.lessThan (sw))
        {
          maxw.sticksTo (sw);
          if (oldmaxw.lessThan (maxw)) maxw.set (oldmaxw);
          thon = false;  // thinning deactivation
        }
        bs.setMaxWidth (maxw);
      }
    }

    // Resets the scan stripe
    if (dynamicScans && count > fittingDelay && bs.getLine () != NULL)
    {
      // Stops the detection if the segment gets crosswise
      if (count == fittingDelay + 1)
      {
        Vr2i dirn = bs.getSupportVector ();
        if (4 * dirn.squaredScalarProduct (scandir)
            < dirn.norm2 () * scandir.norm2 ())
        {
          scanningLeft = false;
          scanningRight = false;
          fail += FAILURE_LOST_ORIENTATION;
        }
      }
      int ppa, ppb, ppc;
      bs.getLine()->getMedialAxis (ppa, ppb, ppc);
      ds->bindTo (ppa, ppb, ppc);
    }

    // Extends on right
    if (scanningRight)
    {
      added = false;
      if (ds->nextOnRight (pix) < MIN_SCAN)
      {
        fail += FAILURE_IMAGE_BOUND_ON_RIGHT;
        scanningRight = false;
      }
      else
      {
        if (recordScans)
        {
          scanBound1.push_back (pix.front ());
          scanBound2.push_back (pix.back ());
          scanLine.push_back (pix);
        }
        added = false;
        nbc = gMap->localMax (cand, pix, normal);
        for (int i = 0; ! added && i < nbc; i++)
          added = bs.addRight (pix[cand[i]]);
        if (added)
        {
          rscan = count;
          if (rstop == 0) rstart = 0;
          else
          {
            rstart ++;
            //if (rstart >= minRestart || rstart >= rstop)
            if (rstart >= rstop)
            {
              rstop = 0;
              rstart = 0;
            }
          }
        }
        else
        {
          if (++rstop - rstart > acceptedLacks)
          {
            if (bs.size () <= 3) fail = FAILURE_NO_START;
            scanningRight = false;
          }
        }
      }
    }

    // Extends on left
    if (scanningLeft)
    {
      if (ds->nextOnLeft (pix) < MIN_SCAN)
      {
        fail += FAILURE_IMAGE_BOUND_ON_LEFT;
        scanningLeft = false;
      }
      else
      {
        if (recordScans)
        {
          scanBound1.push_back (pix.front ());
          scanBound2.push_back (pix.back ());
          scanLine.push_back (pix);
        }
        added = false;
        nbc = gMap->localMax (cand, pix, normal);
        for (int i = 0; ! added && i < nbc; i++)
          added = bs.addLeft (pix[cand[i]]);
        if (added)
        {
          lscan = count;
          if (lstop == 0) lstart = 0;
          else
          {
            lstart ++;
            //if (lstart >= minRestart || lstart >= lstop)
            if (lstart >= lstop)
            {
              lstop = 0;
              lstart = 0;
            }
          }
        }
        else
        {
          if (++lstop - lstart > acceptedLacks)
          {
            if (bs.size () <= 3) fail = FAILURE_NO_START;
            scanningLeft = false;
          }
        }
      }
    }
  }
  if (rstart) bs.removeRight (rstart);
  if (lstart) bs.removeLeft (lstart);
  delete ds;
  return (bs.endOfBirth ());
}


void BSTracker::switchOrthoScans ()
{
  orthoScan = ! orthoScan;
  scanp.setOrtho (orthoScan);
}


void BSTracker::switchScanExtent ()
{
  maxScan = (maxScan == gMap->getHeightWidthMax () ?
             DEFAULT_MAX_SCAN : gMap->getHeightWidthMax ());
}