#include "camera_calibration.h"
#include "ui_camera_calibration.h"

camera_calibration::camera_calibration(QWidget *parent) :
    QDialog(parent),
    ui(new Ui::camera_calibration)
{
    ui->setupUi(this);
    this->setWindowTitle("Cameras calibration - Configuration Menu");
}

camera_calibration::~camera_calibration()
{
    delete ui;
}

void camera_calibration::on_selectFront_clicked()
{
    QString dir = QFileDialog::getExistingDirectory(this, tr("Select Folder.."), QDir::homePath(),
                                                         QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks);
    std::string pathF = dir.toStdString()+"/*.png";
    cv::glob(pathF, imagesF);
    ui->show_pathF->setText(pathF.c_str());
}

void camera_calibration::on_selectSide_clicked()
{
    QString dir = QFileDialog::getExistingDirectory(this, tr("Select Folder.."), QDir::homePath(),
                                                         QFileDialog::ShowDirsOnly | QFileDialog::DontResolveSymlinks);
    std::string pathS = dir.toStdString()+"/*.png";
    cv::glob(pathS, imagesS);
    ui->show_pathS->setText(pathS.c_str());
}


void camera_calibration::on_ChessRow_valueChanged(int arg1)
{
    this->chessRow = arg1-1;
}

void camera_calibration::on_ChessColumn_valueChanged(int arg1)
{
    this->chessCol = arg1-1;
}

void camera_calibration::on_buttonBox_accepted()
{
    this->showFlag = ui->tmp_checkBox->checkState();
    try {
        calibrate();
    } catch (...) {
        QMessageBox text;
        text.setWindowTitle("WARNING");
        text.setText("Check setted parameters");
        text.exec();
    }
}

void camera_calibration::calibrate()
{
    cv::Mat frameF, frameS, grayF, grayS;

    // Creating vector to store vectors of 3D points for each checkerboard image
    std::vector<std::vector<cv::Point3f> > objpoints;

    // Creating vector to store vectors of 2D points for each checkerboard image
    std::vector<std::vector<cv::Point2f> > imgpointsF, imgpointsS;

    // Defining the world coordinates for 3D points
    std::vector<cv::Point3f> objp;

    for(int i{0}; i<this->chessCol; i++){
        for(int j{0}; j<this->chessRow; j++){
            objp.push_back(cv::Point3f(j,i,0));
        }
    }

    // vector to store the pixel coordinates of detected checker board corners
    std::vector<cv::Point2f> corner_ptsF, corner_ptsS;
    bool successF, successS;
    int iter = 0, detected = 0;

    for(int i{0}; i<int(imagesF.size()); i++){
        frameF = cv::imread(imagesF[i]);
        frameS = cv::imread(imagesS[i]);
        cv::cvtColor(frameF,grayF,cv::COLOR_BGR2GRAY);
        cv::cvtColor(frameS,grayS,cv::COLOR_BGR2GRAY);

        successF = cv::findChessboardCorners(grayF,cv::Size(this->chessRow,this->chessCol), corner_ptsF,cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK | cv::CALIB_CB_NORMALIZE_IMAGE);
        successS = cv::findChessboardCorners(grayS,cv::Size(this->chessRow,this->chessCol), corner_ptsS,cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK | cv::CALIB_CB_NORMALIZE_IMAGE);

        if(successF && successS){
            cv::TermCriteria criteria/*( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 );*/(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, this->checker_size, 0.001);

            // refining pixel coordinates for given 2d points.
            cv::cornerSubPix(grayF,corner_ptsF,cv::Size(11,11), cv::Size(-1,-1),criteria);
            cv::cornerSubPix(grayS,corner_ptsS,cv::Size(11,11), cv::Size(-1,-1),criteria);

            if(this->showFlag){
                // Displaying the detected corner points on the checker board
                cv::drawChessboardCorners(frameF, cv::Size(this->chessRow,this->chessCol), corner_ptsF,successF);
                cv::drawChessboardCorners(frameS, cv::Size(this->chessRow,this->chessCol), corner_ptsS,successS);
            }

            objpoints.push_back(objp);
            imgpointsF.push_back(corner_ptsF);
            imgpointsS.push_back(corner_ptsS);

            detected++;

            if(this->showFlag){
                cv::namedWindow("Front view", cv::WINDOW_NORMAL);
                cv::imshow("Front view",frameF);
                cv::namedWindow("Side view", cv::WINDOW_NORMAL);
                cv::imshow("Side view",frameS);
                cv::waitKey(0);
            }
        }
        iter++;
    }
    cv::destroyAllWindows();

    cv::Mat Rot_F, Tran_F, Rot_S, Tran_S;

    cv::calibrateCamera(objpoints, imgpointsF, cv::Size(grayF.rows, grayF.cols),
                        this->parameters.Front_cam.K_matrix, this->parameters.Front_cam.Dist_coeffs, Rot_F, Tran_F);
    cv::calibrateCamera(objpoints, imgpointsS, cv::Size(grayS.rows, grayS.cols),
                        this->parameters.Side_cam.K_matrix, this->parameters.Side_cam.Dist_coeffs, Rot_S, Tran_S);

        std::cout<<"Front: "<<std::endl;
        std::cout << "cameraMatrix: " << this->parameters.Front_cam.K_matrix << std::endl;
        std::cout << "distCoeffs: " << this->parameters.Front_cam.Dist_coeffs << std::endl;
        std::cout<<"Side: "<<std::endl;
        std::cout << "cameraMatrix: " << this->parameters.Side_cam.K_matrix << std::endl;
        std::cout << "distCoeffs: " << this->parameters.Side_cam.Dist_coeffs << std::endl;


    cv::stereoCalibrate(objpoints, imgpointsF, imgpointsS,
                        this->parameters.Front_cam.K_matrix, this->parameters.Front_cam.Dist_coeffs,
                        this->parameters.Side_cam.K_matrix, this->parameters.Side_cam.Dist_coeffs, cv::Size(grayF.rows, grayF.cols),
                        this->parameters.Rotation_matrix, this->parameters.Translation_vector, this->parameters.Essential_matrix, this->parameters.Fundamental_matrix);

    cv::stereoRectify(this->parameters.Front_cam.K_matrix, this->parameters.Front_cam.Dist_coeffs,
                      this->parameters.Side_cam.K_matrix, this->parameters.Side_cam.Dist_coeffs, cv::Size(grayF.rows, grayF.cols),
                      this->parameters.Rotation_matrix, this->parameters.Translation_vector,
                      this->parameters.Front_cam.Rectification_transform, this->parameters.Side_cam.Rectification_transform,
                      this->parameters.Front_cam.Projection_matrix, this->parameters.Side_cam.Projection_matrix, this->parameters.Q_matrix);

        std::cout<< "Q matrix: " << this->parameters.Q_matrix << std::endl;

    QMessageBox report;
    report.setWindowTitle("Calibration Finished");
    std::string text = "Was detected "+std::to_string(detected)+"/"+std::to_string(iter)+" frame(s)";
    report.setText(QString(text.c_str()));
    report.exec();
}

bool camera_calibration::isValid()
{
    this->validity = (this->parameters.Side_cam.Calibrated && this->parameters.Front_cam.Calibrated) ? true : false;
    return this->validity;
}

calibration::stereo_camera camera_calibration::getData(){
    return this->parameters;
}

calibration::_Point camera_calibration::getCalibrationPoint(){
    return this->calibration_point;
}

void camera_calibration::setCalibrationPoint(calibration::_Point point){
    this->calibration_point = point;
}

void camera_calibration::on_CheckerSize_valueChanged(double arg1){
    this->checker_size = arg1;
}
