#include "AbstractFSController.h"
#include "ZZ_Math_HDRONLY.h"
#include <math.h>

void Delay_MSec(ZZ_U16 usMS) {
    QEventLoop qeLoop;
    QTimer::singleShot(usMS, &qeLoop, SLOT(quit()));
    qeLoop.exec();
}

CAbstractFSController::CAbstractFSController(QObject *parent /*= nullptr*/) {
    iFlagInit = 0;
    m_pFSCtrl = NULL;
    m_iThreadID = -1;

    m_vecDataFrameDark.clear();
    m_vecDataFrameSignal.clear();

    m_qstrCalFilePath = "/home/data/Cal";

    m_vecNonLinearCalP.clear();
    for (int i = 0; i < 10; i++) {
        m_taginfolast.ITtimeChange[i] = true;
        m_taginfolast.LastMAXValue[i] = 0;
        m_taginfolast.lastITTIME[i] = 0;
    }
}

CAbstractFSController::~CAbstractFSController() {
    if (m_pFSCtrl != 0) {
        delete m_pFSCtrl;
    }
}

int CAbstractFSController::SetRunParas(int iThreadID, FSInfo fsInfo) {
    connect(this, &CAbstractFSController::SignalInit_Self, this, &CAbstractFSController::InitializeFSControl);
    m_iThreadID = iThreadID;
    m_fsInfo = fsInfo;
    return 0;
}


int CAbstractFSController::InitializeFSControl() {
    using namespace ZZ_MISCDEF::IRIS;

    int iRes = 0;

    if (m_iThreadID == -1/*|| m_iDeviceType == -1*/) {
        qDebug() << "Params Err. Call SetRunParas first";
        return 1;
    }
    switch (m_fsInfo.ucDeviceModel) {
        case DeviceModel::OSIFAlpha:
            m_pFSCtrl = new OceanOptics_lib;
            if (m_pFSCtrl->Initialize(false, m_fsInfo.strInterface, m_fsInfo.strSN) != 0) {
                qDebug() << "OSIFAlpha Not Opened";
                return 2;
            }
            iRes = LoadQEProLinearCalibrationFile();
            if (iRes != 0) {
                qDebug() << "LoadQEProLinearCalibrationFile Failed" << iRes;
                //return 5;
            }
            break;
        case DeviceModel::OSIFBeta:
            m_pFSCtrl = new OceanOptics_lib;
            if (m_pFSCtrl->Initialize(false, m_fsInfo.strInterface, m_fsInfo.strSN) != 0) {
                qDebug() << "OSIFBeta Not Opened";
                return 2;
            }
            iRes = LoadQEProLinearCalibrationFile();
            if (iRes != 0) {
                qDebug() << "LoadQEProLinearCalibrationFile Failed" << iRes;
                //return 5;
            }
            break;
        case DeviceModel::ISIF:
            m_pFSCtrl = new ZZ_ATPControl_Serial_Qt;
            //m_pFSCtrl->Initialize(false, m_fsInfo.strInterface, NULL);
            if (m_pFSCtrl->Initialize(false, m_fsInfo.strInterface, m_fsInfo.strSN) != 0) {
                qDebug() << "ISIF Not Opened";
                return 3;
            }
            break;
        case DeviceModel::IS1:
            m_pFSCtrl = new ZZ_ATPControl_Serial_Qt;
            //m_pFSCtrl->Initialize(false, m_fsInfo.strInterface, NULL);
            if (m_pFSCtrl->Initialize(false, m_fsInfo.strInterface, m_fsInfo.strSN) != 0) {
                qDebug() << "IS1 Not Opened";
                return 3;
            }
            break;
        case DeviceModel::IS2:
            m_pFSCtrl = new ZZ_ATPControl_Serial_Qt;
            //m_pFSCtrl->Initialize(false, m_fsInfo.strInterface, NULL);
            if (m_pFSCtrl->Initialize(false, m_fsInfo.strInterface, m_fsInfo.strSN) != 0) {
                qDebug() << "IS2 Not Opened";
                return 3;
            }
            break;
        default:
            break;
    }

    iRes = m_pFSCtrl->GetDeviceAttribute(m_daDeviceAttr);
    if (iRes != 0) {
        qDebug() << "GetDeviceAttribute Failed" << iRes;
        return 4;
    }

    iRes = m_pFSCtrl->SetDeviceTemperature(0);
    if (iRes != 0) {
        qDebug() << "SetDeviceTemperature Failed" << iRes;
        //return 5;
    }
    iFlagInit = 1;
    return 0;
}

