#ifndef CVUTILS_HPP
#define CVUTILS_HPP

#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"

class CvCapture;

/** Projects a point with the homography matrix
 use perspectiveTransform for arrays of points. */
//cv::Point2f project(const cv::Point2f& p, const cv::Mat& homography);
template<typename T>
inline cv::Point_<T> project(const cv::Point_<T>& p, const cv::Mat_<T>& homography) {
  if (homography.empty())
    return p;
  else {
    T x, y;
    T w = homography.template at<T>(2,0)*p.x+homography.template at<T>(2,1)*p.y+homography.template at<T>(2,2);
    if (w != 0.) {
      x = (homography.template at<T>(0,0)*p.x+homography.template at<T>(0,1)*p.y+homography.template at<T>(0,2))/w;
      y = (homography.template at<T>(1,0)*p.x+homography.template at<T>(1,1)*p.y+homography.template at<T>(1,2))/w;
    } else {
      x = 0.;
      y = 0.;
    }
    return cv::Point_<T>(x, y);
  }
}

/** Loads a cv mat from a text file where the numbers are saved line by line separated by separator */
cv::Mat loadMat(const std::string& filename, const std::string& separator);

inline bool inImage(const  cv::Point2f& p, const cv::Size s) { return (p.x >= 0) && (p.x <= s.width) && (p.y >= 0) && (p.y <= s.height); }

//template<typename T> 
//float scalarProduct(const cv::Point_<T>& v1, const cv::Point_<T>& v2) { return v1.x*v2.x+v1.y*v2.y;}

void keyPoints2Points(const std::vector<cv::KeyPoint>& kpts, std::vector<cv::Point2f>& pts, const bool& clearPts = true);

/** Goes to the target frame number, by querying frame, 
    supposing the video input is currently at current frame number.
    Returns the frame number that was reached.*/
// int goToFrameNum(CvCapture* inputVideo, const int& currentFrameNum, const int& targetFrameNum);

/// Pre-defined colors
class Colors {
public:
  static const int nColors = 8;
  static const cv::Scalar color[];

  static cv::Scalar black(void);
  static cv::Scalar red(void);
  static cv::Scalar green(void);
  static cv::Scalar blue(void);
  static cv::Scalar white(void);
  static cv::Scalar magenta(void);
  static cv::Scalar cyan(void);
  static cv::Scalar yellow(void);

  /** Maps integers to primary colors. */
  static cv::Scalar color3(const int& num);
  static cv::Scalar color8(const int& num);
};

#endif