#include "contours_extractor_impl.hpp"
#include <algorithm>
#include <cstdlib>
#include "opencv2/opencv.hpp"
#include "opencv2/imgproc/imgproc_c.h"
#include "blob/blob.h"
#include "blob/BlobDetector.h"
#include "blob/BlobGroup.h"
#include "blob/BlobContour.h"
#include "opencv2/imgproc.hpp"
#include "bspline.hpp"

#define MIN_VALUE 1e-8

#define IS_DOUBLE_ZERO(d)  (abs(d) < MIN_VALUE)

namespace cvpr
{
    ContoursExtractorImpl::ContoursExtractorImpl()
    {

    }

    void ContoursExtractorImpl::init()
    {

    }

    void ContoursExtractorImpl::destroy()
    {

    }

	void ContoursExtractorImpl::setAreaThreshold(double threshold)
	{
		m_areaThreshold = threshold;
	}

	double ContoursExtractorImpl::areaThreshold()
	{
		return m_areaThreshold;
	}

    void ContoursExtractorImpl::setContourOffset(int32_t offset)
    {
        m_contourOffset = offset;
    }

    int32_t ContoursExtractorImpl::contourOffset()
    {
        return m_contourOffset;
    }

    void ContoursExtractorImpl::setApproxPolyEpsilon(double epsilon)
    {
        m_approxPolyEpsilon = epsilon;
    }

    double ContoursExtractorImpl::approxPolyEpsilon()
    {
        return m_approxPolyEpsilon;
    }

    void ContoursExtractorImpl::setSmoothMethod(SMOOTH_METHOD method)
    {
        m_smoothMethod = method;
    }

    SMOOTH_METHOD ContoursExtractorImpl::smoothMethod()
    {
        return m_smoothMethod;
    }

	void ContoursExtractorImpl::setAngelThreshold(double threshold)
	{
		m_angelThreshold = threshold;
	}

	double ContoursExtractorImpl::angelThreshold()
	{
		return m_angelThreshold;
	}

	void ContoursExtractorImpl::setDefectThreshold(double threshold)
	{
		m_defectThreshold = threshold;
	}

	double ContoursExtractorImpl::defectThreshold()
	{
		return m_defectThreshold;
	}

    void ContoursExtractorImpl::setSmoothStep(double step)
    {
        m_smoothStep = step;
    }

    double ContoursExtractorImpl::smoothStep()
    {
        return m_smoothStep;
    }

