#include <assert.h>
#include <math.h>
#include <string.h>

#include "NPBGSubtractor.h"

//#ifdef _DEBUG
//#undef THIS_FILE
//static char THIS_FILE[]=__FILE__;
//#define new DEBUG_NEW
//#endif
//using namespace bgslibrary::algorithms::kde;

namespace bgslibrary
{
  namespace algorithms
  {
    namespace kde
    {
      void BGR2SnGnRn(unsigned char * in_image,
        unsigned char * out_image,
        unsigned int rows,
        unsigned int cols)
      {
        unsigned int i;
        unsigned int r2, r3;
        unsigned int r, g, b;
        double s;

        for (i = 0; i < rows*cols * 3; i += 3)
        {
          b = in_image[i];
          g = in_image[i + 1];
          r = in_image[i + 2];

          // calculate color ratios
          s = (double)255 / (double)(b + g + r + 30);

          r2 = (unsigned int)((g + 10) * s);
          r3 = (unsigned int)((r + 10) * s);

          out_image[i] = (unsigned char)(((unsigned int)b + g + r) / 3);
          out_image[i + 1] = (unsigned char)(r2 > 255 ? 255 : r2);
          out_image[i + 2] = (unsigned char)(r3 > 255 ? 255 : r3);
        }
      }

      void UpdateDiffHist(unsigned char * image1, unsigned char * image2, DynamicMedianHistogram * pHist)
      {
        unsigned int j;
        int bin, diff;

        unsigned int  imagesize = pHist->imagesize;
        unsigned char histbins = pHist->histbins;
        unsigned char *pAbsDiffHist = pHist->Hist;

        int histbins_1 = histbins - 1;

        for (j = 0; j < imagesize; j++)
        {
          diff = (int)image1[j] - (int)image2[j];
          diff = abs(diff);
          // update histogram
          bin = (diff < histbins ? diff : histbins_1);
          pAbsDiffHist[j*histbins + bin]++;
        }
      }

      void FindHistMedians(DynamicMedianHistogram * pAbsDiffHist)
      {
        unsigned char * Hist = pAbsDiffHist->Hist;
        unsigned char * MedianBins = pAbsDiffHist->MedianBins;
        unsigned char * AccSum = pAbsDiffHist->AccSum;
        unsigned char histsum = pAbsDiffHist->histsum;
        unsigned char histbins = pAbsDiffHist->histbins;
        unsigned int imagesize = pAbsDiffHist->imagesize;

        int sum;
        int bin;
        unsigned int histindex;
        unsigned char medianCount = histsum / 2;
        unsigned int j;

        // find medians
        for (j = 0; j < imagesize; j++)
        {
          // find the median
          bin = 0;
          sum = 0;

          histindex = j*histbins;

          while (sum < medianCount)
          {
            sum += Hist[histindex + bin];
            bin++;
          }

          bin--;

          MedianBins[j] = bin;
          AccSum[j] = sum;
        }
      }

      DynamicMedianHistogram BuildAbsDiffHist(unsigned char * pSequence,
        unsigned int rows,
        unsigned int cols,
        unsigned int color_channels,
        unsigned int SequenceLength,
        unsigned int histbins)
      {

        unsigned int imagesize = rows*cols*color_channels;
        unsigned int i;

        DynamicMedianHistogram Hist;

        unsigned char *pAbsDiffHist = new unsigned char[rows*cols*color_channels*histbins];
        unsigned char *pMedianBins = new unsigned char[rows*cols*color_channels];
        unsigned char *pMedianFreq = new unsigned char[rows*cols*color_channels];
        unsigned char *pAccSum = new unsigned char[rows*cols*color_channels];

        memset(pAbsDiffHist, 0, rows*cols*color_channels*histbins);

        Hist.Hist = pAbsDiffHist;
        Hist.MedianBins = pMedianBins;
        Hist.MedianFreq = pMedianFreq;
        Hist.AccSum = pAccSum;
        Hist.histbins = histbins;
        Hist.imagesize = rows*cols*color_channels;
        Hist.histsum = SequenceLength - 1;

        unsigned char *image1, *image2;
        for (i = 1; i < SequenceLength; i++)
        {
          // find diff between frame i,i-1;
          image1 = pSequence + (i - 1)*imagesize;
          image2 = pSequence + (i)*imagesize;

          UpdateDiffHist(image1, image2, &Hist);
        }

        FindHistMedians(&Hist);

        return Hist;
      }

