#include "Processor_PlaneCalibration.h"
#include <QDebug>
#include "mat_utils.h"

int Processor_PlaneCalibration::start()
{
    inputMat.push_back(new cv::Mat());
    outputMat.push_back(new cv::Mat());

    return 0;
}

int Processor_PlaneCalibration::process(int,int) {
    cv::Size boardSize(m_checkerboard_columns,m_checkerboard_rows);
    
    vector<cv::Point2f> imageCorners;
    
    bool grayscale = (inputMat[0]->channels()==1);
    
    switch(mode_) {
        case Processor_PlaneCalibration_Mode_Waiting:
        case Processor_PlaneCalibration_Mode_Acquire:
            if(inputMat[0]->channels()==1) {
                cv::cvtColor(*(inputMat[0]), *(outputMat[0]), CV_GRAY2BGR);
            } else {
                inputMat[0]->copyTo(*outputMat[0]);
            }
            if(cv::findChessboardCorners(*(inputMat[0]), boardSize, imageCorners, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS)) {
                
                if (grayscale) {
                    cv::cornerSubPix(*inputMat[0], imageCorners, cv::Size(11,11),
                                 cv::Size(-1,-1), cv::TermCriteria( CV_TERMCRIT_EPS|CV_TERMCRIT_ITER, 30, 0.1 ));
                } else {
                    cv::Mat gray;
                    cv::cvtColor(*(inputMat[0]), gray, CV_BGR2GRAY);
                    cv::cornerSubPix(gray, imageCorners, cv::Size(11,11),
                                     cv::Size(-1,-1), cv::TermCriteria( CV_TERMCRIT_EPS|CV_TERMCRIT_ITER, 30, 0.1 ));
                }
                
                vector<cv::Point2f> board_corners_image = {imageCorners[0], imageCorners[boardSize.width-1], imageCorners[boardSize.width*(boardSize.height-1)], imageCorners[boardSize.width*boardSize.height-1]};
                
                double checkerboard_size_x = m_checkerboard_raster*(m_checkerboard_columns-1);
                double checkerboard_size_y = m_checkerboard_raster*(m_checkerboard_rows-1);
                
                float xmin = (m_workspace_x_offset/m_workspace_x_size) * m_picture_width;
                float xmax = (m_workspace_x_offset + checkerboard_size_x)/m_workspace_x_size * m_picture_width;
                
                float ymin = (1.0 - (m_workspace_y_offset/m_workspace_y_size)) * m_picture_height;
                float ymax = (1.0 - (m_workspace_y_offset + checkerboard_size_y)/m_workspace_y_size) * m_picture_height;
                
                vector<cv::Point2f> board_corners_physical = {{xmin, ymax}, {xmax, ymax}, {xmin, ymin}, {xmax, ymin}};
                
                const std::string corner_labels[] = {"P2", "", "P0", "P1"};
                
                for (size_t i = 0; i < board_corners_image.size(); i++) {
                    circle(*outputMat[0], board_corners_image[i], 20, cv::Scalar(255,255,255), -1);
                    circle(*outputMat[0], board_corners_image[i], 14, cv::Scalar(0,0,0), -1);
                    
                    putText(*outputMat[0], corner_labels[i], cv::Point(board_corners_image[i].x+10, board_corners_image[i].y+10), cv::FONT_HERSHEY_PLAIN, 5.0, cvScalar(255,255,255), 12);
                    putText(*outputMat[0], corner_labels[i], cv::Point(board_corners_image[i].x+10, board_corners_image[i].y+10), cv::FONT_HERSHEY_PLAIN, 5.0, cvScalar(0,0,0), 3);
                }
                writeInfo("Chessboard found!");
                
                if (mode_ == Processor_PlaneCalibration_Mode_Acquire) {
                    perspective_mat_ = getPerspectiveTransform(board_corners_image, board_corners_physical);
                    mode_ = Processor_PlaneCalibration_Mode_Calibrated;
                    std::cout << perspective_mat_ << std::endl;
                }
            } else {
                writeInfo("Chessboard not found!");
                mode_ = Processor_PlaneCalibration_Mode_Waiting;
            }
            
            break;
        case Processor_PlaneCalibration_Mode_Calibrated:
            warpPerspective(*inputMat[0], *outputMat[0], perspective_mat_, cv::Size(m_picture_width, m_picture_height));
            break;
    }
    return 0;

}

int Processor_PlaneCalibration::processSpecial(string s)
{
    if(s == "calibrate")
    {
        mode_ = Processor_PlaneCalibration_Mode_Acquire;
    }

    if(s == "reset")
    {
        mode_ = Processor_PlaneCalibration_Mode_Waiting;
        perspective_mat_ = cv::Mat();
    }

    return 0;
}

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

void Processor_PlaneCalibration::writeXml(QXmlStreamWriter& stream) {
    stream.writeStartElement(sXmlKey());
    metaDataXmlWriter(stream,*this);
    if (!perspective_mat_.empty()) {
        write64BaseData(stream, "perspective_mat", Mat_to_QVector2D(perspective_mat_));
    }
    stream.writeEndElement();
}

void Processor_PlaneCalibration::readXml(QXmlStreamReader& stream) {
    while(!(stream.tokenType() == QXmlStreamReader::EndElement && stream.name() == sXmlKey())) {
        cXmlIO::readXml(stream);
        if(stream.tokenType() == QXmlStreamReader::StartElement) {
            if(stream.name()=="perspective_mat") {
                QVector<QVector<double> > perspective_mat_qvector;
                read64BaseData(stream, perspective_mat_qvector);
                perspective_mat_ = QVector2D_to_Mat(perspective_mat_qvector, CV_64F);
                mode_ = Processor_PlaneCalibration_Mode_Calibrated;
            }
        }
        stream.readNext();
    }
}