    void ContoursExtractorImpl::extract(const cv::Mat& alpha, std::vector<std::vector<cv::Point>>& outputContours)
    {
		assert(!alpha.empty());
		if (alpha.empty()) {
			return;
		}

		cv::Mat edges;
		cv::threshold(alpha, edges, 20, 255, cv::THRESH_BINARY);

		cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3));
		cv::erode(edges, edges, kernel);

		if (m_contourOffset == 0) {
			cv::findContours(edges, outputContours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
			return;
		}

#if DEBUG_CONTOURS_EXTRACTOR
		cv::imshow("edges", edges);
		cv::waitKey(0);
#endif

		BlobDetector detector(edges);

		BlobGroup group = detector.getBlobGroup();
		group.filter(group, FilterAction::FLT_EXCLUDE, BlobGetArea(), FilterCondition::FLT_LESS, m_areaThreshold);

		cv::Point center(edges.cols / 2, edges.rows / 2);
		Blob* centerBlob = group.getBlobNearestTo(center);

		cv::Point pt = centerBlob->getCentroid(true);
		for (int i = 0; i < group.getNumBlobs(); i++) {
			auto* blob = group.getBlob(i);
			if (blob != centerBlob) {
				auto center = blob->getCenter();
				cv::line(edges, center, pt, cv::Scalar(255, 255, 255), m_contourOffset, 1);
			}
		}

		std::vector<std::vector<cv::Point>> contours;
		cv::findContours(edges, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);

		cv::Mat tmp = cv::Mat::zeros(edges.rows, edges.cols, CV_8UC3);

#if DEBUG_CONTOURS_EXTRACTOR
		cv::drawContours(tmp, contours, 0, cv::Scalar(0, 0, 255), 1);
#endif
		for (int i = 0; i < contours.size(); ++i) {
			for (int j = 0; j < contours[i].size(); ++j) {
				cv::circle(tmp, contours[i][j], m_contourOffset, cv::Scalar(255, 255, 255), -1);
			}
		}

#if DEBUG_CONTOURS_EXTRACTOR
		cv::imshow("tmp", tmp);
		cv::waitKey(0);
#endif
		cv::Mat bin;
		cv::cvtColor(tmp, bin, CV_BGR2GRAY);

		std::vector<std::vector<cv::Point>> output;
		cv::findContours(bin, output, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);

#if DEBUG_CONTOURS_EXTRACTOR
		cv::drawContours(tmp, output, 0, cv::Scalar(0, 0, 255), 1);
		cv::imshow("tmp", tmp);
		cv::waitKey(0);
#endif
		if (m_smoothMethod == SMOOTH_METHOD::BSPLINE) {
			std::vector<cv::Point> approxCurve;
			cv::approxPolyDP(output[0], approxCurve, m_approxPolyEpsilon, false);

			//output[0].emplace_back(output[0][0]);

#if DEBUG_CONTOURS_EXTRACTOR		
			cv::Mat color = cv::imread("C:/Users/ouxia/source/repos/Contour/data/origin-03.png", cv::ImreadModes::IMREAD_UNCHANGED);

			std::vector<std::vector<cv::Point >> cs;
			cs.emplace_back(approxCurve);
			cv::drawContours(color, cs, 0, cv::Scalar(255, 255, 255), 1);

			std::vector<cv::Point> hullPoints;
			cv::convexHull(approxCurve, hullPoints, true, true);

			std::vector<std::vector<cv::Point >> hullContours;
			hullContours.emplace_back(hullPoints);

			cv::drawContours(color, hullContours, 0, cv::Scalar(0, 255, 0), 1);
#endif
			//std::vector<std::vector<int>> indices;
			//getSubCurves(approxCurve, 150.0, indices);

			//for (const auto& indice : indices) {
			//	if (indice.size() < 3) {
			//		continue;
			//	}
			//	std::vector<std::vector<double>> psArr;
			//	for (int32_t idx = 0; idx < indice.size(); ++idx) {
			//		std::vector<double> pt;
			//		pt.emplace_back(approxCurve[indice[idx]].x);
			//		pt.emplace_back(approxCurve[indice[idx]].y);
			//		psArr.emplace_back(pt);
			//	}

			//	BSpline spline(psArr, 2);

			//	for (double t = 0.0; t < 1.0; t += 0.05) {
			//		std::vector<double> point = spline.eval(t);
			//		if (point.size() > 1) {
			//			cv::Point pt;
			//			pt.x = point[0];
			//			pt.y = point[1];

			//			cv::circle(color, pt, 1, cv::Scalar(0, 0, 255), -1);
			//		}
			//	}
			//}

			auto curve = approxCurve;

			if (!IS_DOUBLE_ZERO(m_defectThreshold)) {
				std::vector<int32_t> hull;
				cv::convexHull(approxCurve, hull, true, false);

				std::vector<cv::Vec4i> defects;
				cv::convexityDefects(approxCurve, hull, defects);

				for (const auto& defect : defects) {
					cv::Point startPoint = approxCurve[defect[0]];

					cv::Point endPoint = approxCurve[defect[1]];

#if DEBUG_CONTOURS_EXTRACTOR
					cv::Point farthestPoint = cs[0][defect[2]];
					float depth = defect[3] / 256.0;

					cv::circle(color, startPoint, 5, cv::Scalar(255, 0, 0), -1);
					cv::circle(color, endPoint, 5, cv::Scalar(255, 127, 0), -1);
					cv::circle(color, farthestPoint, 5, cv::Scalar(0, 255, 255), -1);

					std::string depthText = "Depth: " + std::to_string(depth);
					cv::putText(color, depthText, farthestPoint + cv::Point(0, -10), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255, 255, 255));

					cv::imshow("Convexity Defects", color);
#endif
					int start = -1;
					int end = -1;

					for (int i = 0; i < curve.size(); ++i) {
						if (startPoint == curve[i]) {
							start = i;
						}
						if (endPoint == curve[i]) {
							end = i;
						}
					}

					if (start != -1 && end != -1 && start != end) {
						if (euclideanDistance(startPoint, endPoint) < m_defectThreshold) {
							cv::Point connected = (startPoint + endPoint) / 2;
							std::vector<cv::Point> sub{ connected };
							if (start > end) {
								std::swap(start, end);
							}
							replaceSubsequence(curve, start, end, sub);
						}
					}
				}
			}

			curve.emplace_back(curve[0]);

			std::vector<std::vector<double>> psArr;
			for (int i = 0; i < curve.size(); ++i) {
				std::vector<double> pt;
				pt.emplace_back(curve[i].x);
				pt.emplace_back(curve[i].y);
				psArr.emplace_back(pt);
			}

			BSpline spline(psArr, 2);

			if (spline.isValid()) {
				std::vector<double> weights;
				std::vector<cv::Point> points;
				for (double t = 0.0; t < 1.0; t += m_smoothStep) {
					std::vector<double> point = spline.eval(t);
					if (point.size() > 1) {
						cv::Point pt;
						pt.x = point[0];
						pt.y = point[1];
						points.emplace_back(pt);
					}
				}

				outputContours.emplace_back(points);
			}
			else {
				outputContours = output;
			}
		}
		else {
			outputContours = output;
		}
