/*! \file CVUtils.h
	\brief Some functions extend opencv classes
	
	Created: 2015/06/22, author: Jin Bingwen.
*/

#ifndef __CVUtils_h_
#define __CVUtils_h_

#include "CyclopsVersion.h"
#include "StdUtils.h"
#include <opencv2/opencv.hpp>
#include <opencv2/opencv_modules.hpp>
#include <memory>
#include <vector>
#include <iostream>
#include <fstream>
#include <map>

#if defined(_DEBUG) && defined(_VIEW_INTERNAL_MAT)
#define DEBUG_VIEW_INTERNAL_MAT
#endif

#if defined(_DEBUG) && defined(_DETAIL_LOG)
#define DEBUG_DETAIL_LOG
#endif

#ifndef M_PI
#define M_PI CV_PI
#endif

#define INTER_POINT_Y(p0x, p0y, p1x, p1y, p2x) ((p1y - p0y)/(p1x - p0x)*(p2x - p0x) + p0y)
#define INTER_POINT_X(p0x, p0y, p1x, p1y, p2y) ((p1x - p0x)/(p1y - p0y)*(p2y - p0y) + p0x)

using std::vector;
using std::pair;
using namespace cv;

typedef Size_<double>   Sized;
typedef Size_<float>    Sizef;