      void EstimateSDsFromAbsDiffHist(DynamicMedianHistogram * pAbsDiffHist,
        unsigned char * pSDs,
        unsigned int imagesize,
        double MinSD,
        double MaxSD,
        unsigned int kernelbins)
      {
        double v;
        double kernelbinfactor = (kernelbins - 1) / (MaxSD - MinSD);
        int medianCount;
        int sum;
        int bin;
        unsigned int histindex;
        unsigned int j;
        unsigned int x1, x2;

        unsigned char *Hist = pAbsDiffHist->Hist;
        unsigned char *MedianBins = pAbsDiffHist->MedianBins;
        unsigned char *AccSum = pAbsDiffHist->AccSum;
        unsigned char histsum = pAbsDiffHist->histsum;
        unsigned char histbins = pAbsDiffHist->histbins;

        medianCount = (histsum) / 2;

        for (j = 0; j < imagesize; j++)
        {
          histindex = j*histbins;

          bin = MedianBins[j];
          sum = AccSum[j];

          x1 = sum - Hist[histindex + bin];
          x2 = sum;

          // interpolate to get the median
          // x1 < 50 % < x2

          v = 1.04 * ((double)bin - (double)(x2 - medianCount) / (double)(x2 - x1));
          v = (v <= MinSD ? MinSD : v);

          // convert sd to kernel table bin

          bin = (int)(v >= MaxSD ? kernelbins - 1 : floor((v - MinSD)*kernelbinfactor + .5));

          assert(bin >= 0 && bin < kernelbins);

          pSDs[j] = bin;
        }
      }

      //////////////////////////////////////////////////////////////////////
      // Construction/Destruction
      //////////////////////////////////////////////////////////////////////

      NPBGSubtractor::NPBGSubtractor() {}

      NPBGSubtractor::~NPBGSubtractor()
      {
        delete AbsDiffHist.Hist;
        delete AbsDiffHist.MedianBins;
        delete AbsDiffHist.MedianFreq;
        delete AbsDiffHist.AccSum;
        delete KernelTable;
        delete BGModel->SDbinsImage;
        delete BGModel;
        delete Pimage1;
        delete Pimage2;
        delete tempFrame;
        delete imageindex->List;
        delete imageindex;
      }

      int NPBGSubtractor::Intialize(unsigned int prows,
        unsigned int pcols,
        unsigned int pcolor_channels,
        unsigned int SequenceLength,
        unsigned int pTimeWindowSize,
        unsigned char pSDEstimationFlag,
        unsigned char pUseColorRatiosFlag)
      {

        rows = prows;
        cols = pcols;
        color_channels = pcolor_channels;
        imagesize = rows*cols*color_channels;
        SdEstimateFlag = pSDEstimationFlag;
        UseColorRatiosFlag = pUseColorRatiosFlag;
        //SampleSize = SequenceLength;

        AdaptBGFlag = FALSE;
        //
        SubsetFlag = TRUE;

        UpdateSDRate = 0;

        BGModel = new NPBGmodel(rows, cols, color_channels, SequenceLength, pTimeWindowSize, 500);

        Pimage1 = new double[rows*cols];
        Pimage2 = new double[rows*cols];

        tempFrame = new unsigned char[rows*cols * 3];

        imageindex = new ImageIndex;
        imageindex->List = new unsigned int[rows*cols];

        // error checking
        if (BGModel == NULL)
          return 0;

        return 1;
      }

      void NPBGSubtractor::AddFrame(unsigned char *ImageBuffer)
      {
        if (UseColorRatiosFlag && color_channels == 3)
          BGR2SnGnRn(ImageBuffer, ImageBuffer, rows, cols);

        BGModel->AddFrame(ImageBuffer);
      }

