#include <filesystem>
#include <string>
#include <fstream>
#include <iostream>
#include <opencv2\opencv.hpp>
#include <openMvs/Interface.h>

struct photo_data
{
	std::string url;
	unsigned int img_width;
	unsigned int img_heihgt;
	double focal_pix_i;
	double focal_pix_j;
	double principal_shift_i;
	double principal_shift_j;
	double distortion_k1;
	double distortion_k2;
	double distortion_p1;
	double distortion_p2;
	double geoc_ref_x;
	double geoc_ref_y;
	double geoc_ref_z;
	double geoc_rel_x;
	double geoc_rel_y;
	double geoc_rel_z;
	double yaw_rad;
	double pitch_rad;
	double roll_rad;
	double r_mat[9];
};

static bool UndistortImage(const std::string& input_img, const std::string& output_path, const MVS::Interface::Mat33d& K, const photo_data& telem, const std::string& mask_path = "")
{
	cv::Mat img_in = cv::imread(input_img);
	cv::Mat img_out;
	cv::Mat distCoeffs = (cv::Mat_<double>(1, 4) << telem.distortion_k1, -telem.distortion_k2, telem.distortion_p1, telem.distortion_p2);
	cv::undistort(img_in, img_out, K, distCoeffs);
	cv::imwrite(output_path, img_out);
	if (!mask_path.empty()) cv::imwrite(mask_path, img_out != 0);
	return true;
}

static bool ReadPointToMvsPlatform(std::istream& iss, MVS::Interface& scene, const std::string& mvs_dir, unsigned int nObj)
{
	scene.vertices.resize(scene.vertices.size() + 1);
	auto& v = *scene.vertices.rbegin();
	double i, j;
	uint32_t img_id, nPixels;
	iss >> v.X.x >> v.X.y >> v.X.z >> nPixels;
	v.views.resize(nPixels);
	for (int idx = 0; idx < nPixels; ++idx)
	{
		iss >> img_id >> i >> j;
		v.views[idx].imageID = img_id;
	}
	return true;
}

static bool ReadViewToMvsPlatform(std::istream& iss, MVS::Interface& tiltan2mvs, const std::string& mvs_dir, unsigned int nObj, bool do_undistort = true)
{
	auto& platform = *tiltan2mvs.platforms.begin();
	photo_data photo;
	iss >> photo.url >> photo.img_width >> photo.img_heihgt >> photo.focal_pix_i >> photo.focal_pix_j >> photo.principal_shift_i >> photo.principal_shift_j >> photo.geoc_ref_x >> photo.geoc_ref_y >> photo.geoc_ref_z >> photo.geoc_rel_x >> photo.geoc_rel_y >> photo.geoc_rel_z >> photo.yaw_rad >> photo.pitch_rad >> photo.roll_rad;
	iss >> photo.distortion_k1 >> photo.distortion_k2 >> photo.distortion_p1 >> photo.distortion_p2;
	for (int i = 0; i < 9; ++i) iss >> photo.r_mat[i];

	auto& data = photo;
	std::string img_path = data.url;
	MVS::Interface::Mat33d K;
	K = K.eye();
	K(0, 0) = data.focal_pix_i;
	K(1, 1) = data.focal_pix_j;
	K(0, 2) = 0.5*data.img_width + data.principal_shift_i;
	K(0, 2) = 0.5*data.img_heihgt + data.principal_shift_j;
	MVS::Interface::Mat33d	R;
	memcpy(R.val, data.r_mat, 9 * sizeof(double));
	MVS::Interface::Pos3d C(data.geoc_rel_x, data.geoc_rel_y, data.geoc_rel_z);
	uint32_t img_w = data.img_width, img_h = data.img_heihgt;

	std::string img_name = std::experimental::filesystem::path(img_path).filename().replace_extension("").string();
	std::string distroted_path = mvs_dir + "/" + img_name + ".tif";
	std::string mask_path = mvs_dir + "/" + img_name + "_mask.tif";
	if (do_undistort && !UndistortImage(img_path, distroted_path, K, data, mask_path))
	{
		std::cout << "Error undistorting image: " << img_path << std::endl;
		return false;
	}

	platform.cameras.push_back(MVS::Interface::Platform::Camera());
	auto& cam = *platform.cameras.rbegin();
	cam.K = K;
	cam.R = R;
	cam.C = C;
	cam.height = img_h;
	cam.width = img_w;
	cam.name = "";
	cam.bandName = "RGB";
	tiltan2mvs.images.push_back(MVS::Interface::Image());
	auto& img = *tiltan2mvs.images.rbegin();
	img.ID = nObj;
	img.cameraID = nObj;
	img.platformID = 0;
	img.name = distroted_path;

	img.maskName = mask_path;
}
int ConvertToMVS(const std::string& block_csv, const std::string& mvs_dir)
{
	std::ifstream is(block_csv);
	std::string line, word;
	std::getline(is, line);
	photo_data photo;
	unsigned int img_id;

	MVS::Interface tiltan2mvs;
	tiltan2mvs.platforms.resize(1);
	auto& platform = *tiltan2mvs.platforms.begin();
	platform.name = "";
	unsigned int nObj = 0;
	while (std::getline(is, line))
	{
		for (auto& c : line) if (c == ',') c = ' ';
		std::istringstream iss(line);
		if (!(iss >> img_id)) break;
		ReadViewToMvsPlatform(iss, tiltan2mvs, mvs_dir, nObj++, true);
	}
	nObj = 0;
	while (std::getline(is, line))
	{
		for (auto& c : line) if (c == ',') c = ' ';
		std::istringstream iss(line);
		if (!(iss >> img_id)) break;
		ReadPointToMvsPlatform(iss, tiltan2mvs, mvs_dir, nObj++);
	}
	MVS::ARCHIVE::SerializeSave(tiltan2mvs, mvs_dir + "/scene.mvs");

	return 0;
}