/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          https://www.mrpt.org/                            |
   |                                                                           |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
   | See: https://www.mrpt.org/Authors - All rights reserved.                  |
   | Released under BSD License. See details in https://www.mrpt.org/License   |
   +---------------------------------------------------------------------------+
 */

#include "vision-precomp.h"	 // Precompiled headers
//
#include <mrpt/core/exceptions.h>
#include <mrpt/vision/CFeatureLines.h>
#include <mrpt/vision/utils.h>

// Universal include for all versions of OpenCV
#include <mrpt/3rdparty/do_opencv_includes.h>

using namespace mrpt::vision;
using namespace std;

#if MRPT_HAS_OPENCV

static void extractLines_CannyHough(
	const cv::Mat& canny_image, const std::vector<cv::Vec2f> lines,
	std::vector<cv::Vec4i>& segments, size_t threshold)
{
	// Some variables to change the coordinate system from polar to cartesian
	double rho, theta;
	double cosTheta, sinTheta;
	double m, c, cMax;

	// Variables to define the two extreme points on the line, clipped on the
	// window The constraint is x <= xF
	int x, y, xF, yF;

	segments.clear();

	// For each line
	for (size_t n(0); n < lines.size(); ++n)
	{
		// OpenCV implements the Hough accumulator with theta from 0 to PI, thus
		// rho could be negative We want to always have rho >= 0
		if (lines[n][0] < 0)
		{
			rho = -lines[n][0];
			theta = lines[n][1] - CV_PI;
		}
		else
		{
			rho = lines[n][0];
			theta = lines[n][1];
		}

		if (rho == 0 && theta == 0)
		{  // That case appeared at least once, so a test is needed
			continue;
		}

		// Since the step for theta in cv::HoughLines should not be too small,
		// theta should exactly be 0 (and the line vertical) when this condition
		// is true
		if (fabs(theta) < 0.00001)
		{
			x = xF = lround(rho);
			y = 0;
			yF = canny_image.rows - 1;
		}
		else
		{
			// Get the (m, c) slope and y-intercept from (rho, theta), so that y
			// = mx + c <=> (x, y) is on the line
			cosTheta = cos(theta);
			sinTheta = sin(theta);
			m = -cosTheta / sinTheta;  // We are certain that sinTheta != 0
			c = rho * (sinTheta - m * cosTheta);

			// Get the two extreme points (x, y) and (xF, xF) for the line
			// inside the window, using (m, c)
			if (c >= 0)
			{
				if (c < canny_image.rows)
				{
					// (x, y) is at the left of the window
					x = 0;
					y = static_cast<int>(c);
				}
				else
				{
					// (x, y) is at the bottom of the window
					y = canny_image.rows - 1;
					x = static_cast<int>((y - c) / m);
				}
			}
			else
			{
				// (x, y) is at the top of the window
				x = static_cast<int>(-c / m);
				y = 0;
			}
			// Define the intersection with the right side of the window
			cMax = m * (canny_image.cols - 1) + c;
			if (cMax >= 0)
			{
				if (cMax < canny_image.rows)
				{
					// (xF, yF) is at the right of the window
					xF = canny_image.cols - 1;
					yF = static_cast<int>(cMax);
				}
				else
				{
					// (xF, yF) is at the bottom of the window
					yF = canny_image.rows - 1;
					xF = static_cast<int>((yF - c) / m);
				}
			}
			else
			{
				// (xF, yF) is at the top of the window
				xF = static_cast<int>(-c / m);
				yF = 0;
			}
		}

		// Going through the line using the Bresenham algorithm
		// dx1, dx2, dy1 and dy2 are increments that allow to be successful for
		// each of the 8 octants (possible directions while iterating)
		bool onSegment = false;
		int memory;
		int memoryX = 0, memoryY = 0;
		int xPrev = 0, yPrev = 0;
		size_t nbPixels = 0;

		int w = xF - x;
		int h = yF - y;
		int dx1, dy1, dx2, dy2 = 0;

		int longest, shortest;
		int numerator;

		if (w < 0)
		{
			longest = -w;
			dx1 = -1;
			dx2 = -1;
		}
		else
		{
			longest = w;
			dx1 = 1;
			dx2 = 1;
		}

		if (h < 0)
		{
			shortest = -h;
			dy1 = -1;
		}
		else
		{
			shortest = h;
			dy1 = 1;
		}

		// We want to know whether the direction is more horizontal or vertical,
		// and set the increments accordingly
		if (longest <= shortest)
		{
			memory = longest;
			longest = shortest;
			shortest = memory;
			dx2 = 0;
			if (h < 0) { dy2 = -1; }
			else
			{
				dy2 = 1;
			}
		}

		numerator = longest / 2;

		// cout << n << " numerator " << numerator << endl;
		for (int i(0); i <= longest; ++i)
		{
			// For each pixel, we don't want to use a classic "plot", but to
			// look into canny_image for a black or white pixel
			if (onSegment)
			{
				if (canny_image.at<char>(y, x) == 0 || i == longest)
				{
					// We are leaving a segment
					onSegment = false;
					if (nbPixels >= threshold)
					{
						segments.emplace_back(memoryX, memoryY, xPrev, yPrev);
						// cout << "new segment " << segments.back() << endl;
					}
				}
				else
				{
					// We are still on a segment
					++nbPixels;
				}
			}
			else if (canny_image.at<char>(y, x) != 0)
			{
				// We are entering a segment, and keep this first position in
				// (memoryX, memoryY)
				onSegment = true;
				nbPixels = 0;
				memoryX = x;
				memoryY = y;
			}

			// xPrev and yPrev are used when leaving a segment, to keep in
			// memory the last pixel on it
			xPrev = x;
			yPrev = y;

			// Next pixel using the condition of the Bresenham algorithm
			numerator += shortest;
			if (numerator >= longest)
			{
				numerator -= longest;
				x += dx1;
				y += dy1;
			}
			else
			{
				x += dx2;
				y += dy2;
			}
		}
	}
}
#endif

