
#include "BilFilFast.h"
#include "universalbmp.h"
#define _USE_MATH_DEFINES
#include <math.h>

const float BlurFloatsSideWeightDefault = 0.29f; //0.25f;
float BlurFloatsSideWeight = BlurFloatsSideWeightDefault;

void BlurFloats(float *Data, int Items, int ToNextItem, int Rows, int ToNextRow, int Groups, int ToNextGroup)

// max (Groups-1)*ToNextGroup +
//     (Row-1   )*ToNextRow +
//     (Item-1  )*ToNextItem
{
  if (BlurFloatsSideWeight == 0.0f) return;
  float BlurFloatsCenterWeight = 1.0f - (2.0f*BlurFloatsSideWeight);
  float BlurFloatsFirstWeight = BlurFloatsCenterWeight / (BlurFloatsCenterWeight + BlurFloatsSideWeight);
  float BlurFloatsSecondWeight = BlurFloatsSideWeight / (BlurFloatsCenterWeight + BlurFloatsSideWeight);

  for (int Group=0; Group<Groups; Group++)
  {
    float *GroupStart = Data + (Group*ToNextGroup);
    for (int Row=0; Row<Rows; Row++)
    {
      float *RowStart = GroupStart + (Row*ToNextRow);
      float Last = *RowStart;
      float This;
      float *ThisAddr = RowStart+ToNextItem;
      *RowStart = BlurFloatsFirstWeight * Last +
                  BlurFloatsSecondWeight * RowStart[ToNextItem];
      for (int Item=1; Item<Items-1; Item++)
      {
        This = *ThisAddr;
        *ThisAddr = BlurFloatsCenterWeight * This +
                    BlurFloatsSideWeight   * Last +
                    BlurFloatsSideWeight   * ThisAddr[ToNextItem];
        Last = This;
        ThisAddr+= ToNextItem;
      }
      *ThisAddr = BlurFloatsFirstWeight  * (*ThisAddr) +
                  BlurFloatsSecondWeight * Last;
    }
  }
}

//
//  Blur by Exponential Moving Average
//
//  EMA:
//  alpha:  smoothing factor, 0.0 to 1.0 typically small (0.1) 
//  EMA(x) = alpha*Func(x) + (1-alpha)*EMA(x-1)
//  EMA(x) = alpha*SUM n=0 to -infinity[ (1-alpha)^n * Func(-n) ]

float EMA_Weight        = 1.0 * 0.5; // always half of, because it's one half of the convolution
float EMA_OneMinusAlpha = 0.5;
float EMA_WeightAlpha   = EMA_Weight*(1-EMA_OneMinusAlpha);

void BlurFloatsEMA(float *Data, float *DataTmp, int Items, int ToNextItem, int Rows, int ToNextRow, int Groups, int ToNextGroup)

// max (Groups-1)*ToNextGroup +
//     (Row-1   )*ToNextRow +
//     (Item-1  )*ToNextItem
{
  for (int Group=0; Group<Groups; Group++)
  {
    float *GroupStart = Data + (Group*ToNextGroup);
    for (int Row=0; Row<Rows; Row++)
    {
      float *RowStart = GroupStart + (Row*ToNextRow);   // Forward EMA
      float EMA = EMA_Weight*RowStart[0];
      DataTmp[0] = EMA;    // first item unchanged - blur should behave
                           // as if the first and last were repeated to infinity
      float *PItem=RowStart+ToNextItem;
      float *PTmp=DataTmp+1;
      int Item=1;
      for (; Item<Items; Item++, PItem+=ToNextItem, PTmp++)
      {
        EMA = EMA_WeightAlpha*(*PItem) + EMA*EMA_OneMinusAlpha;
        *PTmp = EMA;
      }

                                                        // Backward EMA
      Item--, PItem-=ToNextItem, PTmp--; // rewind to the last item
      EMA = EMA_Weight * (*PItem);
      float WriteAhead = EMA; // again, first item unchanged,
                              // just add forward data
      for (; Item>0; Item--, PItem-=ToNextItem, PTmp--)
      {
        EMA = EMA_WeightAlpha*(*PItem) + EMA*EMA_OneMinusAlpha;
        *PItem = (*PTmp) + WriteAhead;
           // in backward stage write to one item ahead
           // it avoids summing the convolution peaks
        WriteAhead = EMA;
      }
      *PItem = (*PTmp) + WriteAhead; // add last item
      
    }
  }
}

