/* The copyright in this software is being made available under the BSD
 * License, included below. This software may be subject to other third party
 * and contributor rights, including patent rights, and no such rights are
 * granted under this license.
 *
 * Copyright (c) 2010-2017, ITU/ISO/IEC
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *  * Neither the name of the ITU/ISO/IEC nor the names of its contributors may
 *    be used to endorse or promote products derived from this software without
 *    specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "BilateralFilter.h"

#include <tmmintrin.h>
#include <smmintrin.h>

#if JEM_TOOLS

#include <stdio.h>
#include <math.h>
#include <string.h>
#include <algorithm>

#define DOTIME 0

#if DOTIME
#include <mach/mach_time.h>
#endif


const unsigned short maxPosList[34] = {6, 12, 18, 23, 29, 35, 41, 46, 52, 58, 64, 69, 75, 81, 87, 92, 98, 104, 110, 115, 121, 127, 133, 138, 144, 150, 156, 161, 167, 173, 179, 184, 190, 196};

#define DISTANCE_TABLE_FRACTIONAL_BITS_SB 10

BilateralFilter::BilateralFilter()
{
}

BilateralFilter::~BilateralFilter()
{
}


#if DOTIME
int getUptimeInMilliseconds() // From StackOverflow
{
  const int64_t kOneMillion = 1000 * 1000;
  static mach_timebase_info_data_t s_timebase_info;
  
  if (s_timebase_info.denom == 0) {
    (void) mach_timebase_info(&s_timebase_info);
  }
  
  // mach_absolute_time() returns billionth of seconds,
  // so divide by one million to get milliseconds
  return (int)((mach_absolute_time() * s_timebase_info.numer) / (kOneMillion * s_timebase_info.denom));
}
#endif

void BilateralFilter::create()
{
}

void BilateralFilter::destroy()
{
}

#define NUM_FRAC_BITS_K 4
// Largest possible value for NUM_FRAC_BITS_K_HAT is 6+11 (that still makes it possible to calculate the LUT value using int32_t).
#define NUM_FRAC_BITS_K_HAT 6
#define TO_SHIFT_TO_GET_K_HAT (NUM_FRAC_BITS_K+DISTANCE_TABLE_FRACTIONAL_BITS_SB-NUM_FRAC_BITS_K_HAT)
#define TO_ADD_BEFORE_SHIFT_K_HAT   (((int32_t)(1)) << (TO_SHIFT_TO_GET_K_HAT-1))

#define TO_SHIFT_TO_GET_K_HAT_HIRES (NUM_FRAC_BITS_K+DISTANCE_TABLE_FRACTIONAL_BITS_HIRES-NUM_FRAC_BITS_K_HAT)
#define TO_ADD_BEFORE_SHIFT_K_HAT_HIRES   (((int32_t)(1)) << (TO_SHIFT_TO_GET_K_HAT_HIRES-1))


#if NUM_FRAC_BITS_K_HAT < 10
#define TO_SHIFT_TO_GET_M_HAT (DISTANCE_TABLE_FRACTIONAL_BITS_SB-NUM_FRAC_BITS_K_HAT)
#define TO_ADD_BEFORE_SHIFT_M_HAT   (((int32_t)(1)) << (TO_SHIFT_TO_GET_M_HAT-1))
#else
#define TO_SHIFT_TO_GET_M_HAT (NUM_FRAC_BITS_K_HAT-DISTANCE_TABLE_FRACTIONAL_BITS_SB)
#define TO_ADD_BEFORE_SHIFT_M_HAT   0
#endif

#define DISTANCE_TABLE_FRACTIONAL_BITS_HIRES 15

#if NUM_FRAC_BITS_K_HAT < 15
#define TO_SHIFT_TO_GET_M_HAT_HIRES (DISTANCE_TABLE_FRACTIONAL_BITS_HIRES-NUM_FRAC_BITS_K_HAT)
#define TO_ADD_BEFORE_SHIFT_M_HAT_HIRES   (((int32_t)(1)) << (TO_SHIFT_TO_GET_M_HAT_HIRES-1))
#else
#define TO_SHIFT_TO_GET_M_HAT_HIRES (NUM_FRAC_BITS_K_HAT-DISTANCE_TABLE_FRACTIONAL_BITS_HIRES)
#define TO_ADD_BEFORE_SHIFT_M_HAT_HIRES 0
#endif



#define TO_SHIFT_CURRENT_INFLUENCE (NUM_FRAC_BITS_K_HAT+8)
#define TO_ADD_CURRENT_INFLUENCE (1<<(TO_SHIFT_CURRENT_INFLUENCE-1))
// 8 is because we have used 256 as 1 in the LUT table before


// k approximated using 4 fractional bits. Largest value < 64, hence at most 10 bits, fits in an int16_t which has 15 bits.

int16_t approx_k[46] = {
  816, 408, 291, 215, 170, 141, 124, 107, 95, 85, 78, 72, 66, 61, 57, 54, 50, 47, 45, 43, 41, 39, 37, 36, 34, 33, 32, 31, 30, 29, 28, 27, 26, 25, 25, 24, 23, 23, 22, 21, 21, 20, 20, 20, 19, 19,
};

// k approximated using 22 fractional bits. Largest value < 64, hence at most 28 bits, fits in an int32_t which has 31 bits.
//int32_t approx_k[46] = {213909504, 106954752, 76396251, 56291975, 44564480, 36880949, 32410531, 28145987, 24873198, 22282240, 20568222, 18763992, 17250766, 15963396, 15064050, 14072994, 13204290, 12436599, 11883861, 11258395, 10695475, 10186167, 9812363, 9381996, 8987794, 8625383, 8355840, 8041711, 7750344, 7479353, 7275833, 7036497, 6812405, 6602145, 6443057, 6254664, 6076975, 5909102, 5781338, 5629197, 5484859, 5347738, 5242880, 5117452, 4997886, 4883779};

//int32_t approx_k[46] = {
//    213909504, 118838613, 76396251, 56291975, 44564480, 38198126, 32410531, 28145987, 25465417, 22756330, 20568222, 19099063, 17533566, 16205265, 15279250, 14260634, 13369344, 12582912, 12017388, 11378165, 10803510, 10383957, 9903218, 9465022, 9141432, 8766783, 8421634, 8102633, 7864320, 7585443, 7325668, 7130317, 6900307, 6684672, 6521631, 6328684, 6146825, 5975126, 5844522, 5689083, 5541697, 5429175, 5294790, 5166896, 5068946, 4951609,
//  };

short buffer[66 * 66];
short buffer2[68 * 68];
void BilateralFilter::smoothBlockBilateralFilter(unsigned uiWidth, unsigned uiHeight, short block[], int isInterBlock, int qp) const
{
  int32_t length = (int32_t)std::min(uiWidth, uiHeight);
  int32_t rightPixel, centerPixel;
  int32_t rightInfluenceCalc, bottomInfluenceCalc;
  int32_t leftInfluenceCalc;
  uint8_t distanceFactor5pixelsSB;
  int32_t influenceRowCalc[MAX_CU_SIZE];
  int32_t blockLengthIndex;

  int32_t dIB, dIR;

  if( length >= 16 )
  {
    blockLengthIndex = 3;
  }
  else if ( length >= 8 )
  {
    blockLengthIndex = 2;
  }
  else if (uiWidth == 4 && uiHeight == 4)
  {
    blockLengthIndex = 0; // special case for 4x4 block
  }
  else
  {
    blockLengthIndex = 1;
  }

  distanceFactor5pixelsSB = m_bilateralDistanceTable5pixelsSB[blockLengthIndex + 4 * isInterBlock];


  // my_k:
  // largest possible value before shift is 213909504*246+33554432
  // and after shift it is (213909504*246+33554432) >> 26 = 785 < 1024 < 10 bits, whereof 4 bits are integer bits and 6 bits are fractional
  int32_t my_k_calc = (((int32_t)(approx_k[qp-18])) * ((int32_t)(distanceFactor5pixelsSB)) + TO_ADD_BEFORE_SHIFT_K_HAT) >> (TO_SHIFT_TO_GET_K_HAT);
  int32_t my_k = -my_k_calc;
#if NUM_FRAC_BITS_K_HAT < 10
  int32_t my_m_calc = (((int32_t)(255)) * ((int32_t)(distanceFactor5pixelsSB)) + TO_ADD_BEFORE_SHIFT_M_HAT) >> (TO_SHIFT_TO_GET_M_HAT);
#else
  int32_t my_m_calc = (((int32_t)(255)) * ((int32_t)(distanceFactor5pixelsSB)) + TO_ADD_BEFORE_SHIFT_M_HAT) << (TO_SHIFT_TO_GET_M_HAT);
#endif
  int32_t my_m = my_m_calc;



  if (uiWidth >= 8 /*&& uiHeight == 8*/)
  {
    /*for (int i = -1; i < (int)uiHeight + 1; i++)
    {
    for (int j = -1; j < (int)uiWidth + 1; j++)
    {
    int getX = j;
    if (getX < 0)
    getX = 0;
    if (getX >= uiWidth)
    getX = uiWidth - 1;
    int getY = i;
    if (getY < 0)
    getY = 0;
    if (getY >= uiHeight)
    getY = uiHeight - 1;
    buffer[(i + 1)*(uiWidth + 2) + (j + 1)] = block[getY*uiWidth + getX];

    }
    }*/

    for (int row = 0; row < uiHeight; row++)
    {
      //left one
      buffer[(row + 1)*(uiWidth + 2)] = block[row*uiWidth];
      //middle ones
      memcpy(buffer + (row + 1) * (uiWidth + 2) + 1, block + row * uiWidth, uiWidth * sizeof(short));
      buffer[(row + 1)*(uiWidth + 2) + uiWidth + 1] = block[row*uiWidth + uiWidth - 1];
    }
    //memcpy(buffer, buffer+uiWidth+2, (uiWidth+2) * sizeof(short));
    memcpy(buffer + (uiWidth + 2)*(uiHeight + 1), buffer + (uiWidth + 2)*(uiHeight), (uiWidth + 2) * sizeof(short));



    /*
    printf("k, m = %d, %d", my_k, my_m);
    printf("ok, orig: \n");
    for (int i = 0; i < uiHeight; i++)
    {
    for (int j = 0; j < uiWidth; j++)
    {
    printf("%d ", block[i*uiWidth + j]);
    }
    printf("\n");
    }

    printf("ok, big buffer: \n");
    for (int i = 0; i < uiHeight+2; i++)
    {
    for (int j = 0; j < uiWidth+2; j++)
    {
    printf("%d ", buffer[i * (uiWidth+2) + j]);
    }
    printf("\n");
    }
    */

    __m128i center, left, right, down, diffabs, k_vec, m_vec, low, high, leftresult, leftresult2, rightresult, rightresult2, upresult, upresult2, downresult, downresult2, zero, sum1, sum2, toadd, output;//m_vec16;
    k_vec = _mm_set1_epi16((short)my_k);
    m_vec = _mm_set1_epi32(my_m);
    //m_vec16 = _mm_set1_epi16(my_m);
    zero = _mm_set1_epi32(0);
    toadd = _mm_set1_epi32(TO_ADD_CURRENT_INFLUENCE);


    for (int col = 0; col < uiWidth; col += 8)
    {
      upresult = zero;
      upresult2 = zero;
      int tl = 1 + col;
      short* point = buffer + tl;
      for (int row = 0; row<uiHeight; row++)
      {
        //int tl = ((int)uiWidth + 2)*(row + 1) + 1+col;
        point += ((int)uiWidth + 2);
        center = _mm_loadu_si128((__m128i*)(point));

        //load neighbours
        left = _mm_loadu_si128((__m128i*)(point - 1));
        right = _mm_loadu_si128((__m128i*)(point + 1));
        //up = _mm_loadu_si128((__m128i*)(buffer + tl - ((int)uiWidth) - 2));
        down = _mm_loadu_si128((__m128i*)(point + ((int)uiWidth) + 2));

        //calculate diffs
        left = _mm_sub_epi16(left, center);
        right = _mm_sub_epi16(right, center);



        //up = _mm_sub_epi16(up, center);
        down = _mm_sub_epi16(down, center);

        //------LEFT------
        //calculate abs
        diffabs = _mm_abs_epi16(left);

        //multiply left by k
        low = _mm_mullo_epi16(diffabs, k_vec);
        high = _mm_mulhi_epi16(diffabs, k_vec);
        leftresult = _mm_unpacklo_epi16(low, high);
        leftresult2 = _mm_unpackhi_epi16(low, high);

        //max(0, x+m)
        leftresult = _mm_add_epi32(leftresult, m_vec);
        leftresult = _mm_max_epi32(leftresult, zero);
        leftresult2 = _mm_add_epi32(leftresult2, m_vec);
        leftresult2 = _mm_max_epi32(leftresult2, zero);

        //aand multibly by diff
        leftresult = _mm_mullo_epi32(leftresult, _mm_cvtepi16_epi32(left));
        leftresult2 = _mm_mullo_epi32(leftresult2, _mm_cvtepi16_epi32(_mm_srli_si128(left, 8)));

        /*
        low = _mm_mullo_epi16(diffabs, k_vec);
        leftresult = _mm_add_epi16(low, m_vec);
        */

        //------RIGHT------
        //calculate abs
        diffabs = _mm_abs_epi16(right);

        //multiply left by k
        low = _mm_mullo_epi16(diffabs, k_vec);
        high = _mm_mulhi_epi16(diffabs, k_vec);
        rightresult = _mm_unpacklo_epi16(low, high);
        rightresult2 = _mm_unpackhi_epi16(low, high);

        //max(0, x+m)
        rightresult = _mm_add_epi32(rightresult, m_vec);
        rightresult = _mm_max_epi32(rightresult, zero);
        rightresult2 = _mm_add_epi32(rightresult2, m_vec);
        rightresult2 = _mm_max_epi32(rightresult2, zero);

        //aand multibly by diff
        rightresult = _mm_mullo_epi32(rightresult, _mm_cvtepi16_epi32(right));
        rightresult2 = _mm_mullo_epi32(rightresult2, _mm_cvtepi16_epi32(_mm_srli_si128(right, 8)));
        /*
        //---up---
        //calculate abs
        diffabs = _mm_abs_epi16(up);

        //multiply left by k
        low = _mm_mullo_epi16(diffabs, k_vec);
        high = _mm_mulhi_epi16(diffabs, k_vec);
        upresult = _mm_unpacklo_epi16(low, high);
        upresult2 = _mm_unpackhi_epi16(low, high);

        //max(0, x+m)
        upresult = _mm_add_epi32(upresult, m_vec);
        upresult = _mm_max_epi32(upresult, zero);
        upresult2 = _mm_add_epi32(upresult2, m_vec);
        upresult2 = _mm_max_epi32(upresult2, zero);

        //aand multibly by diff
        upresult = _mm_mullo_epi32(upresult, _mm_cvtepi16_epi32(up));
        upresult2 = _mm_mullo_epi32(upresult2, _mm_cvtepi16_epi32(_mm_srli_si128(up, 8)));
        */

        //------DOWN------
        //calculate abs
        diffabs = _mm_abs_epi16(down);

        //multiply left by k
        low = _mm_mullo_epi16(diffabs, k_vec);
        high = _mm_mulhi_epi16(diffabs, k_vec);
        downresult = _mm_unpacklo_epi16(low, high);
        downresult2 = _mm_unpackhi_epi16(low, high);

        //max(0, x+m)
        downresult = _mm_add_epi32(downresult, m_vec);
        downresult = _mm_max_epi32(downresult, zero);
        downresult2 = _mm_add_epi32(downresult2, m_vec);
        downresult2 = _mm_max_epi32(downresult2, zero);

        //aand multibly by diff
        downresult = _mm_mullo_epi32(downresult, _mm_cvtepi16_epi32(down));
        downresult2 = _mm_mullo_epi32(downresult2, _mm_cvtepi16_epi32(_mm_srli_si128(down, 8)));

        /*
        int* intpoint = (int*)(block);
        _mm_store_si128((__m128i*)(intpoint), rightresult);
        printf("\nRIGHT: ");

        for (int i = 0; i < 4; i++)
        {
        printf("%d ", intpoint[i]);
        }
        printf("\nLEFT: ");
        _mm_store_si128((__m128i*)(intpoint), leftresult);
        for (int i = 0; i < 4; i++)
        {
        printf("%d ", intpoint[i]);
        }
        sum1 = _mm_slli_si128(rightresult, 4);
        sum1=_mm_sub_epi32(zero, sum1);
        printf("\nNEWLEFT: ");
        _mm_store_si128((__m128i*)(intpoint), sum1);
        for (int i = 0; i < 4; i++)
        {
        printf("%d ", intpoint[i]);
        }
        exit(1);*/

        //calculate sum
        sum1 = leftresult;
        sum2 = leftresult2;
        sum1 = _mm_add_epi32(sum1, rightresult);
        sum2 = _mm_add_epi32(sum2, rightresult2);
        sum1 = _mm_add_epi32(sum1, upresult);
        sum2 = _mm_add_epi32(sum2, upresult2);
        sum1 = _mm_add_epi32(sum1, downresult);
        sum2 = _mm_add_epi32(sum2, downresult2);
        sum1 = _mm_add_epi32(sum1, toadd);
        sum2 = _mm_add_epi32(sum2, toadd);
        sum1 = _mm_srai_epi32(sum1, TO_SHIFT_CURRENT_INFLUENCE);
        sum2 = _mm_srai_epi32(sum2, TO_SHIFT_CURRENT_INFLUENCE);

        output = _mm_add_epi16(center, _mm_packs_epi32(sum1, sum2));
        _mm_store_si128((__m128i*)(block + row * uiWidth + col), output);
        //_mm_store_si128((__m128i*)(block), output);
        //calculate the up vector for the next row
        upresult = _mm_sub_epi32(zero, downresult);
        upresult2 = _mm_sub_epi32(zero, downresult2);
      }
    }


    //_mm_store_si128((__m128i*)block, difflabs);
    /*
    printf("test orig: \n");
    for (int i = 0; i < 8; i++)
    {
    for (int j = 0; j < 8; j++)
    {
    printf("%d ", block[i*uiWidth + j]);
    }
    printf("\n");
    }
    exit(1);*/
    return;
  }

  // for each pixel in block

  // These are the types of pixels:
  //
  // A BB C
  //
  // D EE F
  // D EE F
  //
  // G HH I
  //
  // If the block is larger than 4x4, the E-part is larger.
  //
  // Filter types:
  //
  // AA  BBB   CC
  // A    B     C
  //
  // D    E     F
  // DD  EEE   FF
  // D    E     F
  //
  // G    H     I
  // GG  HHH   II
  // C uses a filter of type x
  
  int32_t currentInfluenceSumCalc;
  int16_t *blockCurrentPixelPtr = block;
  int16_t *blockRightPixelPtr = blockCurrentPixelPtr+1;
  int16_t *blockNextLinePixelPtr = blockCurrentPixelPtr + uiWidth;
  int32_t *influenceRowPtrCalc = influenceRowCalc;

  // A pixel. uses filter type xx
  //                           x
  //
  // No information from previous row
  // No information from previous pixel
  // Storing information to next row
  // Storing information to next pixel

  // top left pixel; i = 0, j = 0;
  
  /////////////////////////////////////////////////////////////////////
  // New style filtering
  /////////////////////////////////////////////////////////////////////
  
  centerPixel = *(blockCurrentPixelPtr);
  rightPixel = *(blockRightPixelPtr++);
  dIR = rightPixel - centerPixel;
  dIB = *(blockNextLinePixelPtr++) - centerPixel;
  rightInfluenceCalc = std::max(0, my_k * abs(dIR) + my_m) * dIR;
  bottomInfluenceCalc = std::max(0, my_k * abs(dIB) + my_m) * dIB;
  currentInfluenceSumCalc = rightInfluenceCalc + bottomInfluenceCalc;
  leftInfluenceCalc = - rightInfluenceCalc; // When the pixel to the right is center, its left influence will be the current's right influence negated.
  *(influenceRowPtrCalc++) = - bottomInfluenceCalc; // When the pixel below is center, its top influence will be the current's bottom influence negated.
  int32_t new_style_filtering_calc = (int16_t)(centerPixel + (uint16_t)((currentInfluenceSumCalc + TO_ADD_CURRENT_INFLUENCE) >> TO_SHIFT_CURRENT_INFLUENCE));
  *(blockCurrentPixelPtr++) = new_style_filtering_calc;
  
  for (int32_t i = 1; i < (uiWidth-1); i++)
  {
    // B pixel. uses filter type xxx
    //                            x
    //
    // No information from previous row
    // Information reused from previous pixel
    // Storing information to next row
    // Storing information to next pixel

    /////////////////////////////////////////////////////////////////////
    // New style filtering
    /////////////////////////////////////////////////////////////////////
    
    centerPixel = rightPixel;
    rightPixel = *(blockRightPixelPtr++);
    dIR = rightPixel - centerPixel;
    dIB = *(blockNextLinePixelPtr++) - centerPixel;
    rightInfluenceCalc = std::max(0, my_k * abs(dIR) + my_m) * dIR;
    bottomInfluenceCalc = std::max(0, my_k * abs(dIB) + my_m) * dIB;
    currentInfluenceSumCalc = rightInfluenceCalc + bottomInfluenceCalc + leftInfluenceCalc;
    leftInfluenceCalc = - rightInfluenceCalc; // When the pixel to the right is center, its left influence will be the current's right influence negated.
    *(influenceRowPtrCalc++) = - bottomInfluenceCalc; // When the pixel below is center, its top influence will be the current's bottom influence negated.
    int32_t new_style_filtering_calc = (int16_t)(centerPixel + (uint16_t)((currentInfluenceSumCalc + TO_ADD_CURRENT_INFLUENCE) >> TO_SHIFT_CURRENT_INFLUENCE));
    *(blockCurrentPixelPtr++) = new_style_filtering_calc;

  }

  // C pixel. uses filter type xx
  //                            x
  //
  // No information from previous row
  // Information reused from previous pixel
  // Storing information to next row
  // No information to store to next pixel

  /////////////////////////////////////////////////////////////////////
  // New style filtering
  /////////////////////////////////////////////////////////////////////
  
  centerPixel = rightPixel;
  blockRightPixelPtr++;
  dIB = *(blockNextLinePixelPtr++) - centerPixel;
  bottomInfluenceCalc = std::max(0, my_k * abs(dIB) + my_m) * dIB;
  currentInfluenceSumCalc = bottomInfluenceCalc + leftInfluenceCalc;
  *(influenceRowPtrCalc++) = - bottomInfluenceCalc; // When the pixel below is center, its top influence will be the current's bottom influence negated.
  new_style_filtering_calc = (int16_t)(centerPixel + (uint16_t)((currentInfluenceSumCalc + TO_ADD_CURRENT_INFLUENCE) >> TO_SHIFT_CURRENT_INFLUENCE));
  *(blockCurrentPixelPtr++) = new_style_filtering_calc;

  for (int32_t j = 1; j < (uiHeight-1); j++)
  {
    //                           x
    // D pixel. uses filter type xx
    //                           x
    //
    // Uses information from previous row
    // No information from previous pixel
    // Storing information to next row
    // Storing information to next pixel
    
    /////////////////////////////////////////////////////////////////////
    // New style filtering
    /////////////////////////////////////////////////////////////////////
    
    influenceRowPtrCalc = influenceRowCalc; // Reset pointer to beginning of line

    centerPixel = *(blockCurrentPixelPtr);
    rightPixel = *(blockRightPixelPtr++);
    dIR = rightPixel - centerPixel;
    dIB = *(blockNextLinePixelPtr++) - centerPixel;

    rightInfluenceCalc = std::max(0, my_k * abs(dIR) + my_m) * dIR;
    bottomInfluenceCalc = std::max(0, my_k * abs(dIB) + my_m) * dIB;
    currentInfluenceSumCalc = rightInfluenceCalc + bottomInfluenceCalc + *(influenceRowPtrCalc);
    leftInfluenceCalc = - rightInfluenceCalc; // When the pixel to the right is center, its left influence will be the current's right influence negated.
    *(influenceRowPtrCalc++) = - bottomInfluenceCalc; // When the pixel below is center, its top influence will be the current's bottom influence negated.
    int32_t new_style_filtering_calc = (int16_t)(centerPixel + (uint16_t)((currentInfluenceSumCalc + TO_ADD_CURRENT_INFLUENCE) >> TO_SHIFT_CURRENT_INFLUENCE));
    *(blockCurrentPixelPtr++) = new_style_filtering_calc;

    for (int32_t i = 1; i < (uiWidth-1); i++)
    {
      //                            x
      // E pixel. uses filter type xxx
      //                            x
      //
      // Uses information from previous row
      // Uses information from previous pixel
      // Storing information to next row
      // No information to store to next pixel

      /////////////////////////////////////////////////////////////////////
      // New style filtering
      /////////////////////////////////////////////////////////////////////
      
      centerPixel = rightPixel;
      rightPixel = *(blockRightPixelPtr++);
      dIR = rightPixel - centerPixel;
      dIB = *(blockNextLinePixelPtr++) - centerPixel;
      
      rightInfluenceCalc = std::max(0, my_k * abs(dIR) + my_m) * dIR;
      bottomInfluenceCalc = std::max(0, my_k * abs(dIB) + my_m) * dIB;
      currentInfluenceSumCalc = leftInfluenceCalc + rightInfluenceCalc + bottomInfluenceCalc + *(influenceRowPtrCalc);
      leftInfluenceCalc = - rightInfluenceCalc; // When the pixel to the right is center, its left influence will be the current's right influence negated.
      *(influenceRowPtrCalc++) = - bottomInfluenceCalc; // When the pixel below is center, its top influence will be the current's bottom influence negated.
      int32_t new_style_filtering_calc = (int16_t)(centerPixel + (uint16_t)((currentInfluenceSumCalc + TO_ADD_CURRENT_INFLUENCE) >> TO_SHIFT_CURRENT_INFLUENCE));
      *(blockCurrentPixelPtr++) = new_style_filtering_calc;

    }

    //                            x
    // F pixel. uses filter type xx
    //                            x
    //
    // Uses information from previous row
    // Uses information from previous pixel
    // Storing information to next row
    // Storing information to next pixel

    /////////////////////////////////////////////////////////////////////
    // New style filtering
    /////////////////////////////////////////////////////////////////////
    
    centerPixel = rightPixel;
    blockRightPixelPtr++;
    dIB = *(blockNextLinePixelPtr++) - centerPixel;
    bottomInfluenceCalc = std::max(0, my_k * abs(dIB) + my_m) * dIB;
    currentInfluenceSumCalc = leftInfluenceCalc + bottomInfluenceCalc + *(influenceRowPtrCalc);
    *(influenceRowPtrCalc++) = - bottomInfluenceCalc; // When the pixel below is center, its top influence will be the current's bottom influence negated.
    new_style_filtering_calc = (int16_t)(centerPixel + (uint16_t)((currentInfluenceSumCalc + TO_ADD_CURRENT_INFLUENCE) >> TO_SHIFT_CURRENT_INFLUENCE));
    *(blockCurrentPixelPtr++) = new_style_filtering_calc;

  }

  influenceRowPtrCalc = influenceRowCalc;

  //                           x
  // G pixel. uses filter type xx
  //
  // Uses information from previous row
  // No information from previous pixel
  // No information to store to next row
  // Storing information to next pixel

  /////////////////////////////////////////////////////////////////////
  // New style filtering
  /////////////////////////////////////////////////////////////////////
  
  centerPixel = *(blockCurrentPixelPtr);
  rightPixel = *(blockRightPixelPtr++);
  dIR = rightPixel - centerPixel;
  rightInfluenceCalc = std::max(0, my_k * abs(dIR) + my_m) * dIR;
  currentInfluenceSumCalc = rightInfluenceCalc + *(influenceRowPtrCalc++);
  leftInfluenceCalc = - rightInfluenceCalc; // When the pixel to the right is center, its left influence will be the current's right influence negated.
  new_style_filtering_calc = (int16_t)(centerPixel + (uint16_t)((currentInfluenceSumCalc + TO_ADD_CURRENT_INFLUENCE) >> TO_SHIFT_CURRENT_INFLUENCE));
  *(blockCurrentPixelPtr++) = new_style_filtering_calc;

  for (int32_t i = 1; i < (uiWidth-1); i++)
  {
    //                            x
    // H pixel. uses filter type xxx
    //
    // Uses information from previous row
    // Uses information from previous pixel
    // No information to store to next row
    // Storing information to next pixel

    /////////////////////////////////////////////////////////////////////
    // New style filtering
    /////////////////////////////////////////////////////////////////////
    
    centerPixel = rightPixel;
    rightPixel = *(blockRightPixelPtr++);
    dIR = rightPixel - centerPixel;
    rightInfluenceCalc = std::max(0, my_k * abs(dIR) + my_m) * dIR;
    currentInfluenceSumCalc = leftInfluenceCalc + rightInfluenceCalc + *(influenceRowPtrCalc++);
    leftInfluenceCalc = - rightInfluenceCalc; // When the pixel to the right is center, its left influence will be the current's right influence negated.
    new_style_filtering_calc = (int16_t)(centerPixel + (uint16_t)((currentInfluenceSumCalc + TO_ADD_CURRENT_INFLUENCE) >> TO_SHIFT_CURRENT_INFLUENCE));
    *(blockCurrentPixelPtr++) = new_style_filtering_calc;

  }

  //                            x
  // I pixel. uses filter type xx
  //
  // Uses information from previous row
  // Uses information from previous pixel
  // No information to store to next row
  // No information to store to nex pixel

  /////////////////////////////////////////////////////////////////////
  // New style filtering
  /////////////////////////////////////////////////////////////////////
  
  centerPixel = rightPixel;
  currentInfluenceSumCalc = leftInfluenceCalc + *(influenceRowPtrCalc);
  new_style_filtering_calc = (int16_t)(centerPixel + (uint16_t)((currentInfluenceSumCalc + TO_ADD_CURRENT_INFLUENCE) >> TO_SHIFT_CURRENT_INFLUENCE));
  *(blockCurrentPixelPtr) = new_style_filtering_calc;

}

