/*
 * Copyright 2016 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 "detectcorr.hpp"

#define MIN_COORD(center, kernel, min, out) \
  do{ \
    out = center - (kernel / 2); \
    if( out < min ) out = min; \
  } while(0)

#define MAX_COORD(center, kernel, max, out) \
  do{ \
    out = center + (kernel / 2); \
    if( out > max ) out = max; \
  } while(0)

DetectCorr::DetectCorr(int tempKernel, int inKernel, double threshold,
                       int corrType)
 : mTempKernel(tempKernel), mInKernel(inKernel), mThreshold(threshold),
   mCorrType(corrType)
{  
}


void DetectCorr::corr(InputArray tempA, InputArray inA, OutputArray out) const
{
    Mat temp = tempA.getMat();
    Mat in = inA.getMat();
    
    assert(temp.rows == in.rows && temp.cols == in.cols);
    
    Mat outMat(temp.rows, temp.cols, CV_32F);
    Mat correlation;

    for( int y = 0; y < temp.rows; y++) {
        float *outRow = outMat.ptr<float>(y);

        for( int x = 0; x < temp.cols; x++) {

            int xt_min, xt_max, yt_min, yt_max;
            int xi_min, xi_max, yi_min, yi_max;

            MIN_COORD(x, mTempKernel, 0, xt_min);
            MIN_COORD(y, mTempKernel, 0, yt_min);
            MAX_COORD(x, mTempKernel, temp.cols - 1, xt_max);
            MAX_COORD(y, mTempKernel, temp.rows - 1, yt_max);

            MIN_COORD(x, mInKernel, 0, xi_min);
            MIN_COORD(y, mInKernel, 0, yi_min);
            MAX_COORD(x, mInKernel, in.cols - 1, xi_max);
            MAX_COORD(y, mInKernel, in.rows - 1, yi_max);

            Rect rectt(xt_min, yt_min, xt_max - xt_min + 1, yt_max - yt_min +
                       1);
            Rect recti(xi_min, yi_min, xi_max - xi_min + 1, yi_max - yi_min +
                       1);
            Mat roit = temp(rectt);
            Mat roii = in(recti);

            // Correlate
            matchTemplate(roii, roit, correlation, mCorrType);

            // Find max correlation
            double max, min;
            minMaxIdx(correlation, &min, &max);

            // Store correlation coeficient in output matrix
            if(mCorrType == CV_TM_SQDIFF_NORMED)
              outRow[x] = min;
            else
              outRow[x] = max;

        }
    }

    // Return result
    outMat.copyTo(out);
}

void DetectCorr::process(DataUnit& data)
{
  assert(!data.mMat[TEMP].empty());
  assert(!data.mMat[TROI].empty());
  assert(!data.mMat[INPU].empty());
  
  // Calculate correlation
  Mat c;
  corr(data.mMat[TEMP], data.mMat[INPU], c);

  // Threshold correlation
  if( mCorrType == CV_TM_SQDIFF_NORMED )
    data.mMat[OUTP] = c > mThreshold;
  else
    data.mMat[OUTP] = c < mThreshold;
  
  // Keep changes in ROI only
  multiply(data.mMat[OUTP], data.mMat[TROI], data.mMat[OUTP]);
}