int CAbstractFSController::InitializeFSControl_Self() {
    //InitializeFSControl();
    emit SignalInit_Self();
    return 0;
}

int CAbstractFSController::GetDeviceAttr(DeviceAttribute &daAttr) {
    daAttr = m_daDeviceAttr;
    return 0;
}

int CAbstractFSController::PerformAutoExposure(int position) {
    if (position != 0) {
        // m_taginfolast.ITtimeChange[position] = true;
        m_taginfolast.lastITTIME[position] = m_taginfolast.lastITTIME[0] * 0.5;
        m_pFSCtrl->SetExposureTime(m_taginfolast.lastITTIME[position]);
        return 0;
    }


    qDebug() << "--------------------------Starting PerformAutoExposure" << " Thread ID:" << m_iThreadID;
    using namespace ZZ_MATH;
    float fPredictedExposureTime;
    int iDeviceDepth = (int) m_fsInfo.lDepth;

    qDebug() << "MAX---Min" << m_fsInfo.fMaxFactor << "---" << m_fsInfo.fMinFactor << " Thread ID:" << m_iThreadID;

    bool bFlagIsOverTrying = false;
    bool bFlagIsLowerMinExposureTime = false;
    bool bFlagIsOverMaxExposureTime = false;
    bool bFlagIsAutoExposureOK = false;
    bool bFlagIsAutoExposureFailed = false;

    bool bIsValueOverflow = false;
    bool bIsLastValueOverflow = false;

    int iExposureTime = 1000;
    float fTempExposureTime = 0;
    double fLastExposureTime = 0.1;
    int iRepeatCount = 0;

    //int iRes = m_pFSCtrl->SetExposureTime(1000);//need change to load from files
    int iRes = 0;
    if (iRes != 0) {
        qDebug() << "Err:PerformAutoExposure Failed.Exit Code:1" << " Thread ID:" << m_iThreadID;
        return 1;
    }

    while (!bFlagIsAutoExposureOK && !bFlagIsAutoExposureFailed) {
        DataFrame dfTemp;

        if (iRepeatCount++ > 30) {
            bFlagIsAutoExposureFailed = true;
            bFlagIsOverTrying = true;
            break;
        }
        //m_pFSCtrl->SetExposureTime(5000);
        // m_pFSCtrl->GetExposureTime(iExposureTime);
        // qDebug() << "Current ExpTime:" << iExposureTime << " Thread ID:" << m_iThreadID;
        m_pFSCtrl->SetExposureTime(iExposureTime);
        //fExposureTime = (float)m_daDeviceAttr.iMinIntegrationTimeInMS;
        fTempExposureTime = iExposureTime;

        iRes = m_pFSCtrl->SingleShot(dfTemp);
        //iRes = m_pFSCtrl->SingleShot(dfTemp);
        if (iRes != 0) {
            qDebug() << "Err:PerformAutoExposure Failed.Exit Code:2" << " Thread ID:" << m_iThreadID;
            return 2;
        }

        HeapSort(dfTemp.lData, m_daDeviceAttr.iPixels);

        double dSum = 0;
        int iCount = m_daDeviceAttr.iPixels / 200;
        for (int i = 0; i < iCount; i++) {
            dSum += dfTemp.lData[i];
        }
        double dTemp = dSum / iCount;

        qDebug() << "Avg " << dTemp << " Thread ID:" << m_iThreadID;

        if (dTemp >= iDeviceDepth * 0.99) {
            bIsValueOverflow = true;
            if (!bIsLastValueOverflow) {
                iExposureTime = (float) (fLastExposureTime + iExposureTime) / 2;
            } else {
                iExposureTime = iExposureTime / 2;
            }
        } else if (iDeviceDepth * m_fsInfo.fMaxFactor >= dTemp && dTemp >= iDeviceDepth * m_fsInfo.fMinFactor) {
            qDebug() << "trace bFlagIsAutoExposureOK =1 " << iExposureTime << " Thread ID:" << m_iThreadID;
            bFlagIsAutoExposureOK = 1;
        } else if (dTemp > iDeviceDepth * m_fsInfo.fMaxFactor) {
            bIsValueOverflow = true;
            if (!bIsLastValueOverflow) {
                iExposureTime = (float) (fLastExposureTime + iExposureTime) / 2;
            } else {
                iExposureTime = iExposureTime * 3 / 4;
            }
        } else if (dTemp < iDeviceDepth * m_fsInfo.fMinFactor) {
            bIsValueOverflow = false;
            if (bIsLastValueOverflow) {
                iExposureTime = (float) (fLastExposureTime + iExposureTime) / 2;
            } else {
                double dFactor;
                dFactor = dTemp / (iDeviceDepth * m_fsInfo.fMaxFactor);
                iExposureTime = (float) (iExposureTime / dFactor);
            }
        }
        bIsLastValueOverflow = bIsValueOverflow;
        fLastExposureTime = fTempExposureTime;


        if (/*fExposureTime > 100 || */iExposureTime <= m_daDeviceAttr.iMinIntegrationTimeInMS) {
            bFlagIsAutoExposureOK = false;
            bFlagIsAutoExposureFailed = true;
            bFlagIsLowerMinExposureTime = true;

            // 			qDebug() << "Warning:PerformAutoExposure lower than min integration time.Will be limited to " << m_daDeviceAttr.iMinIntegrationTimeInMS - 1 << "MS" << " Thread ID:" << m_iThreadID;
            // 			iRes = m_pFSCtrl->SetExposureTime((int)iExposureTime);
            // 			if (iRes != 0)
            // 			{
            // 				qDebug() << "Err:PerformAutoExposure Failed.Exit Code:4" << " Thread ID:" << m_iThreadID;
            // 				return 3;
            // 			}
            // 			else
            // 			{
            // 				qDebug() << "Success:PerformAutoExposure. Value" << iExposureTime << " Thread ID:" << m_iThreadID;
            // 			}
            m_taginfolast.ITtimeChange[position] = true;
            m_taginfolast.lastITTIME[position] = m_daDeviceAttr.iMinIntegrationTimeInMS;
            iRes = m_pFSCtrl->SetExposureTime(m_daDeviceAttr.iMinIntegrationTimeInMS);
            if (iRes != 0) {
                qDebug() << "Err:PerformAutoExposure Failed.Exit Code:3" << " Thread ID:" << m_iThreadID;
                return 3;
            } else {
                qDebug() << "Warning:PerformAutoExposure lower than min integration time.Will be limited to " <<
                        m_daDeviceAttr.iMinIntegrationTimeInMS << "MS" << " Thread ID:" << m_iThreadID;
            }

            break;
        }

        if (iExposureTime > m_daDeviceAttr.iMaxIntegrationTimeInMS - 1) {
            bFlagIsAutoExposureOK = false;
            bFlagIsAutoExposureFailed = true;
            bFlagIsOverMaxExposureTime = true;
            //float fPredictedExposureTime = m_daDeviceAttr.iMaxIntegrationTimeInMS-1;
            //iRes = m_pFSCtrl->SetExposureTime(m_daDeviceAttr.iMaxIntegrationTimeInMS-1);
            //if (iRes != 0)
            //{
            //qDebug() << "Err:PerformAutoExposure Failed.Exit Code:3" << " Thread ID:" << m_iThreadID;
            //return 3;
            //}
            //else
            //{
            //qDebug() << "Warning:PerformAutoExposure exceed max integration time.Will be limited to 30sec";
            //}
            m_taginfolast.ITtimeChange[position] = true;
            m_taginfolast.lastITTIME[position] = m_daDeviceAttr.iMaxIntegrationTimeInMS - 1;
            iRes = m_pFSCtrl->SetExposureTime(m_daDeviceAttr.iMaxIntegrationTimeInMS - 1);
            if (iRes != 0) {
                qDebug() << "Err:PerformAutoExposure Failed.Exit Code:3" << " Thread ID:" << m_iThreadID;
                return 3;
            } else {
                qDebug() << "Warning:PerformAutoExposure exceed max integration time.Will be limited to " <<
                        m_daDeviceAttr.iMaxIntegrationTimeInMS - 1 << "MS" << " Thread ID:" << m_iThreadID;
            }

            break;
        }
        m_taginfolast.ITtimeChange[position] = true;
        m_taginfolast.lastITTIME[position] = (int) iExposureTime;
        iRes = m_pFSCtrl->SetExposureTime((int) iExposureTime);
        if (iRes != 0) {
            qDebug() << "Err:PerformAutoExposure Failed.Exit Code:4" << " Thread ID:" << m_iThreadID;
            return 3;
        } else {
            qDebug() << "Success:PerformAutoExposure. Value" << iExposureTime << " Thread ID:" << m_iThreadID;
        }
    }

    fPredictedExposureTime = iExposureTime;
    qDebug() << "--------------------------Stop PerformAutoExposure" << " Thread ID:" << m_iThreadID;
    //emit SignalAcqFinished(m_iThreadID, 1);

    return 0;
}

