// This file is part of VisionCPP, a lightweight C++ template library
// for computer vision and image processing.
//
// Copyright (C) 2016 Codeplay Software Limited. All Rights Reserved.
//
// Contact: visioncpp@codeplay.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.

/// \file harris.cc
///
///             -------Example--------
///             Harris Corner Detector
///
/// \brief This example implements Harris Corner Detector
/// The Harris Corner Detector creates a matrix M based on its derivatives
/// M = sum w | Dx^2  Dx*Dy |
///            | Dx*Dy Dy^2 |
/// where w is the a 3x3 window of ones
/// the corners are detected based on the formula
/// H = det(M)-k*(trace(M)^2)

// include OpenCV for camera display
#include 

// include VisionCpp
#include 

// tunable parameter for the Harris
constexpr float k_param = 0.04f;   // k parameter (usually 0.02 - 0.04)
constexpr float threshold = 0.5f;  // threhold parameter
constexpr int windowSize = 7;      // window size for non-maximal suppresion
constexpr int halfWindowSize = windowSize / 2;  // half window size

// Below a set of operators created to implement the algorithm

// operator created to perform a power of 2 of the image
struct PowerOf2 {
  template 
  const float operator()(const T &t) {
    return t * t;
  }
};

// operator for element-wise multiplication of two images
struct Mul {
  template 
  float operator()(const T1 &t1, const T2 &t2) {
    return t1 * t2;
  }
};

// operator to add two images
struct Add {
  template 
  float operator()(const T1 &t1, const T2 &t2) {
    return t1 + t2;
  }
};

// operator to subtract two images
struct Sub {
  template 
  float operator()(const T1 &t1, const T2 &t2) {
    return t1 - t2;
  }
};

// convolution for a custom filter in a one-dimensional image
struct Filter2D {
  template 
  float operator()(const T1 &nbr, const T2 &fltr) {
    int hs_c = (fltr.cols / 2);
    int hs_r = (fltr.rows / 2);

    float out = 0;
    for (int i2 = -hs_c, i = 0; i2