// vc++ settings
// include	C:\opencv400\opencv\build\include
// libpath	C:\opencv400\opencv\build\x64\vc15\lib
// lib		opencv_world400.lib;winmm.lib;

#define	WINDOWS

#ifdef	WINDOWS
#include <windows.h>
#include <mmsystem.h>		// link	winmm.lib
#include <WinBase.h>
#endif	// WINDOWS

#include <math.h>

#include <opencv2/opencv.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

#define PI				3.14159265358979
#define rad2deg(x)		(((x) / PI) * 180.0)
#define deg2rad(x)		(((x) / 180.0) * PI)

////////////////////////////////////////////////////////////////////////////

#define	L_FILE	"left.jpg"
#define	R_FILE	"right.jpg"

// for 2592x1944 image

#define	L_CTR_X			1277.0
#define	L_CTR_Y			891.0
#define	L_COEF			9.0

#define	R_CTR_X			1420.0
#define	R_CTR_Y			926.0
#define	R_COEF			9.0

#define	DIST_L_R		0.1			// unit: m

#define	MATCH_THRESHOLD		10.0

#define	SEARCH_STEP			1.0			// pixels
#define	SEARCH_MAX			20.0		// pixels
#define	SEARCH_THRESHOLD	1.0			// meter

#define	SHORT_RANGE			1.0
#define	MIDDLE_RANGE		4.0
#define	LONG_RANGE			20.0

#define	OUTNUM_SHORT_RANGE	2
#define	OUTNUM_MIDDLE_RANGE	4
#define	OUTNUM_LONG_RANGE	2

#define	L_DISP_X_OFFSET		(-235)
#define	R_DISP_X_OFFSET		(15)

#define	SHOW_RESULT
//#undef	SHOW_RESULT
#define WINNAME 				"distance"	// window name

////////////////////////////////////////////////////////////////////////////

void distance_calc_agl(
	double	d,			// L-R distance
	double	cx_l,		// L:center
	double	cy_l,
	double	x_l,		// L:pos
	double	y_l,
	double	ratio_l,	// L:coef
	double	cx_r,		// R:center
	double	cy_r,
	double	x_r,		// R:pos
	double	y_r,
	double	ratio_r,	// R:coef
	double	*d_l,		// L:result
	double	*d_r		// R:result
)
{
	double	dx_l, dy_l;
	double	dx_r, dy_r;

	// 左レンズ
	dx_l	= x_l  - cx_l;
	dy_l	= cy_l - y_l;

	// 右レンズ
	dx_r	= x_r  - cx_r;
	dy_r	= cy_r - y_r;

	////////////////////////////////////////////////////////////////////////////////////
	// θrot_+y計算
	////////////////////////////////////////////////////////////////////////////////////
	double	r_l, theta_l_deg, theta_l;
	double	r_r, theta_r_deg, theta_r;

	// 左レンズ
	r_l		= sqrt(dx_l * dx_l + dy_l * dy_l);
	if (ratio_l != 0.0)
		theta_l_deg	= r_l / ratio_l;
	else
		theta_l_deg	= 0.0;
	theta_l	= deg2rad(theta_l_deg);

	// 右レンズ
	r_r		= sqrt(dx_r * dx_r + dy_r * dy_r);
	if (ratio_r	!= 0.0)
		theta_r_deg	= r_r / ratio_r;
	else
		theta_r_deg	= 0.0;
	theta_r	= deg2rad(theta_r_deg);

	////////////////////////////////////////////////////////////////////////////////////
	// φ計算
	////////////////////////////////////////////////////////////////////////////////////
	double	phi_l, phi_r;

	// 左レンズ
	phi_l	= atan2(dy_l, dx_l);

	// 右レンズ
	phi_r	= atan2(dy_r, dx_r);

	////////////////////////////////////////////////////////////////////////////////////%
	// 距離計算
	////////////////////////////////////////////////////////////////////////////////////%
	double	divider;

	divider	= sin(theta_l) * cos(phi_l) * cos(theta_r) - sin(theta_r) * cos(phi_r) * cos(theta_l);

	if (divider != 0.0) {
		*d_l	= (cos(theta_r) / divider) * d;
		*d_r	= (cos(theta_l) / divider) * d;
	}
	else {
		*d_l	= -1.0;
		*d_r	= -1.0;
	}
}


