#include "opencv2/opencv.hpp"
#include <fstream>
#include <vector>
void CalcBoardCornerPositions(cv::Size boardSize, std::vector<cv::Point3f>& corners) {
    corners.clear();
    for (int i = 0; i < boardSize.height; ++i)
        for (int j = 0; j < boardSize.width; ++j)
            corners.push_back(cv::Point3f(float(j), float(i), 0));
}
int main() {

    std::vector<cv::Point2f> left_corners;
    std::vector<cv::Point2f> right_corners;

    cv::VideoCapture cam1(1);
    cv::VideoCapture cam2(2);

    cv::namedWindow("Left");
    cv::namedWindow("Right");
    cv::Mat frame1, frame2;
    cv::Mat cameraMatrix[2], distCoeffs[2];
    cv::Mat R, T, E, F;
    cv::Size boardSize(9, 6);
    double rms;

    std::vector<cv::Point3f> obj;
    CalcBoardCornerPositions(boardSize, obj);
    std::vector<std::vector<cv::Point2f> >imagePoints1;
    std::vector<std::vector<cv::Point2f> >imagePoints2;
    std::vector<std::vector<cv::Point3f> >objectPoints;
    int success = -1;

    while (1) {
        cam1 >> frame1;
        cam2 >> frame2;

        bool foundLeft = false, foundRight = false;

        cv::Mat cameraMatrix[2], distCoeffs[2];
        cameraMatrix[0] = cv::Mat::eye(3, 3, CV_64F);
        cameraMatrix[1] = cv::Mat::eye(3, 3, CV_64F);

        foundLeft = cv::findChessboardCorners(frame1, boardSize, left_corners);
        foundRight = cv::findChessboardCorners(frame2, boardSize, right_corners);
        cv::drawChessboardCorners(frame1, boardSize, left_corners, foundLeft);
        cv::drawChessboardCorners(frame2, boardSize, right_corners, foundRight);
        if (foundLeft && foundRight) {
            imagePoints1.push_back(left_corners);
            imagePoints2.push_back(left_corners);
            objectPoints.push_back(obj);
            printf("Corners stored\n");
            ++success;
        }
        cv::imshow("Left", frame1);
        cv::imshow("Right", frame2);

        if (cv::waitKey(1000) == 32 || success > 10) break;

    }

    std::ofstream file("param.txt");

    rms = cv::stereoCalibrate(objectPoints, imagePoints1, imagePoints2,
        cameraMatrix[0], distCoeffs[0],
        cameraMatrix[1], distCoeffs[1],
        frame1.size(), R, T, E, F);
    file << "RMS Error: " << rms << "\n";
    file << "global_rms: " << rms << std::endl;
    file << "global_R: " << R << std::endl;
    file << "global_T: " << T << std::endl;
    file << "global_E: " << E << std::endl;
    file << "global_F: " << F << std::endl;
    file << "global_cameraMatrix[0]: " << cameraMatrix[0] << std::endl;
    file << "global_cameraMatrix[1]: " << cameraMatrix[0] << std::endl;
    file << "global_distCoeffs[0]: " << distCoeffs[0] << std::endl;
    file << "global_distCoeffs[1]: " << distCoeffs[1] << std::endl;
    file.close();
    cv::Mat R1, R2, P1, P2, Q;
    cv::Rect validROI[2];
    stereoRectify(cameraMatrix[0], distCoeffs[0], cameraMatrix[1], distCoeffs[1], frame1.size(),
        R, T, R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY, 1, frame1.size(), &validROI[0], &validROI[1]);

    cv::Mat rmap[2][2];
    cv::Size imageSize = frame1.size();
    cv::initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, imageSize, CV_16SC2, rmap[0][0], rmap[0][1]);
    cv::initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]);
    cv::Mat canvas;
    double sf;
    int w, h;
    w = cvRound(imageSize.width);
    h = cvRound(imageSize.height);
    canvas.create(h, w * 2, CV_8UC3);


    cv::namedWindow("rectified");

    imshow("rectified", canvas);


    for (int j = 0; j < 2; j++) {

        cv::Mat img, rimg, cimg;
        if (j == 0)
            img = frame1;
        else
            img = frame2;
        remap(img, rimg, rmap[j][0], rmap[j][1], cv::INTER_LINEAR);
        cimg = rimg;
        cv::Mat canvasPart = canvas(cv::Rect(w*j, 0, w, h));
        resize(cimg, canvasPart, canvasPart.size(), 0, 0, cv::INTER_AREA);
        cv::Rect vroi(cvRound(validROI[j].x), cvRound(validROI[j].y),
            cvRound(validROI[j].width), cvRound(validROI[j].height));
        cv::rectangle(canvasPart, vroi, cv::Scalar(0, 0, 255), 3, 8);
    }
    for (int j = 0; j < canvas.rows; j += 16)
        line(canvas, cv::Point(0, j), cv::Point(canvas.cols, j), cv::Scalar(0, 255, 0), 1, 8);
    imshow("rectified", canvas);
    cv::waitKey();
    return 0;
}