#include "LocalBinaryPattern.h"

#include "opencv2/core/version.hpp"
#if CV_MAJOR_VERSION >= 2 && CV_MAJOR_VERSION <= 3 && CV_MINOR_VERSION <= 4 && CV_VERSION_REVISION <= 7

using namespace bgslibrary::algorithms::multilayer;

CLocalBinaryPattern::CLocalBinaryPattern() {
  m_ppOrgImgs = NULL;
  m_pRadiuses = NULL;
  m_fRobustWhiteNoise = 3.0f;
  m_pNeigPointsNums = NULL;
  m_pXYShifts = NULL;
  m_pShiftedImg = NULL;
}

CLocalBinaryPattern::~CLocalBinaryPattern() {
  FreeMemories();
}

void CLocalBinaryPattern::Initialization(IplImage **first_imgs, int imgs_num, int level_num, float *radius, int *neig_pt_num, float robust_white_noise, int type)
{
  m_nImgsNum = imgs_num;
  m_nLBPLevelNum = level_num;
  m_pRadiuses = new float[m_nLBPLevelNum];
  m_pNeigPointsNums = new int[m_nLBPLevelNum];
  m_ppOrgImgs = first_imgs;
  int a, b;
  for (a = 0; a < m_nImgsNum; a++) {
    m_cvImgSize = cvSize(first_imgs[a]->width, first_imgs[a]->height);
    if (first_imgs[a]->nChannels > 1) {
      printf("Input image channel must be 1!");
      exit(1);
    }
  }

  int tot_neig_pts_num = 0;
  for (a = 0; a < m_nLBPLevelNum; a++) {
    m_pRadiuses[a] = radius[a];
    m_pNeigPointsNums[a] = neig_pt_num[a];
    tot_neig_pts_num += neig_pt_num[a];
    if (m_pNeigPointsNums[a] % 2 != 0) {
      printf("Even number must be given for points number for LBP!\n");
      exit(1);
    }
  }

  m_pShiftedImg = cvCloneImage(m_ppOrgImgs[0]);
  m_pXYShifts = new CvPoint[tot_neig_pts_num];
  m_nMaxShift.x = 0;
  m_nMaxShift.y = 0;
  int shift_idx = 0;
  for (a = 0; a < m_nLBPLevelNum; a++)
    for (b = 0; b < m_pNeigPointsNums[a]; b++) {
      // compute the offset of neig point
      CalNeigPixelOffset(m_pRadiuses[a], m_pNeigPointsNums[a], b, m_pXYShifts[shift_idx].x, m_pXYShifts[shift_idx].y);
      m_nMaxShift.x = MAX(m_nMaxShift.x, m_pXYShifts[shift_idx].x);
      m_nMaxShift.y = MAX(m_nMaxShift.y, m_pXYShifts[shift_idx].y);
      shift_idx++;
    }

  m_fRobustWhiteNoise = robust_white_noise;
}

void CLocalBinaryPattern::SetNewImages(IplImage **new_imgs)
{
  m_ppOrgImgs = new_imgs;
}

void CLocalBinaryPattern::ComputeLBP(PixelLBPStruct *PLBP, CvRect *roi)
{
  float *dif_pattern;
  float *_dif_pattern;
  PixelLBPStruct *_PLBP;
  int data_length;
  float *cur_pattern;

  // allocate memories
  if (roi)
    data_length = roi->height*roi->width;
  else
    data_length = m_cvImgSize.width*m_cvImgSize.height;

  dif_pattern = new float[data_length];

  int img_idx, pt_idx, yx, level;
  int pattern_idx = 0;
  for (img_idx = 0; img_idx < m_nImgsNum; img_idx++) {
    for (level = 0; level < m_nLBPLevelNum; level++) {
      for (pt_idx = 0; pt_idx < m_pNeigPointsNums[level]; pt_idx++) {

        // computing the shifted image
        CalShiftedImage(m_ppOrgImgs[img_idx], m_pXYShifts[pattern_idx].x, m_pXYShifts[pattern_idx].y, m_pShiftedImg, roi);

        // computing the different binary images
        CalImageDifferenceMap(m_ppOrgImgs[img_idx], m_pShiftedImg, dif_pattern, roi);

        // set the binary values
        _PLBP = PLBP;
        _dif_pattern = dif_pattern;

        if (roi) {
          int x, y;
          for (y = 0; y < roi->height; y++) {
            _PLBP = PLBP + (y + roi->y)*m_cvImgSize.width + roi->x;
            for (x = 0; x < roi->width; x++) {
              cur_pattern = (*_PLBP++).cur_pattern;
              cur_pattern[pattern_idx] = *_dif_pattern++;
            }
          }
        }
        else {
          for (yx = 0; yx < data_length; yx++) {
            cur_pattern = (*_PLBP++).cur_pattern;
            cur_pattern[pattern_idx] = *_dif_pattern++;
          }
        }

        pattern_idx++;

      }
    }
  }

  // release memories
  delete[] dif_pattern;

  //delete [] shifted_dif_pattern;
  //cvReleaseImage(&shifted_img);
  //cvReleaseImage(&pattern_img);
}