void search_distance_calc_agl(
	double	d,			// L-R distance
	double	cx_l,		// L:center
	double	cy_l,
	double	x_l,		// L:pos
	double	y_l,
	double	ratio_l,	// L:coef
	double	cx_r,		// R:center
	double	cy_r,
	double	x_r,		// R:pos
	double	y_r,
	double	ratio_r,	// R:coef
	double	*d_l,		// L:result
	double	*d_r		// R:result
)
{
	double	diff_l, diff2_l;
	double	new_l, new_r;
	double	prev_l, prev_r;

	double	idx;
	int		initflag;

	initflag	= 1;

	for (idx = 0.0; idx < SEARCH_MAX; idx += SEARCH_STEP) {
		distance_calc_agl(
					d,			// L-R distance
					cx_l,		// L:center
					cy_l,
					x_l + idx,	// *** L:pos
					y_l,
					ratio_l,	// L:coef
					cx_r,		// R:center
					cy_r,
					x_r - idx,	// *** R:pos
					y_r,
					ratio_r,	// R:coef
					&new_l,		// L:result
					&new_r		// R:result
		);

		if (initflag == 1) {
			initflag	= 0;
			*d_l		= new_l;
			*d_r		= new_r;
			diff2_l		= SEARCH_THRESHOLD * 10000.0;	// 10000.0 ... margin
			diff_l		= SEARCH_THRESHOLD * 10000.0;
		}
		else {
			if (new_l <= 0.0 || new_r <= 0.0) {
				new_l	= SEARCH_THRESHOLD * (-10000.0);
				new_r	= SEARCH_THRESHOLD * (-10000.0);
				diff2_l	= SEARCH_THRESHOLD * 10000.0;
				diff_l	= SEARCH_THRESHOLD * 10000.0;
				prev_l	= new_l;
			}
			else {	// normal case
				diff2_l	= diff_l;
				diff_l	= prev_l - new_l;
				prev_l	= new_l;

				if (diff2_l < SEARCH_THRESHOLD && diff_l < SEARCH_THRESHOLD) {
					*d_l	= new_l;
					*d_r	= new_r;
					break;
				}
			}
		}
	}
}


int orthant(int cx, int cy, int x, int y)
{
	if (x >= cx && y >= cy)
		return 1;
	else if (x < cx && y >= cy)
		return 2;
	else if (x < cx && y < cy)
		return 3;
	else
		return 4;
}