void BilateralFilter::nonlocalSmoothBlockBilateralFilter(uint32_t uiWidth, uint32_t uiHeight, int16_t block[], int32_t length, int32_t isInterBlock,
                                                         int32_t qp, int16_t blkFilt[], const ClpRng& clpRng) const
{
  int16_t rightPixel, centerPixel;
  int32_t mySumDeltaCalc[MAX_CU_SIZE + NUMBER_PADDED_SAMPLES]; // INLOOP_BILATERAL_NO_SAMPLES

  int16_t alphaRow[MAX_CU_SIZE + NUMBER_PADDED_SAMPLES]; // INLOOP_BILATERAL_NO_SAMPLES
  int16_t betaRow[MAX_CU_SIZE + NUMBER_PADDED_SAMPLES]; // INLOOP_BILATERAL_NO_SAMPLES
  int16_t gammaRow[MAX_CU_SIZE + NUMBER_PADDED_SAMPLES]; // INLOOP_BILATERAL_NO_SAMPLES
  int16_t omegaRow[MAX_CU_SIZE + NUMBER_PADDED_SAMPLES]; // INLOOP_BILATERAL_NO_SAMPLES
  int32_t   blockLengthIndexSB;
  uint8_t distanceFactor5pixels;
  // our code
  if( length >= 16 )
  {
    blockLengthIndexSB = 3;
  }
  else if ( length >= 8 )
  {
    blockLengthIndexSB = 2;
  }
  else if (uiWidth == 4 && uiHeight == 4)
  {
    blockLengthIndexSB = 0; // special case for 4x4 block
  }
  else
  {
    blockLengthIndexSB = 1;
  }
  distanceFactor5pixels = m_bilateralDistanceTable5pixelsSB[blockLengthIndexSB + 4 * isInterBlock];

  // BEGIN setting up line equation values y = my_k * x + my_m used once per block
  
#if 1
  
  // my_k:
  // largest possible value before shift is 213909504*246+33554432
  // and after shift it is (213909504*246+33554432) >> 26 = 785 < 1024 < 10 bits, whereof 4 bits are integer bits and 6 bits are fractional
  int32_t my_k_calc = (((int32_t)(approx_k[qp-18])) * ((int32_t)(distanceFactor5pixels)) + TO_ADD_BEFORE_SHIFT_K_HAT) >> (TO_SHIFT_TO_GET_K_HAT);
  int32_t my_k = -my_k_calc;
#if NUM_FRAC_BITS_K_HAT < 10
  int32_t my_m_calc = (((int32_t)(255)) * ((int32_t)(distanceFactor5pixels)) + TO_ADD_BEFORE_SHIFT_M_HAT) >> (TO_SHIFT_TO_GET_M_HAT);
#else
  int32_t my_m_calc = (((int32_t)(255)) * ((int32_t)(distanceFactor5pixels)) + TO_ADD_BEFORE_SHIFT_M_HAT) << (TO_SHIFT_TO_GET_M_HAT);
#endif
  int32_t my_m = my_m_calc;
  // my_m:
  // largest possible value is (255 * 246 + 8 ) = 62 738
  // and after shift with >> 4 it is 3921
  
  // in summary my_k is between 0 and -785
  //            my_m is between 0 and 3921
  
#else

  int32_t hires_bilateralDistanceTable5pixelsSB[7] = {5319, 4009, 3686, 2250, 4913, 3907, 3567};
  int32_t distanceFactor5pixelsHires;
  distanceFactor5pixelsHires = hires_bilateralDistanceTable5pixelsSB[blockLengthIndexSB + 4 * isInterBlock];
  int32_t my_k = (((int32_t)(approx_k[qp-18])) * ((int32_t)(distanceFactor5pixelsHires)) + TO_ADD_BEFORE_SHIFT_K_HAT_HIRES) >> (TO_SHIFT_TO_GET_K_HAT_HIRES);
  my_k = -my_k;
#if NUM_FRAC_BITS_K_HAT < 15
  int32_t my_m = (((int32_t)(255)) * ((int32_t)(distanceFactor5pixelsHires)) + TO_ADD_BEFORE_SHIFT_M_HAT_HIRES) >> (TO_SHIFT_TO_GET_M_HAT_HIRES);
#else
  int32_t my_m = (((int32_t)(255)) * ((int32_t)(distanceFactor5pixelsHires)) + TO_ADD_BEFORE_SHIFT_M_HAT_HIRES) << (TO_SHIFT_TO_GET_M_HAT_HIRES);
#endif

#endif
  // END setting up line parameters
  
  
 
  if (uiWidth >= 8)
    //if (uiWidth >=8)
  {
    /*
    printf("test orig: \n");
    for (int i = 0; i < 12; i++)
    {
    for (int j = 0; j < 12; j++)
    {
    printf("%d ", block[i*12 + j]);
    }
    printf("\n");
    }
    */
    /*
    for (int row = 0; row < uiHeight; row++)
    {
    //left one
    buffer[(row + 1)*(uiWidth + 2)] = block[(row+ NUMBER_PADDED_SAMPLES)*(uiWidth+ NUMBER_PADDED_SAMPLES*2)+NUMBER_PADDED_SAMPLES];
    //middle ones
    memcpy(buffer + (row + 1) * (uiWidth + 2) + 1, block + (row + NUMBER_PADDED_SAMPLES) * (uiWidth + NUMBER_PADDED_SAMPLES*2) + NUMBER_PADDED_SAMPLES, uiWidth * sizeof(short));
    buffer[(row + 1)*(uiWidth + 2) + uiWidth + 1] = block[(row + NUMBER_PADDED_SAMPLES)*(uiWidth + NUMBER_PADDED_SAMPLES*2) + (uiWidth + NUMBER_PADDED_SAMPLES) - 1];
    }
    memcpy(buffer, buffer+uiWidth+2, (uiWidth+2) * sizeof(short));
    memcpy(buffer + (uiWidth + 2)*(uiHeight + 1), buffer + (uiWidth + 2)*(uiHeight), (uiWidth + 2) * sizeof(short));
    */
    /*
    for (int row = 0; row < (int)uiHeight+2; row++)
    {
    //left area
    memcpy(buffer2 + row * (uiWidth + 4), block + row * (uiWidth + 4), (uiWidth + 2) * sizeof(short));
    //right two..
    buffer2[row * (uiWidth + 4) + uiWidth + 2] = block[row * (uiWidth + 4) + uiWidth + 1];
    buffer2[row * (uiWidth + 4) + uiWidth + 3] = block[row * (uiWidth + 4) + uiWidth + 1];
    }
    memcpy(buffer2 + (uiWidth + 4)*(uiHeight + 2), buffer2 + (uiWidth + 4)*(uiHeight+1), (uiWidth + 4) * sizeof(short));
    memcpy(buffer2 + (uiWidth + 4)*(uiHeight + 3), buffer2 + (uiWidth + 4)*(uiHeight + 1), (uiWidth + 4) * sizeof(short));
    */
    for (int row = 0; row < (int)uiHeight + 2; row++)
    {
      //left area
      //memcpy(buffer2 + row * (uiWidth + 4), block + row * (uiWidth + 4), (uiWidth + 2) * sizeof(short));
      //right two..
      //buffer2[row * (uiWidth + 4) + uiWidth + 2] = block[row * (uiWidth + 4) + uiWidth + 1];
      block[row * (uiWidth + 4) + uiWidth + 3] = block[row * (uiWidth + 4) + uiWidth + 1];
    }
    //memcpy(buffer2 + (uiWidth + 4)*(uiHeight + 2), buffer2 + (uiWidth + 4)*(uiHeight + 1), (uiWidth + 4) * sizeof(short));
    memcpy(block + (uiWidth + 4)*(uiHeight + 3), block + (uiWidth + 4)*(uiHeight + 1), (uiWidth + 4) * sizeof(short));
    /*
    printf("test orig: \n");
    for (int i = 0; i < 10; i++)
    {
    for (int j = 0; j < 10; j++)
    {
    printf("%d ", buffer[i * 10 + j]);
    }
    printf("\n");
    }

    printf("test orig: \n");
    for (int i = 0; i < 12; i++)
    {
    for (int j = 0; j < 12; j++)
    {
    printf("%d ", buffer2[i * 12 + j]);
    }
    printf("\n");
    }
    */
    __m128i c1, l1, r1, d1, c, l, r, d, dl, dr, dd, ddl, ddr, u, ul, ur, /*ull,*/ll, dll,/*urr,*/rr, drr, toprow, midrow, toprowleft, midrowleft, bottomrowleft, bottomrow, toprowright, midrowright, bottomrowright, sum, k_vec, m_vec, zero, toadd, low, high, downresult, downresult2, down, leftresult, leftresult2, left, rightresult, rightresult2, upresult, upresult2, right, sum1, sum2;
    k_vec = _mm_set1_epi16((short)my_k);
    m_vec = _mm_set1_epi32(my_m);
    zero = _mm_set1_epi32(0);
    toadd = _mm_set1_epi32(TO_ADD_CURRENT_INFLUENCE);

    int eWidth = uiWidth + 4;
    short* temppoint;
    for (int col = 0; col < uiWidth; col += 8)
    {
      upresult = zero;
      upresult2 = zero;
      int tl = ((int)uiWidth + 4)*(-1 + 2) + 2 + col;
      temppoint = block + tl;
      c = _mm_loadu_si128((__m128i*)(temppoint));
      l = _mm_loadu_si128((__m128i*)(temppoint - 1));
      r = _mm_loadu_si128((__m128i*)(temppoint + 1));
      d = _mm_loadu_si128((__m128i*)(temppoint + eWidth));
      dl = _mm_loadu_si128((__m128i*)(temppoint + eWidth - 1));
      dr = _mm_loadu_si128((__m128i*)(temppoint + eWidth + 1));
      u = _mm_loadu_si128((__m128i*)(temppoint - eWidth));
      ul = _mm_loadu_si128((__m128i*)(temppoint - eWidth - 1));
      ur = _mm_loadu_si128((__m128i*)(temppoint - eWidth + 1));
      //for left only
      //ull = _mm_loadu_si128((__m128i*)(temppoint - eWidth - 2));
      ll = _mm_loadu_si128((__m128i*)(temppoint - 2));
      dll = _mm_loadu_si128((__m128i*)(temppoint + eWidth - 2));

      //for right only
      //urr = _mm_loadu_si128((__m128i*)(temppoint - eWidth + 2));
      rr = _mm_loadu_si128((__m128i*)(temppoint + 2));
      drr = _mm_loadu_si128((__m128i*)(temppoint + eWidth + 2));

      //for down only
      dd = _mm_loadu_si128((__m128i*)(temppoint + 2 * eWidth));
      ddl = _mm_loadu_si128((__m128i*)(temppoint + 2 * eWidth - 1));
      ddr = _mm_loadu_si128((__m128i*)(temppoint + 2 * eWidth + 1));
      toprow = _mm_abs_epi16(_mm_sub_epi16(ul, l));
      toprow = _mm_add_epi16(toprow, _mm_abs_epi16(_mm_sub_epi16(u, c)));
      toprow = _mm_add_epi16(toprow, _mm_abs_epi16(_mm_sub_epi16(ur, r)));

      midrow = _mm_abs_epi16(_mm_sub_epi16(l, dl));
      midrow = _mm_add_epi16(midrow, _mm_abs_epi16(_mm_sub_epi16(c, d)));
      midrow = _mm_add_epi16(midrow, _mm_abs_epi16(_mm_sub_epi16(r, dr)));

      bottomrow = _mm_abs_epi16(_mm_sub_epi16(dl, ddl));
      bottomrow = _mm_add_epi16(bottomrow, _mm_abs_epi16(_mm_sub_epi16(d, dd)));
      bottomrow = _mm_add_epi16(bottomrow, _mm_abs_epi16(_mm_sub_epi16(dr, ddr)));

      sum = _mm_add_epi16(toprow, midrow);
      sum = _mm_add_epi16(sum, bottomrow);
      sum = _mm_srai_epi16(sum, 3);

      low = _mm_mullo_epi16(sum, k_vec);
      high = _mm_mulhi_epi16(sum, k_vec);
      downresult = _mm_unpacklo_epi16(low, high);
      downresult2 = _mm_unpackhi_epi16(low, high);

      downresult = _mm_add_epi32(downresult, m_vec);
      downresult = _mm_max_epi32(downresult, zero);
      downresult2 = _mm_add_epi32(downresult2, m_vec);
      downresult2 = _mm_max_epi32(downresult2, zero);

      //aand multibly by diff
      down = _mm_sub_epi16(d, c);
      downresult = _mm_mullo_epi32(downresult, _mm_cvtepi16_epi32(down));
      downresult2 = _mm_mullo_epi32(downresult2, _mm_cvtepi16_epi32(_mm_srli_si128(down, 8)));

      upresult = _mm_sub_epi32(zero, downresult);
      upresult2 = _mm_sub_epi32(zero, downresult2);


      midrowleft = _mm_abs_epi16(_mm_sub_epi16(ll, l));
      midrowleft = _mm_add_epi16(midrowleft, _mm_abs_epi16(_mm_sub_epi16(l, c)));
      midrowleft = _mm_add_epi16(midrowleft, _mm_abs_epi16(_mm_sub_epi16(c, r)));

      bottomrowleft = _mm_abs_epi16(_mm_sub_epi16(dll, dl));
      bottomrowleft = _mm_add_epi16(bottomrowleft, _mm_abs_epi16(_mm_sub_epi16(dl, d)));
      bottomrowleft = _mm_add_epi16(bottomrowleft, _mm_abs_epi16(_mm_sub_epi16(d, dr)));

      midrowright = _mm_abs_epi16(_mm_sub_epi16(rr, r));
      midrowright = _mm_add_epi16(midrowright, _mm_abs_epi16(_mm_sub_epi16(r, c)));
      midrowright = _mm_add_epi16(midrowright, _mm_abs_epi16(_mm_sub_epi16(c, l)));

      bottomrowright = _mm_abs_epi16(_mm_sub_epi16(drr, dr));
      bottomrowright = _mm_add_epi16(bottomrowright, _mm_abs_epi16(_mm_sub_epi16(dr, d)));
      bottomrowright = _mm_add_epi16(bottomrowright, _mm_abs_epi16(_mm_sub_epi16(d, dl)));

      for (int row = 0; row < uiHeight; row++)
      {
        tl = ((int)uiWidth + 4)*(row + 2) + 2 + col;
        //int tl1 = ((int)uiWidth + 2)*(row + 1) + 1 + col;
        temppoint = block + tl;
        c1 = _mm_loadu_si128((__m128i*)(temppoint));
        l1 = _mm_loadu_si128((__m128i*)(temppoint - 1));
        r1 = _mm_loadu_si128((__m128i*)(temppoint + 1));
        d1 = _mm_loadu_si128((__m128i*)(temppoint + (eWidth)));
        //fix down!
        c = _mm_loadu_si128((__m128i*)(temppoint));
        //l = _mm_loadu_si128((__m128i*)(temppoint - 1));
        //r = _mm_loadu_si128((__m128i*)(temppoint + 1));

        d = dd;
        dl = ddl;
        dr = ddr;
        //d = _mm_loadu_si128((__m128i*)(temppoint + eWidth));
        //dl = _mm_loadu_si128((__m128i*)(temppoint + eWidth - 1));
        //dr = _mm_loadu_si128((__m128i*)(temppoint + eWidth + 1));

        //u = _mm_loadu_si128((__m128i*)(temppoint - eWidth));
        //ul = _mm_loadu_si128((__m128i*)(temppoint - eWidth - 1));
        //ur = _mm_loadu_si128((__m128i*)(temppoint - eWidth + 1));

        //for down only
        dd = _mm_loadu_si128((__m128i*)(temppoint + 2 * eWidth));
        ddl = _mm_loadu_si128((__m128i*)(temppoint + 2 * eWidth - 1));
        ddr = _mm_loadu_si128((__m128i*)(temppoint + 2 * eWidth + 1));

        //for left only
        //ull = _mm_loadu_si128((__m128i*)(temppoint - eWidth - 2));
        //ll = _mm_loadu_si128((__m128i*)(temppoint - 2));
        dll = _mm_loadu_si128((__m128i*)(temppoint + eWidth - 2));

        //for right only
        //urr = _mm_loadu_si128((__m128i*)(temppoint - eWidth + 2));
        //rr = _mm_loadu_si128((__m128i*)(temppoint + 2));
        drr = _mm_loadu_si128((__m128i*)(temppoint + eWidth + 2));

        //DOWN
        //toprow = _mm_abs_epi16(_mm_sub_epi16(ul, l));
        //toprow = _mm_add_epi16(toprow, _mm_abs_epi16(_mm_sub_epi16(u, c)));
        //toprow = _mm_add_epi16(toprow, _mm_abs_epi16(_mm_sub_epi16(ur, r)));
        toprow = midrow;
        //midrow = _mm_abs_epi16(_mm_sub_epi16(l, dl));
        //midrow = _mm_add_epi16(midrow, _mm_abs_epi16(_mm_sub_epi16(c, d)));
        //midrow = _mm_add_epi16(midrow, _mm_abs_epi16(_mm_sub_epi16(r, dr)));
        midrow = bottomrow;
        bottomrow = _mm_abs_epi16(_mm_sub_epi16(dl, ddl));
        bottomrow = _mm_add_epi16(bottomrow, _mm_abs_epi16(_mm_sub_epi16(d, dd)));
        bottomrow = _mm_add_epi16(bottomrow, _mm_abs_epi16(_mm_sub_epi16(dr, ddr)));

        sum = _mm_add_epi16(toprow, midrow);
        sum = _mm_add_epi16(sum, bottomrow);
        sum = _mm_srai_epi16(sum, 3);

        low = _mm_mullo_epi16(sum, k_vec);
        high = _mm_mulhi_epi16(sum, k_vec);
        downresult = _mm_unpacklo_epi16(low, high);
        downresult2 = _mm_unpackhi_epi16(low, high);

        //max(0, x+m)
        downresult = _mm_add_epi32(downresult, m_vec);
        downresult = _mm_max_epi32(downresult, zero);
        downresult2 = _mm_add_epi32(downresult2, m_vec);
        downresult2 = _mm_max_epi32(downresult2, zero);

        //aand multibly by diff
        down = _mm_sub_epi16(d1, c1);
        downresult = _mm_mullo_epi32(downresult, _mm_cvtepi16_epi32(down));
        downresult2 = _mm_mullo_epi32(downresult2, _mm_cvtepi16_epi32(_mm_srli_si128(down, 8)));


        //LEFT
        //toprowleft = _mm_abs_epi16(_mm_sub_epi16(ull, ul));
        //toprowleft = _mm_add_epi16(toprowleft, _mm_abs_epi16(_mm_sub_epi16(ul, u)));
        //toprowleft = _mm_add_epi16(toprowleft, _mm_abs_epi16(_mm_sub_epi16(u, ur)));
        toprowleft = midrowleft;
        //midrowleft = _mm_abs_epi16(_mm_sub_epi16(ll, l));
        //midrowleft = _mm_add_epi16(midrowleft, _mm_abs_epi16(_mm_sub_epi16(l, c)));
        //midrowleft = _mm_add_epi16(midrowleft, _mm_abs_epi16(_mm_sub_epi16(c, r)));
        midrowleft = bottomrowleft;
        bottomrowleft = _mm_abs_epi16(_mm_sub_epi16(dll, dl));
        bottomrowleft = _mm_add_epi16(bottomrowleft, _mm_abs_epi16(_mm_sub_epi16(dl, d)));
        bottomrowleft = _mm_add_epi16(bottomrowleft, _mm_abs_epi16(_mm_sub_epi16(d, dr)));

        sum = _mm_add_epi16(toprowleft, midrowleft);
        sum = _mm_add_epi16(sum, bottomrowleft);
        sum = _mm_srai_epi16(sum, 3);

        low = _mm_mullo_epi16(sum, k_vec);
        high = _mm_mulhi_epi16(sum, k_vec);
        leftresult = _mm_unpacklo_epi16(low, high);
        leftresult2 = _mm_unpackhi_epi16(low, high);

        //max(0, x+m)
        leftresult = _mm_add_epi32(leftresult, m_vec);
        leftresult = _mm_max_epi32(leftresult, zero);
        leftresult2 = _mm_add_epi32(leftresult2, m_vec);
        leftresult2 = _mm_max_epi32(leftresult2, zero);


        //aand multibly by diff
        left = _mm_sub_epi16(l1, c1);
        leftresult = _mm_mullo_epi32(leftresult, _mm_cvtepi16_epi32(left));
        leftresult2 = _mm_mullo_epi32(leftresult2, _mm_cvtepi16_epi32(_mm_srli_si128(left, 8)));


        //RIGHT
        //toprowright = _mm_abs_epi16(_mm_sub_epi16(urr, ur));
        //toprowright = _mm_add_epi16(toprowright, _mm_abs_epi16(_mm_sub_epi16(ur, u)));
        //toprowright = _mm_add_epi16(toprowright, _mm_abs_epi16(_mm_sub_epi16(u, ul)));
        toprowright = midrowright;
        //midrowright = _mm_abs_epi16(_mm_sub_epi16(rr, r));
        //midrowright = _mm_add_epi16(midrowright, _mm_abs_epi16(_mm_sub_epi16(r, c)));
        //midrowright = _mm_add_epi16(midrowright, _mm_abs_epi16(_mm_sub_epi16(c, l)));
        midrowright = bottomrowright;
        bottomrowright = _mm_abs_epi16(_mm_sub_epi16(drr, dr));
        bottomrowright = _mm_add_epi16(bottomrowright, _mm_abs_epi16(_mm_sub_epi16(dr, d)));
        bottomrowright = _mm_add_epi16(bottomrowright, _mm_abs_epi16(_mm_sub_epi16(d, dl)));

        sum = _mm_add_epi16(toprowright, midrowright);
        sum = _mm_add_epi16(sum, bottomrowright);
        sum = _mm_srai_epi16(sum, 3);
        low = _mm_mullo_epi16(sum, k_vec);
        high = _mm_mulhi_epi16(sum, k_vec);
        rightresult = _mm_unpacklo_epi16(low, high);
        rightresult2 = _mm_unpackhi_epi16(low, high);

        //max(0, x+m)
        rightresult = _mm_add_epi32(rightresult, m_vec);
        rightresult = _mm_max_epi32(rightresult, zero);
        rightresult2 = _mm_add_epi32(rightresult2, m_vec);
        rightresult2 = _mm_max_epi32(rightresult2, zero);

        //aand multibly by diff
        right = _mm_sub_epi16(r1, c1);
        rightresult = _mm_mullo_epi32(rightresult, _mm_cvtepi16_epi32(right));
        rightresult2 = _mm_mullo_epi32(rightresult2, _mm_cvtepi16_epi32(_mm_srli_si128(right, 8)));


        //calculate sum
        sum1 = leftresult;
        sum2 = leftresult2;
        //sum1 = zero;
        //sum2 = zero;
        sum1 = _mm_add_epi32(sum1, rightresult);
        sum2 = _mm_add_epi32(sum2, rightresult2);
        sum1 = _mm_add_epi32(sum1, upresult);
        sum2 = _mm_add_epi32(sum2, upresult2);
        sum1 = _mm_add_epi32(sum1, downresult);
        sum2 = _mm_add_epi32(sum2, downresult2);
        sum1 = _mm_add_epi32(sum1, toadd);
        sum2 = _mm_add_epi32(sum2, toadd);
        sum1 = _mm_srai_epi32(sum1, TO_SHIFT_CURRENT_INFLUENCE);
        sum2 = _mm_srai_epi32(sum2, TO_SHIFT_CURRENT_INFLUENCE);

        sum = _mm_add_epi16(c, _mm_packs_epi32(sum1, sum2));
        _mm_store_si128((__m128i*)(blkFilt + (row + 2) * (eWidth + 4) + col + 2), sum);
        //calculate the up vector for the next row
        upresult = _mm_sub_epi32(zero, downresult);
        upresult2 = _mm_sub_epi32(zero, downresult2);
      }
    }
    /*
    printf("test output: \n");
    for (int i = 2; i < 10; i++)
    {
    for (int j = 2; j < 10; j++)
    {
    printf("%d ", blkFilt[i * 12 + j]);
    }
    printf("\n");
    }
    */
    //exit(1);

    return;
  }

  int64_t bottomInfluenceCalc;
  int64_t rightInfluenceCalc;
  int64_t leftInfluenceCalc;


  int32_t dIB, dIR, dNLIB, dNLIR;
  int32_t alpha, beta, gamma, delta;
  int32_t omega;

  int32_t currentInfluenceCalcSum;

  uiWidth += (NUMBER_PADDED_SAMPLES << 1);  // INLOOP_BILATERAL_NO_SAMPLES
  uiHeight += (NUMBER_PADDED_SAMPLES << 1); // INLOOP_BILATERAL_NO_SAMPLES
  
  int16_t *blockCurrentPixelPtr = block + uiWidth + 1; // start from the second pixel of the second row in the padded block
  int16_t *blkFiltCurrentPixelPtr = blkFilt + uiWidth + 1;
  int16_t *blockRightPixelPtr = blockCurrentPixelPtr + 1;
  int16_t *blockNextLinePixelPtr = blockCurrentPixelPtr + uiWidth;
  int32_t *mySumDeltaCalcPtr = mySumDeltaCalc;
  int16_t *alphaRowPtr = alphaRow;
  int16_t *betaRowPtr = betaRow;
  int16_t *gammaRowPtr = gammaRow;
  int16_t deltaRowValue;
  int16_t *omegaRowPtr = omegaRow;

  blockCurrentPixelPtr++;
  rightPixel = *(blockRightPixelPtr++);
  
  blkFiltCurrentPixelPtr++;
  blockNextLinePixelPtr++;
  
  int32_t offsetY0 = -NL_HALFW * uiWidth;
  int32_t twoOffsetY0 = 2*offsetY0;

  for (int32_t i = NUMBER_PADDED_SAMPLES; i < (uiWidth - NUMBER_PADDED_SAMPLES); i++)
  { // only store the bottom weights for the second row of the padded block
    
    centerPixel = rightPixel;
    rightPixel = *(blockRightPixelPtr++);
    
    dIB = *(blockNextLinePixelPtr++) - centerPixel;

    // update non-local values
    *(alphaRowPtr) = abs(*(blockCurrentPixelPtr+offsetY0-1) - *(blockCurrentPixelPtr-1)) + abs(*(blockCurrentPixelPtr+offsetY0) - *(blockCurrentPixelPtr)) + abs(*(blockCurrentPixelPtr+offsetY0+1) - *(blockCurrentPixelPtr+1));
    *(betaRowPtr) = abs(*(blockCurrentPixelPtr-1) - *(blockCurrentPixelPtr-offsetY0-1)) + abs(*(blockCurrentPixelPtr) - *(blockCurrentPixelPtr-offsetY0)) + abs(*(blockCurrentPixelPtr+1) - *(blockCurrentPixelPtr-offsetY0+1));
    *(gammaRowPtr) = abs(*(blockCurrentPixelPtr-offsetY0-1) - *(blockCurrentPixelPtr-twoOffsetY0-1)) + abs(*(blockCurrentPixelPtr-offsetY0) - *(blockCurrentPixelPtr-twoOffsetY0)) + abs(*(blockCurrentPixelPtr-offsetY0+1) - *(blockCurrentPixelPtr-twoOffsetY0+1));
    *(omegaRowPtr) = *(alphaRowPtr++) + *(betaRowPtr++) + *(gammaRowPtr++);
    dNLIB = (*(omegaRowPtr++)) >> 3;
    blockCurrentPixelPtr++;
    
    // reminder:  my_k is between 0 and -785
    //            my_m is between 0 and 3921
    
    // dNLIB can be clamped to be between 0 an 219 < 256. Otherwise it is 0 to 1023
    // -my_k * dNLIB < 785*219 = 171915   or   < 785*1023  =  803055
    // smallest possible value: -171915 + 0 = -171915   or 803055
    // largest possible value: 0 + 3921 = 3921
    // in summary: my_y is between -171915 and 3921 before max    or between -803055 and 3921
    //             we can add 13 bits before we run out of int32_t     or we can add 11 bits before we run out
    
    bottomInfluenceCalc = std::max(0, my_k * dNLIB + my_m) * dIB;
    
    // after max operation: between 0 and 3921
    // after multiplying by dIB: between -1023*3921 and 1023*3921, i.e., -4011183 and 4011183
    // before: LUT * dIB, i.e, 255 * 1023

    *(mySumDeltaCalcPtr++) = (int32_t)-bottomInfluenceCalc;
    blkFiltCurrentPixelPtr++;
  }
  
  blockCurrentPixelPtr += (NUMBER_PADDED_SAMPLES + 1);   // second pixel of next row
  blkFiltCurrentPixelPtr += (NUMBER_PADDED_SAMPLES + 1); // second pixel of next row
  
  blockRightPixelPtr = blockCurrentPixelPtr + 1;
  blockNextLinePixelPtr = blockCurrentPixelPtr + uiWidth;
  
  for (int32_t j = NUMBER_PADDED_SAMPLES; j < (uiHeight - NUMBER_PADDED_SAMPLES - 1); j++)
  { // for second pixel of each row, only store right weight
    alphaRowPtr = alphaRow;
    betaRowPtr = betaRow;
    gammaRowPtr = gammaRow;
    omegaRowPtr = omegaRow;

    centerPixel = *(blockCurrentPixelPtr);
    rightPixel = *(blockRightPixelPtr++);
    dIR = rightPixel - centerPixel;

    // update non-local values
    alpha = abs(*(blockCurrentPixelPtr+offsetY0-1) - *(blockCurrentPixelPtr+offsetY0)) + abs(*(blockCurrentPixelPtr-1) - *(blockCurrentPixelPtr)) + abs(*(blockCurrentPixelPtr-offsetY0-1) - *(blockCurrentPixelPtr-offsetY0));
    beta = abs(*(blockCurrentPixelPtr+offsetY0) - *(blockCurrentPixelPtr+offsetY0+1)) + abs(*(blockCurrentPixelPtr) - *(blockCurrentPixelPtr+1)) + abs(*(blockCurrentPixelPtr-offsetY0) - *(blockCurrentPixelPtr-offsetY0+1));
    gamma = abs(*(blockCurrentPixelPtr+offsetY0+1) - *(blockCurrentPixelPtr+offsetY0+2)) + abs(*(blockCurrentPixelPtr+1) - *(blockCurrentPixelPtr+2)) + abs(*(blockCurrentPixelPtr-offsetY0+1) - *(blockCurrentPixelPtr-offsetY0+2));
    omega = alpha + beta + gamma;
    dNLIR = (omega) >> 3;
    blockCurrentPixelPtr++;

    mySumDeltaCalcPtr = mySumDeltaCalc;
    rightInfluenceCalc = std::max(0, my_k * dNLIR + my_m) * dIR;
    leftInfluenceCalc = -rightInfluenceCalc;
    blkFiltCurrentPixelPtr++;
    blockNextLinePixelPtr++;
    
    for (int32_t i = NUMBER_PADDED_SAMPLES; i < (uiWidth - NUMBER_PADDED_SAMPLES - 1); i++)
    { // store both right and bottom weight
      centerPixel = rightPixel;
      rightPixel = *(blockRightPixelPtr++);
      dIR = rightPixel - centerPixel;
      dIB = *(blockNextLinePixelPtr++) - centerPixel;

      // update non-local values
      delta = abs(*(blockCurrentPixelPtr+offsetY0+1) - *(blockCurrentPixelPtr+offsetY0+2)) + abs(*(blockCurrentPixelPtr+1) - *(blockCurrentPixelPtr+2)) + abs(*(blockCurrentPixelPtr-offsetY0+1) - *(blockCurrentPixelPtr-offsetY0+2));
      omega = omega - alpha + delta;
      dNLIR = (omega) >> 3;
      alpha = beta;
      beta = gamma;
      gamma = delta;
      
      // update non-local values
      deltaRowValue = abs(*(blockCurrentPixelPtr-offsetY0-1) - *(blockCurrentPixelPtr-twoOffsetY0-1)) + abs(*(blockCurrentPixelPtr-offsetY0) - *(blockCurrentPixelPtr-twoOffsetY0)) + abs(*(blockCurrentPixelPtr-offsetY0+1) - *(blockCurrentPixelPtr-twoOffsetY0+1));
      *(omegaRowPtr) = *(omegaRowPtr) - *(alphaRowPtr) + deltaRowValue;
      *(alphaRowPtr++) = *(betaRowPtr);
      *(betaRowPtr++) = *(gammaRowPtr);
      *(gammaRowPtr++) = deltaRowValue;
      dNLIB = (*(omegaRowPtr++)) >> 3;
      blockCurrentPixelPtr++;
      
      rightInfluenceCalc = std::max(0, my_k * dNLIR + my_m) * dIR;
      bottomInfluenceCalc = std::max(0, my_k * dNLIB + my_m) * dIB;
      currentInfluenceCalcSum = (int32_t)(rightInfluenceCalc + bottomInfluenceCalc + leftInfluenceCalc + *(mySumDeltaCalcPtr));
      leftInfluenceCalc = -rightInfluenceCalc;
      *(mySumDeltaCalcPtr++) = (int32_t)-bottomInfluenceCalc;
      
      int32_t new_style_filtering_calc = (int16_t)(centerPixel + (uint16_t)((currentInfluenceCalcSum + TO_ADD_CURRENT_INFLUENCE) >> TO_SHIFT_CURRENT_INFLUENCE));
      *(blkFiltCurrentPixelPtr++) = ClipPel(new_style_filtering_calc, clpRng);
    }
    
    // last pixel of each row of unpadded block
    centerPixel = rightPixel;
    dIB = *(blockNextLinePixelPtr)-centerPixel;

    // update non-local values
    deltaRowValue = abs(*(blockCurrentPixelPtr-offsetY0-1) - *(blockCurrentPixelPtr-twoOffsetY0-1)) + abs(*(blockCurrentPixelPtr-offsetY0) - *(blockCurrentPixelPtr-twoOffsetY0)) + abs(*(blockCurrentPixelPtr-offsetY0+1) - *(blockCurrentPixelPtr-twoOffsetY0+1));
    *(omegaRowPtr) = *(omegaRowPtr) - *(alphaRowPtr) + deltaRowValue;
    *(alphaRowPtr++) = *(betaRowPtr);
    *(betaRowPtr++) = *(gammaRowPtr);
    *(gammaRowPtr++) = deltaRowValue;
    dNLIB = (*(omegaRowPtr++) ) >> 3;

    bottomInfluenceCalc = std::max(0, my_k * dNLIB + my_m) * dIB;
    currentInfluenceCalcSum = (int32_t)(bottomInfluenceCalc + leftInfluenceCalc + *(mySumDeltaCalcPtr));
    *(mySumDeltaCalcPtr++) = (int32_t)-bottomInfluenceCalc;

    int32_t new_style_filtering_calc = (int16_t)(centerPixel + (uint16_t)((currentInfluenceCalcSum + TO_ADD_CURRENT_INFLUENCE) >> TO_SHIFT_CURRENT_INFLUENCE));
    *(blkFiltCurrentPixelPtr) = ClipPel(new_style_filtering_calc, clpRng);

    blockCurrentPixelPtr += (NUMBER_PADDED_SAMPLES << 1);   // second pixel of next row
    blkFiltCurrentPixelPtr += (NUMBER_PADDED_SAMPLES << 1); // second pixel of next row
    blockRightPixelPtr = blockCurrentPixelPtr + 1;
    blockNextLinePixelPtr = blockCurrentPixelPtr + uiWidth;
  }
  
  // last line of the unpadded block
  centerPixel = *(blockCurrentPixelPtr);
  rightPixel = *(blockRightPixelPtr++);
  dIR = rightPixel - centerPixel;

  // update non-local values
  alpha = abs(*(blockCurrentPixelPtr+offsetY0-1) - *(blockCurrentPixelPtr+offsetY0)) + abs(*(blockCurrentPixelPtr-1) - *(blockCurrentPixelPtr)) + abs(*(blockCurrentPixelPtr-offsetY0-1) - *(blockCurrentPixelPtr-offsetY0));
  beta = abs(*(blockCurrentPixelPtr+offsetY0) - *(blockCurrentPixelPtr+offsetY0+1)) + abs(*(blockCurrentPixelPtr) - *(blockCurrentPixelPtr+1)) + abs(*(blockCurrentPixelPtr-offsetY0) - *(blockCurrentPixelPtr-offsetY0+1));
  gamma = abs(*(blockCurrentPixelPtr+offsetY0+1) - *(blockCurrentPixelPtr+offsetY0+2)) + abs(*(blockCurrentPixelPtr+1) - *(blockCurrentPixelPtr+2)) + abs(*(blockCurrentPixelPtr-offsetY0+1) - *(blockCurrentPixelPtr-offsetY0+2));
  omega = alpha + beta + gamma;
  dNLIR = (omega) >> 3;
  blockCurrentPixelPtr++;

  mySumDeltaCalcPtr = mySumDeltaCalc;
  rightInfluenceCalc = std::max(0, my_k * dNLIR + my_m) * dIR;
  leftInfluenceCalc = -rightInfluenceCalc;
  
  blkFiltCurrentPixelPtr++;
  blockNextLinePixelPtr++;
  
  for (int32_t i = NUMBER_PADDED_SAMPLES; i < (uiWidth - NUMBER_PADDED_SAMPLES - 1); i++)
  { // store both right and bottom weight
    centerPixel = rightPixel;
    rightPixel = *(blockRightPixelPtr++);
    dIR = rightPixel - centerPixel;

    // update non-local values
    delta = abs(*(blockCurrentPixelPtr+offsetY0+1) - *(blockCurrentPixelPtr+offsetY0+2)) + abs(*(blockCurrentPixelPtr+1) - *(blockCurrentPixelPtr+2)) + abs(*(blockCurrentPixelPtr-offsetY0+1) - *(blockCurrentPixelPtr-offsetY0+2));
    omega = omega - alpha + delta;
    alpha = beta;
    beta = gamma;
    gamma = delta;
    dNLIR = (omega ) >> 3;
    blockCurrentPixelPtr++;

    rightInfluenceCalc = std::max(0, my_k * dNLIR + my_m) * dIR;
    currentInfluenceCalcSum = (int32_t)(rightInfluenceCalc + leftInfluenceCalc + *(mySumDeltaCalcPtr++));
    leftInfluenceCalc = (int32_t)-rightInfluenceCalc;

    int32_t new_style_filtering_calc = (int16_t)(centerPixel + (uint16_t)((currentInfluenceCalcSum + TO_ADD_CURRENT_INFLUENCE) >> TO_SHIFT_CURRENT_INFLUENCE));
    *(blkFiltCurrentPixelPtr++) = ClipPel(new_style_filtering_calc, clpRng);
  }
  
  // last pixel of the last line of unpadded block
  centerPixel = rightPixel;
  currentInfluenceCalcSum = (int32_t)(leftInfluenceCalc + *(mySumDeltaCalcPtr));

  int32_t new_style_filtering_calc = (int16_t)(centerPixel + (uint16_t)((currentInfluenceCalcSum + TO_ADD_CURRENT_INFLUENCE) >> TO_SHIFT_CURRENT_INFLUENCE));
  *(blkFiltCurrentPixelPtr) = ClipPel(new_style_filtering_calc, clpRng);

}