namespace CyclopsUtils {

// Hold an empty dummy mat which is useful when you want to return a Mat() incase of some failure,
// or pass a Mat() as defualt function parameter.
// This will save some computation cost for initialization and deallocation of a local variable.
extern Mat gDummyMat;

// Hold an empty dummy Scalar which is useful when you want to return a Scalar() incase of some failure,
// or pass a Scalar() as defualt function parameter.
// This will save some computation cost for initialization and deallocation of a local variable.
extern Scalar gDummyScalar;

string templateMatchMethod2Str(int method);

// Carmack fast InvSqrt!
inline float InvSqrt(float x)
{
    float xhalf = 0.5f*x;
    int i = *(int*)&x;
    i = 0x5f3759df - (i >> 1); // ������һ�����Ƹ�
    x = *(float*)&i;
    x = x*(1.5f - xhalf*x*x); // ţ�ٵ�����
    return x;
}

inline bool isPow2(int n)
{
	return (n & (n - 1)) == 0;
}

inline unsigned int makePow2(unsigned int n)
{
	return n <= 2 ? n : pow(2, cvFloor(std::log2(n)));
}

template<typename T>
bool isEyeMat(const Mat& m) {
	for (int j = 0; j < m.rows; ++j) {
		const T* p = m.ptr<T>(j);
		if (j < m.cols && p[j] != 1) return false;
		for (int i = 0; i < j; ++i) {
			if (p[i] != 0) return false;
		}
		for (int i = j + 1; i < m.cols; ++i) {
			if (p[i] != 0) return false;
		}
	}
	return true;
}

void printIndent(int indentNum, int indent = 4);

template<typename _Ty>
void printProperty(int indentNum, string label, _Ty val)
{
    printIndent(indentNum);
    std::cout << label << ": " << val << "; " << std::endl;
}

void printMatInfo(const Mat& m, int baseIndentNum = 0, int indent = 4);

void openOper(Mat& img, int w);
void openOper(Mat& img, const Mat& kernel);
void closeOper(Mat& img, int w);
void closeOper(Mat& img, const Mat& kernel);
void localCloseOper(Mat& img, vector< vector<Point> >& contours, float longerScale);

Mat genCircleMask(int rows, int cols, int type,
    Point cen, double r,
    const Scalar& color, int thickness, int lineType = 8, int shift = 0);

void closeShapesToConvex(const Mat& mask, Mat& closedMask);

double longShortRatio(const Size& s);

float lpContourArea(const vector<Point>& contour);


float circleDegree(const vector<Point>& contour);


enum LogicOper
{
    LP_LOGIC_LARGER,
    LP_LOGIC_SMALLER
};

Mat gridThre(const Mat& img, int gridWidth, int gridHeight, float threStdDevScale, LogicOper oper /*= LP_LOGIC_SMALLER*/, float majority /*= 0.8*/);
Mat gridWhiteThre(const Mat& img, int gridWidth, int gridHeight, float threStdDevScale, float majority /*= 0.8*/);
Mat gridBlackThre(const Mat& img, int gridWidth, int gridHeight, float threStdDevScale, float majority = 0.8);

void converToType(cv::Mat& mat, int mattype);

void genScharrImage(Mat& img);
void genSobelImage(Mat& img, Mat* pSobelx = NULL, Mat* pSobely = NULL);
void genSobelDir(Mat& img);
Mat genSobelDir(const Mat& sobelX, const Mat& sobelY);
Mat genDirColorImg(const Mat& img, Mat* pValueMat = NULL);

void magnitude_16s32f(const Mat& sobelX, const Mat& sobelY, Mat& magMat);

template<typename _T>
void histMeanStddev(const Mat& hist, double* pMean, double* pStddev)
{
    _T* pData = (_T*)hist.data;
    double sum = 0;
    int count = 0;
    for (int i = 0; i < hist.cols; ++i)
    {
        if (!pData[i])
        {
            continue;
        }
        count += pData[i];
        sum += pData[i]*i;
    }
    if (count == 1)
    {
        if (pMean)
        {
            *pMean = sum;
        }
        if (pStddev)
        {
            *pStddev = 0;
        }
        return;
    }
    *pMean = sum / (double)count;
    sum = 0;
    for (int i = 0; i < hist.cols; ++i)
    {
        if (!pData[i])
        {
            continue;
        }
        double d = i - *pMean;
        sum += d*d*pData[i];
    }
    *pStddev = sqrt(sum / (double)(count - 1));
}

bool localMeanVarNorm(const Mat& srcMat, Mat& dstMat, int winRadius, double tarMean = 120, double tarStd = 30);

Mat genXDeriLineKernel(int w, int type, double absVal);

Mat genXDeriMat(const Mat& img, int w, double absVal);

Mat GaussianDownSamplerWithBox(Mat in, Rect bbox, float scaleX, float scaleY = -1);

bool PoolingByReduce(const Mat& src, Mat& des,const Size& BlockSize, ReduceTypes emReducedtype);

//Precisely line-interpolation: {1,3,5,7}=>{1,2,3,4,5,6,7}
Mat interpMat(const Mat& src);
/************************************************************************/
/*                            Normalize                                 */
/************************************************************************/
bool zsorceNorm(const cv::Mat &mtSrc, cv::Mat &mtDst, int rtype = -1, const Mat& mask= Mat::Mat());

Mat normEachRow(const Mat& img);

enum PixelSelectionMethod
{
    LP_PIXEL_SELECT_ALL,
    LP_PIXEL_SELECT_MAJORITY,
    LP_PIXEL_SELECT_MAJORITY_FAST
};

Mat cocentricNorm(const Mat& img, Point2f center, const Mat& mask, float dstMeanVal);

Mat meanNorm(const Mat& img, const Mat& mask, float dstMeanVal,
    float majority = 1.0,
    PixelSelectionMethod method = LP_PIXEL_SELECT_ALL);

Mat minMaxNorm(const Mat& img, double tarMin, double tarMax, const Mat& mask = Mat());

/**********************************************************************/

Mat genGradientDir4EachRow(const Mat& img);

void findMatElementsEquals(const Mat& img, vector<Point>& pointVec, float val, int xPadding);

double localMatSum(const Mat& mat, const Rect& roi);

void findEdgePointsEachRow(const Mat& img, vector<Point>& edgePointVec, int xPadding);

template<typename _Ty>
Mat sumEachRow(const Mat& iMat)
{
	_ASSERTE(iMat.channels() == 1);
	Mat oVec(iMat.rows, 1, DataType<_Ty>::type);
	for (int i = 0; i < iMat.rows; ++i)
	{
		Mat rowVec = iMat.row(i);
		Scalar s = cv::sum(rowVec);
		oVec.at<_Ty>(i) = s.val[0]/rowVec.cols;
	}
	return oVec;
}

template<typename _Ty, int cn>
Mat sumEachRowN(const Mat& iMat)
{
	_ASSERTE(iMat.channels() == cn);
	Mat oVec(iMat.rows, 1, CV_MAKETYPE(DataType<_Ty>::type, cn));
	for (int i = 0; i < iMat.rows; ++i)
	{
		Mat rowVec = iMat.row(i);
		Scalar s = cv::sum(rowVec);
		Vec<_Ty, cn>& d = oVec.at<Vec<_Ty, cn> >(i);
		for (int j = 0; j < cn; ++j)
			d[j] = s[j] / iMat.cols;
	}
	return oVec;
}

CYCLOPS_UTILS_DLLSPEC Mat sumEachRow2(const Mat& img);

template<typename _Ty>
Mat sumEachCol(const Mat& iMat)
{
	_ASSERTE(iMat.channels() == 1);
	Mat oVec(1, iMat.cols, DataType<_Ty>::type);
	for (int i = 0; i < iMat.cols; ++i)
	{
		Mat colVec = iMat.col(i);
		Scalar s = cv::sum(colVec);
		oVec.at<_Ty>(i) = s.val[0]/colVec.rows;
	}
	return oVec;
}

template<typename _Ty, int cn>
Mat sumEachColN(const Mat& iMat)
{
	_ASSERTE(iMat.channels() == cn);
	Mat oVec(1, iMat.cols, CV_MAKETYPE(DataType<_Ty>::type, cn));
	for (int i = 0; i < iMat.cols; ++i)
	{
		Mat colVec = iMat.col(i);
		Scalar s = cv::sum(colVec);
		Vec<_Ty, cn>& d = oVec.at<Vec<_Ty, cn> >(i);
		for (int j = 0; j < cn; ++j)
			d[j] = s[j] / colVec.rows;
	}
	return oVec;
}

CYCLOPS_UTILS_DLLSPEC Mat sumEachCol2(const Mat& img);

/************************************************************************/
/*                            Threshold                                 */
/************************************************************************/
Mat thresholdEachRowLocally(const Mat& img, int localRange = 501, int C = 10);

Mat thresholdEachRow(const Mat& img, float threScale = 0.5);

Mat thresholdEachRow(const Mat& img, const Mat& rowThre, float threScale);

// As OpenCV doesn't provide any API to get automatic threshold value only without actually apply the threshold,
// however in some cases, we need this value only to apply our own threshold type.
// So, copied out the private getThreshVal_Otsu_8u and getThreshVal_Triangle_8u function here,
// and we provide the uniformed getAutoThresValue function to call the above two.
// Pass in THRESH_OTSU for Otsu threshold, and THRESH_TRIANGLE for the other.
double getAutoThresValue(const Mat& img, int type = THRESH_OTSU);
/**********************************************************************/

Mat getFirstChannel(const Mat& img);
Mat getChannel(const Mat& img, int i);
void setChannel(Mat& img, int i, Mat chnMat);

void mulEachRow(Mat& img, const Mat& scaleRow);

void genRandomPoints(const Mat& img, vector<Point>& points, RNG rng, int sampleCnt = 1000);

void plot8uVec(const Mat& vec, float scale = 1.0);

void plot32fVec(const Mat& vec, float scale = 1.0);

CYCLOPS_UTILS_DLLSPEC Mat calcHist(const Mat& img, const Mat& mask, int histSize = 256, int minVal = 0, int maxVal = 256);

Mat resizeSum(const Mat& img, const Size& s);

void writeFile(const Mat& mat, std::string filePath, std::string matName = "mat");

Mat readFile(std::string filePath, std::string matName);

void gaussianBlurEachRow(const Mat& src, Mat& dst, int ksize = 3);

void medianBlurEachRow(const Mat& src, Mat& dst, int ksize = 3);

double maxLaplacianX(const Mat& rowMat, float scale = 1.0);


double minLaplacianX(const Mat& rowMat, float scale = 1.0);

void Laplacian1D(const Mat& src, Mat& dst, int ddepth, int ksize = 1);

void filterKeyPointsByNeighborDistance(vector<KeyPoint>& vec, float dis);

void filterKeyPointsByRotationInvariants(vector<KeyPoint>& vec, const Mat& img,
    FeatureDetector* pDesExtractor, float tor);

double localIC_Angle(const Mat& img, Point2f center, int patchSize);
double localAngle_WeightedCen(const Mat& img, Point2f center);
double localAngle_(const Mat& img, Point2f center, Mat mask);

class GroupMatcher : public BFMatcher
{
public:
    CV_WRAP GroupMatcher(int normType = NORM_L2, bool crossCheck = false);
    virtual ~GroupMatcher() {}