///////////////////////////////////////////////////////////////////////////////////////////////////
int	main(int argc, char* argv[])
{
	cv::Mat	img1	= imread(L_FILE, cv::IMREAD_COLOR);
	cv::Mat	img2	= imread(R_FILE, cv::IMREAD_COLOR);

	cv::Mat	dest;
	cv::hconcat(img1, img2, dest);

	int		clr		= 1;

	// 特徴点抽出
	auto akaze	= cv::AKAZE::create();
	std::vector<cv::KeyPoint>	keypoint1, keypoint2;
	akaze->detect(img1, keypoint1);
	akaze->detect(img2, keypoint2);

	// 特徴記述
	cv::Mat	descriptor1, descriptor2;
	akaze->compute(img1, keypoint1, descriptor1);
	akaze->compute(img2, keypoint2, descriptor2);

	// マッチング
	cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce");
	std::vector<cv::DMatch> match, match_use;
	matcher->match(descriptor1, descriptor2, match);

	double	min_dist	= -1.0;
	for (size_t i = 0; i < match.size(); i++) {
		cv::DMatch forward	= match[i];
		if (min_dist < 0 || forward.distance < min_dist)
			min_dist	= forward.distance;
	}

	int	cnt_short	= 0;
	int	cnt_mid		= 0;
	int	cnt_long	= 0;

	for (size_t i = 0; i < match.size(); i++) {
		cv::DMatch forward	= match[i];

		if (forward.distance < min_dist * MATCH_THRESHOLD) {
			int			idx1, idx2;
			cv::Point2f	coor1, coor2;
			int			x1, y1, x2, y2;
			double		l1, l2;

			// 対応点の座標を求める
			idx1	= (int)(forward.queryIdx);
			coor1	= (keypoint1[idx1]).pt;
			x1		= (int)coor1.x;
			y1		= (int)coor1.y;

			idx2	= (int)(forward.trainIdx);
			coor2	= (keypoint2[idx2]).pt;
			x2		= (int)coor2.x;
			y2		= (int)coor2.y;

			search_distance_calc_agl(
					DIST_L_R,
					L_CTR_X, L_CTR_Y, (double)x1, (double)y1, L_COEF,
					R_CTR_X, R_CTR_Y, (double)x2, (double)y2, R_COEF,
					&l1, &l2
				);

			int	orthant1	= orthant(L_CTR_X, L_CTR_Y, x1, y1);
			int	orthant2	= orthant(R_CTR_X, R_CTR_Y, x2, y2);

			// 対応点のうち距離が正常なものを登録
			if (l1 > 0.0 && l2 > 0.0
					&& (   (l2 <= l1 && l1 < DIST_L_R + l2)
						|| (l1 <= l2 && l2 < DIST_L_R + l1) )
					&& (   (orthant1 == 1 && orthant2 != 3)
						|| (orthant1 == 2 && orthant2 != 4)
						|| (orthant1 == 3 && orthant2 != 1)
						|| (orthant1 == 4 && orthant2 != 2) )
				) {

				int	push_back	= 0;
				if (l1 <= SHORT_RANGE && cnt_short < OUTNUM_SHORT_RANGE) {
					cnt_short++;
					push_back	= 1;
				}
				else if (SHORT_RANGE < l1 && l1 <= LONG_RANGE && cnt_mid < OUTNUM_MIDDLE_RANGE) {
					cnt_mid++;
					push_back	= 1;
				}
				else if (LONG_RANGE < l1 && cnt_long < OUTNUM_LONG_RANGE) {
					cnt_long++;
					push_back	= 1;
				}

				if (push_back == 1) {
					char	strbuf[1024];
					int		r, g, b;

					r	= ((clr & 0x01) == 0) ? 0 : 255;
					g	= ((clr & 0x02) == 0) ? 0 : 255;
					b	= ((clr & 0x04) == 0) ? 0 : 255;
					clr++;
					if (clr >= 8) {
						clr	= 1;
					}

					match_use.push_back(forward);
					fprintf(stderr, "1[x=%d, y=%d] l=%f <==> 2[x=%d, y=%d] l=%f\n", x1, y1, l1, x2, y2, l2);

					// note: 16 == CV_AA

					sprintf(strbuf, "%.2f", l1);
					cv::putText(dest, strbuf, cv::Point(x1 + L_DISP_X_OFFSET, y1), cv::FONT_HERSHEY_DUPLEX, 3.0, cv::Scalar(0,0,0), 15, 16);
					cv::putText(dest, strbuf, cv::Point(x1 + L_DISP_X_OFFSET, y1), cv::FONT_HERSHEY_DUPLEX, 3.0, cv::Scalar(b,g,r), 5, 16);

					sprintf(strbuf, "%.2f", l2);
					cv::putText(dest, strbuf, cv::Point(x2 + R_DISP_X_OFFSET + img2.cols, y2), cv::FONT_HERSHEY_DUPLEX, 3.0, cv::Scalar(0,0,0), 15, 16);
					cv::putText(dest, strbuf, cv::Point(x2 + R_DISP_X_OFFSET + img2.cols, y2), cv::FONT_HERSHEY_DUPLEX, 3.0, cv::Scalar(b,g,r), 5, 16);

					cv::line(dest, cv::Point(x1, y1), cv::Point(x2 + img2.cols, y2), cv::Scalar(b, g, r), 3, 16);
				}
			}
		}
	}
	

	// マッチング結果の描画
//	cv::drawMatches(img1, keypoint1, img2, keypoint2, match_use, dest);
	cv::imwrite("result.jpg", dest);

#ifdef	SHOW_RESULT
	cv::namedWindow(WINNAME, cv::WINDOW_NORMAL);
	cv::imshow(WINNAME, dest);
	while (1) {
		int	key;
		key	= cv::waitKey(1);
		if (key >= 0)
			break;
	}
	cv::destroyWindow(WINNAME);
#endif

}
