/** @brief Функция ищет на переданной картинке наш калибровочный паттерн и
	запоминает найденные точки паттерна.
	Size boardSize (9, 6) - это конфигурация калибровочного паттерна, количества квадратов по горизонтали и вертикали,
	в sampleFound сохраняется был ли найден паттерн на изображении
*/
void CameraCalibrator::findSample (const cv::Mat& img) {

	currentSamplePoints.clear();

	int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
	sampleFound = findChessboardCorners( img, boardSize, currentSamplePoints, chessBoardFlags);
	currentImage = &img;
}

bool CameraCalibrator::isSampleFound () {
	return sampleFound;
}

/** @brief Функция сохранения найденного сэмпла
*/
void CameraCalibrator::acceptSample () {
	// немного улучшает найденные координаты сэмпла
	Mat viewGray;
	cvtColor(*currentImage, viewGray, COLOR_BGR2GRAY);
	cornerSubPix( viewGray, currentSamplePoints, Size(11,11),
	Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 ));
	
	// рисует на изображении найденные точки (полезно видеть что именно было найдено)
	drawChessboardCorners(*currentImage, boardSize, Mat(currentSamplePoints), sampleFound);
	
	//сохраняет сэмпл
	samplesPoints.push_back(currentSamplePoints);
}

/** @brief Функция инициирующая процесс калибровки
*/
void CameraCalibrator::makeCalibration () {
	vector<Mat> rvecs, tvecs;
	vector<float> reprojErrs;
	double totalAvgErr = 0;
	//мы знаем заранее, что у нас картинка всегда квадратная
	//поэтому можно этот параметр не калибровать,
	//если есть еще какая-то информация о камерах, ее тоже
	//можно использовать с помощью других флагов, которые
	//можно здесь указать
	float aspectRatio = 1.0;
	int flags = CV_CALIB_FIX_ASPECT_RATIO;

	bool ok = runCalibration(samplesPoints, imageSize, boardSize, squareSize,
                        aspectRatio, flags, cameraMatrix, distCoeffs,
                        rvecs, tvecs, reprojErrs, totalAvgErr);
	
	// выводим в лог результат калибровки
	stringstream sstr;
	sstr << "--- calib result: " << (ok ? "Calibration succeeded" : "Calibration failed") << ". avg reprojection error = " << totalAvgErr;
	DebugLog(sstr.str());
	
	saveCameraParams(imageSize, boardSize, squareSize, aspectRatio, flags, cameraMatrix, distCoeffs, rvecs, tvecs, reprojErrs, samplesPoints, totalAvgErr);
}

/** @brief Функция рассчета геометрии калибровочного паттерна
*/
void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners)
{
	corners.resize(0);
	for( int i = 0; i < boardSize.height; ++i )
		for( int j = 0; j < boardSize.width; ++j )
			corners.push_back(Point3f(j*squareSize, i*squareSize, 0));
}

/** @brief Функция выполняющая калибровку
*/
bool runCalibration(
	vector<vector<Point2f> > imagePoints,
	Size imageSize, Size boardSize,
	float squareSize, float aspectRatio,
	int flags, Mat& cameraMatrix, Mat& distCoeffs,
	vector<Mat>& rvecs, vector<Mat>& tvecs,
	vector<float>& reprojErrs,
	double& totalAvgErr
) {
	//инициализация матрицы внутренних параметров камеры
	cameraMatrix = Mat::eye(3, 3, CV_64F);
	if( flags & CALIB_FIX_ASPECT_RATIO )
	cameraMatrix.at<double>(0,0) = aspectRatio;

	//инициализация коэффициентов дисторсии
	distCoeffs = Mat::zeros(8, 1, CV_64F);

	//рассчитываем координаты точек на калибровочном паттерне
	//в системе калибровочного паттерна
	//это необходимо для калибровки
	vector<vector<Point3f> > objectPoints(1);
	calcChessboardCorners(boardSize, squareSize, objectPoints[0]);

	objectPoints.resize(imagePoints.size(),objectPoints[0]);

	//вызов OpenCV функции калибровки
	//objectPoints - заранее известная геометрия калировочного паттерна
	//imagePoints - калибровочные сэмплы
	//imageSize - размеры изображения камеры
	double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix,
                    distCoeffs, rvecs, tvecs, flags|CALIB_FIX_K4|CALIB_FIX_K5);
                    ///*|CALIB_FIX_K3*/|CALIB_FIX_K4|CALIB_FIX_K5);
	
	//rms - оценка качества калибровки, при хорошей калибровке,
	//при задании всех размеров в пикселах, должно около 1
	printf("RMS error reported by calibrateCamera: %g\n", rms);

	bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);

	totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints,
                rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs);

	return ok;
}

