#include "bsdetector.h"



const int BSDetector::RESULT_UNDETERMINED = -1;
const int BSDetector::RESULT_OK = 0;
const int BSDetector::RESULT_INITIAL_NO_DETECTION = 11;
const int BSDetector::RESULT_INITIAL_TOO_FEW = 12;
const int BSDetector::RESULT_INITIAL_TOO_SPARSE = 13;
const int BSDetector::RESULT_INITIAL_TOO_MANY_OUTLIERS = 14;
const int BSDetector::RESULT_FINAL_NO_DETECTION = 21;
const int BSDetector::RESULT_FINAL_TOO_FEW = 22;
const int BSDetector::RESULT_FINAL_TOO_SPARSE = 23;
const int BSDetector::RESULT_FINAL_TOO_MANY_OUTLIERS = 24;

const int BSDetector::DEFAULT_BS_MIN_SIZE = 5;
const int BSDetector::ABSOLUTE_BS_MIN_SIZE = 3;
const int BSDetector::DEFAULT_CONNECT_MIN_SIZE = 5;
const int BSDetector::DEFAULT_AUTO_RESOLUTION = 10;


BSDetector::BSDetector ()
{
  gMap = NULL;

  bst1 = new BSTracker ();
  bst1->setPixelLackTolerence (bst1->proximityThreshold ());
  bst2 = new BSTracker ();

  prefilteringOn = true;
  lsf1 = (prefilteringOn ? new LineSpaceFilter () : NULL);
  filteringOn = false;
  lsf2 = (filteringOn ? new LineSpaceFilter () : NULL);

  edgeDirection = 0;   // detects line (not only edges)
  bsMinSize = DEFAULT_BS_MIN_SIZE;
  connectMinSize = DEFAULT_CONNECT_MIN_SIZE;
  minScanLength = bst1->defaultMinScan ();
  autowidth = true;
  ccOn = true;
  densityTestOn = true;
  multiSelection = false;
  autoResol = DEFAULT_AUTO_RESOLUTION;

  bsini = NULL;
  bsf = NULL;
  resultValue = RESULT_UNDETERMINED;
}


BSDetector::~BSDetector ()
{
  delete bst1;
  delete bst2;
  if (lsf1 != NULL) delete lsf1;
  if (lsf2 != NULL) delete lsf2;
  if (bsini != NULL) delete bsini;
  if (bsf != NULL) delete bsf;
  vector<BlurredSegment *>::iterator it = mbsini.begin ();
  while (it != mbsini.end ()) delete (*it++);
  it = mbsf.begin ();
  while (it != mbsf.end ()) delete (*it++);
}


void BSDetector::setGradientMap (VMap *data)
{
  gMap = data;
  bst1->setGradientMap (data);
  bst2->setGradientMap (data);
}


void BSDetector::detectAll ()
{
  // Deletes former blurred segments
  vector<BlurredSegment *>::iterator it = mbsini.begin ();
  while (it != mbsini.end ()) delete (*it++);
  mbsini.clear ();
  it = mbsf.begin ();
  while (it != mbsf.end ()) delete (*it++);
  mbsf.clear ();

  gMap->setMasking (true);

  int width = gMap->getWidth ();
  int height = gMap->getHeight ();
  for (int x = width / 2; x > 0; x -= autoResol)
    runMultiDetection (Pt2i (x, 0), Pt2i (x, height - 1), Vr2i (0, 1));
  for (int x = width / 2 + autoResol; x < width - 1; x += autoResol)
    runMultiDetection (Pt2i (x, 0), Pt2i (x, height - 1), Vr2i (0, 1));
  for (int y = height / 2; y > 0; y -= autoResol)
    runMultiDetection (Pt2i (0, y), Pt2i (width - 1, y), Vr2i (1, 0));
  for (int y = height / 2 + autoResol; y < height - 1; y += autoResol)
    runMultiDetection (Pt2i (0, y), Pt2i (width - 1, y), Vr2i (1, 0));

  gMap->clearMask ();
  gMap->setMasking (false);
}


void BSDetector::multidetect (const Pt2i &p1, const Pt2i &p2)
{
  // Deletes former blurred segments
  vector<BlurredSegment *>::iterator it = mbsini.begin ();
  while (it != mbsini.end ()) delete (*it++);
  mbsini.clear ();
  it = mbsf.begin ();
  while (it != mbsf.end ()) delete (*it++);
  mbsf.clear ();

  gMap->setMasking (true);
  runMultiDetection (p1, p2, p1.vectorTo (p2));
  gMap->clearMask ();
  gMap->setMasking (false);
}


void BSDetector::runMultiDetection (const Pt2i &p1, const Pt2i &p2,
                                    const Vr2i &dir)
{
  vector<Pt2i> pts;
  p1.draw (pts, p2);
  int locmax[pts.size ()];
  gMap->releaseOrientationConstraint ();
  int nlm = gMap->localMax (locmax, pts, dir);
  gMap->restoreOrientationConstraint ();
  for (int i = 0; i < nlm; i++)
  {
    detect (p1, p2, &(pts.at (locmax[i])));
    if (bsf != NULL)
    {
      gMap->setMask (bsf->getAllPoints ());
      mbsf.push_back (bsf);
      mbsini.push_back (bsini);
      bsf = NULL; // to avoid BS deletion
      bsini = NULL; // to avoid BS deletion
    }
  }
}


