#include "Processor_CameraCalibration.h"
#include <QDebug>

int Processor_CameraCalibration::start()
{
    qDebug()<<"Processor_CameraCalibration::start()";
    inputMat.push_back(new cv::Mat());
    outputMat.push_back(inputMat[0]);

    acquiredImageDisplayTimeout=-1;

    imageCorners.clear();
    objectCorners.clear();
    imagePoints.clear();
    objectPoints.clear();
    cv::Size boardSize(m_boardSize_X,m_boardSize_Y);

    return 0;
}

int Processor_CameraCalibration::process(int,int)
{
    switch(mode)
    {
    case Processor_CameraCalibration_Mode_Normal:
    {
        if(acquiredImageDisplayTimeout == 0)
        {
            delete outputMat[0];
            outputMat[0] = inputMat[0];
        }
        if(acquiredImageDisplayTimeout >= 0)
        {
            --acquiredImageDisplayTimeout;
        }
        break;
    }

    case Processor_CameraCalibration_Mode_Acquire:
    {
        cv::Size boardSize(m_boardSize_X,m_boardSize_Y);
        //cv::OutputArray imageCorners_l =  imageCorners;
        //imageCorners_l.push_back(cv::Point2f(1,1));
        
        if(cv::findChessboardCorners(*(inputMat[0]), boardSize, imageCorners,CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS))
        {
            //cv::Mat m = imageCorners_l.getMat();
            outputMat[0] = new cv::Mat();
            if(inputMat[0]->channels()==1)
                cv::cvtColor(*(inputMat[0]), *(outputMat[0]), CV_GRAY2BGR);
            if(inputMat[0]->channels()==3)
                inputMat[0]->copyTo(*outputMat[0]);
            cv::Mat viewGray;
            cv::cvtColor(*(outputMat[0]), viewGray, CV_BGR2GRAY);

            cv::cornerSubPix( viewGray, imageCorners, cv::Size(11,11),
                              cv::Size(-1,-1), cv::TermCriteria( CV_TERMCRIT_EPS|CV_TERMCRIT_ITER, 30, 0.1 ));
            cv::drawChessboardCorners(*(outputMat[0]),boardSize,imageCorners,1);

            //imageCorners = imageCorners_l;
            //imageCorners_l.release();
            //imageCorners_l.release();
            
            objectCorners.clear();
            for (int i=0; i<boardSize.height; i++) {
                for (int j=0; j<boardSize.width; j++) {
                    objectCorners.push_back(cv::Point3f(i*m_squareSize, j*m_squareSize, 0.0f));
                }
            }
            
            imagePoints.push_back(imageCorners);
            objectPoints.push_back(objectCorners);
            acquiredImageDisplayTimeout = 5;
            writeInfo("Chessboard found!");
        }
        else
        {
            writeInfo("Chessboard not found!");
        }
        break;
    }

    case Processor_CameraCalibration_Mode_Calibrate:
    {
        cv::Mat cameraMatrix;
        cv::Mat distCoeffs;
        std::vector<cv::Mat> rvecs, tvecs;
        //qDebug()<<"object Points size:"<<objectPoints.size()<<"points:"<<objectPoints[0].size()<< "Image Points size:"<<imagePoints.size()<<" elements:"<<imagePoints[0].size();
        cv::calibrateCamera(objectPoints,imagePoints, cv::Size(inputMat[0]->cols, inputMat[0]->rows),cameraMatrix, distCoeffs, rvecs, tvecs, 0);


        string path;
        char cstr_path[200];
        path = (m_path + m_filename).toStdString();
        
        qDebug() << "path" << QString(path.c_str());
        qDebug() << "*cameraName[0]" << *cameraName[0];
        
        
        sprintf(cstr_path,path.c_str(),qPrintable(*cameraName[0]));
        path = cstr_path;

        cv::FileStorage write;
        write.open(path, cv::FileStorage::WRITE);
        write << "mtxCam" << cameraMatrix;
        write << "mtxDist" << distCoeffs;
        write.release();
        break;
    }

    default:
        break;
    }

    mode = Processor_CameraCalibration_Mode_Normal;
    return 0;

}

int Processor_CameraCalibration::processSpecial(string s)
{
    if(s == "acquire")
    {
        mode = Processor_CameraCalibration_Mode_Acquire;
    }

    if(s == "calibrate")
    {
        mode = Processor_CameraCalibration_Mode_Calibrate;
    }

    return 0;
}

int Processor_CameraCalibration::stop()
{
    delete inputMat[0];
    inputMat.clear();
    outputMat.clear();
    return 0;
}