      void NPBGSubtractor::Estimation()
      {
        int SampleSize = BGModel->SampleSize;

        memset(BGModel->TemporalMask, 0, rows*cols*BGModel->TemporalBufferLength);

        //BGModel->AccMask= new unsigned int [rows*cols];
        memset(BGModel->AccMask, 0, rows*cols * sizeof(unsigned int));

        unsigned char *pSDs = new unsigned char[rows*cols*color_channels];

        //DynamicMedianHistogram AbsDiffHist;

        int Abshistbins = 20;

        TimeIndex = 0;

        // estimate standard deviations

        if (SdEstimateFlag)
        {
          AbsDiffHist = BuildAbsDiffHist(BGModel->Sequence, rows, cols, color_channels, SampleSize, Abshistbins);
          EstimateSDsFromAbsDiffHist(&AbsDiffHist, pSDs, imagesize, SEGMAMIN, SEGMAMAX, SEGMABINS);
        }
        else
        {
          unsigned int bin;
          bin = (unsigned int)floor(((DEFAULTSEGMA - SEGMAMIN)*SEGMABINS) / (SEGMAMAX - SEGMAMIN));
          memset(pSDs, bin, rows*cols*color_channels * sizeof(unsigned char));
        }

        BGModel->SDbinsImage = pSDs;

        // Generate the Kernel
        KernelTable = new KernelLUTable(KERNELHALFWIDTH, SEGMAMIN, SEGMAMAX, SEGMABINS);
      }

      /*********************************************************************/

      void BuildImageIndex(unsigned char * Image,
        ImageIndex * imageIndex,
        unsigned int rows,
        unsigned int cols)
      {
        unsigned int i, j;
        unsigned int r, c;
        unsigned int * image_list;

        j = cols + 1;
        i = 0;
        image_list = imageIndex->List;

        for (r = 1; r < rows - 1; r++)
        {
          for (c = 1; c < cols - 1; c++)
          {
            if (Image[j])
              image_list[i++] = j;

            j++;
          }
          j += 2;
        }

        imageIndex->cnt = i;
      }

      /*********************************************************************/

      void HystExpandOperatorIndexed(unsigned char * inImage,
        ImageIndex * inIndex,
        double * Pimage,
        double hyst_th,
        unsigned char * outImage,
        ImageIndex * outIndex,
        unsigned int urows,
        unsigned int ucols)
      {
        unsigned int * in_list;
        unsigned int in_cnt;
        // unsigned int * out_list;

        int rows, cols;

        int Nbr[9];
        unsigned int i, j;
        // unsigned int k;
        unsigned int idx;

        rows = (int)urows;
        cols = (int)ucols;

        in_cnt = inIndex->cnt;
        in_list = inIndex->List;

        Nbr[0] = -cols - 1;
        Nbr[1] = -cols;
        Nbr[2] = -cols + 1;
        Nbr[3] = -1;
        Nbr[4] = 0;
        Nbr[5] = 1;
        Nbr[6] = cols - 1;
        Nbr[7] = cols;
        Nbr[8] = cols + 1;

        memset(outImage, 0, rows*cols);

        // out_list = outIndex->List;
        // k = 0;

        for (i = 0; i < in_cnt; i++)
        {
          for (j = 0; j < 9; j++)
          {
            idx = in_list[i] + Nbr[j];

            if (Pimage[idx] < hyst_th)
              outImage[idx] = 255;
          }
        }

        // build index for out image
        BuildImageIndex(outImage, outIndex, urows, ucols);
      }

      /*********************************************************************/

