#include <filesystem>
#include <fstream>
#include <nlohmann/json.hpp>
#include "utility.h"

using namespace cv;
using namespace std;

using json = nlohmann::json;

// Constructors
utility::Frame::Frame() : Mat() {}
utility::Frame::Frame(const Mat& m) : Mat(m) {}
utility::Frame::Frame(const Frame& f) : Mat(f) {}
// Operators
utility::Frame& utility::Frame::operator=(const Mat& m) {
	Mat::operator=(m);
	return *this;
}
utility::Frame& utility::Frame::operator=(const Frame& f) {
	Mat::operator=(f);
	return *this;
}
// Methods
utility::Frame utility::Frame::clone() const {
	return utility::Frame(Mat::clone());
}
utility::Frame& utility::Frame::downsample(int factor) {
	pyrDown(*this, *this, Size(size().width / factor, size().height / factor));
	return *this;
}
utility::Frame& utility::Frame::convertColor(int code) {
	cvtColor(*this, *this, code);
	return *this;
}
utility::Frame utility::Frame::difference(Frame& f) {
	return Frame(utility::difference(*this, f));
}
utility::Frame& utility::Frame::crop(Size rect_size, Point2f center) {
	cv::getRectSubPix(*this, rect_size, center, *this);
	return *this;
}
utility::Frame& utility::Frame::warp(cv::Mat rotationMatrix) {
	cv::warpAffine(*this, *this, rotationMatrix, this->size(), INTER_CUBIC);
	return *this;
}
pair<utility::Frame, utility::Frame> utility::Frame::deinterlace() const {
	Frame odd_frame(cv::Mat(this->rows/2, this->cols, CV_8UC3));
	Frame even_frame(cv::Mat(this->rows/2, this->cols, CV_8UC3));

	utility::separateFrame(*this, odd_frame, even_frame);

	return make_pair(odd_frame, even_frame);
}

void utility::detectShape(Ptr<GeneralizedHoughGuil> alg, Mat templateShape, int posThresh, vector<Vec4f> &positivePositions, Mat &positiveVotes, vector<Vec4f> &negativePositions, Mat &negativeVotes, Mat processingArea) {

	alg -> setPosThresh(posThresh);
	alg -> setTemplate(templateShape);

	int oldSizePositive = 0;
	int i = 0;
	int maxVote = 0;

	// Process shapes with positive angles
	alg -> setMinAngle(0);
	alg -> setMaxAngle(3);
	while (true) {
		alg -> detect(processingArea, positivePositions, positiveVotes);
		int currentSize = positivePositions.size();
		if (currentSize == 1) {
			// We detected the most interesting shape
			break;
		} else if (currentSize == 0 && oldSizePositive > 0) {
			// It is not possible to detect only one shape with the current parameters
			alg -> setPosThresh(posThresh+i-1); // Decrease position value
			alg -> detect(processingArea, positivePositions, positiveVotes); // Detect all available shapes
			break;
		} else if (currentSize == 0 && oldSizePositive == 0) {
			// Impossible to find with these parameters
			break;
		}
		oldSizePositive = currentSize;
		// Find maximum vote
		for (int j = 0; j < positiveVotes.cols / 3; j++) {
			if (positiveVotes.at<int>(3*j) > maxVote)
				maxVote = positiveVotes.at<int>(3*j);
		}

		if (currentSize > 10) {
			i += 5; // To speed up computation when there are too many matches
		} else if (maxVote - (posThresh + i) > 100) {
			i += 100; // To speed up computation when there are few super high matches
		} else {
			i++;
		}
		alg -> setPosThresh(posThresh+i);
	}

	int oldSizeNegative = 0;
	// Reset incremental position value
	i = 0;
	maxVote = 0;
	// Process shapes with negative angles
	alg -> setMinAngle(357);
	alg -> setMaxAngle(360);
	while (true) {
		alg -> detect(processingArea, negativePositions, negativeVotes);
		int currentSize = negativePositions.size();
		if (currentSize == 1) {
			// We detected the most interesting shape
			break;
		} else if (currentSize == 0 && oldSizeNegative > 0) {
			// It is not possible to detect only one shape with the current parameters
			alg -> setPosThresh(posThresh+i-1); // Decrease position value
			alg -> detect(processingArea, negativePositions, negativeVotes); // Detect all available shapes
			break;
		} else if (currentSize == 0 && oldSizeNegative == 0) {
			// Impossible to found with these parameters
			break;
		}
		oldSizeNegative = currentSize;

		// Find maximum vote
		for (int j = 0; j < positiveVotes.cols / 3; j++) {
			if (positiveVotes.at<int>(3*j) > maxVote)
				maxVote = positiveVotes.at<int>(3*j);
		}

		if (currentSize > 10) {
			i += 5; // To speed up computation when there are too many matches
		} else if (maxVote - (posThresh + i) > 100) {
			i += 100; // To speed up computation when there are few super high matches
		} else {
			i++;
		}
		alg -> setPosThresh(posThresh+i);
	}
}

