#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/legacy/legacy.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <iostream>
#include <string>
#include <vector>
#include <time.h>
using namespace std;


// This function initializes the object points of chessboard corners.
// The coordinates are stored in a vector<vector<cv::Point3f>>.
// The object points can be used for calibrateCamera().

void chessboardObjectPoints(const  cv::Size patternSize,  // # of inner corners per a chessboard row and column
                            int    nViews,                // # of views (images) of chessboard
                            double squareSizeX,           // width of square (x distance between corners)
                            double squareSizeY,           // height of square (y distance between corners)
                            vector<vector<cv::Point3f>> & object_points) // object points of chessboard corners
{
    int iView, iY, iX, nY, nX, ic;
    nY = patternSize.height; 
    nX = patternSize.width;

    // resize the object_points object
    object_points.clear();
    object_points.resize(nViews);
    for (iView = 0; iView < nViews; iView++) {
        object_points[iView].resize(nY*nX);
    }

    // fill the object points
    for (iView = 0; iView < nViews; iView++) {
        ic = 0;
        for (iY = nY - 1; iY >= 0; iY--) {
            for (iX = 0; iX < nX; iX++) {
                object_points[iView][ic].x = (float)(iX * squareSizeX);
                object_points[iView][ic].y = (float)(iY * squareSizeY);
                object_points[iView][ic].z = (float) 0.0;
                ic++;
            }
        }
    }
}


int main() {
    double t0;    

    // read the image file and put the image into my variable.
    vector<string> calFilenames;
    calFilenames.push_back("sample/20130120_222211.JPG");
    calFilenames.push_back("sample/20130120_222233.JPG");
    calFilenames.push_back("sample/20130120_222247.JPG");
    calFilenames.push_back("sample/20130120_222254.JPG");
    calFilenames.push_back("sample/20130120_222313.JPG");
    calFilenames.push_back("sample/20130120_222320.JPG");

    // open an OpenCV window
    cv::namedWindow("Chessboard");

    cv::Size patternSize( 10, 6 ); // patternSize – # of inner corners per a chessboard row and column
    cv::Size myCalibImgSize; 
    vector<cv::Mat> cornersVec;
    cornersVec.resize(calFilenames.size());

    for (int i=0; i< (int) calFilenames.size(); i++) {
        cout << "Reading cal image " << i << "...";
        t0 = clock();
        cv::Mat myCalibImg= cv::imread(calFilenames[i], CV_LOAD_IMAGE_GRAYSCALE );
        cout << (clock()-t0)/CLOCKS_PER_SEC << " sec.\n";
        myCalibImgSize.width = myCalibImg.cols;
        myCalibImgSize.height = myCalibImg.rows;
//        cout << "Size: " << myCalibImg.cols << "x" << myCalibImg.rows << endl;

        // find chessboard corner
        bool found;        
        cv::Mat corners(10, 6, CV_32FC2);
        cout << "Finding cal image " << i << "...";
        t0 = clock();
        found = cv::findChessboardCorners( myCalibImg, patternSize, corners, CV_CALIB_CB_ADAPTIVE_THRESH );
        cout << (clock()-t0)/CLOCKS_PER_SEC << " sec.\n";
        if ( found == true ) {
            cout << "Finding subpixel of image " << i << "...";
            t0 = clock();
            cv::cornerSubPix( myCalibImg, corners, cv::Size(5,5), cv::Size(-1,-1), 
                cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.01));
            cout << (clock()-t0)/CLOCKS_PER_SEC << " sec.\n";
        } else {
          cout << "Can not find corners of image " << i << "\n";
        }
           
        // draw chessboard 
        drawChessboardCorners(myCalibImg, patternSize, corners, found);
        cv::resize(myCalibImg, myCalibImg, cv::Size(800,600), 0, 0, cv::INTER_LINEAR ); 

        // show image
        cv::imshow("Chessboard", myCalibImg );
        cv::waitKey( 100 );

        // data copy
        cornersVec[i] = corners;
    } 


    // generate object points
    vector<vector<cv::Point3f>> object_points;
    chessboardObjectPoints( patternSize, calFilenames.size(), 23.8, 23.8, 
                            object_points );

    // calibration
    cv::Mat cameraMatrix(3, 3, CV_64FC1);
    cv::Mat distCoeffs  (1, 8, CV_64FC1);
    vector<vector<double>> rvecs, tvecs;
    int flag = CV_CALIB_RATIONAL_MODEL ; 

    double reprojectionError = 
        cv::calibrateCamera(object_points, cornersVec, myCalibImgSize, 
                            cameraMatrix, 
                            distCoeffs, 
                            rvecs, tvecs, 
                            flag, 
                            cv::TermCriteria( cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, DBL_EPSILON) ); 
    cout << "Camera matrix:\n" << cameraMatrix << endl;
    cout << "Image size: " << myCalibImgSize.width << " " << 
                              myCalibImgSize.height << endl;
    cout << "Camera matrix:\n" << cameraMatrix << endl;
    cout << "Dist. coeffic:\n" << distCoeffs << endl;
    cout << "Fx: " << cameraMatrix.at<double>(0,0) << endl;
    cout << "Fy: " << cameraMatrix.at<double>(1,1) << endl;
    cout << "Cx: " << cameraMatrix.at<double>(0,2) << endl;
    cout << "Cy: " << cameraMatrix.at<double>(1,2) << endl;
    cout << "k1: " << distCoeffs  .at<double>(0) << endl;
    cout << "k2: " << distCoeffs  .at<double>(1) << endl;
    cout << "p1: " << distCoeffs  .at<double>(2) << endl;
    cout << "p2: " << distCoeffs  .at<double>(3) << endl;
    cout << "k3: " << distCoeffs  .at<double>(4) << endl;
    cout << "k4: " << distCoeffs  .at<double>(5) << endl;
    cout << "k5: " << distCoeffs  .at<double>(6) << endl;
    cout << "k6: " << distCoeffs  .at<double>(7) << endl;
    cout << "Reproj. error: "  << reprojectionError << endl;
    cout << "Finished.\n";

    cv::waitKey( 0 );

    return 0;  // Or return 1; 
}