    virtual bool isMaskSupported() const { return false; }

    virtual Ptr<DescriptorMatcher> clone(bool emptyTrainData = false) const;

// no longer supported in opencv3.4.1
#if (CV_MAJOR_VERSION < 3)
    AlgorithmInfo* info() const;
#endif
protected:
    virtual void knnMatchImpl(const Mat& queryDescriptors, vector<vector<DMatch> >& matches, int k,
        const vector<Mat>& masks = vector<Mat>(), bool compactResult = false);
    virtual void radiusMatchImpl(const Mat& queryDescriptors, vector<vector<DMatch> >& matches, float maxDistance,
        const vector<Mat>& masks = vector<Mat>(), bool compactResult = false);

    int normType;
    bool crossCheck;
};

void upperMajorityMask(const Mat& img, Mat& mask, float majority);
void majorityHistLower(const Mat& img, const Mat& mask, Mat& hist, float majority);
void majorityHistUpper(const Mat& img, const Mat& mask, Mat& hist, float majority);
Mat lowerMajorityMask(const Mat& img, const Mat& mask, float majority);
void meanStdDev(const Mat& img, const Mat& mask, double* pMean, double* pStdDev, float majority, int type = 0);

CYCLOPS_UTILS_DLLSPEC float getMajor(const Mat& img, Mat mask, float tfactor = 0.8);

float getMajor(const Mat& img, Mat mask, Range binSize, Range binCount, int binCountExpect,
	double* pMajorMean, double* pMajorStdDev);

Mat getRoiImg(const Mat& img, Point2i cen, int radius);


Mat getRoiImg(const Mat& img, vector<Point2i> ptVec, int radius);


Mat filterY(const Mat& img, float* kernel, int size, int ddepth = CV_32FC1);

Mat getContourBoundedRoiImg(const Mat& src, const vector<Point>& contour, Rect* pRoiRect = NULL);

void filterContours(Mat hsvColorThresImg,
	int minArea, double minLength, double minLongShortRatio,
	double maxCircleDegree, vector<Point>* pAreaMaxContour = NULL);
void filterContours(Mat& img, double minArea);
void filterContours(const vector< vector<Point> >& contours, const Mat& srcImg, 
    vector< vector<Point> >& filteredContours, int minArea, double minLength,
    double minLongShortRatio, double maxCircleDegree, vector<Point>* pAreaMaxContour = NULL);
int filterSmallAndFindMaxContours(vector< vector<Point> >& contours, double minArea);
Mat genInRangeMask(const Mat& src, std::map<int, pair<Scalar, Scalar>>& colorRanges);

void genContrastImg(const Mat& img, Mat& contrastImg, const Mat& kernelMat);
void genContrastImg(const Mat& img, Mat& contrastImg, int ksize);
void genYContrastImg(const Mat& img, Mat& constrastImg, int ksize);

void genHalfContrastImg(const Mat& img, Mat& contrastImg, const Mat& kernelMat);
void genHalfContrastImg(const Mat& img, Mat& contrastImg, int ksize);

void cvtColor(Mat& img, int t);

CYCLOPS_UTILS_DLLSPEC Mat cropImage(const Mat& img, const Rect& roi);
Mat rotateImage(const Mat& img, Point2f cen, float degree);
Mat normAngle(const Mat& img, Mat mask);

Mat genSimpleXGradient(const Mat& m);

void uniformLocalMinExtremas(const Mat& vec, Mat& localExtremaValVec,
    vector<int>& idxVec, int n, int refineRange);

void equalizeHist(const Mat& src, Mat& dst, const Mat& mask = gDummyMat);

void removeHighlights(const Mat& src, Mat& dst, float thre);

void genSectorSumVec(const Mat& img, const Mat& weightMat, Mat& hist,
    Mat& weightHist, const Point2f& cen, float angleStep,
    Mat* pAngMat = NULL, Mat* pMagMat = NULL);
void applySectorScales(Mat& img, const Mat& hist, const Mat& angMat);
void normSectors(Mat& img, Mat weightMat, const Point2f& cen, float angleStep, 
    float tarVal);
void normSectors_tarImg(Mat& img, Mat weightMat, const Point2f& cen, float angleStep, 
    const Mat& tarImg);
void normSectorsMeans_tarHist(Mat& img, const Mat& hist, const Point2f& cen, 
    float angleStep, const Mat& tarHist, const Mat& angMat);

Point2f imgCen(const Mat& img);

CYCLOPS_UTILS_DLLSPEC bool ensureGrayImg(const Mat& img, Mat& gray);

CYCLOPS_UTILS_DLLSPEC bool ensureColorImg(const Mat& img, Mat& color);

Scalar ensureScalarRng(const Scalar& c, int min, int max);

CYCLOPS_UTILS_DLLSPEC void gradiant(const Mat& img, Mat& orResult, int dx, int dy, bool ignoreSign = false, int ddepth = -1);

CYCLOPS_UTILS_DLLSPEC double sharpness(const Mat& img, Mat const* pMask = nullptr, Mat* pRetMat = nullptr);

/************************************************************************/
/*                            Color Space                               */
/************************************************************************/

double hsvDis(const Vec3b& v0, const Vec3b& v1);
Mat genHSVColorDisMat(const Mat& m0, const Mat& m1);
Mat genHSVColorDisMat(const Mat& m, Vec3b baseColor);
Mat genHSVColorDisMat(const Mat& m);
Mat genBlueGreenRatioMat(const Mat& m, float alpha = 166.32, float beta = -16.414);
Mat genColorFuncDisMat(const Mat& m, float a = 0.001, float b = 0.4061, float c = -5.6845);

CYCLOPS_UTILS_DLLSPEC void addColorRngToLUT(const Scalar& c1, const Scalar& c2, Mat& lut, uchar val);

template<typename T>
float validate_diff(T result, T target, float err_tol)
{
	T diff = result - target;
	float normDiff = cv::norm(diff);
	return normDiff <= err_tol ? 0 : normDiff;
}

template<typename T, typename S>
inline S _bilinear(const T* p0, const T* p1, int x0, int x1,
	double a, double b, double c, double d)
{
	return (a * (S)(p0[x0]) + b * (S)(p0[x1])) * c +
		(a * (S)(p1[x0]) + b * (S)(p1[x1])) * d;
}

template<typename T, typename S>
S getImgSubPixBilinear(const Mat& img, float x, float y)
{
	_ASSERTE(!img.empty());
	_ASSERTE(x >= 0 && y >= 0 && x < img.cols - 1 && y < img.rows - 1);

	int x0 = x;
	int y0 = y;
	int x1 = x0 + 1;
	int y1 = y0 + 1;

	double a = x1 - x;
	double b = 1. - a;
	double c = y1 - y;
	double d = 1. - c;

	const T* p0 = img.ptr<T>(y0);
	const T* p1 = img.ptr<T>(y1);

	return _bilinear<T, S>(p0, p1, x0, x1, a, b, c, d);
}

template<typename T, typename S>
void getImgSubPixBilinear2(const Mat& img1, const Mat& img2, float x, float y, S& ret1, S& ret2)
{
	_ASSERTE(!img1.empty() && !img2.empty());
	_ASSERTE(x >= 0 && y >= 0 && x < img1.cols - 1 && y < img1.rows - 1);
	_ASSERTE(img1.size == img2.size);

	int x0 = x;
	int y0 = y;
	int x1 = x0 + 1;
	int y1 = y0 + 1;

	double a = x1 - x;
	double b = 1. - a;
	double c = y1 - y;
	double d = 1. - c;

	const T* p0 = img1.ptr<T>(y0);
	size_t rstep = img1.step.p[0] / sizeof(T);
	const T* p1 = p0 + rstep;

	ret1 = _bilinear<T, S>(p0, p1, x0, x1, a, b, c, d);

	p0 = img2.ptr<T>(y0);
	p1 = p0 + rstep;

	ret2 = _bilinear<T, S>(p0, p1, x0, x1, a, b, c, d);
}

template<typename T, typename S>
S getImgSubPixBilinearSafeBorder(const Mat& img, float x, float y)
{
	_ASSERTE(!img.empty());
	_ASSERTE(x >= 0 && y >= 0 && x < img.cols && y < img.rows);

	int x0 = x;
	int y0 = y;
	int x1 = x0 + 1;
	int y1 = y0 + 1;

	double a = x1 - x;
	double b = 1. - a;
	double c = y1 - y;
	double d = 1. - c;

	if (x0 < 0) x0 = x1;
	if (x1 > img.cols - 1) x1 = x0;
	if (y0 < 0) y0 = y1;
	if (y1 > img.rows - 1) y1 = y0;

	const T* p0 = img.ptr<T>(y0);
	const T* p1 = img.ptr<T>(y1);

	return _bilinear<T, S>(p0, p1, x0, x1, a, b, c, d);
}

template<typename T>
inline T __cubicHermite(T A, T B, T C, T D, double t3, double t2, double t)
{
	T A2 = -A / 2.0f;
	T D2 = D / 2.0f;
	T a = A2 + 1.5f * B - 1.5f * C + D2;
	T b = A - 2.5f * B + 2.0f * C - D2;
	T c = A2 + C / 2.0f;

	return a * t3 + b * t2 + c * t + B;
}

template<typename T, typename S>
inline S _cubicHermite(const T* p0, const T* p1, const T* p2, const T* p3,
	int x0_1, int x0, int x1, int x2,
	double dx3, double dx2, double dx, double dy3, double dy2, double dy)
{
	S col0 = __cubicHermite<S>(p0[x0_1], p0[x0], p0[x1], p0[x2], dx3, dx2, dx);
	S col1 = __cubicHermite<S>(p1[x0_1], p1[x0], p1[x1], p1[x2], dx3, dx2, dx);
	S col2 = __cubicHermite<S>(p2[x0_1], p2[x0], p2[x1], p2[x2], dx3, dx2, dx);
	S col3 = __cubicHermite<S>(p3[x0_1], p3[x0], p3[x1], p3[x2], dx3, dx2, dx);

	return __cubicHermite<S>(col0, col1, col2, col3, dy3, dy2, dy);
}

template<typename T, typename S>
S getImgSubPixCubic(const Mat& img, float x, float y)
{
	_ASSERTE(!img.empty());
	_ASSERTE(x >= 1 && y >= 1 && x < img.cols - 2 && y < img.rows - 2);

	int x0 = x;
	int y0 = y;
	double dx = x - x0;
	double dy = y - y0;

	size_t rstep = img.step.p[0];
	const T* p1 = (const T*)(img.data + rstep * y0);
	const T* p0 = (const T*)((const uchar*)(p1) - rstep);
	const T* p2 = (const T*)((const uchar*)(p1) + rstep);
	const T* p3 = (const T*)((const uchar*)(p2) + rstep);

	double dx2 = dx * dx;
	double dx3 = dx2 * dx;
	double dy2 = dy * dy;
	double dy3 = dy2 * dy;

	int x0_1 = x0 - 1;
	int x1 = x0 + 1;
	int x2 = x0 + 2;

	return _cubicHermite<T, S>(p0, p1, p2, p3, x0_1, x0, x1, x2, dx3, dx2, dx, dy3, dy2, dy);
}

template<typename T, typename S>
void getImgSubPixCubic2(const Mat& img1, const Mat& img2, float x, float y, S& ret1, S& ret2)
{
	_ASSERTE(!img1.empty() && !img2.empty());
	_ASSERTE(x >= 1 && y >= 1 && x < img1.cols - 2 && y < img1.rows - 2);
	_ASSERTE(img1.size == img2.size);

	int x0 = x;
	int y0 = y;
	double dx = x - x0;
	double dy = y - y0;

	size_t rstep = img1.step.p[0];
	const T* p1 = (const T*)(img1.data + rstep * y0);
	const T* p0 = (const T*)((const uchar*)(p1)-rstep);
	const T* p2 = (const T*)((const uchar*)(p1)+rstep);
	const T* p3 = (const T*)((const uchar*)(p2)+rstep);

	double dx2 = dx * dx;
	double dx3 = dx2 * dx;
	double dy2 = dy * dy;
	double dy3 = dy2 * dy;

	int x0_1 = x0 - 1;
	int x1 = x0 + 1;
	int x2 = x0 + 2;

	ret1 = _cubicHermite<T, S>(p0, p1, p2, p3, x0_1, x0, x1, x2, dx3, dx2, dx, dy3, dy2, dy);

	p1 = (const T*)(img2.data + rstep * y0);
	p0 = (const T*)((const uchar*)(p1)-rstep);
	p2 = (const T*)((const uchar*)(p1)+rstep);
	p3 = (const T*)((const uchar*)(p2)+rstep);

	ret2 = _cubicHermite<T, S>(p0, p1, p2, p3, x0_1, x0, x1, x2, dx3, dx2, dx, dy3, dy2, dy);
}

template<typename T, typename S>
S getVecSubPixBilinear(const std::vector<T>& vec, float x)
{
	_ASSERTE(x >= 0 && x < vec.size() - 1);

	int x0 = x;
	double dx = x - x0;
	return saturate_cast<S>(dx * vec[x0 + 1] + (1 - dx) * vec[x0]);
}

template<typename T>
double interpolatePoly2(T v, T v0, T v1)
{
	// y = ax^2 + bx + c, y -> v,
	// x represent the position of the pixel point and its neighbor, x - 1 and x + 1
	float c = v;
	float a = (v0 + v1) / 2.f - c;
	float b = (v1 - v0) / 2.f;
	return abs(a) < FLT_EPSILON ? 0 : -b / (2.f * a); // -b/2a
}

template<typename T>
vector<T> cvtMat2Vec(const Mat& img)
{
	_ASSERTE(!img.empty());
	int count = img.cols * img.rows;

	std::vector<T> vals(count);
	if (img.isContinuous()) {
		memcpy((T*)vals.data(), (T*)img.data, sizeof(T)*count);
	}
	else {
		int rows = img.rows;
		T* vals_p = vals.data();
		size_t rowStep = sizeof(T)*img.cols;
		for (int i = 0; i < rows; ++i) {
			memcpy(vals_p, img.ptr<T>(i), rowStep);
			vals_p += img.cols;
		}
	}

	return vals;
}

template<typename T>
vector<T> cvtMat2Vec(const Mat& img, const Mat& mask)
{
	_ASSERTE(!img.empty());
	int count = img.cols * img.rows;

	std::vector<T> vals;
	vals.reserve(count);
	for (int i = 0; i < img.rows; ++i) {
		const T* img_p = img.ptr<T>(i);
		const uchar* mask_p = mask.ptr<uchar>(i);
		for (int j = 0; j < img.cols; ++j) {
			if (mask_p[j]) {
				vals.push_back(saturate_cast<T>(img_p[j]));
			}
		}
	}

	return vals;
}

template<typename T, typename S>
std::list<T> cvtMat2List(const Mat& img)
{
	_ASSERTE(!img.empty());
	int rows = img.rows;
	int cols = img.cols;
	int count = cols * rows;

	std::list<T> vals;
	for (int i = 0; i < rows; ++i) {
		const S* img_p = img.ptr<S>(i);
		for (int j = 0; j < cols; ++j) {
			vals.push_back(saturate_cast<T>(img_p[j]));
		}
	}

	return vals;
}

template<typename T>
Mat cvtVec2Mat(const vector<T>& vec)
{
	_ASSERTE(vec.size());
	return Mat(1, vec.size(), DataType<T>::type, (T*)vec.data());
}

template<typename T>
T getNthElement(const Mat& img, int n)
{
	_ASSERTE(!img.empty());
	int count = img.cols * img.rows;
	_ASSERTE(n < count);

	std::vector<T> vals = cvtMat2Vec<T>(img);

	std::nth_element(vals.begin(), vals.begin() + n, vals.end());

	return vals[n];
}

int getLocalMaximun(const Mat& vec, double thres, std::vector<int>* pMaxIdxVec);
int getLocalMinimun(const Mat& vec, double thres, std::vector<int>* pMaxIdxVec);

int getLocalMaximun(const Mat& vec, const Mat& mask, std::vector<int>* pMaxIdxVec);
int getLocalMinimun(const Mat& vec, const Mat& mask, std::vector<int>* pMaxIdxVec);

// naive method for finding local extremes
int getLocalMaximun(const Mat& vec, std::vector<int>* pMaxIdxVec);
int getLocalMinimun(const Mat& vec, std::vector<int>* pMaxIdxVec);

enum EnumAbnormalType {
	AbTypePositive = 0,
	AbTypeNegative,
	AbTypeBoth
};
CYCLOPS_UTILS_DLLSPEC int detectAbnormal(const Mat& vec, int kernelSize, EnumAbnormalType abnormalType, double abnormalThreshold,
	Mat* pScores = nullptr, std::vector<int>* pAbIdxVec = nullptr);

typedef std::vector<Point> PointVec;
typedef std::vector<Point2f> Point2fVec;
typedef std::vector<Point3f> Point3fVec;
typedef std::vector<Point2d> Point2dVec;
typedef std::vector<Point3d> Point3dVec;

Point2fVec toPoint2fVec(const Point3fVec& pnts3d);
void toPoint2fVec(const Point3fVec& pnts3d, Point2fVec& pnts2d);
Point3fVec toPoint3fVec(const Point2fVec& pnts2d, float z = 0);
void toPoint3fVec(const Point2fVec& pnts2d, Point3fVec& pnts3d, float z = 0);

CYCLOPS_UTILS_DLLSPEC Point2f detectValley(const Mat& mask);

void filterWithMask(const Mat& src, Mat& dst, const Mat& mask, int ddepth, Size kSize);

CYCLOPS_UTILS_DLLSPEC Point2f findMaskCenter(const Mat& mask);

CYCLOPS_UTILS_DLLSPEC void createMaskByContour(Mat& mask,
	const PointVec& contour, const vector<PointVec>& holes, const Point& offset = Point(0, 0), bool isDense = true);

vector<double> resampleVec(const vector<double>& v, float start, float end, int expectSize);


// add new helper functions above
};