void BilateralFilter::myCopyDataForward(Pel *myDest, Pel *mySource, int32_t uiHeight, int32_t uiWidth, int32_t uiStride)
{
  for( unsigned j = 0; j < uiHeight; j++)
  {
    //             dest                              source                 num
    memcpy(myDest + j * uiWidth, mySource + j * uiStride, uiWidth * sizeof(Pel));
    //                   start                      end                         dest
    //      std::copy(piReco + j * uiStride, piReco + j * uiStride + uiWidth, tempblock.data() + (j + NUMBER_PADDED_SAMPLES) * uiWidthExt + NUMBER_PADDED_SAMPLES);
    
    //      std::copy(piReco + j * uiStride, piReco + j * uiStride + uiWidth, myTempblock.data() + j*uiWidth);
    
    
  }
}


void BilateralFilter::bilateralFilterIntra( PelBuf& recoBuf, int qp)
{
  const unsigned uiWidth  = recoBuf.width;
  const unsigned uiHeight = recoBuf.height;
  const unsigned uiStride = recoBuf.stride;
  Pel             *piReco = recoBuf.buf;
  __m128i tempblocki[100 * 100];
  short * tempblock = (short*)tempblocki;
  // Exclude 4x4, 4x8 and 8x4 (slush2)
//  if( (uiWidth == 4 && uiHeight == 4) || (uiWidth == 4 && uiHeight == 8) || (uiWidth == 8 && uiHeight == 4) )
//    return;

    if( (uiWidth == 4 && uiHeight == 4) )
      return;
  
  if( recoBuf.stride == recoBuf.width )
  {
    smoothBlockBilateralFilter(uiWidth, uiHeight, piReco, 0, qp);
  }
  else
  {
    for( unsigned j = 0; j < uiHeight; j++)
    {
      memcpy(tempblock + j * uiWidth, piReco + j * uiStride, uiWidth * sizeof(Pel));
    }
    
    smoothBlockBilateralFilter(uiWidth, uiHeight, tempblock, 0, qp);
    
    for( unsigned j = 0; j < uiHeight; j++)
    {
      memcpy(piReco + j * uiStride, tempblock + j * uiWidth, uiWidth * sizeof(Pel));
    }
  }
}

