#include "opencv2/opencv.hpp"
#include "opencv2/aruco.hpp"

int main(int argc, char *argv[])
{
    cv::Mat frame = cv::imread(argc > 1 ? argv[1] : "img_fail.png");

    // Intrinsic camera parameters.
    cv::Mat camMatrix = cv::Mat::eye(3, 3, CV_64F);
    cv::Mat distortionCoeffs = cv::Mat::zeros(8, 1, CV_64F);
    camMatrix.at<double>(0, 0) = 2.3396381685789738e+03;
    camMatrix.at<double>(0, 2) = 960.;
    camMatrix.at<double>(1, 1) = 2.3396381685789738e+03;
    camMatrix.at<double>(1, 2) = 540.;
    std::cout << "camMatrix:\n" << camMatrix << std::endl;

//    distortionCoeffs.at<double>(0, 0) = -1.0982746232841779e-01;
//    distortionCoeffs.at<double>(1, 0) = 2.2689585715220828e-01;
//    distortionCoeffs.at<double>(2, 0) = 0.;
//    distortionCoeffs.at<double>(3, 0) = 0.;
//    distortionCoeffs.at<double>(4, 0) = -2.2112148171171589e-01;
    std::cout << "distortionCoeffs: " << distortionCoeffs.t() << std::endl;

    std::vector<cv::Point3d> objectPoints;
    objectPoints.push_back( cv::Point3d(-2.5, 2.5, 0.0) );
    objectPoints.push_back( cv::Point3d(2.5, 2.5, 0.0) );
    objectPoints.push_back( cv::Point3d(2.5, -2.5, 0.0) );
    objectPoints.push_back( cv::Point3d(-2.5, -2.5, 0.0) );

    std::vector<cv::Point2d> imagePoints;
    imagePoints.push_back( cv::Point2d(988, 512) );
    imagePoints.push_back( cv::Point2d(945, 575) );
    imagePoints.push_back( cv::Point2d(849, 544) );
    imagePoints.push_back( cv::Point2d(893, 480) );

    std::vector<int> solvePnP_methods;
    solvePnP_methods.push_back(cv::SOLVEPNP_ITERATIVE);
    solvePnP_methods.push_back(cv::SOLVEPNP_EPNP);
    solvePnP_methods.push_back(cv::SOLVEPNP_P3P);
    solvePnP_methods.push_back(cv::SOLVEPNP_AP3P);

    std::map<int, std::string> solvePnP_method_names;
    solvePnP_method_names[cv::SOLVEPNP_ITERATIVE] = "cv::SOLVEPNP_ITERATIVE";
    solvePnP_method_names[cv::SOLVEPNP_EPNP] = "cv::SOLVEPNP_EPNP";
    solvePnP_method_names[cv::SOLVEPNP_P3P] = "cv::SOLVEPNP_P3P";
    solvePnP_method_names[cv::SOLVEPNP_AP3P] = "cv::SOLVEPNP_AP3P";

    for (size_t idx = 0; idx < solvePnP_methods.size(); idx++)
    {
        cv::Mat debugFrame = frame.clone();
        cv::putText(debugFrame, solvePnP_method_names[solvePnP_methods[idx]], cv::Point(20,20), cv::FONT_HERSHEY_SIMPLEX , 0.5, cv::Scalar(0,0,255));

        cv::line(debugFrame, imagePoints[0], imagePoints[1], cv::Scalar(0,0,255), 2);
        cv::line(debugFrame, imagePoints[1], imagePoints[2], cv::Scalar(0,255,0), 2);
        cv::line(debugFrame, imagePoints[2], imagePoints[3], cv::Scalar(255,0,0), 2);

        cv::Mat rvec, tvec;
        cv::solvePnP(objectPoints, imagePoints, camMatrix, distortionCoeffs, rvec, tvec, false, solvePnP_methods[idx]);

        cv::Mat rotation_matrix;
        cv::Rodrigues(rvec, rotation_matrix);

        std::cout << "\nrotation_matrix:\n" << rotation_matrix << std::endl;
        std::cout << "R.R^t=" << rotation_matrix*rotation_matrix.t() << std::endl;
        std::cout << "det(R)=" << cv::determinant(rotation_matrix) << std::endl << std::endl;

        cv::aruco::drawAxis(debugFrame, camMatrix, distortionCoeffs, rvec, tvec, 5.0);

        std::vector<cv::Point3d> axis_vec;
        axis_vec.push_back( cv::Point3d(0, 0, 0) );
        axis_vec.push_back( cv::Point3d(1, 0, 0) );
        axis_vec.push_back( cv::Point3d(0, 1, 0) );
        axis_vec.push_back( cv::Point3d(0, 0, 1) );

        std::vector<cv::Point2d> axis_vec_proj;
        cv::projectPoints(axis_vec, rvec, tvec, camMatrix, distortionCoeffs, axis_vec_proj);
        for (size_t i = 0; i < axis_vec_proj.size(); i++)
        {
            std::cout << "axis_vec_proj[" << i << "]=" << axis_vec_proj[i].x << " ; " << axis_vec_proj[i].y << std::endl;
        }

        // Project cube.
        float length = 20.0f;
        std::vector<cv::Point3f> testObj3d;
        testObj3d.push_back(cv::Point3f(0, 0, 0));
        testObj3d.push_back(cv::Point3f(length, 0, 0));
        testObj3d.push_back(cv::Point3f(0, length, 0));
        testObj3d.push_back(cv::Point3f(length, length, 0));
        testObj3d.push_back(cv::Point3f(0, 0, length));
        testObj3d.push_back(cv::Point3f(length, 0, length));
        testObj3d.push_back(cv::Point3f(0, length, length));
        testObj3d.push_back(cv::Point3f(length, length, length));
        std::vector<cv::Point2f> testObj2d;
        cv::projectPoints(testObj3d, rvec, tvec, camMatrix, distortionCoeffs, testObj2d);

        cv::line(debugFrame, testObj2d[0], testObj2d[1], cv::Scalar(0,0,0), 1);
        cv::line(debugFrame, testObj2d[0], testObj2d[2], cv::Scalar(0,0,0), 1);
        cv::line(debugFrame, testObj2d[1], testObj2d[3], cv::Scalar(0,0,0), 1);
        cv::line(debugFrame, testObj2d[2], testObj2d[3], cv::Scalar(0,0,0), 1);
        cv::line(debugFrame, testObj2d[0], testObj2d[4], cv::Scalar(0,0,0), 1);
        cv::line(debugFrame, testObj2d[1], testObj2d[5], cv::Scalar(0,0,0), 1);
        cv::line(debugFrame, testObj2d[2], testObj2d[6], cv::Scalar(0,0,0), 1);
        cv::line(debugFrame, testObj2d[3], testObj2d[7], cv::Scalar(0,0,0), 1);
        cv::line(debugFrame, testObj2d[4], testObj2d[5], cv::Scalar(0,0,0), 1);
        cv::line(debugFrame, testObj2d[4], testObj2d[6], cv::Scalar(0,0,0), 1);
        cv::line(debugFrame, testObj2d[5], testObj2d[7], cv::Scalar(0,0,0), 1);
        cv::line(debugFrame, testObj2d[6], testObj2d[7], cv::Scalar(0,0,0), 1);

        cv::imshow("debug", debugFrame);
        cv::waitKey(0);
    }
}