#include <opencv2/opencv.hpp>
#include <iostream>
#include <string>
#include <vector>

int main(int argc, char **argv) {
	const int MAX_FRAME_WIDTH = 640;
	const int MAX_FRAME_HEIGHT = 360;
	const int CENTER_X = MAX_FRAME_WIDTH / 2;
	const int CENTER_Y = MAX_FRAME_HEIGHT / 2;
	const int OFFSET = 50;

	// Read image path from stdin
	std::string path_image;
	getline(std::cin, path_image);
	cv::Mat bgr_image = cv::imread(path_image);

	cv::Mat orig_image = bgr_image.clone();

	// Convert input image to HSV
	cv::Mat hsv_image;
	cv::cvtColor(bgr_image, hsv_image, cv::COLOR_BGR2HSV);

	// Threshold the HSV image, keep only the green pixels
	cv::Mat lower_color_hue_range;
	cv::Mat upper_color_hue_range;
	cv::inRange(hsv_image, cv::Scalar(60, 100, 100), cv::Scalar(80, 255, 255), lower_color_hue_range);
	cv::inRange(hsv_image, cv::Scalar(60, 100, 100), cv::Scalar(80, 255, 255), upper_color_hue_range);

	// Combine the above two images
	cv::Mat color_hue_image;
	cv::addWeighted(lower_color_hue_range, 1.0, upper_color_hue_range, 1.0, 0.0, color_hue_image);

	cv::GaussianBlur(color_hue_image, color_hue_image, cv::Size(9, 9), 2, 2);

	// Use the Hough transform to detect circles in the combined threshold image
	std::vector<cv::Vec3f> circles;
	cv::HoughCircles(color_hue_image, circles, CV_HOUGH_GRADIENT, 1, color_hue_image.rows/8, 100, 20, 20, 0);

	if(circles.size() == 0) std::exit(-1);

	// Process the first detected circle and outline them on the original image
	size_t current_circle = 0;
	int x = std::round(circles[current_circle][0]);
	int y = std::round(circles[current_circle][1]);
	cv::Point center(x, y);
	int radius = std::round(circles[current_circle][2]);

	cv::circle(orig_image, center, radius, cv::Scalar(200, 0, 0), 5);

	// Position correction
	int dx = 0;
	int dy = 0;
	std::string cmd_x = "HOLD";
	std::string cmd_y = "HOLD";

	if (x < (CENTER_X - OFFSET)) {
		dx = CENTER_X - x;
		cmd_x = "GO_LEFT";
	}
	else if (x > (CENTER_X + OFFSET)) {
		dx = CENTER_X - x;
		cmd_x = "GO_RIGHT";
	}

	if (y < (CENTER_Y - OFFSET)) {
		dy = CENTER_Y - y;
		cmd_y = "GO_FRONT";
	}
	else if (y > (CENTER_Y + OFFSET)) {
		dy = CENTER_Y - y;
		cmd_y = "GO_BACK";
	}

	std::cout << "{\'" << cmd_x << "\'," << dx << ",\'" << cmd_y << "\'," << dy << "}"; // << std::endl;

	// Show image
	cv::imshow("Tracked target (green circle)", orig_image);
	cv::waitKey(0);

	return 0;
}