#include <iostream>
#include <opencv2/highgui.hpp>
#include <opencv2/core/ocl.hpp>
#include <unistd.h>
#include <vector>
#include <string> 
#include "argpopt.h"
#include "unwrap.h"
#include "bgremoval.h"
#include "normalize.h"
#include "log.h"
#include "charuco.h"
#include "harris.h"

void cd_to_exe_dir( char *argv[] )
{
	std::string path = argv[0];
	int ii = path.length();
	while ( !( path[ii] == '/' || path[ii] == '\\' ) && ii > 0 )
	{
		ii--;
	}
	path.erase( ii, 100 );
	chdir( path.c_str() );
}

std::vector<cv::Mat> loadImages(char** fileNames)
{ 
	std::vector<cv::Mat> images;
	for(size_t i = 0; fileNames[i]; ++i)
	{
		cv::Mat tmpImage = cv::imread(fileNames[i]);
		if(tmpImage.data)images.push_back(tmpImage);
		else std::cout<<"can not read image "<<i<<" from "<<fileNames[i]<<'\n';
	}
	return images;
}

int main(int argc, char* argv[])
{
	cv::ocl::setUseOpenCL(false);
	
	cd_to_exe_dir(argv);
	
	Config config;
    argp_parse(&argp, argc, argv, 0, 0, &config);
	
	Log::level = config.quiet ? Log::WARN : config.verbose ? Log::DEBUG : Log::INFO;
	
	Log(Log::INFO)<<"UVOS Optical lubricant thikness mapper "<<argp_program_version;
	
	if(!config.charuco.empty())
	{
		createCharucoBoard(config.size, config.charuco);
		Log(Log::INFO)<<"exporting charuco map";
		return 0;
	}
	
	std::vector<cv::Mat> inImages = loadImages(config.inFileNames);
	
	if(inImages.empty()) 
	{
		Log(Log::ERROR)<<"Input images must be provided!";
		return -1;
	}
	
	if(config.verbose) 
	{
		cv::namedWindow( "Viewer", cv::WINDOW_NORMAL );
		cv::resizeWindow("Viewer", 960, 500);
	}
	
	if(config.maps.empty())
	{
		cv::Mat mask;
		if(config.verbose)
		{
			cv::imshow( "Viewer", inImages[0] );
			cv::waitKey(0);
		}
		if(!config.bg.empty())
		{
			cv::Mat bg = cv::imread(config.bg);
		
			if(bg.data)
			{
				createMask(inImages[0], mask, bg);
				if(config.verbose)
				{
					cv::Mat masked;
					inImages[0].copyTo(masked, mask);
					cv::imshow( "Viewer", masked );
					cv::waitKey(0);
				}
			}
			else Log(Log::WARN)<<"can not read background image from "<<config.bg;
		}
		std::vector<cv::Point2f > points;
		std::vector<cv::Point2i> coordiantes;
		
		if(config.harris)
		{
			points = harrisDetectPoints(inImages[0], mask);
		}
		else
		{
			points = detectCharucoPoints(inImages[0], &coordiantes);
		}
		
		Log(Log::INFO)<<"Found "<<points.size()<<" points";
		if(points.size() < 8) 
		{
			Log(Log::ERROR)<<"Error creating map, insufficant points detected";
			return -1;
		}
		
		createRemapMap(inImages[0], points, coordiantes, config.inFileNames[0], config.verbose);
	}
	else 
	{
		cv::FileStorage fs(config.maps, cv::FileStorage::READ);
		if (!fs.isOpened())
		{
			Log(Log::ERROR)<<"could not open maps file " <<config.maps;
			return -1;
		}
		cv::Mat xMat;
		cv::Mat yMat;
		fs["xmat"]>>xMat;
		fs["ymat"]>>yMat;
		
		cv::Mat norm;
		if(!config.norm.empty())
		{
			cv::Mat tmp = cv::imread(config.norm);
			if(!tmp.data)
			{
				Log(Log::WARN)<<"could not open normalize file " <<config.norm;
			}
			applyRemap(tmp, norm, xMat, yMat, config.size);
			if(config.verbose)
			{
				cv::imshow("Viewer", norm );
				cv::waitKey(0);
			}
		}
		
		for(auto& image : inImages)
		{
			if(config.verbose)
			{
				cv::imshow( "Viewer", image );
				cv::waitKey(0);
			}
			cv::Mat remaped;
			applyRemap(image, remaped, xMat, yMat, config.size);
			if(norm.data) normalize(remaped, norm);
			cv::imshow( "Viewer", remaped );
			cv::waitKey(0);
		}
	}
	
	if(config.verbose)
	{
		cv::destroyWindow("Viewer");  
		cv::waitKey(0);
	}
	
	return 0;
}