using namespace CyclopsUtils;

#include "GeomUtils.h"

// This macro provide 4 functions for serialization:
// serializeToMemory, serializeToFile, deserializeFromMemory, deserializeFromFile
// It does the error case handling and FileStorage initialization for you,
// you only need to write serialize() and deserialize() to do the real job.
// see CameraCalibrator and BlobDetector for example
#define DECL_SERIALIZE_FUNCS public:\
bool serializeToMemory(std::string& str, bool base64 = true) {\
	try {\
		int writeFlag = base64 ? FileStorage::WRITE_BASE64 : FileStorage::WRITE;\
		FileStorage fs(".json", writeFlag + FileStorage::MEMORY + FileStorage::FORMAT_JSON);\
		if (!fs.isOpened()) return false;\
		bool ret = serialize(fs);\
		if (!ret) return false;\
		str = fs.releaseAndGetString();\
	}\
	catch (const cv::Exception& exc) {\
		std::cout << cvErrorStr(exc.code) << std::endl;\
		return false;\
	}\
	catch (...) {\
		return false;\
	}\
	return true;\
}\
bool serializeToFile(const char* filename, bool base64 = true) {\
	if (!filename || strlen(filename) == 0) return false;\
	try {\
		int writeFlag = base64 ? FileStorage::WRITE_BASE64 : FileStorage::WRITE;\
		FileStorage fs(filename, writeFlag + FileStorage::FORMAT_JSON);\
		if (!fs.isOpened()) return false;\
		bool ret = serialize(fs);\
		if (!ret) return false;\
	}\
	catch (const cv::Exception& exc) {\
		std::cout << cvErrorStr(exc.code) << std::endl;\
		return false;\
	}\
	catch (...) {\
		return false;\
	}\
	return true;\
}\
bool serializeToFile(const std::string& filename, bool base64 = true) { return serializeToFile(filename.c_str(), base64); }\
bool deserializeFromMemory(const std::string& str) {\
	if (str.empty()) return false;\
	try {\
		FileStorage fs(str, FileStorage::READ + FileStorage::MEMORY + FileStorage::FORMAT_JSON);\
		if (!fs.isOpened()) return false;\
		bool ret = deserialize(fs.root());\
		if (!ret) return false;\
	}\
	catch (const cv::Exception& exc) {\
		std::cout << cvErrorStr(exc.code) << std::endl;\
		return false;\
	}\
	catch (...) {\
		return false;\
	}\
	return true;\
}\
bool deserializeFromFile(const char* filename) {\
	if (!filename || strlen(filename) == 0) return false;\
	try {\
		FileStorage fs(filename, FileStorage::READ + FileStorage::FORMAT_JSON);\
		if (!fs.isOpened()) return false;\
		bool ret = deserialize(fs.root());\
		if (!ret) return false;\
	}\
	catch (const cv::Exception& exc) {\
		std::cout << cvErrorStr(exc.code) << std::endl;\
		return false;\
	}\
	catch (...) {\
		return false;\
	}\
	return true;\
}\
bool deserializeFromFile(const std::string& filename) { return deserializeFromFile(filename.c_str()); }\

