#include "twopoint.h"

twoPoint::twoPoint(QObject *parent)
    : QObject{parent}
{
    thread = new QThread();
    thread->start();
    this->moveToThread(thread);
    ready = false;

    b_savelow = false;
    b_savehigh = false;
    n_savelow = 8;
    n_savehigh = 8;
    b_onepoint = false;
    vardim = 4;
    opnum = 0;
    c = 0;
    bdb = false;
    dbrange = 1;
}

void twoPoint::addlow(cv::Mat img, int id)
{
    cv::Mat temp;
    img.convertTo(temp,CV_32FC1,1.0/32.0);
    if(lowtemp.empty()){
        //        img.copyTo(lowtemp);
        //        img.convertTo(lowtemp,CV_32FC1);
        lowtemp = temp;
        //        lowtemp = lowtemp/8;
    }
    if(id == 0){
        //        img.convertTo(lowtemp,CV_32FC1);
        lowtemp = temp;
    }
    else if(id<32) {
        //        lowtemp+= img;
        lowtemp+= temp;
    }
    else{
        b_savelow = false;
    }
}

void twoPoint::addhigh(cv::Mat img, int id)
{
    cv::Mat temp;
    img.convertTo(temp,CV_32FC1,1.0/32.0);
    if(hightemp.empty()){
        //        img.copyTo(hightemp);
        //        img.convertTo(hightemp,CV_32FC1);
        hightemp = temp;
        //        lowtemp = lowtemp/8;
    }
    if(id == 0){
        //        hightemp = img/32;
        //        img.convertTo(hightemp,CV_32FC1);
        hightemp = temp;
    }
    else if(id<32) {
        //        hightemp+= img;
        hightemp += temp;
    }
    else{
        b_savehigh = false;
    }
}

void twoPoint::onepoint(cv::Mat img)
{
    //    ready = false;

    if(kmat.empty() || bmat.empty()){
        return;
    }
    cv::Mat temp;
    img.convertTo(temp,CV_32FC1,1.0/32.0);
    if(opnum == 32){
        temp.copyTo(bmat);
    }
    else{
        bmat += temp;
    }

    opnum-=1;
    //    if(opnum == 0){
    //        base =
    //    }

    //    cv::Mat dst = cv::Mat(img.rows,img.cols,CV_16UC1);
    //    double sum = 0;
    //    for(int i =0 ;i< img.rows;i++){
    //        for (int j = 0;j< img.cols;j++){
    //            int temp = kmat.at<float>(i,j)*img.at<ushort>(i,j)+bmat.at<float>(i,j);
    //            if(temp<0) temp = 0;
    //            if(temp>65535) temp = 65535;
    ////            dst.at<ushort>(i,j) = temp;
    //            sum+=temp;
    //        }
    //    }
    //    sum = sum/img.rows/img.cols;


    //    for(int i =0 ;i< img.rows;i++){
    //        for (int j = 0;j< img.cols;j++){
    //            bmat.at<float>(i,j) = sum-kmat.at<float>(i,j)*img.at<ushort>(i,j);
    //        }
    //    }

    /// k(i,j)*img(i,j)+b(i,j) = out(i,j)
    /// b'(i,j) = out^-k(i,j)*img(i,j)
    ///

    //    ready = true;
}

void twoPoint::onepoint()
{
    b_onepoint = true;
    opnum = 32;
}

void twoPoint::calc()
{
    if(hightemp.empty() || lowtemp.empty()){
        return;
    }
    kmat = cv::Mat(hightemp.rows,hightemp.cols,CV_32FC1);
    bmat = cv::Mat(hightemp.rows,hightemp.cols,CV_32FC1);
    //    double
    high = 0,low = 0;
    for(int i = 0;i<hightemp.rows;i++){
        for(int j = 0;j< hightemp.cols;j++){
            high += hightemp.at<float>(i,j);
            low += lowtemp.at<float>(i,j);
        }
    }
    high = high/hightemp.total();
    high = 16383*4;
    low = low/hightemp.total();
    low = 0;
    /// (temp(i,j) - b(i,j))*k(i,j)+low = fix(i,j)
    /// low(i,j) = b(i,j)
    /// (high(i,j) - b(i,j))*k(i,j) = high - low
    /// k(i,j) = (high-low)/(high(i,j)-low(i,j))
    /// base = low

    for(int i = 0;i<hightemp.rows;i++){
        for(int j = 0;j<hightemp.cols;j++){
            kmat.at<float>(i,j) =(high-low)/(hightemp.at<float>(i,j)-lowtemp.at<float>(i,j));
            //            bmat.at<float>(i,j) = low - lowtemp.at<ushort>(i,j)*kmat.at<float>(i,j);
            bmat.at<float>(i,j) = lowtemp.at<float>(i,j);
        }
    }
    kmean = cv::mean(kmat)[0];
    base = low;
    ready =true;
}

void twoPoint::savek()
{
    if(ready)
        cv::imwrite("k.tiff",kmat);
}

void twoPoint::saveb()
{
    if(ready)
        cv::imwrite("b.tiff",bmat);
}

void twoPoint::savelow()
{
    ready = false;
    b_savelow = true;
    n_savelow = 0;
}

void twoPoint::savehigh()
{
    ready = false;
    b_savehigh = true;
    n_savehigh = 0;
}