int CAbstractFSController::ComputExposure(int position) {
    if (position == 0) {
        int lastittime = m_taginfolast.lastITTIME[position];
        int lastmaxvalue = m_taginfolast.LastMAXValue[position];

        int maxallowvalue = m_fsInfo.lDepth * m_fsInfo.fMaxFactor;
        int minallowvalue = m_fsInfo.lDepth * m_fsInfo.fMinFactor;
        if (lastmaxvalue > maxallowvalue) {
            if (lastmaxvalue >= m_fsInfo.lDepth) {
                lastittime = lastittime / 2;
            } else {
                lastittime = lastittime * (maxallowvalue * 1.0 / lastmaxvalue) * 0.95;
            }

            m_taginfolast.ITtimeChange[position] = true;
            m_taginfolast.lastITTIME[position] = lastittime;
            m_pFSCtrl->SetExposureTime(lastittime);
            qDebug() << "Set exposure time for position" << position << " to " << lastittime;
            return lastittime;
        }
        if (lastmaxvalue < minallowvalue) {
            lastittime = lastittime * (minallowvalue * 1.0 / lastmaxvalue) * 1.05;
            if (lastittime >= 60000) {
                lastittime = 60000;
            }
            m_taginfolast.ITtimeChange[position] = true;
            m_taginfolast.lastITTIME[position] = lastittime;
            m_pFSCtrl->SetExposureTime(lastittime);
            qDebug() << "Set exposure time for position" << position << " to " << lastittime;
            return lastittime;
        }
        m_taginfolast.ITtimeChange[position] = false;
        m_pFSCtrl->SetExposureTime(lastittime);
        qDebug() << "Set exposure time for position" << position << " to " << lastittime;
        return lastittime;
    }
    int lasttimeindev = 0;
    m_pFSCtrl->GetExposureTime(lasttimeindev);
    float scalofit = 1.5;
    m_pFSCtrl->SetExposureTime(m_taginfolast.lastITTIME[0] * scalofit);
    if (lasttimeindev != m_taginfolast.lastITTIME[0] * scalofit) {
        Delay_MSec(lasttimeindev);
    }
    qDebug() << "Set exposure time for position" << position << " to " << m_taginfolast.lastITTIME[0] * scalofit;
    return m_taginfolast.lastITTIME[0] * scalofit;
}

