#include "CameraObject.h"

#include <QSemaphore>
#include <QMetaObject>
#include <QMessageBox>
#include <QDir>
#include "Camera.h"

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <iostream>
#include <fstream>
#include <QDebug>

void CameraObject::readCallibrationFile()
{
//    QFile ser(cameraName+".set");
//    ser.open(QIODevice::ReadOnly);
//    //markersGenerator=new  cImagePocessing;
//    if(ser.isOpen())
//    {
//        QDataStream out(&ser);
//        out>>*this;
//        out>>*cam;
//
//        ser.close();
//    }
}

void CameraObject::writeXml(QXmlStreamWriter &stream)
{
    stream.writeStartElement(sXmlKey());
    metaDataXmlWriter(stream,*this);
    
    cam->writeXml(stream);

    //stream.writeEndElement();
    //    qDebug()<<"cam Mat";
    //    QVector<double> v={1,2,3,5};
    //    QVector<QVector<double>> v2={v,v,v,v};
    //    write64BaseData(stream,"cameraMat",v2);
    //qDebug()<<"cam Csv";
    //    writeCsvData(stream,"cameraMatCsv",v2);
    //    stream.writeEndElement();

}

void CameraObject::readXml(QXmlStreamReader& stream)
{
    cXmlIO::readXml(stream);
    while(!(stream.tokenType() == QXmlStreamReader::EndElement &&
            stream.name() == sXmlKey())) {

        if(stream.tokenType() == QXmlStreamReader::StartElement) {

            if(stream.name()==cam->sXmlKey()) //class name
            {
                cam->readXml(stream);
            }
        }
        stream.readNext();
    }
}


CameraObject::CameraObject(Camera *c, int camType, int camId, QSemaphore *ss)
{
    qDebug() << "CameraObject::CameraObject(Camera *c, int camType, int camId, QSemaphore *ss)";
    qRegisterMetaType<QList<CameraObject*>>();
    
    //QVariant(
    cameraName = QString("T") + QVariant(camType).toString() + QString("_I") + QVariant(camId).toString();
    c->cameraName = cameraName;
    setXmlID(c->getXmlID());

    m_displayData = CameraObject::DisplayData::DisplayRawImage;
    n_img = 0;
    matType = CV_8UC1;
    first_grab=false;

    cam=c;
    cameraType=camType;
    cameraID=camId;
    syncSem=ss;
    SetcalibrationPath(QString("calibration/"));
    setCalibrationFileName( QString("%s.yml"));
    isRunning = false;

}

void CameraObject::cStart()
{
    if(isRunning)
    {
        syncSem->release(1);
        return;
    }
    isRunning = true;
    first_grab = true;
    cam->start();

    //	if(m_showThresholdBox)
    //		markersGenerator->setwindowSettings();
    string path;
    char cstr_path[200];
    path = (m_calibrationPath + m_calibrationFileName).toStdString();
    sprintf(cstr_path,path.c_str(),qPrintable(cameraName));
    path = cstr_path;

    cv::FileStorage read;
    if(read.open(path, cv::FileStorage::READ))
    {
        read["mtxCam"] >> cameraMatrix;
        read["mtxDist"] >> distCoeffs;
        read.release();
        doRemap = true;
    }
    else
    {
        doRemap = false;
    }

    if(cam->cameraColor() == Camera::CameraColor_Mono)
    {
        matType = CV_8UC1;
    }

    if(cam->cameraColor() == Camera::CameraColor_BGR)
    {
        matType = CV_8UC3;
    }

    syncSem->release(1);
}

void CameraObject::cSync()
{
    cam->sync();
    syncSem->release(1);
}