/** @brief Функция, выполняющая калибровку стереопары
*/
void StereoCalibrator::makeCalibration (
	const std::vector<std::vector<cv::Point2f>>& camera1SamplesPoints,
	const std::vector<std::vector<cv::Point2f>>& camera2SamplesPoints,
	cv::Mat& camera1Matrix,
	cv::Mat& camera1DistCoeffs,
	cv::Mat& camera2Matrix,
	cv::Mat& camera2DistCoeffs
) {

	//снова нам нужно передать геометрию калибровочного паттерна
	std::vector<vector<Point3f>> objectPoints;
	for( int i = 0; i < camera1SamplesPoints.size(); i++ ) {
		objectPoints.push_back(chessboardCorners);
	}

	double rms = stereoCalibrate(
		objectPoints, camera1SamplesPoints, camera2SamplesPoints,
		//матрицы и параметры искажений будут уточнены и перезаписаны
		camera1Matrix, camera1DistCoeffs,
		camera2Matrix, camera2DistCoeffs,
		//разрешение картинка камеры
		imageSize,
		//здесь будет рассчитанная матрица поворота второй камеры относительно первой
		rotationMatrix,
		//здесь будет вектор трансляции второй камеры относительно первой
		translationVector,
		//здесь будет существенная матрица
		essentialMatrix,
		//здесь будет фундаментальная матрица
		fundamentalMatrix,
		//флаг, указывающий что нажно использовать переданные матрицы камер
		//и параметры искажений в качестве начального приближения
		CV_CALIB_USE_INTRINSIC_GUESS,
		TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-5)
	);
	
	//функция также возвращает оценку качества калибровки -
	//ошибку репроекции, она, по идее, должна тоже быть в районе 1
	stringstream outs;
	outs << "--- stereo calib: done with RMS error=" << rms;
	DebugLog(outs.str());
	
}

	//512, 70 deg
	Mat cam1 = (Mat_<double>(3, 3) <<
	355.3383989449604, 0, 258.0008490063121,
	 0, 354.5068750418187, 255.7252273330564,
	 0, 0, 1);

	Mat dist1 = (Mat_<double>(5, 1) <<
	-0.02781875153957544,
	 0.05084431574408409,
	 0.0003262438299225566,
	 0.0005420218184546293,
	 -0.06711413339515834);

	Mat cam2 = (Mat_<double>(3, 3) <<
	354.8366825622115, 0, 255.7668702403205,
	 0, 353.9950515096826, 254.3218524455621,
	 0, 0, 1);

	Mat dist2 = (Mat_<double>(12, 1) <<
	-0.03429254591232522,
	 0.04304840389703278,
	 -0.0005799461588668822,
	 0.0005396568753307817,
	 -0.01867317550268149);

	Mat R = (Mat_<double>(3, 3) <<
	0.9999698145104303, 3.974878365893637e-06, 0.007769816740176146,
	 -3.390471048492443e-05, 0.9999925806915616, 0.003851936175643478,
	 -0.00776974378253147, -0.003852083336451321, 0.9999623955607145);

	Mat T = (Mat_<double>(3, 1) <<
	498.2890078004688,
	 0.3317087752736566,
	 -6.137837861924672);

//Создание алгоритмов получения карт смещений
DisparityMapCalculator::DisparityMapCalculator () {
	bm = StereoBM::create(16,9);
}