int CAbstractFSController::TakeDarkFrame(int position) {
    int shuttertime = 0;
    m_pFSCtrl->GetExposureTime(shuttertime);
    qDebug() << "Starting TakeDarkFrame" << " position ID:" << position << " ITtime:" << shuttertime << "\n";
    if (m_taginfolast.ITtimeChange[0]) {
        m_taginfolast.LastDarkframe[position] = TakeOneFrame();
    }


    m_vecDataFrameDark.push_back(m_taginfolast.LastDarkframe[position]);


    qDebug() << "Stop TakeDarkFrame" << " Thread ID:" << m_iThreadID;
    //emit SignalAcqFinished(m_iThreadID, 1);
    return 0;
}


int CAbstractFSController::ComputeMaxValue(DataFrame Data) {
    ZZ_S32 *pData = Data.lData;
    int number = 4096;
    int maxvalue = 0;
    for (int i = 0; i < number - 1; i++) {
        if (pData[i] > maxvalue) {
            maxvalue = pData[i];
        }
    }
    return maxvalue;

    // if (number <= 10) {
    // 	// 如果数据量小，直接排序计算
    // 	int sum = 0;
    // 	for (int i = 0; i < number; i++) {
    // 		sum += pData[i];
    // 	}
    // 	return number > 0 ? sum / number : 0;
    // }
    //
    // // 复制数据（或者直接操作原数组如果允许修改）
    // std::vector<int> temp(pData, pData + number);
    //
    // // 使用nth_element找到第10大的元素
    // std::nth_element(temp.begin(), temp.begin() + 9, temp.end(), std::greater<int>());
    //
    // // 计算前10个最大值的平均
    // int sum = 0;
    // for (int i = 0; i < 10; i++) {
    // 	sum += temp[i];
    // }
    //
    // return sum / 10;


    // int maxvalue[10]={0};
    // ZZ_S32 *pData=Data.lData;
    // int number=4096;
    // //获取pdata中最大的10个值
    // for (int i=0;i<number;i++) {
    // 	int val=pData[i];
    // 	for (int j=0;j<10;j++) {
    // 		if (val>maxvalue[j]) {
    // 			//插入到maxvalue中
    // 			for (int k=9;k>j;k--) {
    // 				maxvalue[k]=maxvalue[k-1];
    // 			}
    // 			maxvalue[j]=val;
    // 			break;
    // 		}
    // 	}
    // }
}