      void HystShrinkOperatorIndexed(unsigned char * inImage,
        ImageIndex * inIndex,
        double * Pimage,
        double hyst_th,
        unsigned char * outImage,
        ImageIndex * outIndex,
        unsigned int urows,
        unsigned int ucols)
      {
        unsigned int * in_list;
        unsigned int in_cnt;
        // unsigned int * out_list;

        int rows, cols;

        int Nbr[9];
        unsigned int i, j;
        unsigned int idx; // k

        rows = (int)urows;
        cols = (int)ucols;

        in_cnt = inIndex->cnt;
        in_list = inIndex->List;

        Nbr[0] = -cols - 1;
        Nbr[1] = -cols;
        Nbr[2] = -cols + 1;
        Nbr[3] = -1;
        Nbr[4] = 0;
        Nbr[5] = 1;
        Nbr[6] = cols - 1;
        Nbr[7] = cols;
        Nbr[8] = cols + 1;

        memset(outImage, 0, rows*cols);

        // out_list = outIndex->List;
        // k = 0;

        for (i = 0; i < in_cnt; i++)
        {
          idx = in_list[i];
          j = 0;

          while (j < 9 && inImage[idx + Nbr[j]])
            j++;

          if (j >= 9 || Pimage[idx] <= hyst_th)
            outImage[idx] = 255;
        }

        BuildImageIndex(outImage, outIndex, rows, cols);
      }

      /*********************************************************************/

      void ExpandOperatorIndexed(unsigned char * inImage,
        ImageIndex * inIndex,
        unsigned char * outImage,
        ImageIndex * outIndex,
        unsigned int urows,
        unsigned int ucols)
      {
        unsigned int * in_list;
        unsigned int in_cnt;
        // unsigned int * out_list;

        int rows, cols;

        int Nbr[9];
        unsigned int i, j;
        // unsigned int k;
        unsigned int idx;

        rows = (int)urows;
        cols = (int)ucols;

        in_cnt = inIndex->cnt;
        in_list = inIndex->List;

        Nbr[0] = -cols - 1;
        Nbr[1] = -cols;
        Nbr[2] = -cols + 1;
        Nbr[3] = -1;
        Nbr[4] = 0;
        Nbr[5] = 1;
        Nbr[6] = cols - 1;
        Nbr[7] = cols;
        Nbr[8] = cols + 1;


        memset(outImage, 0, rows*cols);


        // out_list = outIndex->List;
        // k = 0;
        for (i = 0; i < in_cnt; i++)
          for (j = 0; j < 9; j++) {
            idx = in_list[i] + Nbr[j];
            outImage[idx] = 255;
          }


        // build index for out image

        BuildImageIndex(outImage, outIndex, rows, cols);

      }

      /*********************************************************************/

      void ShrinkOperatorIndexed(unsigned char * inImage,
        ImageIndex * inIndex,
        unsigned char * outImage,
        ImageIndex * outIndex,
        unsigned int urows,
        unsigned int ucols)
      {

        unsigned int * in_list;
        unsigned int in_cnt;
        // unsigned int * out_list;

        int rows, cols;

        int Nbr[9];
        unsigned int i, j;
        unsigned int idx; // k

        rows = (int)urows;
        cols = (int)ucols;

        in_cnt = inIndex->cnt;
        in_list = inIndex->List;

        Nbr[0] = -cols - 1;
        Nbr[1] = -cols;
        Nbr[2] = -cols + 1;
        Nbr[3] = -1;
        Nbr[4] = 0;
        Nbr[5] = 1;
        Nbr[6] = cols - 1;
        Nbr[7] = cols;
        Nbr[8] = cols + 1;


        memset(outImage, 0, rows*cols);

        // out_list = outIndex->List;
        // k = 0;
        for (i = 0; i < in_cnt; i++) {
          idx = in_list[i];
          j = 0;

          while (j < 9 && inImage[idx + Nbr[j]]) {
            j++;
          }

          if (j >= 9) {
            outImage[idx] = 255;
          }
        }

        BuildImageIndex(outImage, outIndex, rows, cols);
      }

      /*********************************************************************/

