#include "imagepipeline.h"
#include <uvosunwrap/unwrap.h>
#include <uvosunwrap/normalize.h>
#include <uvosunwrap/curve.h>
#include <QtConcurrent/QtConcurrentRun>
#include <QFuture>
#include <QDebug>
#include <algorithm>
#include <opencv2/highgui.hpp>

ImagePipeline::ImagePipeline(Cameras* cameras, bool simpleStichingAlg, QObject *parent):
	QObject(parent), cameras_(cameras), simpleStichingAlg_(simpleStichingAlg)
{
	connect(cameras_, &Cameras::newImages, this, &ImagePipeline::apply);
}

void ImagePipeline::applyDarkMap(cv::Mat& image, const cv::Mat& darkmap)
{
	cv::Mat localDarkMap;
	if(image.size() != darkmap.size())
	{
		qWarning()<<"image and darkmap not of the same size"<<image.size().height<<'x'<<image.size().width<<darkmap.size().height<<'x'<<darkmap.size().width;
		return;
	}
	else if(image.channels() != darkmap.channels())
	{
		qWarning()<<"image and darkmap do not have the same number of channels";
		if(image.channels() == 1 && darkmap.channels() == 3)
			cv::cvtColor(darkmap, localDarkMap,  cv::COLOR_BGR2GRAY);
		else if(image.channels() == 3 && darkmap.channels() == 1)
			cv::cvtColor(darkmap, localDarkMap,  cv::COLOR_GRAY2BGR);

		if(!localDarkMap.data || image.channels() != localDarkMap.channels())
			return;
	}
	else
	{
		localDarkMap = darkmap;
	}
	cv::Mat subtracted;
	image.copyTo(subtracted);
	subtracted = image - localDarkMap;
	image = subtracted;
}

cv::Mat ImagePipeline::process(const Profile profile, std::vector<Camera::Image> images, bool simpleStichingAlg)
{
	qDebug()<<__func__<<"got"<<images.size()<<"images";
	std::vector<RemapedImage> remapedImages;
	remapedImages.reserve(images.size());

	try
	{
		if(profile.nodistort && images.size() > 0)
		{
			for(auto& image : images)
			{
				if(profile.cameras[0].id == image.cameraId)
				{
					RemapedImage img;
					img.origin.x = 0;
					img.origin.y = 0;
					image.mat.copyTo(img.image);
					if(profile.cameras[0].darkmap.data)
						applyDarkMap(img.image, profile.cameras[0].darkmap);
					remapedImages.push_back(img);
					break;
				}
			}
		}
		else
		{
			for(Camera::Image& image : images)
			{
				qDebug()<<__func__<<"image cam id"<<image.cameraId;
				bool matched = false;
				for(auto& camera : profile.cameras)
				{
					if(camera.id == image.cameraId)
					{
						if(!camera.remapMap.xMat.data || !camera.remapMap.yMat.data)
							return cv::Mat();
						if(camera.darkmap.data)
							applyDarkMap(image.mat, camera.darkmap);
						RemapedImage remaped = applyRemap(image.mat, camera.remapMap);
						qDebug()<<"Camera"<<camera.id<<"image remaped to"<<remaped.image.data<<remaped.image.rows<<remaped.image.cols
								<<"at"<<remaped.origin.x<<'x'<<remaped.origin.y;
						remapedImages.push_back(applyRemap(image.mat, camera.remapMap));
						matched = true;
						break;
					}
				}
				if(!matched)
					qWarning()<<"non matching image recived";
			}
		}
	}
	catch(cv::Exception& ex)
	{
		qDebug()<<ex.err.c_str();
		qDebug()<<"Image pipe failure";
		return cv::Mat();
	}

	if(remapedImages.size() > 0)
	{
		std::sort(remapedImages.begin(), remapedImages.end(),  [](const RemapedImage& imgA, const RemapedImage& imgB) -> bool {return imgA.origin.x < imgB.origin.x;});
		cv::Mat output;
		if(simpleStichingAlg)
			output = simpleStich(remapedImages);
		else
			output = stich(remapedImages, true);

		if(output.depth() != CV_8U)
			output.convertTo(output, CV_8U);
		if(output.channels() == 3)
			cv::cvtColor(output, output, cv::COLOR_BGR2GRAY);

		output.convertTo(output, CV_32FC1, 1.0/255.0, 0);

		if(profile.lightmap.data)
		{
			qDebug()<<"output"<<output.type()<<output.cols<<output.rows;
			qDebug()<<"profile.lightmap"<<profile.lightmap.type()<<profile.lightmap.cols<<profile.lightmap.rows;
			try
			{
				output = output.mul(profile.lightmap);
			}
			catch(cv::Exception& ex)
			{
				qDebug()<<ex.err.c_str();
				qDebug()<<"Image pipe failure";
				return cv::Mat();
			}
		}

		if(profile.calcurve.data)
			applyCurve(output, profile.calcurve);
		return output;
	}
	else
	{
		qDebug()<<"Image pipe failure insufficant matched images";
		return cv::Mat();
	}
}

void ImagePipeline::apply(std::vector<Camera::Image> images)
{
	if(!invalid_)
	{
		futureImageWatchers_.push_back(new QFutureWatcher<cv::Mat>());
		connect(futureImageWatchers_.back(), &QFutureWatcher<cv::Mat>::finished, this, &ImagePipeline::imageFinished);

		statusMsg("Processing");
		QFuture<cv::Mat> futureImage = QtConcurrent::run(&ImagePipeline::process, profile_, images, simpleStichingAlg_);
		futureImageWatchers_.back()->setFuture(futureImage);
	}
}

void ImagePipeline::setProfile(const Profile& profile)
{
	profile_ = profile;
	cameras_->setExposureTime(profile_.exposureTime);
	cameras_->setLighting(profile_.lighting);
	cameras_->setSetup(profile_.cameras);
	qDebug()<<"setting profile "<<profile_.getName();
	for(CameraSetup& setup : profile_.cameras)
	{
		qDebug()<<setup.id;
		//TODO: dehardcode this
		setup.remapMap.outputCellSize=200;
	}
	invalid_ = false;
	if(!profile_.camerasSufficant(cameras_->getCameras()))
	{
		invalid_ = true;
		sigInvalidProfile("A camera required for this profile is not available");
	}
	statusMsg("set profile " + profile_.getName());
}

void ImagePipeline::imageFinished()
{
	for(size_t i = 0; i < futureImageWatchers_.size(); ++i)
	{
		if(futureImageWatchers_[i]->isFinished())
		{
			cv::Mat result = futureImageWatchers_[i]->result();
			if(result.data)
			{
				sigResult(Camera::Image(result, 0));
				delete futureImageWatchers_[i];
				futureImageWatchers_.erase(futureImageWatchers_.begin()+i);
				i--;
				statusMsg("Finished");
			}
			else
			{
				sigInvalidProfile("Image pipe failure");
			}
		}
	}
}