/** @brief Задание параметров калибровки и
	инициализация параметров ректификации изображений
*/
void DisparityMapCalculator::set (
	cv::Mat camera1Matrix,
	cv::Mat camera2Matrix,
	cv::Mat camera1distCoeff,
	cv::Mat camera2distCoeff,
	cv::Mat rotationMatrix,
	cv::Mat translationVector,
	cv::Size imageSize
) {
	this->camera1Matrix = camera1Matrix;
	this->camera2Matrix = camera2Matrix;
	this->camera1distCoeff = camera1distCoeff;
	this->camera2distCoeff = camera2distCoeff;
	this->rotationMatrix = rotationMatrix;
	this->translationVector = translationVector;
	
	//вычисляет параметры ректификации (выпрямления) изображений
	stereoRectify( camera1Matrix, camera1distCoeff, camera2Matrix, camera2distCoeff, imageSize, rotationMatrix, translationVector, R1, R2, P1, P2, Q, /*CALIB_ZERO_DISPARITY*/0, -1, imageSize, &roi1, &roi2 );
	
	//инициализация преобразований ректификации изображений
	//для левого и правого изображения
	initUndistortRectifyMap(camera1Matrix, camera1distCoeff, R1, P1, imageSize, CV_16SC2, map11, map12);
	initUndistortRectifyMap(camera2Matrix, camera2distCoeff, R2, P2, imageSize, CV_16SC2, map21, map22);

	bm->setROI1(roi1);
	bm->setROI2(roi2);
}

/** @brief Функция задания параметров алгоритма рассчета карты смещений
*/
void DisparityMapCalculator::setBMParameters (
	int preFilterSize,
	int preFilterCap,
	int blockSize,
	int minDisparity,
	int numDisparities,
	int textureThreshold,
	int uniquenessRatio,
	int speckleWindowSize,
	int speckleRange,
	int disp12maxDiff
) {
	bm->setPreFilterSize(preFilterSize);
	bm->setPreFilterCap(preFilterCap);
	bm->setBlockSize(blockSize);
	bm->setMinDisparity(minDisparity);
	bm->setNumDisparities(numDisparities);
	bm->setTextureThreshold(textureThreshold);
	bm->setUniquenessRatio(uniquenessRatio);
	bm->setSpeckleWindowSize(speckleWindowSize);
	bm->setSpeckleRange(speckleRange);
	bm->setDisp12MaxDiff(disp12maxDiff);
}

/** @brief Рассчет карты смещений
*/
void DisparityMapCalculator::compute (
	const cv::Mat& image1,
	const cv::Mat& image2,
	cv::Mat& image1recified,
	cv::Mat& image2recified,
	cv::Mat& disparityMap
) {

	// ректификация изображений
	remap(image1, image1recified, map11, map12, INTER_LINEAR);
	remap(image2, image2recified, map21, map22, INTER_LINEAR);
	
	// для алгоритма очень важно не перепутать левое изображение и правое.
	// при считывании изображений из OpenGL получается так, что они
	// оказываются зеркально отображены,
	// сказываются особенности хранения текстур в OpenGL и OpenCV
	// нам важно отразить изображения по горизонтали, ниаче алгоритм не сработает
	flip(image1recified, L, 1);
	flip(image2recified, R, 1);

	// stereo bm - мне понравился больше чем sgbm,
	// проще настроился и работает быстрее
	// StereoBM принимает на входе изображения в градациях серого
	cv::cvtColor(L, image1gray, CV_RGBA2GRAY, 1);
	cv::cvtColor(R, image2gray, CV_RGBA2GRAY, 1);
	
	int numberOfDisparities = bm->getNumDisparities();
	//вычисление карты смещений
	bm->compute(image1gray, image2gray, disp);

	//конвертируем карту смещений в изображение, которое можно отобразить
	disp.convertTo(disp8bit, CV_8U, 255/(numberOfDisparities*16.));
	//отражаем результат, чтобы увидеть его правильно ориентированным в Unity
	flip (disp8bit, disp, 1);

	//текстуры у нас хранятся в 4 канальном виде
	//поэтому нужно конвертировать наше однокальаное изображение
	//в 4 канала
	cv::cvtColor(disp, disparityMap, CV_GRAY2RGBA, 4);
}