/*
 * Copyright 2017 Jan Dorazil <deu439@gmail.com>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include "lbph.hpp"

#include "hamming_map16.cpp"
#include "ternary_map16.cpp"
#include "ternary_map16_2.cpp"
#include "hamming_map8.cpp"

void lbph::mb_lbp(InputArray _src, OutputArray _dst){
  
  float thresh = 0.2;
  
  int indices[][9][2] = {
    {
      {-4, -4}, {-3, -4}, {-2, -4},
      {-4, -3}, {-3, -3}, {-2, -3},
      {-4, -2}, {-3, -2}, {-2, -2}
    },{
      {-1, -4}, { 0, -4}, { 1, -4},
      {-1, -3}, { 0, -3}, { 1, -3},
      {-1, -2}, { 0, -2}, { 1, -2},
    },{
      { 2, -4}, { 3, -4}, { 4, -4},
      { 2, -3}, { 3, -3}, { 4, -3},
      { 2, -2}, { 3, -2}, { 4, -2},
    },{
      { 2, -1}, { 3, -1}, { 4, -1},
      { 2,  0}, { 3,  0}, { 4,  0},
      { 2,  1}, { 3,  1}, { 4,  1},
    },{
      { 2,  2}, { 3,  2}, { 4,  2},
      { 2,  3}, { 3,  3}, { 4,  3},
      { 2,  4}, { 3,  4}, { 4,  4},
    },{
      { -1, 2}, { 0,  2}, { 1,  2},
      { -1, 3}, { 0,  3}, { 1,  3},
      { -1, 4}, { 0,  4}, { 1,  4},
    },{
      { -4, 2}, {-3,  2}, {-2,  2},
      { -4, 3}, {-3,  3}, {-2,  3},
      { -4, 4}, {-3,  4}, {-2,  4},
    },{
      { -4,-1}, {-3, -1}, {-2, -1},
      { -4, 0}, {-3,  0}, {-2,  0},
      { -4, 1}, {-3,  1}, {-2,  1},
    },{
      {-1, -1}, { 0, -1}, { 1, -1},
      {-1,  0}, { 0,  0}, { 1,  0},
      {-1,  1}, { 0,  1}, { 1,  1},
    }
  };
  
  // Allocate destination
  _dst.create(_src.rows() - 8, _src.cols() - 8, CV_32SC1);
  
  // Get mats
  Mat dst = _dst.getMat();
  Mat src = _src.getMat();

  // Zero
  dst.setTo(0);
  
  for(int x = 4; x < dst.cols - 4; x++ ){
    for(int y = 4; y < dst.rows - 4; y++ ){
      float center = 0;
      for( int i = 0; i < 8; i++ ){
        center += src.at<uchar>(y + indices[8][i][1], x + indices[8][i][0]);
      }
      center /= 9;
      
      for( int j = 0; j < 8; j++ ){
        float mean = 0;
        for( int i = 0; i < 9; i++ ){
          mean += src.at<uchar>(y + indices[j][i][1], x + indices[j][i][0]);
        }
        mean /= 9;
        
        dst.at<int>(y, x) |= ((mean - center) < -center*thresh) << j;
        dst.at<int>(y, x) |= ((mean - center) > center*thresh) << (j + 8);
      }
    }
  }
}

template <typename _Tp>
void lbph::olbp_(InputArray _src, OutputArray _dst) {
    static int ind[][2] = {
      {-1, -1}, {-1,  0}, {-1,  1},
      { 0,  1}, { 1,  1}, { 1,  0},
      { 1, -1}, { 0, -1}, {-1, -1}
    };
    
    float threshold = 5;
    // get matrices
    Mat src = _src.getMat();
    // allocate memory for result
    _dst.create(src.rows-2, src.cols-2, CV_8UC1);
    Mat dst = _dst.getMat();
    // zero the result matrix
    dst.setTo(0);
    // calculate patterns
    for(int i=1;i<src.rows-1;i++) {
        for(int j=1;j<src.cols-1;j++) {
            float center = src.at<_Tp>(i,j);
            unsigned char code = 0;
            
            for( int k = 0; k < 8; k++ )
              code |= (abs(src.at<_Tp>( i + ind[k][0], j + ind[k][1]) - center)
                       > threshold) << k;
            
            dst.at<uchar>(i-1,j-1) = code;
        }
    }
}

template <typename _Tp>
void lbph::elbp_(InputArray _src, OutputArray _dst, int radius, int neighbors) {
    //get matrices
    Mat src = _src.getMat();
    // allocate memory for result
    _dst.create(src.rows-2*radius, src.cols-2*radius, CV_32SC1);
    Mat dst = _dst.getMat();
    // zero
    dst.setTo(0);
    
    for(int n=0; n<neighbors; n++) {
        // sample points
        float x = static_cast<float>(radius * cos(2.0*CV_PI*n/static_cast<float>(neighbors)));
        float y = static_cast<float>(-radius * sin(2.0*CV_PI*n/static_cast<float>(neighbors)));
        // relative indices
        int fx = static_cast<int>(floor(x));
        int fy = static_cast<int>(floor(y));
        int cx = static_cast<int>(ceil(x));
        int cy = static_cast<int>(ceil(y));
        // fractional part
        float ty = y - fy;
        float tx = x - fx;
        // set interpolation weights
        float w1 = (1 - tx) * (1 - ty);
        float w2 =      tx  * (1 - ty);
        float w3 = (1 - tx) *      ty;
        float w4 =      tx  *      ty;
        // iterate through your data
        for(int i=radius; i < src.rows-radius;i++) {
            for(int j=radius;j < src.cols-radius;j++) {
                // Center pixel reference
                _Tp &center = src.at<_Tp>(i, j);
                
                // calculate interpolated value
                float t = static_cast<float>(
                    w1*src.at<_Tp>(i+fy,j+fx) + 
                    w2*src.at<_Tp>(i+fy,j+cx) + 
                    w3*src.at<_Tp>(i+cy,j+fx) + 
                    w4*src.at<_Tp>(i+cy,j+cx)
                );
                
                // floating point precision, so check some machine-dependent epsilon
                dst.at<int>(i-radius,j-radius) += ((t > src.at<_Tp>(i,j)) || 
                (std::abs(t-src.at<_Tp>(i,j)) < std::numeric_limits<float>::epsilon())) << n;
            }
        }
    }
}

template <typename _Tp>
void lbph::eltp_(
  InputArray _src, OutputArray _dst, int radius, int neighbors, float th
) {
    static int ind[][2] = {
      {-1, -1}, { 0, -1}, { 1, -1},
      {-1,  0}, { 0,  0}, { 1,  0},
      {-1,  1}, { 0,  1}, { 1,  1}
    };
    float mean;
    //get matrices
    Mat src = _src.getMat();
    // allocate memory for result
    _dst.create(src.rows-2*radius, src.cols-2*radius, CV_32SC1);
    Mat dst = _dst.getMat();
    // zero
    dst.setTo(0);
    
    for(int n=0; n<neighbors; n++) {
        // sample points
        float x = static_cast<float>(radius * cos(2.0*CV_PI*n/static_cast<float>(neighbors)));
        float y = static_cast<float>(-radius * sin(2.0*CV_PI*n/static_cast<float>(neighbors)));
        // relative indices
        int fx = static_cast<int>(floor(x));
        int fy = static_cast<int>(floor(y));
        int cx = static_cast<int>(ceil(x));
        int cy = static_cast<int>(ceil(y));
        // fractional part
        float ty = y - fy;
        float tx = x - fx;
        // set interpolation weights
        float w1 = (1 - tx) * (1 - ty);
        float w2 =      tx  * (1 - ty);
        float w3 = (1 - tx) *      ty;
        float w4 =      tx  *      ty;
        // iterate through your data
        for(int i=radius; i < src.rows-radius;i++) {
            for(int j=radius;j < src.cols-radius;j++) {
                // Center pixel reference
                _Tp &center = src.at<_Tp>(i, j);
                
                // Get the mean to scale the threshold
                mean = 0;
                for( int k = 0; k < 9; k++ )
                  mean += src.at<_Tp>(i + ind[k][1], j + ind[k][0]);
                mean /= 9;
                
                // calculate interpolated value
                float t = static_cast<float>(
                    w1*src.at<_Tp>(i+fy,j+fx) + 
                    w2*src.at<_Tp>(i+fy,j+cx) + 
                    w3*src.at<_Tp>(i+cy,j+fx) + 
                    w4*src.at<_Tp>(i+cy,j+cx)
                );
                
                // floating point precision, so check some machine-dependent epsilon
//                 dst.at<int>(i-radius,j-radius) += ((t > src.at<_Tp>(i,j)) || (std::abs(t-src.at<_Tp>(i,j)) < std::numeric_limits<float>::epsilon())) << n;
                dst.at<int>(i-radius, j-radius) += 
                  ((t - center) < -(mean*th)) << n;
                dst.at<int>(i-radius, j-radius) +=
                  ((t - center) > (mean*th)) << (n + neighbors);
            }
        }
    }
}

template <typename _Tp>
void lbph::lbsp_(InputArray _src, OutputArray _dst, float threshold)
{
    static int ind[][2] = {
      {-2, -2}, {-2,  0}, {-2, 2},
      {-1, -1}, {-1,  0}, {-1, 1},
      { 0, -2}, { 0, -1}, { 0, 1}, { 0, 2},
      { 1, -1}, { 1,  0}, { 1, 1},
      { 2, -2}, { 2,  0}, { 2, 2}
    };
    // get matrices
    Mat src = _src.getMat();
    // allocate memory for result
    _dst.create(src.rows-4, src.cols-4, CV_16UC1);
    Mat dst = _dst.getMat();
    // zero the result matrix
    dst.setTo(0);
    // calculate patterns
    for(int i=2;i<src.rows-2;i++) {
        for(int j=2;j<src.cols-2;j++) {
            float center = (float)src.at<_Tp>(i,j);
            uint16_t code = 0;
            for( int k = 0; k < 16; k++ )
              code |= (abs(src.at<_Tp>( i + ind[k][0], j + ind[k][1]) - center)
                       > threshold) << k;
            dst.at<uint16_t>(i-2,j-2) = code;
        }
    }
}

template <typename _Tp>
void lbph::lbsp_(InputArray _src1, InputArray _src2, OutputArray _dst,
                 float threshold)
{
    static int ind[][2] = {
      {-2, -2}, {-2,  0}, {-2, 2},
      {-1, -1}, {-1,  0}, {-1, 1},
      { 0, -2}, { 0, -1}, { 0, 1}, { 0, 2},
      { 1, -1}, { 1,  0}, { 1, 1},
      { 2, -2}, { 2,  0}, { 2, 2}
    };
    
    // get matrices
    Mat src1 = _src1.getMat();
    Mat src2 = _src2.getMat();
    
    // allocate memory for result
    _dst.create(src1.rows-4, src1.cols-4, CV_16UC1);
    Mat dst = _dst.getMat();
    
    // zero the result matrix
    dst.setTo(0);
    // calculate patterns
    for(int i=2; i < src1.rows-2; i++) {
        for(int j=2; j < src1.cols-2; j++) {
            float center = (float)src1.at<_Tp>(i,j);
            uint16_t code = 0;
            
            for( int k = 0; k < 16; k++ )
              code |= (abs(src2.at<_Tp>( i + ind[k][0], j + ind[k][1]) - center) > threshold) << k;
            
            dst.at<uint16_t>(i-2,j-2) = code;
        }
    }
}

template <typename _Tp>
void lbph::var_(InputArray _src, OutputArray _dst)
{
    static int ind[8][2] = {
      {-1, -1}, {-1, 0}, {-1, 1},
      { 0, -1},          { 0, 1},
      { 1, -1}, { 1, 0}, { 1, 1}
    };
    
    // get matrices
    Mat src = _src.getMat();
    
    // allocate memory for result
    _dst.create(src.rows-2, src.cols-2, CV_8UC1);
    Mat dst = _dst.getMat();
    
    // calculate pattern strength
    for(int i=1;i<src.rows-1;i++) {
        for(int j=1;j<src.cols-1;j++) {
            float P = 0, m = 0;
            _Tp gp;
            for( int k = 0; k < 8; k++ ){
              gp = src.at<uchar>(i + ind[k][0], j + ind[k][1]);
              P += gp * gp; // Power mean
              m += gp;      // Mean
            }
            P = P / 8;
            m = m / 8;
            // Variance
            dst.at<uchar>(i-1,j-1) = (255*sqrt(P - m*m)) / 128;
//             std::cout << P - m*m << std::endl;
        }
    }
}

void lbph::remap(Mat in)
{
  for( int y = 0; y < in.rows; y++ ){
    for(int x = 0; x < in.cols; x++ ){
      int &px = in.at<int>(y,x);
      px = lookup_ternary16_2[px];
//       px = lookup8[px];
    }
  }
}

void lbph::olbp(InputArray src, OutputArray dst)
{
    int type = src.type();
    switch (type) {
    case CV_8SC1:   olbp_<char>(src,dst); break;
    case CV_8UC1:   olbp_<unsigned char>(src, dst); break;
    case CV_16SC1:  olbp_<short>(src,dst); break;
    case CV_16UC1:  olbp_<unsigned short>(src,dst); break;
    case CV_32SC1:  olbp_<int>(src,dst); break;
    case CV_32FC1:  olbp_<float>(src,dst); break;
    case CV_64FC1:  olbp_<double>(src,dst); break;
    default:
        String error_msg = format("Using Original Local Binary Patterns for feature extraction only works on single-channel images (given %d). Please pass the image data as a grayscale image!", type);
        CV_Error(Error::StsNotImplemented, error_msg);
        break;
    }
}

void lbph::elbp(InputArray src, OutputArray dst, int radius, int neighbors)
{
    int type = src.type();
    switch (type) {
    case CV_8SC1:   elbp_<char>(src,dst, radius, neighbors); break;
    case CV_8UC1:   elbp_<unsigned char>(src, dst, radius, neighbors); break;
    case CV_16SC1:  elbp_<short>(src,dst, radius, neighbors); break;
    case CV_16UC1:  elbp_<unsigned short>(src,dst, radius, neighbors); break;
    case CV_32SC1:  elbp_<int>(src,dst, radius, neighbors); break;
    case CV_32FC1:  elbp_<float>(src,dst, radius, neighbors); break;
    case CV_64FC1:  elbp_<double>(src,dst, radius, neighbors); break;
    default:
        String error_msg = format("Using Extended Local Binary Patterns for feature extraction only works on single-channel images (given %d). Please pass the image data as a grayscale image!", type);
        CV_Error(Error::StsNotImplemented, error_msg);
        break;
    }
}

void lbph::eltp(
  InputArray src, OutputArray dst, int radius, int neighbors, float th
)
{
    int type = src.type();
    switch (type) {
    case CV_8SC1:  eltp_<char>(src,dst, radius, neighbors, th); break;
    case CV_8UC1:  eltp_<unsigned char>(src, dst, radius, neighbors, th); break;
    case CV_16SC1: eltp_<short>(src,dst, radius, neighbors, th); break;
    case CV_16UC1: eltp_<unsigned short>(src,dst, radius, neighbors, th); break;
    case CV_32SC1: eltp_<int>(src,dst, radius, neighbors, th); break;
    case CV_32FC1: eltp_<float>(src,dst, radius, neighbors, th); break;
    case CV_64FC1: eltp_<double>(src,dst, radius, neighbors, th); break;
    default:
        String error_msg = format("Using Extended Local Ternary Patterns for feature extraction only works on single-channel images (given %d). Please pass the image data as a grayscale image!", type);
        CV_Error(Error::StsNotImplemented, error_msg);
        break;
    }
}

void lbph::lbsp(InputArray src, OutputArray dst, float threshold)
{
    int type = src.type();
    switch (type) {
    case CV_8SC1:   lbsp_<char>(src, dst, threshold); break;
    case CV_8UC1:   lbsp_<unsigned char>(src, dst, threshold); break;
    case CV_16SC1:  lbsp_<short>(src, dst, threshold); break;
    case CV_16UC1:  lbsp_<unsigned short>(src, dst, threshold); break;
    case CV_32SC1:  lbsp_<int>(src, dst, threshold); break;
    case CV_32FC1:  lbsp_<float>(src, dst, threshold); break;
    case CV_64FC1:  lbsp_<double>(src, dst, threshold); break;
    default:
        String error_msg = format("Using Local Binary Similarity Patterns for feature extraction only works on single-channel images (given %d). Please pass the image data as a grayscale image!", type);
        CV_Error(Error::StsNotImplemented, error_msg);
        break;
    }
}

void lbph::lbsp(InputArray src1, InputArray src2, OutputArray dst,
                float threshold)
{
    int type = src1.type();
    CV_Assert(type == src2.type());
    
    switch (type) {
    case CV_8SC1:   lbsp_<char>(src1, src2, dst, threshold); break;
    case CV_8UC1:   lbsp_<unsigned char>(src1, src2, dst, threshold); break;
    case CV_16SC1:  lbsp_<short>(src1, src2, dst, threshold); break;
    case CV_16UC1:  lbsp_<unsigned short>(src1, src2, dst, threshold); break;
    case CV_32SC1:  lbsp_<int>(src1, src2, dst, threshold); break;
    case CV_32FC1:  lbsp_<float>(src1, src2, dst, threshold); break;
    case CV_64FC1:  lbsp_<double>(src1, src2, dst, threshold); break;
    default:
        String error_msg = format("Using Local Binary Similarity Patterns for feature extraction only works on single-channel images (given %d). Please pass the image data as a grayscale image!", type);
        CV_Error(Error::StsNotImplemented, error_msg);
        break;
    }
}

void lbph::var(InputArray src, OutputArray dst)
{
    int type = src.type();
    switch (type) {
    case CV_8SC1:   var_<char>(src, dst); break;
    case CV_8UC1:   var_<unsigned char>(src, dst); break;
    case CV_16SC1:  var_<short>(src, dst); break;
    case CV_16UC1:  var_<unsigned short>(src, dst); break;
    case CV_32SC1:  var_<int>(src, dst); break;
    case CV_32FC1:  var_<float>(src, dst); break;
    case CV_64FC1:  var_<double>(src, dst); break;
    default:
        String error_msg = format("Using Local Binary Patterns for feature extraction only works on single-channel images (given %d). Please pass the image data as a grayscale image!", type);
        CV_Error(Error::StsNotImplemented, error_msg);
        break;
    }
}

Mat lbph::histc_(const Mat& src, int minVal, int maxVal, bool normed)
{
    Mat result;
    // Establish the number of bins.
    int histSize = maxVal-minVal+1;
    // Set the ranges.
    float range[] = { static_cast<float>(minVal), static_cast<float>(maxVal+1) };
    const float* histRange = { range };
    // calc histogram
    calcHist(&src, 1, 0, Mat(), result, 1, &histSize, &histRange, true, false);
    // normalize
    if(normed) {
        result /= (int)src.total();
    }
    return result.reshape(1,1);
}

Mat lbph::histc(InputArray _src, int minVal, int maxVal, bool normed)
{
    Mat src = _src.getMat();
    switch (src.type()) {
        case CV_8SC1:
            return histc_(Mat_<float>(src), minVal, maxVal, normed);
            break;
        case CV_8UC1:
            return histc_(src, minVal, maxVal, normed);
            break;
        case CV_16SC1:
            return histc_(Mat_<float>(src), minVal, maxVal, normed);
            break;
        case CV_16UC1:
            return histc_(src, minVal, maxVal, normed);
            break;
        case CV_32SC1:
            return histc_(Mat_<float>(src), minVal, maxVal, normed);
            break;
        case CV_32FC1:
            return histc_(src, minVal, maxVal, normed);
            break;
        default:
            CV_Error(Error::StsUnmatchedFormats, "This type is not implemented yet."); break;
    }
    return Mat();
}

Mat lbph::spatial_histogram(InputArray _src, int numPatterns,
                            int grid_x, int grid_y, bool /*normed*/)
{
    Mat src = _src.getMat();
    // calculate LBP patch size
    int width = src.cols/grid_x;
    int height = src.rows/grid_y;
    // allocate memory for the spatial histogram
    Mat result = Mat::zeros(grid_x * grid_y, numPatterns, CV_32FC1);
    // return matrix with zeros if no data was given
    if(src.empty())
        return result.reshape(1,1);
    // initial result_row
    int resultRowIdx = 0;
    // iterate through grid
    for(int i = 0; i < grid_y; i++) {
        for(int j = 0; j < grid_x; j++) {
            Mat src_cell = Mat(src, Range(i*height,(i+1)*height), Range(j*width,(j+1)*width));
            Mat cell_hist = histc(src_cell, 0, (numPatterns-1), true);
            // copy to the result matrix
            Mat result_row = result.row(resultRowIdx);
            cell_hist.reshape(1,1).convertTo(result_row, CV_32FC1);
            // increase row count in result matrix
            resultRowIdx++;
        }
    }
    // return result as reshaped feature vector
    return result.reshape(1,1);
}
