#include <iostream>
#include "opencv2/opencv.hpp"
#include "opencv2/aruco.hpp"

// Get MarkersID
void ShowMarkerIdsFunction(cv::OutputArray in) {
    std::vector<cv::Mat> marker_ids;
    in.getMatVector(marker_ids);
    std::cout << "Marker IDs : "<< std::endl;
    for (auto marker_ids : marker_ids) {
        std::cout << marker_ids << std::endl;
    }
}

int main(int argh, char* argv[])
{
    // Create Marker Dictionary, Type of marker : 7X7,1000
    const cv::aruco::PREDEFINED_DICTIONARY_NAME dictionary_name = cv::aruco::DICT_7X7_1000;
    cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(dictionary_name);
    std::vector<int> marker_ids;
    std::vector<std::vector<cv::Point2f>> marker_corners;
    int sum_ids = 0;
    cv::Mat web_camera_frame;
    cv::VideoCapture cap(1);

    // Confirm web camera connection
    if (!cap.isOpened())
    {
        printf("web camera : Bad Connection\n");
        return -1;
    }

    // Recognition MarkersID with ArUco
    while (cap.read(web_camera_frame))
    {
        // Detect markers
        cv::Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();
        cv::aruco::detectMarkers(web_camera_frame, dictionary, marker_corners, marker_ids, parameters);
        cv::aruco::drawDetectedMarkers(web_camera_frame, marker_corners, marker_ids);

        sum_ids = marker_ids.size();
        cv::imshow("from camera", web_camera_frame);

        // Show MarkerID and  sum of IDs in console
        ShowMarkerIdsFunction(marker_ids);
        std::cout << "sum IDs:" << sum_ids << std::endl;

        const int key = cv::waitKey(1);
        if (key == 'q'){
            break;
        }
        else if (key == 's'){
            cv::imwrite("Marker_ids_recognized.png", web_camera_frame);
        }
    }
    cv::destroyAllWindows();
    return 0;
}