void CameraObject::cGrab()
{
    if(first_grab)
    {
        //cam->grab();
        if(cam->grab())
        {
            syncSem->release(1);
            return;
        }
        frame.create(cam->resultY, cam->resultX, matType);
        thresh.create(cam->resultY, cam->resultX, CV_8UC1);

        first_grab = false;

        if(doRemap)
        {
            cv::initUndistortRectifyMap(
                        cameraMatrix, // computed camera matrix
                        distCoeffs, // computed distortion matrix
                        cv::Mat(), // optional rectification (none)
                        cameraMatrix, // camera matrix to generate undistorted
                        frame.size(), // size of undistorted
                        CV_32FC1, // type of output map
                        map1, map2); // the x and y mapping functions
        }

        if(!(cam->readData))
        {
            if(matType == CV_8UC1)
                memcpy(frame.ptr(), cam->resultBuffer, cam->resultY*cam->resultX);
            else if(matType == CV_8UC3)
                memcpy(frame.ptr(), cam->resultBuffer, cam->resultY*cam->resultX*3);
        }
    }
    else
    {
        //cam->grab(&frame);
        if(cam->grab(&frame))
        {
            syncSem->release(1);
            return;
        }


        if(cam->readData)
        {
            points.clear();
            for(size_t i=0; i<cam->points.size(); ++i)
            {
                points.push_back(cam->points[i]);
            }
        }
    }

    //syncSem->release(1);



    if(!(cam->readData))
    {
        cv::Mat frame_test = cv::Mat(frame);

        //if(matType == CV_8UC3||matType == CV_8UC1)
        //	points=markersGenerator->processFrame(frame_test,m_showThresholdBox);
        /*		if(matType == CV_8UC1)
        {
            //cv::blur(frame,thresh,cv::Size(3,3));
            cv::resize(frame,thresh,cv::Size(), 0.5, 0.5);
        }
        else if(matType == CV_8UC3)
        {
            cv::cvtColor(frame,thresh, CV_BGR2GRAY);
            //cv::blur(thresh,thresh,cv::Size(3,3));
            cv::resize(thresh,thresh,cv::Size(), 0.5, 0.5);
        }

        cv::threshold(thresh,thresh,127,255,cv::THRESH_BINARY);

        contours.clear();
        cv::findContours(thresh,contours,CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

        cv::findContours(thresh,contours,CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);*/

        //points.clear();

        /*for(int i=0; i<contours.size(); ++i)
        {
            cv::Moments mu;
            mu = cv::moments(contours[i],false);
            cv::Point2f mid(2*mu.m10/mu.m00, 2*mu.m01/mu.m00);
            if(mid.x >= 0 && mid.y >= 0 && mid.x <= frame.cols && mid.y <= frame.rows)
                points.push_back(mid);
        }*/
    }

    cv::Mat* out = NULL;

    string path;

    switch(m_displayData)
    {
    case DisplayRawImage:
        out = new cv::Mat(frame);
        if(matType == CV_8UC1)
            cv::cvtColor(*out,*out,CV_GRAY2BGR);
        //else if(matType == CV_8UC3)
        //	cv::cvtColor(*out,*out,CV_BGR2BGR);
        emit frameOut(out, cam->resultX, cam->resultY);
        break;
    case DisplayThreshold:
        out = new cv::Mat(thresh);
        cv::cvtColor(*out,*out,CV_GRAY2BGR);
        emit frameOut(out, cam->resultX, cam->resultY);
        break;
    case DisplayPoints:
        out = new cv::Mat(cam->resultY, cam->resultX, CV_8UC1, cv::Scalar(0));
        for(size_t i=0; i<points.size(); ++i)
        {
            cv::circle(*out,points[i],5,cv::Scalar(255),3);
        }
        cv::cvtColor(*out,*out,CV_GRAY2BGR);
        emit frameOut(out, cam->resultX, cam->resultY);
        break;
    default:
        break;
    }

    syncSem->release(1);

}

void CameraObject::cStop()
{
    if(!isRunning)
    {
        syncSem->release(1);
        return;
    }
    output_file.close();
    cam->stop();
    isRunning = false;
    syncSem->release(1);
}

void CameraObject::requestImage(void* image, void* camSem)
{
    frame.copyTo(*(cv::Mat*)image);
    ((QSemaphore*)camSem)->release(1);
}

void CameraObject::requestPoints2(void* points, void* camSem)
{
    ((vector<cv::Point2f> *)points)->clear();

    for(size_t i=0; i<this->points.size(); ++i)
        ((vector<cv::Point2f> *)points)->push_back(this->points[i]);

    ((QSemaphore*)camSem)->release(1);
}