// more implementation of serialization
template<typename _Tp>
void serializeList(FileStorage& fs, const std::list<_Tp>& vec, const char* n) {
	fs << n << "[";
	for (const _Tp& v : vec) {
		fs << v;
	}
	fs << "]";
}

template<typename _Tp> static inline
void deserializeList(const FileNode& node, std::list<_Tp>& vec, const char* n) {
	FileNode lNode = node[n];
	vec.clear();
	if (!lNode.isNone()) {
		_Tp val;
		for (FileNodeIterator it = lNode.begin(); it != lNode.end(); ++it) {
			*it >> val;
			vec.push_back(std::move(val));
		}
	}
}

template<typename _T, typename _S>
void serializeMap(FileStorage& fs, const std::map<_T, _S>& m, const char* n) {
	fs << n << "{";
	fs << "keys" << "[";
	for (auto it = m.begin(); it != m.end(); ++it) {
		fs << it->first;
	}
	fs << "]";
	fs << "values" << "[";
	for (auto it = m.begin(); it != m.end(); ++it) {
		fs << it->second;
	}
	fs << "]";
	fs << "}";
}

template<typename _T, typename _S>
void deserializeMap(const FileNode& node, std::map<_T, _S>& m, const char* n) {
	FileNode mNode = node[n];
	m.clear();
	if (!mNode.isNone()) {
		_T k; _S v;
		FileNode kNode = mNode["keys"];
		FileNode vNode = mNode["values"];
		_ASSERTE(kNode.size() == vNode.size());
		FileNodeIterator itValue = vNode.begin();
		for (FileNodeIterator itKey = kNode.begin(); itKey != kNode.end() && itValue != vNode.end();
			++itKey, ++itValue)
		{
			*itKey >> k;
			*itValue >> v;
			m[k] = v;
		}
	}
}