float EMA_1OneMinusAlpha = 0.850254f;  //0.795f;
float EMA_2OneMinusAlpha = 0.753384f;  //0.67f;
float EMA_3OneMinusAlpha = 0.612547f;  //0.5f;

float EMA_1WeightFirtItem  =  3.9f;
float EMA_2WeightFirtItem  = -3.9f;
float EMA_3WeightFirtItem  =  1.0f;

float EMA_1Sum = EMA_1WeightFirtItem * (1.0f / (1.0f - EMA_1OneMinusAlpha));  // Sum of all items of core
float EMA_2Sum = EMA_2WeightFirtItem * (1.0f / (1.0f - EMA_2OneMinusAlpha));
float EMA_3Sum = EMA_3WeightFirtItem * (1.0f / (1.0f - EMA_3OneMinusAlpha));

float CorrectAllWeight = 1.0f;
/*
  // correct weights that suma of all 3 fronts would be 0.5
  0.5 /
  (
    EMA_1Sum +
    EMA_2Sum +
    EMA_3Sum
  );
*/

float EMA_1Weight = EMA_1Sum * CorrectAllWeight;
float EMA_2Weight = EMA_2Sum * CorrectAllWeight;
float EMA_3Weight = EMA_3Sum * CorrectAllWeight;

float EMA_1WeightAlpha   = EMA_1Weight*(1.0f-EMA_1OneMinusAlpha);
float EMA_2WeightAlpha   = EMA_2Weight*(1.0f-EMA_2OneMinusAlpha);
float EMA_3WeightAlpha   = EMA_3Weight*(1.0f-EMA_3OneMinusAlpha);


void BlurFloats3EMA(float *Data, float *DataTmp, int Items, int ToNextItem, int Rows, int ToNextRow, int Groups, int ToNextGroup)

// max (Groups-1)*ToNextGroup +
//     (Row-1   )*ToNextRow +
//     (Item-1  )*ToNextItem
{
  for (int Group=0; Group<Groups; Group++)
  {
    float *GroupStart = Data + (Group*ToNextGroup);
    for (int Row=0; Row<Rows; Row++)
    {
      float *RowStart = GroupStart + (Row*ToNextRow);   // Forward EMA
      float EMA1 = EMA_1Weight * RowStart[0];
      float EMA2 = EMA_2Weight * RowStart[0];
      float EMA3 = EMA_3Weight * RowStart[0];

      // first item unchanged - blur should behave
      // as if the first and last were repeated to infinity
      DataTmp[0] = EMA1+EMA2+EMA3;

      float *PItem=RowStart+ToNextItem;
      float *PTmp=DataTmp+1;
      int Item=1;
      for (; Item<Items; Item++, PItem+=ToNextItem, PTmp++)
      {
        EMA1 = EMA_1WeightAlpha*(*PItem) + EMA1*EMA_1OneMinusAlpha;
        EMA2 = EMA_2WeightAlpha*(*PItem) + EMA2*EMA_2OneMinusAlpha;
        EMA3 = EMA_3WeightAlpha*(*PItem) + EMA3*EMA_3OneMinusAlpha;
        *PTmp = EMA1+EMA2+EMA3;
      }

      // Backward EMA
      Item--, PItem-=ToNextItem, PTmp--; // rewind to the last item
      EMA1 = EMA_1Weight * (*PItem);
      EMA2 = EMA_2Weight * (*PItem);
      EMA3 = EMA_3Weight * (*PItem);
      float WriteAhead = EMA1+EMA2+EMA3; // again, first item unchanged,
                                         // just add forward data
      for (; Item>0; Item--, PItem-=ToNextItem, PTmp--)
      {
        EMA1 = EMA_1WeightAlpha*(*PItem) + EMA1*EMA_1OneMinusAlpha;
        EMA2 = EMA_2WeightAlpha*(*PItem) + EMA2*EMA_2OneMinusAlpha;
        EMA3 = EMA_3WeightAlpha*(*PItem) + EMA3*EMA_3OneMinusAlpha;
        *PItem = (*PTmp) + WriteAhead;

        // in backward stage write to one item ahead
        // it avoids summing the convolution peaks
        WriteAhead = EMA1+EMA2+EMA3;
      }
      *PItem = (*PTmp) + WriteAhead; // add last item
      
    }
  }
}