void BilateralFilter::bilateralFilterOtherInter(PelBuf& resiBuf, const CPelBuf& predBuf,
  PelBuf& recoBuf, int32_t qp, bool bAboveLeftAvai, const CPelBuf& recIPredBuf, const ClpRng& clpRng)
{
  const unsigned uiWidth = predBuf.width;
  const unsigned uiHeight = predBuf.height;
  __m128i tempblocki[100 * 100];
  short * tempblock = (short*)tempblocki;
  __m128i tempblockfi[100 * 100];
  short * atempblockFiltered = (short*)tempblockfi;
  atempblockFiltered += 6;
  // Exclude 4x4 (sorbet2)
  //if( (uiWidth == 4 && uiHeight == 4) || (uiWidth == 4 && uiHeight == 8) || (uiWidth == 8 && uiHeight == 4) )
  //  return;

  // Exclude 4x4 (sorbet3: exclude 4x4 here and 4xa4, 4x8 and 8x4 for intra)
  if( (uiWidth == 4 && uiHeight == 4) )
    return;

  const unsigned uiPredStride = predBuf.stride;
  const unsigned uiStrideRes = resiBuf.stride;
  const unsigned uiRecStride = recoBuf.stride;
  const unsigned uiRecIPredStride = recIPredBuf.stride;
  const Pel *piPred = predBuf.buf;
  Pel *piResi = resiBuf.buf;
  Pel *piReco = recoBuf.buf;
  const Pel *piRecIPred = recIPredBuf.buf;
  
  uint32_t uiMinSize = std::min(uiWidth, uiHeight);
  const Pel *piPredTemp = piPred;
  Pel *piResiTemp = piResi;
  Pel *piRecoTemp = piReco;
  // Reco = Pred + Resi

  

  // If above and left are available, do non-local filtering

  Pel *tempBlockPtr;
  int32_t tempBlockStride;

  
  if (bAboveLeftAvai)
  {
    uint32_t   uiWidthExt = uiWidth + (NUMBER_PADDED_SAMPLES << 1);
    uint32_t   uiHeightExt = uiHeight + (NUMBER_PADDED_SAMPLES << 1);

    memset(tempblock, 0, uiWidthExt*uiHeightExt*sizeof(short));
    tempBlockPtr = tempblock + (NUMBER_PADDED_SAMPLES) * uiWidthExt + NUMBER_PADDED_SAMPLES;

    //// Clip and move block to temporary block

    for (uint32_t uiY = 0; uiY < uiHeight; ++uiY)
    {
      for (uint32_t uiX = 0; uiX < uiWidth; ++uiX)
      {
        tempBlockPtr[uiX] = ClipPel(piPred[uiX] + piResi[uiX], clpRng);
      }
      piPred += uiPredStride;
      piResi += uiStrideRes;
      piReco += uiRecStride;
      tempBlockPtr += uiWidthExt;
    }
    
    piPred = piPredTemp;
    piResi = piResiTemp;
    piReco = piRecoTemp;

    //// Pad rest of samples
    
    std::copy(piRecIPred - (uiRecIPredStride << 1) - NUMBER_PADDED_SAMPLES,
              piRecIPred - (uiRecIPredStride << 1) - NUMBER_PADDED_SAMPLES + (uiWidthExt - NUMBER_PADDED_SAMPLES), tempblock);
    tempblock[uiWidthExt - 2] = tempblock[uiWidthExt - 3];

    std::copy(piRecIPred - uiRecIPredStride - NUMBER_PADDED_SAMPLES,
              piRecIPred - uiRecIPredStride - NUMBER_PADDED_SAMPLES + (uiWidthExt - NUMBER_PADDED_SAMPLES), tempblock + uiWidthExt);
    tempblock[(uiWidthExt << 1) - 2] = tempblock[(uiWidthExt << 1) - 3];

    int32_t iOffset = NUMBER_PADDED_SAMPLES * uiWidthExt;
    for (int32_t j = 0; j < uiHeight; j++)
    {
      for (int32_t i = 0; i < NUMBER_PADDED_SAMPLES; i++)
      {
        tempblock[iOffset + i] = piRecIPred[(int32_t)(j * uiRecIPredStride) - NUMBER_PADDED_SAMPLES + i];
      }
      tempblock[iOffset + uiWidthExt - 2] = tempblock[iOffset + uiWidthExt - NUMBER_PADDED_SAMPLES - 1];
      iOffset += uiWidthExt;
    }
    std::copy(tempblock + iOffset - uiWidthExt, tempblock + iOffset, tempblock + iOffset);

    // Now do non-local filtering
    //for (int i = 0; i < 1000; i++)
    {
      nonlocalSmoothBlockBilateralFilter(uiWidth, uiHeight, tempblock, uiMinSize, 1, qp, atempblockFiltered, clpRng);
    }
    // Copy back

    //    for (uint32_t j = 0; j < uiHeight; j++)
    //    {
    //      std::copy(tempblockFiltered + (j + NUMBER_PADDED_SAMPLES) * uiWidthExt + NUMBER_PADDED_SAMPLES,
    //                tempblockFiltered + (j + NUMBER_PADDED_SAMPLES) * uiWidthExt + NUMBER_PADDED_SAMPLES + uiWidth,
    //                piReco + j * uiRecStride);
    //    }
    if (uiWidth >= 8)
    {
      tempBlockPtr = atempblockFiltered + NUMBER_PADDED_SAMPLES * (uiWidthExt + 4) + NUMBER_PADDED_SAMPLES;
      tempBlockStride = uiWidthExt + 4;
    }
    else
    {

      tempBlockPtr = atempblockFiltered + NUMBER_PADDED_SAMPLES * (uiWidthExt)+NUMBER_PADDED_SAMPLES;
      tempBlockStride = uiWidthExt;
    }

  }
  else
  {
    // We dont have above and left -- use the intra version of the filter that is local.
    
    const unsigned uiWidth      = predBuf.width;
    const unsigned uiHeight     = predBuf.height;
    const unsigned uiPredStride = predBuf.stride;
    const unsigned uiStrideRes  = resiBuf.stride;
    const Pel *piPred           = predBuf.buf;
    Pel *piResi           = resiBuf.buf;
    
    for( unsigned  uiY = 0; uiY < uiHeight; ++uiY)
    {
      for( unsigned uiX = 0; uiX < uiWidth; ++uiX)
      {
        tempblock[ uiX + uiY*uiWidth ] = ClipPel( piPred[uiX] + piResi[uiX], clpRng);
      }
      piPred += uiPredStride;
      piResi += uiStrideRes;
    }

    smoothBlockBilateralFilter(uiWidth, uiHeight, tempblock, 1, qp);
    tempBlockPtr = tempblock;
    tempBlockStride = uiWidth;
    
  }
  // need to be performed if residual  is used
  // Resi' = Reco' - Pred
  for (uint32_t uiY = 0; uiY < uiHeight; ++uiY)
  {
    for (uint32_t uiX = 0; uiX < uiWidth; ++uiX)
    {
      piResi[uiX] = tempBlockPtr[uiX] - piPred[uiX];
    }
    piPred += uiPredStride;
    piResi += uiStrideRes;
    tempBlockPtr += tempBlockStride;
  }
}

#endif
