#include <iostream>
#include "opencv2/opencv.hpp"

using namespace std;
using namespace cv;

class SAD
{
public:
	SAD() :winSize(7), DSR(30) {}
	SAD(int _winSize, int _DSR) : winSize(_winSize), DSR(_DSR) {}
	Mat computerSAD(Mat &L, Mat&R);

private:
	int winSize;	//卷积核尺寸
	int DSR;		//视差搜索范围
};

Mat SAD::computerSAD(Mat&L, Mat& R)
{
	int height = L.rows;
	int width = L.cols;
	Mat kernel_L(Size(winSize, winSize), CV_8U, Scalar::all(0));
	Mat kernel_R(Size(winSize, winSize), CV_8U, Scalar::all(0));
	Mat disparity(height, width, CV_8U, Scalar::all(0));
	
	for (size_t i = 0; i < width - winSize; i++)
	{
		for (size_t j = 0; j < height - winSize; j++)
		{
			kernel_L = L(Rect(i, j, winSize, winSize));
			Mat MM(1, DSR, CV_32F, Scalar(0));

			for (size_t k = 0; k < DSR; k++)
			{
				if (i >= k)
				{
					kernel_R = R(Rect(i - k, j, winSize, winSize));
					Mat Dif;
					absdiff(kernel_L, kernel_R, Dif);
					Scalar ADD = sum(Dif);
					MM.at<float>(k) = ADD[0];
				}
			}

			Point minLoc;
			minMaxLoc(MM, NULL, NULL, &minLoc, NULL);

			int loc = minLoc.x;
			disparity.at<char>(j, i) = loc * 16;
		}
	}
	return disparity;
}

#pragma once

#include "opencv2/opencv.hpp"
#include <vector>
#include <iostream>
#include <string>

using namespace std;
using namespace cv;


float sadvalue(const Mat& src1, const Mat& src2)
{
	Mat matdiff = cv::abs(src1 - src2);
	int saddiff = cv::sum(matdiff)[0];
	return saddiff;
}

float GetMinSadIndex(vector<float>& sad)
{
	float minsad = sad[0];
	int index = 0;
	int len = sad.size();
	for (size_t i = 1; i < len; i++)
	{
		if (sad[i] < minsad)
		{
			minsad = sad[i];
			index = i;
		}
	}
	return index;
}

void MatDataNormal(const Mat& src, Mat& dst)
{
	normalize(src, dst, 255, 0, NORM_MINMAX);
	dst.convertTo(dst, CV_8UC1);
}

void GetPointDepthRight(Mat& disparity, Mat& leftimg, Mat& rightimg, const int MaxDisparity, const int winsize)
{
	int row = leftimg.rows;
	int col = leftimg.cols;
	if (leftimg.channels() == 3 && rightimg.channels() == 3)
	{
		cvtColor(leftimg, leftimg, CV_BGR2GRAY);
		cvtColor(rightimg, rightimg, CV_BGR2GRAY);
	}

	int w = winsize;
	int rowrange = row - w;
	int colrange = col - w - MaxDisparity;

	for (size_t i = w; i < rowrange; i++)
	{
		int *ptr = disparity.ptr<int>(i);
		for (size_t j = w; j < colrange; j++)
		{
			Mat rightwin = rightimg(Range(i - w, i + w + 1), Range(j - w, j + w + 1));
			vector<float> sad(MaxDisparity);
			for (size_t d = j; d < j + MaxDisparity; d++)
			{
				Mat leftwin = leftimg(Range(i - w, i + w + 1), Range(d - w, d + w + 1));
				sad[d - j] = sadvalue(leftwin, rightwin);
			}
			*(ptr + j) = GetMinSadIndex(sad);
		}
	}
}

