/*
 * vision.h
 *
 *  Created on: May 14, 2013
 *      Author: cforster
 */

#ifndef VIKIT_VISION_H_
#define VIKIT_VISION_H_

#include <aligned_mem.h>
#include <Eigen/Core>
#include <opencv2/opencv.hpp>

namespace vk
{

//! Return value between 0 and 1
//! WARNING This function does not check whether the x/y is within the border
inline float
interpolateMat_32f(const cv::Mat& mat, float u, float v)
{
  assert(mat.type()==CV_32F);
  float x = floor(u);
  float y = floor(v);
  float subpix_x = u-x;
  float subpix_y = v-y;
  float wx0 = 1.0-subpix_x;
  float wx1 =  subpix_x;
  float wy0 = 1.0-subpix_y;
  float wy1 =  subpix_y;

  float val00 = mat.at<float>(y,x);
  float val10 = mat.at<float>(y,x+1);
  float val01 = mat.at<float>(y+1,x);
  float val11 = mat.at<float>(y+1,x+1);
  return (wx0*wy0)*val00 + (wx1*wy0)*val10 + (wx0*wy1)*val01 + (wx1*wy1)*val11;
}

//! Return value between 0 and 255
//! WARNING This function does not check whether the x/y is within the border
inline float
interpolateMat_8u(const cv::Mat& mat, float u, float v)
{
  if(mat.empty())return 0.0;
  if(mat.type()!=CV_8U)return 0.0;
  int x = floor(u);
  int y = floor(v);
  float subpix_x = u-x;
  float subpix_y = v-y;

  float w00 = (1.0f-subpix_x)*(1.0f-subpix_y);
  float w01 = (1.0f-subpix_x)*subpix_y;
  float w10 = subpix_x*(1.0f-subpix_y);
  float w11 = 1.0f - w00 - w01 - w10;

  const int stride = mat.step.p[0];
  unsigned char* ptr = mat.data + y*stride + x;
  if(isnan(w00) || isnan(w01 )||isnan( w10) || isnan(w11) || isinf(w00) || isinf(w01)|| isinf(w10)|| isinf(w11))return 0.0;
  if(ptr[0] == NULL || ptr[1]== NULL || ptr[stride]==NULL ||ptr[stride+1]==NULL)return 0.0;
  return w00*ptr[0] + w01*ptr[stride] + w10*ptr[1] + w11*ptr[stride+1];
}

void halfSample(const cv::Mat& in, cv::Mat& out);

float shiTomasiScore(const cv::Mat& img, int u, int v);

void calcSharrDeriv(const cv::Mat& src, cv::Mat& dst);

#ifdef __SSE2__

/// Used to convert a Kinect depthmap
/// Code by Christian Kerl DVO, GPL Licence
void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale);

#endif

} // namespace vk

#endif // VIKIT_VISION_H_