RotatedRect utility::drawShapes(Mat frame, Vec4f &positions, Scalar color, int width, int height, int offsetX, int offsetY, float processingScale) {
    RotatedRect rr;
    Point2f rrpts[4];

    Point2f pos(positions[0]+offsetX, positions[1]+offsetY);
    float scale = positions[2];
    float angle = positions[3];

    rr.center = pos * processingScale;
    rr.size = Size2f(width * scale * processingScale, height * scale * processingScale);
    rr.angle = angle;

    rr.points(rrpts);

    line(frame, rrpts[0], rrpts[1], color, 2);
    line(frame, rrpts[1], rrpts[2], color, 2);
    line(frame, rrpts[2], rrpts[3], color, 2);
    line(frame, rrpts[3], rrpts[0], color, 2);

    return rr;
}

void utility::separateFrame(const cv::Mat frame, cv::Mat &odd_frame, cv::Mat &even_frame) {

    int i_odd_frame = 0;
    int i_even_frame = 0;

    for (int i = 0; i < frame.rows; i++) {
        for (int j = 0; j < frame.cols; j++) {
            if (i % 2 == 0) {
                even_frame.at<cv::Vec3b>( i_even_frame, j )[0] = frame.at<cv::Vec3b>(i, j)[0];
                even_frame.at<cv::Vec3b>( i_even_frame, j )[1] = frame.at<cv::Vec3b>(i, j)[1];
                even_frame.at<cv::Vec3b>( i_even_frame, j )[2] = frame.at<cv::Vec3b>(i, j)[2];
            } else {
                odd_frame.at<cv::Vec3b>( i_odd_frame, j )[0] = frame.at<cv::Vec3b>(i, j)[0];
                odd_frame.at<cv::Vec3b>( i_odd_frame, j )[1] = frame.at<cv::Vec3b>(i, j)[1];
                odd_frame.at<cv::Vec3b>( i_odd_frame, j )[2] = frame.at<cv::Vec3b>(i, j)[2];
            }
        }

        if (i % 2 == 0) {
            i_even_frame++;
        } else {
            i_odd_frame++;
        }
    }

    return;

}

cv::Mat utility::difference(cv::Mat &prevFrame, cv::Mat &currentFrame) {
    cv::Mat diff = currentFrame.clone();
    for (int i = 0; i < currentFrame.rows; i++) {
        for (int j = 0; j < currentFrame.cols; j++) {
            if (prevFrame.at<cv::Vec3b>(i, j)[0] != currentFrame.at<cv::Vec3b>(i, j)[0] || prevFrame.at<cv::Vec3b>(i, j)[1] != currentFrame.at<cv::Vec3b>(i, j)[1] || prevFrame.at<cv::Vec3b>(i, j)[2] != currentFrame.at<cv::Vec3b>(i, j)[2]) {
                // Different pixels
                diff.at<cv::Vec3b>(i, j)[0] = 0;
            } else {
                // Identical pixels
                diff.at<cv::Vec3b>(i, j)[0] = 255;
            }
        }
    }
    return diff;
}

SceneObject::SceneObject(int minDist, Threshold threshold) {
	this->minDist = minDist;
	this->threshold = threshold;
}

SceneObject SceneObject::from_file(fs::path path, Object obj) {
	ifstream iConfig(path);
	json j;
	iConfig >> j;

	if (obj == Object::TAPE) {
		return SceneObject(
			j["MinDist"],
			Threshold {
				j["TapeThresholdPercentual"],
				j["AngleThresh"],
				j["ScaleThresh"],
				j["PosThresh"]
			}
		);
	} else {
		return SceneObject(
			j["MinDistCapstan"],
			Threshold {
				j["CapstanThresholdPercentual"],
				j["AngleThreshCapstan"],
				j["ScaleThreshCapstan"],
				j["PosThreshCapstan"]
			}
		);
	}
}