void FastBilateralFilter::ComputeRadius()
{
  int GreaterSide = (MaxX > MaxY) ? MaxX : MaxY;

  ComputedRadius = int((GreaterSide * RadiusFrac) + RadiusPixels);
}

void FastBilateralFilter::ComputeIntensityRadius()
{
  ComputedIntensityRadius =
   (MaxInputIntensity-MinInputIntensity)*IntensityRadiusFrac + IntensityRadius;

  if (ComputedIntensityRadius < 0.000001f) // avoid DIV 0
    ComputedIntensityRadius = InvComputedIntensityRadius = 1;
  else
    InvComputedIntensityRadius = 1.0f/ComputedIntensityRadius;

  if ((MaxInputIntensity-MinInputIntensity) == 0)
  {
    ComputedIntensityRadiusFrac = 1;
    InvInputDynamicRange = 1;
  }
  else
  {
    ComputedIntensityRadiusFrac = ComputedIntensityRadius /
     (MaxInputIntensity-MinInputIntensity);
    InvInputDynamicRange = 1.0f/(MaxInputIntensity-MinInputIntensity);
  }
}


// for EMA histogram bluring

const int HistogramAddItemsToSideSingleEMA = 2;
const int HistogramAddItemsToSideTripleEMA = 10;

const float HistogramItemsPerIntensityBlurRadiusSingleEMA = 2.0f;
const float HistogramItemsPerIntensityBlurRadiusTripleEMA = 10.0f;// / 1.30f;

#define EMA_HISTOGRAM_BLUR