void GetPointDepthLeft(Mat& disparity, Mat& leftimg, Mat& rightimg, const int MaxDisparity, const int winsize)
{
	int row = leftimg.rows;
	int col = leftimg.cols;
	if (leftimg.channels() == 3 && rightimg.channels() == 3)
	{
		cvtColor(leftimg, leftimg, CV_BGR2GRAY);
		cvtColor(rightimg, rightimg, CV_BGR2GRAY);
	}

	int w = winsize;
	int rowrange = row - w;
	int colrange = col - w - MaxDisparity;

	for (size_t i = w; i < rowrange; i++)
	{
		int *ptr = disparity.ptr<int>(i);
		for (size_t j = MaxDisparity + w; j < colrange; j++)
		{
			Mat leftwin = leftimg(Range(i - w, i + w + 1), Range(j - w, j + w + 1));
			vector<float> sad(MaxDisparity);
			for (size_t d = j; d > j - MaxDisparity; d--)
			{
				Mat rightwin = rightimg(Range(i - w, i + w + 1), Range(d - w, d + w + 1));
				sad[j - d] = sadvalue(leftwin, rightwin);
			}
			*(ptr + j) = GetMinSadIndex(sad);
		}
	}
}


// Left-Right Consistency 
void CrossCheckDisparity(const Mat& leftdisp, const Mat& rightdisp, Mat& lastdisp, const int MaxDisparity, const int winsize)
{
	int row = leftdisp.rows;
	int col = rightdisp.cols;
	int w = winsize;
	int rowrange = row - w;
	int colrange = col - MaxDisparity - w;
	int diffthreshold = 2;
	for (size_t i = w; i < row - w; i++)
	{
		const int *ptrleft = leftdisp.ptr<int>(i);
		const int *ptrright = rightdisp.ptr<int>(i);
		int *ptrdisp = lastdisp.ptr<int>(i);
		for (size_t j = MaxDisparity + w; j < col - MaxDisparity - w; j++)
		{
			int leftvalue = *(ptrleft + j);
			int rightvalue = *(ptrright + j);
			int diff = abs(leftvalue - rightvalue);
			if (diff>diffthreshold)
			{
				*(ptrdisp + j) = 0;
			}
			else
			{
				*(ptrdisp + j) = leftvalue;
			}
		}
	}
}

int main(int argc, char* argv[])
{
	Mat img_L = imread("0.png", IMREAD_GRAYSCALE);
	Mat img_R = imread("4.png", IMREAD_GRAYSCALE);
	//Mat img_L = imread("aloeL.jpg", IMREAD_GRAYSCALE);
	//Mat img_R = imread("aloeR.jpg", IMREAD_GRAYSCALE);
	if (img_L.empty() || img_R.empty())
	{
		exit(-1);
	}
	Mat disparity;

	/***************************************************************************/
	// SAD简易实现版本+LRC
	/***************************************************************************/
	int row = img_L.rows;
	int col = img_L.cols;
	Mat depthleft = Mat::zeros(row, col, CV_32S);
	Mat depthright = Mat::zeros(row, col, CV_32S);
	Mat lastdisp = Mat::zeros(row, col, CV_32S);
	int MaxDisparity = 64;
	int winsize = 15;
	GetPointDepthLeft(depthleft, img_L, img_R, MaxDisparity, winsize);
	GetPointDepthRight(depthright, img_L, img_R, MaxDisparity, winsize);
	CrossCheckDisparity(depthleft, depthright, lastdisp, MaxDisparity, winsize);
	
	MatDataNormal(depthleft, depthleft);
	MatDataNormal(depthright, depthright);
	MatDataNormal(lastdisp, lastdisp);
	namedWindow("left", 0);
	namedWindow("right", 0);
	namedWindow("depthleft", 0);
	namedWindow("depthright", 0);
	namedWindow("lastdisp", 0);
	imshow("left", img_L);
	imshow("right", img_R);
	imshow("depthleft", depthleft);
	imshow("depthright", depthright);
	imshow("lastdisp", lastdisp);
	waitKey(0);