void CLocalBinaryPattern::FreeMemories()
{
  delete[] m_pRadiuses;
  delete[] m_pNeigPointsNums;
  delete[] m_pXYShifts;
  cvReleaseImage(&m_pShiftedImg);

  m_pXYShifts = NULL;
  m_pRadiuses = NULL;
  m_pNeigPointsNums = NULL;
  m_pShiftedImg = NULL;
}

void CLocalBinaryPattern::SetShiftedMeshGrid(CvSize img_size, float offset_x, float offset_y, CvMat *grid_map_x, CvMat *grid_map_y)
{
  float *gX = (float*)(grid_map_x->data.ptr);
  float *gY = (float*)(grid_map_y->data.ptr);

  int x, y;
  for (y = 0; y < img_size.height; y++) {
    for (x = 0; x < img_size.width; x++) {
      *gX++ = (float)x + offset_x;
      *gY++ = (float)y + offset_y;
    }
  }
}

void CLocalBinaryPattern::CalShiftedImage(IplImage *src, int offset_x, int offset_y, IplImage *dst, CvRect *roi)
{
  CvRect src_roi, dst_roi;
  int roi_width, roi_height;

  if (roi) {
    src_roi.x = MAX(offset_x + roi->x, 0);
    src_roi.y = MAX(offset_y + roi->y, 0);

    dst_roi.x = MAX(-(offset_x + roi->x), roi->x);
    dst_roi.y = MAX(-(offset_y + roi->y), roi->y);

    roi_width = MIN(MIN(roi->width + (int)fabsf((float)offset_x), src->width - src_roi.x), dst->width - dst_roi.x);
    roi_height = MIN(MIN(roi->height + (int)fabsf((float)offset_y), src->height - src_roi.y), dst->height - dst_roi.y);

    src_roi.width = roi_width;
    src_roi.height = roi_height;

    dst_roi.width = roi_width;
    dst_roi.height = roi_height;
  }
  else {
    roi_width = src->width - (int)fabsf((float)offset_x);
    roi_height = src->height - (int)fabsf((float)offset_y);

    src_roi.x = MAX(offset_x, 0);
    src_roi.y = MAX(offset_y, 0);
    src_roi.width = roi_width;
    src_roi.height = roi_height;

    dst_roi.x = MAX(-offset_x, 0);
    dst_roi.y = MAX(-offset_y, 0);
    dst_roi.width = roi_width;
    dst_roi.height = roi_height;
  }

  cvSet(dst, cvScalar(0));

  if (roi_width <= 0 || roi_height <= 0)
    return;

  cvSetImageROI(src, src_roi);
  cvSetImageROI(dst, dst_roi);
  cvCopy(src, dst);
  cvResetImageROI(src);
  cvResetImageROI(dst);
}

void CLocalBinaryPattern::CalNeigPixelOffset(float radius, int tot_neig_pts_num, int neig_pt_idx, int &offset_x, int &offset_y)
{
  float angle = (float)neig_pt_idx / (float)tot_neig_pts_num*2.0f*PI;
  offset_x = cvRound(radius*cosf(angle));
  offset_y = cvRound(-radius*sinf(angle));
}

void CLocalBinaryPattern::CalImageDifferenceMap(IplImage *cent_img, IplImage *neig_img, float *pattern, CvRect *roi)
{
  COpencvDataConversion<uchar, uchar> ODC;

  if (roi) {
    cvSetImageROI(cent_img, *roi);
    cvSetImageROI(neig_img, *roi);
  }

  uchar *_centI = ODC.GetImageData(cent_img);
  uchar *_neigI = ODC.GetImageData(neig_img);
  uchar *centI = _centI;
  uchar *neigI = _neigI;

  float *tmp_pattern = pattern;

  int xy;
  int length;

  if (roi)
    length = roi->height*roi->width;
  else
    length = cent_img->height*cent_img->width;

  for (xy = 0; xy < length; xy++) {
    *tmp_pattern = (float)BINARY_PATTERM_ELEM(*neigI, *centI, m_fRobustWhiteNoise);
    tmp_pattern++;
    centI++;
    neigI++;
  }

  if (roi) {
    cvResetImageROI(cent_img);
    cvResetImageROI(neig_img);
  }

  // release memories
  delete[] _centI;
  delete[] _neigI;

}

#endif