// more implementation of saturate_cast
namespace cv {
template<> inline float saturate_cast<float>(double v) { return v > FLT_MAX ? FLT_MAX : (v < -FLT_MAX ? -FLT_MAX : v); }
}

template<typename T, typename S> inline Point_<T> saturate_cast_point(const Point_<S>& pnt) {
	return Point_<T>(saturate_cast<T>(pnt.x), saturate_cast<T>(pnt.y));
}
template<typename T, typename S> inline std::vector<Point_<T> > saturate_cast_points(const std::vector<Point_<S> >& pnts) {
	size_t n = pnts.size();
	std::vector<Point_<T> > ret; ret.reserve(n);
	for (size_t i = 0; i < n; ++i) {
		ret.push_back(saturate_cast_point<T, S>(pnts[i]));
	}
	return std::move(ret);
}

template<typename T, typename S> inline Point_<T> saturate_cast_point(const Point_<S>& pnt,
	const Point_<S>& scale, const Point_<S>& shift)
{
	return Point_<T>(saturate_cast<T>(pnt.x * scale.x + shift.x), saturate_cast<T>(pnt.y * scale.y + shift.y));
}
template<typename T, typename S> inline std::vector<Point_<T> > saturate_cast_points(const std::vector<Point_<S> >& pnts,
	const Point_<S>& scale, const Point_<S>& shift)
{
	size_t n = pnts.size();
	std::vector<Point_<T> > ret; ret.reserve(n);
	for (size_t i = 0; i < n; ++i) {
		ret.push_back(saturate_cast_point<T, S>(pnts[i], scale, shift));
	}
	return std::move(ret);
}