void FastBilateralFilter::ComputeBilateralPrecompile(TFloatBmp &InIntensity, bool TripleEMA)
{

  //TEST:
  /*
  {
    float pokus[21] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
    float tmp[21] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
    //BlurFloatsEMA(pokus, tmp, 21, 1, 1, 0, 1, 0);
    BlurFloats3EMA(pokus, tmp, 21, 1, 1, 0, 1, 0);
    tmp[1]=0;
  }
  */


#ifdef __BORLANDC__
  unsigned int AsmCW; // set FloatToInt to TRUNC
  __asm
  {
    wait
    fnstcw word ptr AsmCW
    wait
    mov AX, word ptr AsmCW
    or ah,12
    mov word ptr AsmCW, AX
    fldcw word ptr AsmCW
  }
#endif


  // computes histogram matrix
  MaxX = InIntensity.MaxX;
  MaxY = InIntensity.MaxY;
  ComputeRadius();
  FastBilateralSquareSize =  ComputedRadius;
  if (FastBilateralSquareSize < 1) FastBilateralSquareSize = 1;

  if ((FilterOutput.MaxX != MaxX)||
      (FilterOutput.MaxY != MaxY))
  FilterOutput.New(MaxX,MaxY,1);

  float *PIn = InIntensity.Pixel(0,0);
  MinInputIntensity = *PIn;
  MaxInputIntensity = MinInputIntensity;

  for (int y=0;y<MaxY;y++)
    for (int x=0;x<MaxX;x++)
    {
      if (*PIn > MaxInputIntensity) MaxInputIntensity = *PIn;
      if (*PIn < MinInputIntensity) MinInputIntensity = *PIn;
      PIn++;
    }
  ComputeIntensityRadius();

  
  if (TripleEMA)
  {
    MaxHistogram = int(HistogramItemsPerIntensityBlurRadiusTripleEMA / ComputedIntensityRadiusFrac);
    HistogramSize = MaxHistogram + 2*HistogramAddItemsToSideTripleEMA + 1;
    HistogramAddItemsToSide = HistogramAddItemsToSideTripleEMA;
  }
  else
  {
    MaxHistogram = int(HistogramItemsPerIntensityBlurRadiusSingleEMA / ComputedIntensityRadiusFrac);
    HistogramSize = MaxHistogram + 2*HistogramAddItemsToSideSingleEMA + 1;
    HistogramAddItemsToSide = HistogramAddItemsToSideSingleEMA;
  }



  IntensityHistograms.New((MaxX+FastBilateralSquareSize-1)/FastBilateralSquareSize,
                          (MaxY+FastBilateralSquareSize-1)/FastBilateralSquareSize,
                           HistogramSize);
  IntMultIntHistograms.New(IntensityHistograms.MaxX,
                           IntensityHistograms.MaxY,
                           HistogramSize);

#ifdef EMA_HISTOGRAM_BLUR
  float *BlurTemp = new float[HistogramSize];
#endif

  DynRangeToHistogIndex = InvInputDynamicRange * float(MaxHistogram);

  for (int y=0; y<IntensityHistograms.MaxY; y++) // Square rows
  {
    int StartY = y*FastBilateralSquareSize;
    int EndY = StartY+FastBilateralSquareSize;
    if (EndY>MaxY) EndY = MaxY;
    for (int x=0; x<IntensityHistograms.MaxX; x++) // Squares
    {
      int StartX = x*FastBilateralSquareSize;
      int EndX = StartX+FastBilateralSquareSize;
      if (EndX>MaxX) EndX = MaxX;

      float *IntensityHistogram = IntensityHistograms.Pixel(x,y);
      float *IntMultIntHistogram = IntMultIntHistograms.Pixel(x,y);

      for (int n=0; n<HistogramSize; n++)
      {
        IntensityHistogram[n] = 0;
        IntMultIntHistogram[n] = 0;  // maybe both to same small number to avoid div 0 ?
      }

      for (int InY = StartY; InY<EndY; InY++) // Lines in square
      {
        float *InPixel = InIntensity.Pixel(StartX,InY);
        for (int InX = StartX; InX<EndX; InX++) // Pixels in square
        {
          float Intensity = *InPixel;

            // ZAOKROUHLENI, predelat NA INTERPOLACI                                  /////// !!!!!!!!
          float HistogIndexFloat = ((Intensity - MinInputIntensity) * DynRangeToHistogIndex);

#ifdef EMA_HISTOGRAM_BLUR
  HistogIndexFloat += 0.5; // EMA blur convolution moves signal by 0.5 item to the left
#endif


          // POTENCIALNE POMALE!!
          int HistogIndex;
          //HistogIndex = HistogIndexFloat;  // prumer 1.44s
                    // int HistogIndex = 5;  // prumer 1.1s    // TZN 24% celeho vypoctu!!!

#ifdef __BORLANDC__
          __asm
          {                                   // prumer 1.28s  // tzn 11% zrychleni
            fld HistogIndexFloat
            fistp HistogIndex
          }
#else
          HistogIndex = int(HistogIndexFloat);
#endif

          float HistogIndexWeightNext = HistogIndexFloat - float(HistogIndex);
          HistogIndex += HistogramAddItemsToSide;
          // HistogramBlurRadius = 2

          IntensityHistogram [HistogIndex] += 1.0f-HistogIndexWeightNext;
          IntMultIntHistogram[HistogIndex] += (1.0f-HistogIndexWeightNext) * Intensity;
          HistogIndex++;
          IntensityHistogram [HistogIndex] += HistogIndexWeightNext;
          IntMultIntHistogram[HistogIndex] += HistogIndexWeightNext * Intensity;


          InPixel++;
        } // Pixels in square
      } // Lines in square
    } // Squares
  } // Square rows

#ifdef EMA_HISTOGRAM_BLUR

  if (TripleEMA)
  {
    BlurFloats3EMA(IntensityHistograms.Pixel(0,0),                    // float *Data
                   BlurTemp,                                          // float *DataTmp
                   HistogramSize,                                     // int Items
                   1,                                                 // int ToNextItem
                   IntensityHistograms.MaxX*IntensityHistograms.MaxY, // int Rows
                   HistogramSize,                                     // int ToNextRow
                   1,                                                 // int Groups
                   0                                                  //int ToNextGroup)
                  );
    BlurFloats3EMA(IntMultIntHistograms.Pixel(0,0),                     // float *Data
                   BlurTemp,                                            // float *DataTmp
                   HistogramSize,                                       // int Items
                   1,                                                   // int ToNextItem
                   IntMultIntHistograms.MaxX*IntMultIntHistograms.MaxY, // int Rows
                   HistogramSize,                                       // int ToNextRow
                   1,                                                   // int Groups
                   0                                                    //int ToNextGroup)
                  );

  }
  else
  {
    BlurFloatsEMA(IntensityHistograms.Pixel(0,0),                    // float *Data
                  BlurTemp,                                          // float *DataTmp
                  HistogramSize,                                     // int Items
                  1,                                                 // int ToNextItem
                  IntensityHistograms.MaxX*IntensityHistograms.MaxY, // int Rows
                  HistogramSize,                                     // int ToNextRow
                  1,                                                 // int Groups
                  0                                                  //int ToNextGroup)
                 );
    BlurFloatsEMA(IntMultIntHistograms.Pixel(0,0),                     // float *Data
                  BlurTemp,                                            // float *DataTmp
                  HistogramSize,                                       // int Items
                  1,                                                   // int ToNextItem
                  IntMultIntHistograms.MaxX*IntMultIntHistograms.MaxY, // int Rows
                  HistogramSize,                                       // int ToNextRow
                  1,                                                   // int Groups
                  0                                                    //int ToNextGroup)
                 );
  }

  delete [] BlurTemp;
#endif

#ifndef EMA_HISTOGRAM_BLUR
  BlurFloats(IntensityHistograms.Pixel(0,0),                    // float *Data
             HistogramSize,                                     // int Items
             1,                                                 // int ToNextItem
             IntensityHistograms.MaxX*IntensityHistograms.MaxY, // int Rows
             HistogramSize,                                     // int ToNextRow
             1,                                                 // int Groups
             0                                                  //int ToNextGroup)
            );
  BlurFloats(IntMultIntHistograms.Pixel(0,0),                     // float *Data
             HistogramSize,                                       // int Items
             1,                                                   // int ToNextItem
             IntMultIntHistograms.MaxX*IntMultIntHistograms.MaxY, // int Rows
             HistogramSize,                                       // int ToNextRow
             1,                                                   // int Groups
             0                                                    //int ToNextGroup)
            );
#endif
}