int CAbstractFSController::TakeSignalFrame(int position) {
    int shuttertime = 0;
    m_pFSCtrl->GetExposureTime(shuttertime);
    qDebug() << "Starting TakeSignal" << " position" << position << " ITtime:" << shuttertime << "\n";

    m_vecDataFrameSignal.push_back(TakeOneFrame());
    // todo: compute max ten value 计算前10个值平均值
    m_taginfolast.LastMAXValue[position] = ComputeMaxValue(m_vecDataFrameSignal.at(m_vecDataFrameSignal.size() - 1));
    if (m_taginfolast.LastMAXValue[position] >= 65535) {
        m_vecDataFrameSignal.at(m_vecDataFrameSignal.size() - 1).bIsValid = false;
    }
    qDebug() << "Stop TakeSignal" << " Thread ID:" << m_iThreadID;
    qDebug() << "Max 10 Average Value:" << m_taginfolast.LastMAXValue[position] << " Thread ID:" << m_iThreadID;
    //emit SignalAcqFinished(m_iThreadID, 1);
    return 0;
}

DataFrame CAbstractFSController::TakeOneFrame() {
    using namespace ZZ_MISCDEF::IRIS;
    //int iExpTime = 0;
    DataFrame dfTemp;
    // 	m_pFSCtrl->GetExposureTime(iExpTime);
    // 	dfTemp.usExposureTimeInMS = iExpTime;
    // 	m_pFSCtrl->GetDeviceTemperature(dfTemp.fTemperature);

    if (m_fsInfo.ucDeviceModel == DeviceModel::ISIF) {
        float fTemp;
        m_pFSCtrl->GetDeviceTemperature(fTemp);
        dfTemp.fTemperature = fTemp;
    } else if (m_fsInfo.ucDeviceModel == DeviceModel::IS1) {
        dfTemp.fTemperature = 0;
    }

    int iRes = m_pFSCtrl->SingleShot(dfTemp);
    if (iRes != 0) {
        qDebug() << "Err. SingleShot" << " Thread ID:" << m_iThreadID;
    }

    if (m_fsInfo.ucDeviceModel == DeviceModel::OSIFAlpha || m_fsInfo.ucDeviceModel == DeviceModel::OSIFBeta) {
        if (m_vecNonLinearCalP.size() != 8) {
            qDebug() << "Err.Non Linear calibration parameters not fit.Skip..." << " Thread ID:" << m_iThreadID;
            return dfTemp;
        }
        for (int i = 0; i < m_daDeviceAttr.iPixels; i++) {
            dfTemp.lData[i] = dfTemp.lData[i] / (m_vecNonLinearCalP[0] +
                                                 m_vecNonLinearCalP[1] * dfTemp.lData[i] +
                                                 m_vecNonLinearCalP[2] * pow(dfTemp.lData[i], 2) +
                                                 m_vecNonLinearCalP[3] * pow(dfTemp.lData[i], 3) +
                                                 m_vecNonLinearCalP[4] * pow(dfTemp.lData[i], 4) +
                                                 m_vecNonLinearCalP[5] * pow(dfTemp.lData[i], 5) +
                                                 m_vecNonLinearCalP[6] * pow(dfTemp.lData[i], 6) +
                                                 m_vecNonLinearCalP[7] * pow(dfTemp.lData[i], 7)
                              );
        }
    }


    return dfTemp;
    // 	DataFrame dfTemp;
    // 	int iRes = m_pFSCtrl->SingleShot(dfTemp);
    // 	if (iRes != 0)
    // 	{
    // 		qDebug() << "Err. SingleShot" << " Thread ID:" << m_iThreadID;
    // 	}
    //
    // 	return dfTemp;
}