// more implementation of cv structure's operation 
namespace cv {
static inline bool operator!= (const Size& s1, const Size& s2) { return s1.width != s2.width || s1.height != s2.height; }
static inline bool operator>= (const Size& s1, const Size& s2) { return s1.width >= s2.width && s1.height >= s2.height; }
static inline bool operator> (const Size& s1, const Size& s2) { return s1.width > s2.width && s1.height > s2.height; }
static inline bool operator<= (const Size& s1, const Size& s2) { return s1.width <= s2.width && s1.height <= s2.height; }
static inline bool operator< (const Size& s1, const Size& s2) { return s1.width < s2.width && s1.height < s2.height; }
}

// implementation of float range
namespace cv {
class Rangef
{
public:
	Rangef() : start(0), end(0) {}
	Rangef(float _start, float _end) : start(_start), end(_end) {}
	float size() const { return empty() ? 0 : end - start; }
	float empty() const { return start >= end; }
	void add(float v) {
		start = std::min<float>(v, start);
		end = std::max<float>(v, end);
	}
	void add(const Rangef& other) {
		start = std::min<float>(other.start, start);
		end = std::max<float>(other.end, end);
	}
	Rangef extend(float dstart, float dend) const {
		return Rangef(start + dstart, end + dend);
	}
	Rangef merge(const Rangef& other) const {
		return Rangef(std::min<float>(other.start, start), std::max<float>(other.end, end));
	}
	Rangef overlap(const Rangef& other) const {
		if (start > other.start) return other.overlap(*this);
		else if (end > other.start) return Rangef(other.start, std::min<float>(other.end, end));
		else return Rangef();
	}
	bool equal(const Rangef& other, float tol = FLT_EPSILON) const {
		return abs(other.start - start) < tol && abs(other.end - end) < tol;
	}
	float mid() const { return (start + end) / 2.; }
	bool contain(float v) const { return v >= start && v <= end; }
	static Rangef all() { return Rangef(-FLT_MAX, FLT_MAX); }
	static Rangef none() { return Rangef(FLT_MAX, -FLT_MAX); }

	float start, end;
};

template<> class DataType<Rangef>
{
public:
	typedef Rangef      value_type;
	typedef value_type	work_type;
	typedef float       channel_type;

	enum {
		generic_type = 0,
		channels = 2,
		fmt = traits::SafeFmt<channel_type>::fmt + ((channels - 1) << 8)
#ifdef OPENCV_TRAITS_ENABLE_DEPRECATED
		, depth = DataType<channel_type>::depth
		, type = CV_MAKETYPE(depth, channels)
#endif
	};

	typedef Vec<channel_type, channels> vec_type;
};

namespace traits {
	template<>
	struct Depth< Rangef > { enum { value = Depth<float>::value }; };
	template<>
	struct Type< Rangef > { enum { value = CV_MAKETYPE(Depth<float>::value, 2) }; };
}
}

#endif // __CVUtils_h_