#if DEBUG_CONTOURS_EXTRACTOR
		cv::drawContours(alpha, outputContours, 0, cv::Scalar(255, 255, 255), 1);
		cv::imshow("alpha", alpha);
		cv::waitKey(0);
#endif
	}

	double ContoursExtractorImpl::calculateAngle(const cv::Point& pt1, const cv::Point& pt2, const cv::Point& pt3) 
	{
		cv::Point vec1 = pt1 - pt2; 
		cv::Point vec2 = pt3 - pt2; 

		double dotProduct = vec1.x * vec2.x + vec1.y * vec2.y; 
		double magnitude1 = std::sqrt(vec1.x * vec1.x + vec1.y * vec1.y); 
		double magnitude2 = std::sqrt(vec2.x * vec2.x + vec2.y * vec2.y); 

		double cosTheta = dotProduct / (magnitude1 * magnitude2);
		cosTheta = std::clamp<double>(cosTheta, -1.0, 1.0); 

		return std::acos(cosTheta) * 180.0 / CV_PI;
	}


	double ContoursExtractorImpl::euclideanDistance(const cv::Point& p1, const cv::Point& p2)
	{
		return cv::norm(p1 - p2);
	}

	double ContoursExtractorImpl::curveDistance(const std::vector<cv::Point>& contour, int startIdx, int endIdx)
	{
		double curveDistance = 0.0;
		int n = contour.size();

		for (int i = startIdx; i != endIdx; i = (i + 1) % n) {
			curveDistance += euclideanDistance(contour[i], contour[(i + 1) % n]);
		}

		return curveDistance;
	}

	void ContoursExtractorImpl::getSubCurves(const std::vector<cv::Point>& contour, double thresholdAngle, std::vector<std::vector<int>>& indices)
	{
		std::set<int> currentGroup; 

		for (size_t i = 0; i < contour.size(); ++i) {
			const cv::Point& pt1 = contour[(i + contour.size() - 1) % contour.size()];
			const cv::Point& pt2 = contour[i];
			const cv::Point& pt3 = contour[(i + 1) % contour.size()];

			double angle = calculateAngle(pt1, pt2, pt3);

			if (angle < thresholdAngle) {
				currentGroup.insert((i + contour.size() - 1) % contour.size());
				currentGroup.insert(i); 
				currentGroup.insert((i + 1) % contour.size());
			}
			else if (!currentGroup.empty()) {
				std::vector<int> vec;
				for (auto it = currentGroup.begin(); it != currentGroup.end(); ++it) {
					vec.emplace_back(*it);
				}
				indices.push_back(vec);
				currentGroup.clear();
			}
		}

		if (!currentGroup.empty()) {
			std::vector<int> vec;
			for (auto it = currentGroup.begin(); it != currentGroup.end(); ++it) {
				vec.emplace_back(*it);
			}
			indices.push_back(vec);
		}
	}

	void ContoursExtractorImpl::getOptimalPoints(const std::vector<cv::Point>& contour, int& bestIdx1, int& bestIdx2, double& bestCurveDist, double& bestLineDist)
	{
		bestCurveDist = 0.0;                          
		bestLineDist = std::numeric_limits<double>::max(); 
		int n = contour.size();

		for (int i = 0; i < n; ++i) {
			for (int j = i + 1; j < n; ++j) {
				double curveDist = curveDistance(contour, i, j);

				double lineDist = euclideanDistance(contour[i], contour[j]);

				if (curveDist > bestCurveDist || (curveDist == bestCurveDist && lineDist < bestLineDist)) {
					bestCurveDist = curveDist;
					bestLineDist = lineDist;
					bestIdx1 = i;
					bestIdx2 = j;
				}
			}
		}
	}

	void ContoursExtractorImpl::replaceSubsequence(std::vector<cv::Point>& contour, int startIdx, int endIdx, const std::vector<cv::Point>& newSubsequence)
	{
		int n = contour.size();

		if (startIdx < 0 || endIdx < 0 || startIdx >= n || endIdx >= n) {
			return;
		}

		if (startIdx > endIdx) {
			return;
		}

		contour.erase(contour.begin() + startIdx, contour.begin() + endIdx + 1);

		contour.insert(contour.begin() + startIdx, newSubsequence.begin(), newSubsequence.end());
	}

	cv::Point ContoursExtractorImpl::getPointOnLine(const cv::Point& pt1, const cv::Point& pt2, int32_t x)
	{
		double dx = pt2.x - pt1.x;
		double dy = pt2.y - pt1.y;

		cv::Point pt;
		pt.x = pt1.x + x * dx;
		pt.y = pt1.y + x * dy;

		return pt;
	}
}