void twoPoint::on(cv::Size s)
{
    if(kmat.empty()|| bmat.empty())
    {
        kmat = cv::Mat(s,CV_32FC1);
        bmat = cv::Mat(s,CV_32FC1);
        for(int i = 0;i<s.height;i++){
            for(int j = 0;j<s.width;j++){
                kmat.at<float>(i,j) = 1;
                bmat.at<float>(i,j) = 0;
            }
        }
        base = 0;
    }
    ready = true;
}

void twoPoint::off()
{
    ready = false;
}

void twoPoint::loaddata()
{
    kmat = cv::imread("k.tiff",cv::IMREAD_UNCHANGED);
    bmat = cv::imread("b.tiff",cv::IMREAD_UNCHANGED);
    if(kmat.empty() || bmat.empty()){
        return;
    }
    ready = true;
}

void twoPoint::setdim(int value)
{
    vardim = value;
}

void twoPoint::savedata()
{
    c+=1;
    cv::imwrite( "high"+QString::number(c).toStdString()+".tiff",hightemp);
    cv::imwrite("low"+QString::number(c).toStdString()+".tiff",lowtemp);
}

void twoPoint::dbon()
{
    bdb = true;
}

void twoPoint::dboff()
{
    bdb = false;
}

void twoPoint::setpara(double range)
{
    this->dbrange = range;
}

void twoPoint::saveData(cv::Mat img)
{
    cv::Mat temp;
    img.copyTo(temp);
    datas[cv::mean(img)[0]] = temp;
}

void twoPoint::calcData()
{
    double v0 =  datas.begin().key();
    double vn = datas.end().key();
    stagecount = datas.size();


    kstages = (double*)malloc(sizeof(double)*512*640*(stagecount+1));
    bstages = (double*)malloc(sizeof(double)*512*640*(stagecount+1));

    QMap<double,cv::Mat>::Iterator start = datas.begin();
    QMap<double,cv::Mat>::Iterator end = start++;
    memset(kstages,0,sizeof(double)*512*640);
    memset(bstages,0,sizeof(double)*512*640);
    int k = 1;
    for(;start!=datas.end();start++,end++,k++){
        double vs = start.key();
        double ve = end.key();
        vs = (vs-v0)/(vn-v0)*65535;
        ve = (ve-v0)/(vn-v0)*65535;

        cv::Mat ms = start.value();
        cv::Mat me = end.value();

        double* ks = kstages+k*512*640;
        double* bs = bstages+k*512*640;

        /// y = kx+b
        /// vs = k*x0+b
        /// ve = k*x1+b
        /// k = (ve-vs)/(x1-x0)
        /// b = vs-k*x0
        ///

        for(int i = 0;i<ms.rows;i++){
            for(int j = 0;j< ms.cols;j++){
                *(ks+i*ms.cols+j) = (ve-vs)/(me.at<ushort>(i,j) - ms.at<ushort>(i,j));
                *(bs+i*ms.cols+j) = vs - k*ms.at<ushort>(i,j);
            }
        }

    }

}

void twoPoint::runData(cv::Mat img)
{
    cv::Mat dst(img.rows,img.cols,img.type());
    for(int i = 0; i< img.rows;i++){
        for(int j = 0; j< img.cols;j++){
            int k = 0;
            for(; k<stagecount;k++)
            {
                if( img.at<ushort>(i,j) <  stages[k*img.rows*img.cols+i*img.cols+j])
                    break;
            }
            dst.at<ushort>(i,j) = kstages[k*img.rows*img.cols+i*img.cols+j]*img.at<ushort>(i,j)+ bstages[k*img.rows*img.cols+i*img.cols+j];
        }
    }

}

void twoPoint::meanmat(cv::Mat img)
{
    if(n_save == 32){
        img.convertTo(msave,CV_32FC1,1.0/32.0);
    }
    else if(n_save>0){
        for(int i = 0;i<img.rows;i++){
            for(int j = 0;j<img.cols;j++){
                msave.at<float>(i,j) += img.at<ushort>(i,j)/32.0;
            }
        }

    }
    else{
        saveData(msave);
        b_save = false;
    }
    n_save --;
}

void twoPoint::save()
{
    n_save = 32;

    b_save = true;
}

void twoPoint::slotprocimg(cv::Mat img)
{


    if(b_save)
        meanmat(img);

    if(b_savelow){
        addlow(img,n_savelow);
        n_savelow+=1;
    }
    if(b_savehigh){
        addhigh(img,n_savehigh);
        n_savehigh += 1;
    }
    if(b_onepoint & (opnum>0)){
        onepoint(img);
        //        b_onepoint = false;
    }
    if(ready){
        cv::Mat dst = cv::Mat(img.rows,img.cols,CV_16UC1);
        for(int i =0 ;i< img.rows;i++){
            for (int j = 0;j< img.cols;j++){
                int temp = kmat.at<float>(i,j)*(img.at<ushort>(i,j)-bmat.at<float>(i,j))+base;
                if(temp<low) temp = low;
//                if(temp>65535) temp = 65535;
                if(temp>high) temp = high;
                dst.at<ushort>(i,j) = temp;
            }
        }
        if(bdb){
            for(int i =0 ;i< img.rows;i++){
                for (int j = 1;j< img.cols;j++){

                    if((kmat.at<float>(i,j)/kmean-dbrange<1) && (kmat.at<float>(i,j)/kmean+dbrange>1))
                        continue;
                    dst.at<ushort>(i,j) = dst.at<ushort>(i,j-1);
                }
            }
        }
        emit signalsendimg(dst);
    }
    else{
        cv::Mat temp;
        img.copyTo(temp);
        emit signalsendimg(temp);
    }
}