void FastBilateralFilter::ComputeBilateralFinish(TFloatBmp &InIntensity, bool Lanczos)
{
  // max (Groups-1)*ToNextGroup +
  //     (Row-1   )*ToNextRow +
  //     (Item-1  )*ToNextItem

  BlurFloats(IntensityHistograms.Pixel(0,0),        // float *Data       BLUR THROUGH X
             IntensityHistograms.MaxX,              // int Items
             HistogramSize,                         // int ToNextItem
             HistogramSize,                         // int Rows
             1,                                     // int ToNextRow
             IntensityHistograms.MaxY,              // int Groups
             IntensityHistograms.MaxX*HistogramSize //int ToNextGroup)
            );
  BlurFloats(IntensityHistograms.Pixel(0,0),        // float *Data       BLUR THROUGH Y
             IntensityHistograms.MaxY,              // int Items
             IntensityHistograms.MaxX*HistogramSize,// int ToNextItem
             HistogramSize,                         // int Rows
             1,                                     // int ToNextRow
             IntensityHistograms.MaxX,              // int Groups
             HistogramSize                          // int ToNextGroup)
            );

  BlurFloats(IntMultIntHistograms.Pixel(0,0),        // float *Data       BLUR THROUGH X
             IntMultIntHistograms.MaxX,              // int Items
             HistogramSize,                          // int ToNextItem
             HistogramSize,                          // int Rows
             1,                                      // int ToNextRow
             IntMultIntHistograms.MaxY,              // int Groups
             IntMultIntHistograms.MaxX*HistogramSize //int ToNextGroup)
            );
  BlurFloats(IntMultIntHistograms.Pixel(0,0),        // float *Data       BLUR THROUGH Y
             IntMultIntHistograms.MaxY,              // int Items
             IntMultIntHistograms.MaxX*HistogramSize,// int ToNextItem
             HistogramSize,                          // int Rows
             1,                                      // int ToNextRow
             IntMultIntHistograms.MaxX,              // int Groups
             HistogramSize                           // int ToNextGroup)
            );

  float *LanczosPrecomp;
  if (Lanczos)
  {
    LanczosPrecomp = new float[4*FastBilateralSquareSize+1];

    for (int i=0; i<4*FastBilateralSquareSize+1; i++)
    {
      float Param = float(i) / float(FastBilateralSquareSize);
      Param -= 2.0f;
      Param *= float(M_PI);
      float Lancz;

      if (Param == 0) Lancz = 1;
      else
      {
        Lancz = (sin(    Param) /      Param) *
                (sin(0.5f*Param) / (0.5f*Param));
      }
      LanczosPrecomp[i] = Lancz;
    }
  }

  int HalfSquareSize = (FastBilateralSquareSize+1)/2;
  float LateralWeightMult = 1.0f / float(FastBilateralSquareSize);

  float *PIn = InIntensity.Pixel(0,0);
  float *POut = FilterOutput.Pixel(0,0);

  for (int y=0,yHiWeight=HalfSquareSize,yBox1=-1,yBox2=0; y<MaxY; y++,yHiWeight++)
  {
    if (yHiWeight==FastBilateralSquareSize)
    {
      yBox1++; yBox2++; yHiWeight=0;
    }
    float yHiWeightFloat = LateralWeightMult*yHiWeight;

    //yHiWeightFloat = 0.5 - 0.5*cos(M_PI*yHiWeightFloat);    /// COS SUBSAMPL
    //yHiWeightFloat = 0;

    //int SqY = y/FastBilateralSquareSize;
    //if (SqY>IntensityHistograms.MaxY)
    //{
    //  SqY=IntensityHistograms.MaxY-1;
    //}

    for (int x=0,xHiWeight=HalfSquareSize,xBox1=-1,xBox2=0; x<MaxX; x++,xHiWeight++)
    {
      if (xHiWeight==FastBilateralSquareSize)
      {
        xBox1++; xBox2++; xHiWeight=0;
      }
      float xHiWeightFloat = LateralWeightMult*xHiWeight;

      float Numerator = 0;
      float Denominator = 0;

      float FloatIntensIndex = ((*PIn) - MinInputIntensity) * DynRangeToHistogIndex;
      FloatIntensIndex += HistogramAddItemsToSide;


      int IntensIndex;
      #ifdef __BORLANDC__
      __asm
      {
        fld FloatIntensIndex
        fistp IntensIndex
      }
#else
      IntensIndex = int(FloatIntensIndex);
#endif
      
      float WeightNext = FloatIntensIndex - float(IntensIndex);
      float WeightThis = 1.0f - WeightNext;

      if (Lanczos)
      {
        for (int YBox = yBox1-1, YLancParam = FastBilateralSquareSize-yHiWeight; YBox < yBox1+3; YBox++, YLancParam+=FastBilateralSquareSize)
        if ((YBox >= 0)&&(YBox < IntensityHistograms.MaxY))
        {
          for (int XBox = xBox1-1, XLancParam = FastBilateralSquareSize-xHiWeight; XBox < xBox1+3; XBox++, XLancParam+=FastBilateralSquareSize)
          if ((XBox >= 0)&&(XBox < IntensityHistograms.MaxX))
          if ((XLancParam > 4*FastBilateralSquareSize)||
              (YLancParam > 4*FastBilateralSquareSize))
          {
            // error
          }
          else
          {
            float LateralWeight = LanczosPrecomp[XLancParam]*LanczosPrecomp[YLancParam];
            float *Hist =    IntensityHistograms.Pixel(XBox,YBox);
            float *IntHist = IntMultIntHistograms.Pixel(XBox,YBox);

            // toto nekdy zpusobi vystrelove chyby

            Numerator   += LateralWeight *(WeightThis*IntHist[IntensIndex] + WeightNext*IntHist[IntensIndex+1]);
            Denominator += LateralWeight *(WeightThis*   Hist[IntensIndex] + WeightNext*   Hist[IntensIndex+1]);
          }
        }
      }
      else // bilinear interpolation
      {
        if ((xBox1>=0)&&(yBox1>=0))
        {
          float LateralWeight = (1-xHiWeightFloat)*(1-yHiWeightFloat);
          float *Hist =    IntensityHistograms.Pixel(xBox1,yBox1);
          float *IntHist = IntMultIntHistograms.Pixel(xBox1,yBox1);
          Numerator   += LateralWeight *(WeightThis*IntHist[IntensIndex] + WeightNext*IntHist[IntensIndex+1]);
          Denominator += LateralWeight *(WeightThis*   Hist[IntensIndex] + WeightNext*   Hist[IntensIndex+1]);
        }
        if ((xBox2<IntensityHistograms.MaxX)&&(yBox1>=0))
        {
          float LateralWeight = (xHiWeightFloat)*(1-yHiWeightFloat);
          float *Hist =    IntensityHistograms.Pixel(xBox2,yBox1);
          float *IntHist = IntMultIntHistograms.Pixel(xBox2,yBox1);
          Numerator   += LateralWeight *(WeightThis*IntHist[IntensIndex] + WeightNext*IntHist[IntensIndex+1]);
          Denominator += LateralWeight *(WeightThis*   Hist[IntensIndex] + WeightNext*   Hist[IntensIndex+1]);
        }
        if ((xBox1>=0)&&(yBox2<IntensityHistograms.MaxY))
        {
          float LateralWeight = (1-xHiWeightFloat)*(yHiWeightFloat);
          float *Hist =    IntensityHistograms.Pixel(xBox1,yBox2);
          float *IntHist = IntMultIntHistograms.Pixel(xBox1,yBox2);
          Numerator   += LateralWeight *(WeightThis*IntHist[IntensIndex] + WeightNext*IntHist[IntensIndex+1]);
          Denominator += LateralWeight *(WeightThis*   Hist[IntensIndex] + WeightNext*   Hist[IntensIndex+1]);
        }
        if ((xBox2<IntensityHistograms.MaxX)&&(yBox2<IntensityHistograms.MaxY))
        {
          float LateralWeight = (xHiWeightFloat)*(yHiWeightFloat);
          float *Hist =    IntensityHistograms.Pixel(xBox2,yBox2);
          float *IntHist = IntMultIntHistograms.Pixel(xBox2,yBox2);
          Numerator   += LateralWeight *(WeightThis*IntHist[IntensIndex] + WeightNext*IntHist[IntensIndex+1]);
          Denominator += LateralWeight *(WeightThis*   Hist[IntensIndex] + WeightNext*   Hist[IntensIndex+1]);
        }
      }
      if (Denominator > 0)
        *POut = Numerator / Denominator;
      else
        *POut = MinInputIntensity;

      PIn++;
      POut++;
    }
  }

  if (Lanczos)
  {
    delete [] LanczosPrecomp;
  }

}

void FastBilateralFilter::ComputeBilateralFreeMem()
{
  IntensityHistograms.Delete();
  IntMultIntHistograms.Delete();
}

