#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>

// NB: Don't use this, see https://stackoverflow.com/questions/1452721/why-is-using-namespace-std-considered-bad-practice
// using namespace std;
// using namespace cv;

void process_contour(cv::Mat& frame, std::vector<cv::Point> const& contour)
{
    cv::Scalar TRIANGLE_COLOR(255, 0, 0);
    cv::Scalar QUADRILATERAL_COLOR(0, 255, 0);
    cv::Scalar HEPTAGON_COLOR(0, 0, 255);

    cv::Scalar colour;
    if (contour.size() == 3) {
        colour = TRIANGLE_COLOR;
    } else if (contour.size() == 4) {
        colour = QUADRILATERAL_COLOR;
    } else if (contour.size() == 7) {
        colour = HEPTAGON_COLOR;
    } else {
        return;
    }

    cv::Point const* points(&contour[0]);
    int n_points(static_cast<int>(contour.size()));

    polylines(frame, &points, &n_points, 1, true, colour, 4);
}

void process_frame(cv::Mat const& frame, cv::Mat& result_frame)
{
    cv::Mat feedGrayScale;
    cv::cvtColor(frame, feedGrayScale, cv::COLOR_BGR2GRAY);

    frame.copyTo(result_frame);

    //thresholding the grayscale image to get better results
    cv::threshold(feedGrayScale, feedGrayScale, 128, 255, cv::THRESH_BINARY);

    std::vector<std::vector<cv::Point> > contours;
    std::vector<cv::Vec4i> hierarchy;
    cv::findContours(feedGrayScale, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
    for (size_t k(0); k < contours.size(); ++k) {
        std::vector<cv::Point> approx_contour;
        cv::approxPolyDP(cv::Mat(contours[k]), approx_contour, 3, true);
        process_contour(result_frame, approx_contour);
    }
}

int main()
{
    cv::VideoCapture cap(0); // open the video camera no. 0

    if (!cap.isOpened())  // if not success, exit program
    {
        std::cout << "Cannot open the video cam\n";
        return -1;
    }

    cv::namedWindow("Original", CV_WINDOW_AUTOSIZE);
    cv::namedWindow("Tracked", CV_WINDOW_AUTOSIZE);

    // Process frames from the video stream...
    for(;;) {
        cv::Mat frame, result_frame;

        // read a new frame from video
        if (!cap.read(frame)) {
            std::cout << "Cannot read a frame from video stream\n";
            break;
        }

        process_frame(frame, result_frame);

        cv::imshow("Original", frame);
        cv::imshow("Tracked", result_frame);
        if (cv::waitKey(20) == 27) { // Quit on ESC
            break;
        }
    }

    return 0;
}