      void NoiseFilter_o(unsigned char * Image,
        unsigned char * ResultIm,
        int rows,
        int cols,
        unsigned char th)
      {
        /* assuming input is 1 for on, 0 for off */


        int r, c;
        unsigned char *p, *n, *nw, *ne, *e, *w, *s, *sw, *se;
        unsigned int v;
        unsigned int TH;

        unsigned char * ResultPtr;

        TH = 255 * th;

        memset(ResultIm, 0, rows*cols);

        p = Image + cols + 1;
        ResultPtr = ResultIm + cols + 1;

        for (r = 1; r < rows - 1; r++)
        {
          for (c = 1; c < cols - 1; c++)
          {
            if (*p)
            {
              n = p - cols;
              ne = n + 1;
              nw = n - 1;
              e = p + 1;
              w = p - 1;
              s = p + cols;
              se = s + 1;
              sw = s - 1;

              v = (unsigned int)*nw + *n + *ne + *w + *e + *sw + *s + *se;

              if (v >= TH)
                *ResultPtr = 255;
              else
                *ResultPtr = 0;
            }
            p++;
            ResultPtr++;
          }
          p += 2;
          ResultPtr += 2;
        }
      }

      /*********************************************************************/

      void NPBGSubtractor::SequenceBGUpdate_Pairs(unsigned char * image,
        unsigned char * Mask)
      {
        unsigned int i, ic;
        unsigned char * pSequence = BGModel->Sequence;
        unsigned char * PixelQTop = BGModel->PixelQTop;
        //unsigned int Top = BGModel->Top;
        unsigned int rate;

        int TemporalBufferTop = (int)BGModel->TemporalBufferTop;
        unsigned char * pTemporalBuffer = BGModel->TemporalBuffer;
        unsigned char * pTemporalMask = BGModel->TemporalMask;
        int TemporalBufferLength = BGModel->TemporalBufferLength;

        unsigned int * AccMask = BGModel->AccMask;
        unsigned int ResetMaskTh = BGModel->ResetMaskTh;

        unsigned char *pAbsDiffHist = AbsDiffHist.Hist;
        unsigned char histbins = AbsDiffHist.histbins;
        int histbins_1 = histbins - 1;

        int TimeWindowSize = BGModel->TimeWindowSize;
        int SampleSize = BGModel->SampleSize;

        int TemporalBufferNext;

        unsigned int imagebuffersize = rows*cols*color_channels;
        unsigned int imagespatialsize = rows*cols;

        unsigned char mask;

        unsigned int histindex;
        unsigned char diff;
        unsigned char bin;

        static int TBCount = 0;

        unsigned char * pTBbase1, *pTBbase2;
        unsigned char * pModelbase1, *pModelbase2;

        rate = TimeWindowSize / SampleSize;
        rate = (rate > 2) ? rate : 2;


        TemporalBufferNext = (TemporalBufferTop + 1)
          % TemporalBufferLength;

        // pointers to Masks : Top and Next
        unsigned char * pTMaskTop = pTemporalMask + TemporalBufferTop*imagespatialsize;
        unsigned char * pTMaskNext = pTemporalMask + TemporalBufferNext*imagespatialsize;

        // pointers to TB frames: Top and Next
        unsigned char * pTBTop = pTemporalBuffer + TemporalBufferTop*imagebuffersize;
        unsigned char * pTBNext = pTemporalBuffer + TemporalBufferNext*imagebuffersize;

        if (((TimeIndex) % rate == 0) && TBCount >= TemporalBufferLength)
        {
          for (i = 0, ic = 0; i < imagespatialsize; i++, ic += color_channels)
          {
            mask = *(pTMaskTop + i) | *(pTMaskNext + i); // mask = *(pTMaskTop + i) || *(pTMaskNext + i);

            if (!mask)
            {
              // pointer to TB pixels to be added to the model
              pTBbase1 = pTBTop + ic;
              pTBbase2 = pTBNext + ic;

              // pointers to Model pixels to be replaced
              pModelbase1 = pSequence + PixelQTop[i] * imagebuffersize + ic;
              pModelbase2 = pSequence + ((PixelQTop[i] + 1) % SampleSize)*imagebuffersize + ic;

              // update Deviation Histogram
              if (SdEstimateFlag)
              {
                if (color_channels == 1)
                {
                  histindex = i*histbins;

                  // add new pair from temporal buffer
                  diff = (unsigned char)abs((int)*pTBbase1 - (int)*pTBbase2);
                  bin = (diff < histbins ? diff : histbins_1);
                  pAbsDiffHist[histindex + bin]++;


                  // remove old pair from the model
                  diff = (unsigned char)abs((int)*pModelbase1 - (int)*pModelbase2);
                  bin = (diff < histbins ? diff : histbins_1);
                  pAbsDiffHist[histindex + bin]--;
                }
                else
                {
                  // color

                  // add new pair from temporal buffer
                  histindex = ic*histbins;
                  diff = abs(*pTBbase1 -
                    *pTBbase2);
                  bin = (diff < histbins ? diff : histbins_1);
                  pAbsDiffHist[histindex + bin]++;

                  histindex += histbins;
                  diff = abs(*(pTBbase1 + 1) -
                    *(pTBbase2 + 1));
                  bin = (diff < histbins ? diff : histbins_1);
                  pAbsDiffHist[histindex + bin]++;

                  histindex += histbins;
                  diff = abs(*(pTBbase1 + 2) -
                    *(pTBbase2 + 2));
                  bin = (diff < histbins ? diff : histbins_1);
                  pAbsDiffHist[histindex + bin]++;

                  // remove old pair from the model
                  histindex = ic*histbins;

                  diff = abs(*pModelbase1 -
                    *pModelbase2);
                  bin = (diff < histbins ? diff : histbins_1);
                  pAbsDiffHist[histindex + bin]--;

                  histindex += histbins;
                  diff = abs(*(pModelbase1 + 1) -
                    *(pModelbase2 + 1));
                  bin = (diff < histbins ? diff : histbins_1);
                  pAbsDiffHist[histindex + bin]--;

                  histindex += histbins;
                  diff = abs(*(pModelbase1 + 2) -
                    *(pModelbase2 + 2));
                  bin = (diff < histbins ? diff : histbins_1);
                  pAbsDiffHist[histindex + bin]--;
                }
              }

              // add new pair into the model
              memcpy(pModelbase1, pTBbase1, color_channels * sizeof(unsigned char));

              memcpy(pModelbase2, pTBbase2, color_channels * sizeof(unsigned char));

              PixelQTop[i] = (PixelQTop[i] + 2) % SampleSize;
            }
          }
        } // end if (sampling event)

        // update temporal buffer
        // add new frame to Temporal buffer.
        memcpy(pTBTop, image, imagebuffersize);

        // update AccMask
        // update new Mask with information in AccMask

        for (i = 0; i < rows*cols; i++)
        {
          if (Mask[i])
            AccMask[i]++;
          else
            AccMask[i] = 0;

          if (AccMask[i] > ResetMaskTh)
            Mask[i] = 0;
        }

        // add new mask
        memcpy(pTMaskTop, Mask, imagespatialsize);

        // advance Temporal buffer pointer
        TemporalBufferTop = (TemporalBufferTop + 1) % TemporalBufferLength;

        BGModel->TemporalBufferTop = TemporalBufferTop;

        TBCount++;

        // estimate SDs

        if (SdEstimateFlag && UpdateSDRate && ((TimeIndex) % UpdateSDRate == 0))
        {
          double MaxSD = KernelTable->maxsegma;
          double MinSD = KernelTable->minsegma;
          int KernelBins = KernelTable->segmabins;

          unsigned char * pSDs = BGModel->SDbinsImage;

          FindHistMedians(&(AbsDiffHist));
          EstimateSDsFromAbsDiffHist(&(AbsDiffHist), pSDs, imagebuffersize, MinSD, MaxSD, KernelBins);
        }

        TimeIndex++;
      }

