#ifndef _DynamicProgramSearch_h_
#define _DynamicProgramSearch_h_

#include <opencv2/opencv.hpp>
//#include "CVUtils.h"

using namespace cv;

using std::vector;

void dynamicProgramYWithSmooth(Mat& disMat, int n = 2, float sw = 1.0, const Mat& smoothMat = Mat());
void dynamicProgramYWithSmooth(Mat& disMat0, Mat& disMat1,
	const Mat& smoothMat0, const Mat& smoothMat1, int targetWidth, int n, float sw);

void dynamicProgramY(Mat& disMat, int n = 2);
void dynamicProgramX(Mat& disMat, int n = 2);

void findMaxXPath(const Mat& disMat, vector<int>& vec, vector<float>& energyVec,
	int n = 2);
void findMaxYPath(const Mat& disMat, vector<int>& vec, vector<float>& energyVec,
	int n = 2);

void recoverPathEnergy(vector<float>& vec);

inline float ptLineDisY(const Vec4f& line, const Point& pt)
{
    float p0x = line.val[2];
    float p0y = line.val[3];
    float p1x = line.val[0] + p0x;
    float p1y = line.val[1] + p0y;

    float px = pt.x;
    float py = pt.y;

    float y = p0y + (p1y - p0y) / (p1x - p0x) * (px - p0x);

    float dy = y - py;
    if (dy > 0)
    {
        return dy;
    }
    else
    {
        return -dy;
    }
}
// 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 float ptLineDis(const Vec4f& line, const Point& pt)
{
	float a = line.val[1];
	float b = -line.val[0];
	float c = -a*line.val[2] - b*line.val[3];
	return (a * (float)pt.x + b * (float)pt.y + c) * _InvSqrt(a*a + b*b);
}


inline float ptLineDis(const Vec4f& line, const Point2f& pt)
{
	float a = line.val[1];
	float b = -line.val[0];
	float c = -a*line.val[2] - b*line.val[3];
	return (a * pt.x + b * pt.y + c) * _InvSqrt(a*a + b*b);
}

inline float ptLineDis(const Vec4f& line, const vector<Point>& ptVec)
{
	float ret = 0;
	for (int i = 0; i < ptVec.size(); ++i)
	{
		const Point& pt(ptVec[i]);
		float d = ptLineDis(line, pt);
		ret += abs(d);
	}
	return ret;
}

template<typename _T>
void drawPointsX(vector<int>& xVec, Mat& img, int sx, int sy, int color = 255, int w = 1)
{
    for (int x = 0; x < xVec.size(); ++x)
    {
        int y = xVec[x] + sy;
        img.at<_T>(y, x + sx) = color;
        for (int i = 1; i <= w / 2; ++i)
        {
            if (y - i >= 0)
            {
                img.at<_T>(y - i, x + sx) = color;
            }
            if (y + i < img.rows)
            {
                img.at<_T>(y + i, x + sx) = color;
            }
        }
    }
}

template<typename _T>
void drawPointsY(vector<int>& yVec, Mat& img, int sx, int sy, int color = 255, int w = 1)
{
    for (int y = 0; y < yVec.size(); ++y)
    {
        int x = yVec[y] + sx;
        img.at<_T>(y + sy, x) = color;
        for (int i = 1; i <= w / 2; ++i)
        {
            if (x - i >= 0)
            {
                img.at<_T>(y + sy, x - i) = color;
            }
            if (x + i < img.cols)
            {
                img.at<_T>(y + sy, x + i) = color;
            }
        }
    }
}

#endif // _DynamicProgramSearch_h_