void CFeatureLines::extractLines(
	const mrpt::img::CImage& img_in,
	std::vector<std::array<int, 4>>& segments_out, size_t threshold,
	const bool display)
{
#if MRPT_HAS_OPENCV

	using namespace cv;
	using namespace std;
	const Mat image = img_in.asCvMat<Mat>(mrpt::img::SHALLOW_COPY);

	// Canny edge detector
	cv::Mat canny_img;
	int lowThreshold = 150;
	// int const max_lowThreshold = 100;
	int ratio = 3;
	int kernel_size = 3;
	cv::Canny(
		image, canny_img, lowThreshold, lowThreshold * ratio,
		kernel_size);  // 250, 600 // CAUTION: Both thresholds depend on the
	// input image, they might be a bit hard to set because
	// they depend on the strength of the gradients
	//            cv::namedWindow("Canny detector", cv::WINDOW_AUTOSIZE);
	//            cv::imshow("Canny detector", canny_img);
	//            cv::waitKey(0);
	//            cv::destroyWindow("Canny detector");

	// Get lines through the Hough transform

	// CvMat matCannyStub, *matCannyImg = (CvMat*)canny_img.getAs<IplImage>();
	// CV_CALL(matCannyImg = cvGetMat(canny_img.getAs<IplImage>(),
	// &matCannyStub));

	std::vector<cv::Vec2f> lines;
	cv::HoughLines(
		canny_img, lines, 1, CV_PI / 180.0,
		threshold);	 // CAUTION: The last parameter depends on the input image,
	// it's the smallest number of pixels to consider a line in
	// the accumulator
	//    double minLineLength=50, maxLineGap=5;
	//    cv::HoughLinesP(canny_img, lines, 1, CV_PI / 180.0, threshold,
	//    minLineLength, maxLineGap); // CAUTION: The last parameter depends on
	//    the input image, it's the smallest number of pixels to consider a line
	//    in the accumulator
	// std::cout << lines.size() << " lines detected" << std::endl;

	// Possible dilatation of the canny detector
	// Useful when the lines are thin and not perfectly straight
	cv::Mat filteredCanny;
	// Choose whether filtering the Canny or not, for thin and non-perfect edges
	/*filteredCanny = canny_img.clone();*/
	cv::dilate(canny_img, filteredCanny, cv::Mat());
	//    cv::namedWindow("Filtered Canny detector");
	//    cv::imshow("Filtered Canny detector", filteredCanny);
	//    cv::waitKey(0);
	//    cv::destroyWindow("Filtered Canny detector");

	// Extracting segments (pairs of points) from the filtered Canny detector
	// And using the line parameters from lines
	std::vector<cv::Vec4i> segments;
	extractLines_CannyHough(filteredCanny, lines, segments, threshold);

	// Force the segments to have a predefined order (e1y <= e2y)
	for (auto line = begin(segments); line != end(segments); ++line)
		if ((*line)[1] == (*line)[3])
		{
			if ((*line)[0] > (*line)[1])
				*line =
					cv::Vec4i((*line)[2], (*line)[3], (*line)[0], (*line)[1]);
		}
		else if ((*line)[1] < (*line)[3])
			*line = cv::Vec4i((*line)[2], (*line)[3], (*line)[0], (*line)[1]);

	// convert cv->STL:
	segments_out.resize(segments.size());
	for (size_t i = 0; i < segments.size(); i++)
		for (size_t k = 0; k < segments_out[i].size(); k++)
			segments_out[i][k] = segments[i][k];

	// Display 2D segments
	if (display)
	{
		cv::Mat image_lines;
		image.convertTo(image_lines, CV_8UC1, 1.0 / 2);
		for (auto line = begin(segments); line != end(segments); ++line)
		{
			cv::line(
				image_lines, cv::Point((*line)[0], (*line)[1]),
				cv::Point((*line)[2], (*line)[3]), cv::Scalar(255, 0, 255), 1);
			cv::circle(
				image_lines, cv::Point((*line)[0], (*line)[1]), 3,
				cv::Scalar(255, 0, 255), 3);
			cv::putText(
				image_lines, string(to_string(distance(begin(segments), line))),
				cv::Point(
					((*line)[0] + (*line)[2]) / 2,
					((*line)[1] + (*line)[3]) / 2),
				0, 1.2, cv::Scalar(200, 0, 0), 3);
		}
		cv::imshow("lines", image_lines);
		cv::moveWindow("lines", 20, 100 + 700);
		cv::waitKey(0);
	}
#else
	THROW_EXCEPTION("Requires building MRPT with OpenCV support");
#endif
}