      /*********************************************************************/

      void DisplayPropabilityImageWithThresholding(double * Pimage,
        unsigned char * DisplayImage,
        double Threshold,
        unsigned int rows,
        unsigned int cols)
      {
        double p;

        for (unsigned int i = 0; i < rows*cols; i++)
        {
          p = Pimage[i];

          DisplayImage[i] = (p > Threshold) ? 0 : 255;
        }
      }

      /*********************************************************************/

      void NPBGSubtractor::NPBGSubtraction_Subset_Kernel(
        unsigned char * image,
        unsigned char * FGImage,
        unsigned char * FilteredFGImage)
      {
        unsigned int i, j;
        unsigned char *pSequence = BGModel->Sequence;

        unsigned int SampleSize = BGModel->SampleSize;

        double *kerneltable = KernelTable->kerneltable;
        int KernelHalfWidth = KernelTable->tablehalfwidth;
        //double *KernelSum = KernelTable->kernelsums;
        double KernelMaxSigma = KernelTable->maxsegma;
        double KernelMinSigma = KernelTable->minsegma;
        int KernelBins = KernelTable->segmabins;
        unsigned char * SDbins = BGModel->SDbinsImage;

        //unsigned char * SaturationImage = FilteredFGImage;

        // default sigmas .. to be removed.
        // double sigma1;
        // double sigma2;
        // double sigma3;

        // sigma1 = 2.25;
        // sigma2 = 2.25;
        // sigma3 = 2.25;

        double p;
        double th;

        double alpha;

        alpha = AlphaValue;

        /* intialize FG image */

        memset(FGImage, 0, rows*cols);

        //Threshold=1;
        th = Threshold * SampleSize;

        double sum = 0, kernel1, kernel2, kernel3;
        int k, g;


        if (color_channels == 1)
        {
          // gray scale

          int kernelbase;

          for (i = 0; i < rows*cols; i++)
          {
            kernelbase = SDbins[i] * (2 * KernelHalfWidth + 1);
            sum = 0;
            j = 0;

            while (j < SampleSize && sum < th)
            {
              g = pSequence[j*imagesize + i];
              k = g - image[i] + KernelHalfWidth;
              sum += kerneltable[kernelbase + k];
              j++;
            }

            p = sum / j;
            Pimage1[i] = p;
          }
        }
        else if (UseColorRatiosFlag && SubsetFlag)
        {
          // color ratios

          unsigned int ig;
          int base;

          // int kernelbase1;
          int kernelbase2;
          int kernelbase3;

          unsigned int kerneltablewidth = 2 * KernelHalfWidth + 1;

          double beta = 3.0;    // minimum bound on the range.
          double betau = 100.0;

          double beta_over_alpha = beta / alpha;
          double betau_over_alpha = betau / alpha;


          double brightness_lowerbound = 1 - alpha;
          double brightness_upperbound = 1 + alpha;
          int x1, x2;
          unsigned int SubsampleCount;

          for (i = 0, ig = 0; i < imagesize; i += 3, ig++)
          {
            // kernelbase1 = SDbins[i] * kerneltablewidth;
            kernelbase2 = SDbins[i + 1] * kerneltablewidth;
            kernelbase3 = SDbins[i + 2] * kerneltablewidth;

            sum = 0;
            j = 0;
            SubsampleCount = 0;

            while (j < SampleSize && sum < th)
            {
              base = j*imagesize + i;
              g = pSequence[base];

              if (g < beta_over_alpha)
              {
                x1 = (int)(g - beta);
                x2 = (int)(g + beta);
              }
              else if (g > betau_over_alpha)
              {
                x1 = (int)(g - betau);
                x2 = (int)(g + betau);
              }
              else
              {
                x1 = (int)(g*brightness_lowerbound + 0.5);
                x2 = (int)(g*brightness_upperbound + 0.5);
              }

              if (x1 < image[i] && image[i] < x2)
              {
                g = pSequence[base + 1];
                k = (g - image[i + 1]) + KernelHalfWidth;
                kernel2 = kerneltable[kernelbase2 + k];

                g = pSequence[base + 2];
                k = (g - image[i + 2]) + KernelHalfWidth;
                kernel3 = kerneltable[kernelbase3 + k];

                sum += kernel2*kernel3;

                SubsampleCount++;
              }
              j++;
            }

            p = sum / j;
            Pimage1[ig] = p;
          }
        }
        else if (UseColorRatiosFlag && !SubsetFlag)
        {
          // color ratios

          unsigned int ig;
          int base;
          int bin;

          int kernelbase1;
          int kernelbase2;
          int kernelbase3;

          unsigned int kerneltablewidth = 2 * KernelHalfWidth + 1;

          int gmin, gmax;
          double gfactor;

          gmax = 200;
          gmin = 10;

          gfactor = (KernelMaxSigma - KernelMinSigma) / (double)(gmax - gmin);

          for (i = 0, ig = 0; i < imagesize; i += 3, ig++)
          {

            bin = (int)floor(((alpha * 16 - KernelMinSigma)*KernelBins) / (KernelMaxSigma - KernelMinSigma));

            kernelbase1 = bin*kerneltablewidth;
            kernelbase2 = SDbins[i + 1] * kerneltablewidth;
            kernelbase3 = SDbins[i + 2] * kerneltablewidth;

            sum = 0;
            j = 0;

            while (j < SampleSize && sum < th)
            {
              base = j*imagesize + i;
              g = pSequence[base];

              if (g < gmin)
                bin = 0;
              else if (g > gmax)
                bin = KernelBins - 1;
              else
                bin = (int)((g - gmin) * gfactor + 0.5);

              kernelbase1 = bin*kerneltablewidth;

              k = (g - image[i]) + KernelHalfWidth;
              kernel1 = kerneltable[kernelbase1 + k];

              g = pSequence[base + 1];
              k = (g - image[i + 1]) + KernelHalfWidth;
              kernel2 = kerneltable[kernelbase2 + k];

              g = pSequence[base + 2];
              k = (g - image[i + 2]) + KernelHalfWidth;
              kernel3 = kerneltable[kernelbase3 + k];

              sum += kernel1*kernel2*kernel3;
              j++;
            }

            p = sum / j;
            Pimage1[ig] = p;
          }
        }
        else // RGB color
        {
          unsigned int ig;
          int base;

          int kernelbase1;
          int kernelbase2;
          int kernelbase3;
          unsigned int kerneltablewidth = 2 * KernelHalfWidth + 1;

          for (i = 0, ig = 0; i < imagesize; i += 3, ig++)
          {
            // used extimated kernel width to access the right kernel
            kernelbase1 = SDbins[i] * kerneltablewidth;
            kernelbase2 = SDbins[i + 1] * kerneltablewidth;
            kernelbase3 = SDbins[i + 2] * kerneltablewidth;

            sum = 0;
            j = 0;
            while (j < SampleSize && sum < th)
            {
              base = j*imagesize + i;
              g = pSequence[base];
              k = (g - image[i]) + KernelHalfWidth;
              kernel1 = kerneltable[kernelbase1 + k];

              g = pSequence[base + 1];
              k = (g - image[i + 1]) + KernelHalfWidth;
              kernel2 = kerneltable[kernelbase2 + k];

              g = pSequence[base + 2];
              k = (g - image[i + 2]) + KernelHalfWidth;
              kernel3 = kerneltable[kernelbase3 + k];

              sum += kernel1*kernel2*kernel3;
              j++;
            }

            p = sum / j;
            Pimage1[ig] = p;
          }
        }

        DisplayPropabilityImageWithThresholding(Pimage1, FGImage, Threshold, rows, cols);
      }

      /*********************************************************************/

      void NPBGSubtractor::NBBGSubtraction(unsigned char * Frame,
        unsigned char * FGImage,
        unsigned char * FilteredFGImage,
        unsigned char ** DisplayBuffers)
      {
        if (UseColorRatiosFlag)
          BGR2SnGnRn(Frame, tempFrame, rows, cols);
        else
          memcpy(tempFrame, Frame, rows*cols*color_channels);

        NPBGSubtraction_Subset_Kernel(tempFrame, FGImage, FilteredFGImage);
        /*NoiseFilter_o(FGImage,DisplayBuffers[3],rows,cols,4);
        BuildImageIndex(DisplayBuffers[3],imageindex,rows,cols);

        ExpandOperatorIndexed(DisplayBuffers[3],imageindex,DisplayBuffers[4],imageindex,rows,cols);
        ShrinkOperatorIndexed(DisplayBuffers[4],imageindex,FilteredFGImage,imageindex,rows,cols);

        memset(DisplayBuffers[3],0,rows*cols);*/
      }

      void NPBGSubtractor::Update(unsigned char * FGMask)
      {
        if (UpdateBGFlag)
          SequenceBGUpdate_Pairs(tempFrame, FGMask);
      }
    }
  }
}