void BSDetector::detect (const Pt2i &p1, const Pt2i &p2, Pt2i *p0)
{
  // Clearance
  //----------
  resultValue = RESULT_UNDETERMINED;
  bst1->clear ();
  bst2->clear ();
  if (prefilteringOn) lsf1->clear ();
  if (filteringOn) lsf2->clear ();
  if (bsini != NULL) delete bsini;
  if (bsf != NULL) delete bsf;
  bsf = NULL;
  if (p1.equals (p2)) return;

  // Initial detection based on highest gradient without orientation constraint
  //---------------------------------------------------------------------------
  bsini = bst1->fastTrack (p1, p2, p0);
  if (bsini == NULL || bsini->size () < bsMinSize)
  {
    resultValue = (bsini == NULL ? RESULT_INITIAL_NO_DETECTION
                                 : RESULT_INITIAL_TOO_FEW);
    return;
  }

  // Density test
  //-------------
  if (densityTestOn)
  {
    DigitalStraightLine mydsl (p1, p2, DigitalStraightLine::DSL_NAIVE);
    int mydrlf = mydsl.manhattan (bsini->getLastRight ())
                 - mydsl.manhattan (bsini->getLastLeft ());
    if (mydrlf < 0) mydrlf = -mydrlf; // Case of horizontal P1P2
    int expansion = 1 + mydrlf;
    if (bsini->size () < expansion / 2)
    {
      resultValue = RESULT_INITIAL_TOO_SPARSE;
      return;
    }
  }

  // Hough filtering on the initial segment
  //---------------------------------------
  if (prefilteringOn)
  {
    BlurredSegment *fbs = lsf1->filter (bsini);
    if (fbs != NULL)
    {
      delete bsini;
      bsini = fbs;
    }
  }
  if (bsini->size () < bsMinSize)
  {
    resultValue = RESULT_INITIAL_TOO_MANY_OUTLIERS;
    return;
  }

  // Finer detection based on gradient maxima with orientation constraint
  //---------------------------------------------------------------------
  Pt2i pCenter = bsini->getCenter ();
  Vr2i gRef = gMap->getValue (pCenter.x (), pCenter.y ());
  if (edgeDirection == -1) gRef.invert ();
  int bswidth = bst1->inputMaxWidth ();
  int scanwidth2 = 4 * bswidth;
  if (autowidth)
  {
    bswidth = minScanLength;
    DigitalStraightSegment *dss = bsini->getSegment ();
    if (dss != NULL)
    {
      bswidth = (dss->width () / dss->period ());
      if (bswidth < minScanLength) bswidth = minScanLength;
    }
    scanwidth2 = bswidth;
    pCenter = bsini->getSegment()->centerOfIntersection (p1, p2);
  }
  bsf = bst2->fineTrack (pCenter, bsini->getSupportVector(),
                         scanwidth2, bswidth, gRef);

  if (bsf == NULL || bsf->size () < bsMinSize)
  {
    resultValue = (bsf == NULL ? RESULT_FINAL_NO_DETECTION
                               : RESULT_FINAL_TOO_FEW);
    return;
  }

  // Connected components analysis */
  //------------------------------*/
  if (ccOn)
  {
    int bsccp = bsf->countOfConnectedPoints (connectMinSize);
    int bssize = bsf->getAllPoints().size ();
    if (bsccp < bssize / 2)
    {
      resultValue = RESULT_FINAL_TOO_SPARSE;
      delete bsf;
      bsf = NULL;
      return;
    }
  }

  // Line space filtering
  //---------------------
  if (filteringOn)
  {
    BlurredSegment *fbsf = lsf2->filter (bsf);
    if (fbsf != NULL)
    {
      resultValue = RESULT_FINAL_TOO_MANY_OUTLIERS;
      delete bsf;
      bsf = fbsf;
    }
  }

  resultValue = RESULT_OK;
}



void BSDetector::switchOrthoScans ()
{
  bst1->switchOrthoScans ();
  bst2->switchOrthoScans ();
}


vector<Pt2i> BSDetector::getRejected (int step) const
{
  vector<Pt2i> res;
  if (step != 0)
  {
    if (filteringOn) res = lsf2->getRejected ();
  }
  else if (prefilteringOn) res = lsf1->getRejected ();
  return res;
}


void BSDetector::switchFiltering (int step)
{
  if (step != 0)
  {
    filteringOn = ! filteringOn;
    if (filteringOn && lsf2 == NULL) lsf2 = new LineSpaceFilter ();
  }
  else
  {
    prefilteringOn = ! prefilteringOn;
    if (prefilteringOn && lsf1 == NULL) lsf1 = new LineSpaceFilter ();
  }
}


bool BSDetector::incConnectedComponentMinSize (bool increase)
{
  if ((! increase) && connectMinSize <= 1) return false;
  connectMinSize += (increase ? 1 : -1);
  return true;
}