int CAbstractFSController::SaveDataFile() {
    return 0;
}

int CAbstractFSController::LoadQEProLinearCalibrationFile() {
    m_vecNonLinearCalP.clear();

    QDir qdirPath(m_qstrCalFilePath);
    if (!qdirPath.exists()) {
        qDebug() << "Non-Linear Calibration Folder not exist" << " Thread ID:" << m_iThreadID;
        return 1;
    }
    QString qstrFilePath;

    qstrFilePath = m_qstrCalFilePath + QString("/") + QString::fromStdString(m_fsInfo.strSN) + QString(".NLC");

    QFile qfCalFile(qstrFilePath);
    bool bRes = qfCalFile.open(QFile::ReadOnly);
    if (!bRes) {
        qDebug() << "Non-Linear Calibration File open Failed" << " Thread ID:" << m_iThreadID;
        return 2;
    }

    while (!qfCalFile.atEnd()) {
        QByteArray qbData = qfCalFile.readLine();
        qbData.remove(qbData.size() - 1, 1);
        m_vecNonLinearCalP.push_back(qbData.toDouble());
        //qDebug() << qbData;
    }
    qfCalFile.close();

    qDebug() << "Non-Linear Calibration Params:" << m_vecNonLinearCalP.size() << " Thread ID:" << m_iThreadID;


    return 0;
}

int CAbstractFSController::StartAcquisitionSignal(int position) {
    //
    qDebug() << "Starting acq Signal" << " Thread ID:" << m_iThreadID;

    // 	DataFrame struDF;
    // 	int iii;
    // 	m_pFSCtrl->SetExposureTime(10000000);
    // 	m_pFSCtrl->GetExposureTime(iii);
    // 	m_pFSCtrl->SingleShot(struDF);

    if (m_taginfolast.lastITTIME[position] == 0) {
        PerformAutoExposure(position);
    } else {
        ComputExposure(position);
    }


    TakeSignalFrame(position);

    qDebug() << "Stop acq Signal" << " Thread ID:" << m_iThreadID;
    emit SignalAcqFinished_Signal(m_iThreadID, 1);
    return 0;
}

int CAbstractFSController::StartAcquisitionDark(int position) {
    qDebug() << "Starting acq Dark" << " Thread ID:" << m_iThreadID;
    TakeDarkFrame(position);
    qDebug() << "Stop acq Dark" << " Thread ID:" << m_iThreadID;
    emit SignalAcqFinished_Dark(m_iThreadID, 1);

    return 0;
}

int CAbstractFSController::StopAcquisition() {
    return 0;
}

int CAbstractFSController::ClearBuffer() {
    m_vecDataFrameDark.clear();
    m_vecDataFrameSignal.clear();
    return 0;
}

int CAbstractFSController::GetBuffer(std::vector<DataFrame> &pvecDataFrameDark,
                                     std::vector<DataFrame> &pvecDataFrameSignal) {
    for (size_t i = 0; i < m_vecDataFrameSignal.size(); i++) {
        pvecDataFrameSignal.push_back(m_vecDataFrameSignal[i]);
        pvecDataFrameDark.push_back(m_vecDataFrameDark[i]);
    }
    return 0;
}