#include "getTransmissionData.h"
#include "Function.h"
#include "Function1D.h"
#include "Function2D.h"
#include "common/dataBlockCreation/removeDataFromArrays.h"
#include "log/notify.h"
#include "src//config/config.h"
#include "src/common/getGeometryInfo.h"
#include "src/common/temperatureCalculation/extractTasTemperature.h"
#include "src/common/dataBlockCreation/getAScanBlockPreprocessed.h"
#include "src/common/precalculateChannelList.h"
#include "src/transmissionReconstruction/dataFilter/sensitivityCalculations.h"
#include "src/transmissionReconstruction/dataFilter/estimateNoiseValueFromAScans.h"

#include "Matrix.h"
#include "Function3D.h"
#include "transmissionReconstruction/dataFilter/dataFilter.h"
#include "transmissionReconstruction/dataPreperation.h"
#include "transmissionReconstruction/detection/detection.h"
#include <cstddef>
#include <mkl_cblas.h>

#include <iostream>
#include <mkl_vml_functions.h>
#include <map>
#include <string>
#include <thread>
#include <mutex>
#include <condition_variable>

using namespace Recon;
using namespace Aurora;

namespace 
{
    struct BlockOfTransmissionData
    {
        Matrix tofData;
        Matrix attData;
        Matrix senderBlock;
        Matrix receiverBlock;
        Matrix waterTempBlock;
        MetaInfos metaInfos;

        Matrix ascanBlock;
        Matrix ascanBlockRef;
        Matrix dists;
        Matrix distRefBlock;
        Matrix waterTempRefBlock;
    };

    std::map<std::string, BlockOfTransmissionData> BLOCK_OF_TRANSIMISSIONDARA_BUFFER;
    std::mutex CREATE_BUFFER_MUTEX;
    std::condition_variable CREATE_BUFFER_CONDITION;

    std::mutex PROCESS_BUFFER_MUTEX;
    std::condition_variable PROCESS_BUFFER_CONDITION;
    int BUFFER_COUNT = 0;
    int BUFFER_SIZE = 3;

    Matrix prepareAScansForTransmissionDetection(const Matrix& aAscanBlock, const Matrix& aGainBlock)
    {
        Matrix result = aAscanBlock / repmat(aGainBlock, aAscanBlock.getDimSize(0), 1);
        result = result - repmat(mean(result,FunctionDirection::Column), result.getDimSize(0), 1);
        return result;
    }

    BlockOfTransmissionData getBlockOfTransmissionData(const Matrix& aMp, const Matrix& aMpRef, const Matrix& aSl, const Matrix& aSn, const Matrix& aRlList, const Matrix& aRnList,
                                    const TasTemps& aTasTemps, const Matrix& aExpectedSOSWater, GeometryInfo aGeom, GeometryInfo& aGeomRef,
                                    const Matrix& aSnrRmsNoise, const Matrix& aSnrRmsNoiseRef, const MeasurementInfo& aExpInfo, const MeasurementInfo& aExpInfoRef,
                                    const PreComputes& aPreComputes, Parser* aParser, Parser* aParserRef)
    {
        BlockOfTransmissionData result;
        MetaInfos metaInfos;
        auto blockData = getAscanBlockPreprocessed(aParser, aMp, aSl, aSn, aRlList, aRnList, aGeom, aExpInfo, true, true);
        auto blockDataRef = getAscanBlockPreprocessed(aParserRef, aMpRef, aSl, aSn, aRlList, aRnList, aGeomRef, aExpInfoRef, true, true);
        Matrix ascanBlock = prepareAScansForTransmissionDetection(blockData.ascanBlockPreprocessed, blockData.gainBlock);
        Matrix ascanBlockRef = prepareAScansForTransmissionDetection(blockDataRef.ascanBlockPreprocessed, blockDataRef.gainBlock);
        blockData.ascanBlockPreprocessed = Matrix();
        blockDataRef.ascanBlockPreprocessed = Matrix();
        if(aExpInfo.Hardware == "USCT3dv3")
        {
            Matrix channelList = precalculateChannelList(aRlList, aRnList, aExpInfo, aPreComputes);
            double* channelListSizeData = Aurora::malloc(2);
            channelListSizeData[0] = channelList.getDimSize(0);
            channelListSizeData[1] = channelList.getDimSize(1);
            Matrix channelListSize = Matrix::New(channelListSizeData, 2, 1);
            Matrix ind = sub2ind(channelListSize, {blockData.rlBlock, blockData.rnBlock});
            size_t channelListBlockSize = ind.getDataSize();
            double* channelListBlockData = Aurora::malloc(channelListBlockSize);
            for(size_t i=0; i<channelListBlockSize; ++i)
            {
                channelListBlockData[i] = channelList[ind[i] - 1];
            }
            Matrix channelListBlock = Matrix::New(channelListBlockData, 1, channelListBlockSize);
            Matrix fx = fft(ascanBlock);
            double* fhData =  Aurora::malloc(aExpInfo.matchedFilter.getDimSize(0) * channelListBlockSize, true);
            Matrix fh = Matrix::New(fhData, aExpInfo.matchedFilter.getDimSize(0), channelListBlockSize, 1, Aurora::Complex);
            size_t matchedFilterRowDataSize = aExpInfo.matchedFilter.getDimSize(0)*2;
            for(size_t i=0; i<channelListBlockSize; ++i)
            {
                cblas_dcopy(matchedFilterRowDataSize, aExpInfo.matchedFilter.getData() + (size_t)(channelListBlock[i] - 1) * matchedFilterRowDataSize, 1 , fhData ,1);
                fhData += matchedFilterRowDataSize;
            }
            // Matrix fxReal = Aurora::real(fx);
            // Matrix fhReal = Aurora::real(fh);
            // Matrix fxImag = Aurora::imag(fx);
            // Matrix fhImag = Aurora::imag(fh);
            // Matrix real = fxReal * fhReal + fxImag * fhImag;
            // Matrix image =  fxImag * fhReal - fxReal * fhImag;
            double* value1 = Aurora::malloc(fx.getDataSize());
            vdMulI(fx.getDataSize(), fx.getData(), 2, fh.getData(), 2, value1, 1);
            double* value2 = Aurora::malloc(fx.getDataSize());
            vdMulI(fx.getDataSize(), fx.getData() + 1, 2, fh.getData() + 1, 2, value2, 1);
            double* realData = Aurora::malloc(fx.getDataSize());
            vdAdd(fx.getDataSize(), value1, value2, realData);
            Matrix real = Matrix::New(realData, fx.getDimSize(0), fx.getDimSize(1));

            vdMulI(fx.getDataSize(), fx.getData() + 1, 2, fh.getData(), 2, value1, 1);
            vdMulI(fx.getDataSize(), fx.getData(), 2, fh.getData() + 1, 2, value2, 1);
            double* imagData = Aurora::malloc(fx.getDataSize());
            vdSub(fx.getDataSize(), value1, value2, imagData);
            Matrix image = Matrix::New(imagData, fx.getDimSize(0), fx.getDimSize(1));

            double* complexData = Aurora::malloc(real.getDataSize(), true);
            cblas_dcopy(real.getDataSize(), real.getData(), 1 , complexData ,2);
            cblas_dcopy(image.getDataSize(), image.getData(), 1 , complexData + 1 ,2);
            Matrix complex = Matrix::New(complexData, real.getDimSize(0), real.getDimSize(1), 1, Aurora::Complex);
            ascanBlock = Aurora::real(ifft(complex));

            fx = fft(ascanBlockRef);
            fhData =  Aurora::malloc(aExpInfoRef.matchedFilter.getDimSize(0) * channelListBlockSize, true);
            fh = Matrix::New(fhData, aExpInfoRef.matchedFilter.getDimSize(0), channelListBlockSize, 1, Aurora::Complex);
            matchedFilterRowDataSize = aExpInfoRef.matchedFilter.getDimSize(0)*2;
            for(size_t i=0; i<channelListBlockSize; ++i)
            {
                cblas_dcopy(matchedFilterRowDataSize, aExpInfoRef.matchedFilter.getData() + (size_t)(channelListBlock[i] - 1) * matchedFilterRowDataSize, 1 , fhData ,1);
                fhData += matchedFilterRowDataSize;
            }
            //  real = Aurora::real(fx) * Aurora::real(fh) + Aurora::imag(fx) * Aurora::imag(fh);
            //  image =  Aurora::imag(fx) * Aurora::real(fh) - Aurora::real(fx) * Aurora::imag(fh);
            vdMulI(fx.getDataSize(), fx.getData(), 2, fh.getData(), 2, value1, 1);
            vdMulI(fx.getDataSize(), fx.getData() + 1, 2, fh.getData() + 1, 2, value2, 1);
            realData = Aurora::malloc(fx.getDataSize());
            vdAdd(fx.getDataSize(), value1, value2, realData);
            real = Matrix::New(realData, fx.getDimSize(0), fx.getDimSize(1));

            vdMulI(fx.getDataSize(), fx.getData() + 1, 2, fh.getData(), 2, value1, 1);
            vdMulI(fx.getDataSize(), fx.getData(), 2, fh.getData() + 1, 2, value2, 1);
            imagData = Aurora::malloc(fx.getDataSize());
            vdSub(fx.getDataSize(), value1, value2, imagData);
            image = Matrix::New(imagData, fx.getDimSize(0), fx.getDimSize(1));
            Aurora::free(value1);
            Aurora::free(value2);

            complexData = Aurora::malloc(real.getDataSize(), true);
            cblas_dcopy(real.getDataSize(), real.getData(), 1 , complexData ,2);
            cblas_dcopy(image.getDataSize(), image.getData(), 1 , complexData + 1 ,2);
            complex = Matrix::New(complexData, real.getDimSize(0), real.getDimSize(1), 1, Aurora::Complex);
            ascanBlockRef = Aurora::real(ifft(complex));
        }
        else
        {
            transParams::detectionWindowSOS = transParams::pulseLengthSamples;
            transParams::detectionWindowATT = transParams::pulseLengthSamples;
        }

        if(transParams::applyCalib)
        {
            metaInfos.snrValues = calculateSnr(ascanBlock, aSnrRmsNoise[0]);
            metaInfos.snrValuesRef = calculateSnr(ascanBlockRef, aSnrRmsNoiseRef[0]);
        }

        Matrix dists = distanceBetweenTwoPoints(blockData.senderPositionBlock, blockData.receiverPositionBlock);
        Matrix distRefBlock = distanceBetweenTwoPoints(blockDataRef.senderPositionBlock, blockDataRef.receiverPositionBlock);

        Matrix waterTempBlock = calculateWaterTemperature(aTasTemps.waterTempPreCalc_sl, aTasTemps.waterTempPreCalc_rl, blockData.slBlock, blockData.rlBlock, blockData.mpBlock);
        Matrix waterTempRefBlock  = calculateWaterTemperature(aTasTemps.waterTempRefPreCalc_sl, aTasTemps.waterTempRefPreCalc_rl, blockData.slBlock, blockData.rlBlock, blockDataRef.mpBlock);
        
        if(transParams::saveDetection || transParams::outlierOnTasDetection || transParams::saveDebugInfomation)
        {
            metaInfos.mpBlock = blockData.mpBlock;
            metaInfos.slBlock = blockData.slBlock;
            metaInfos.snBlock = blockData.snBlock;
            metaInfos.rlBlock = blockData.rlBlock;
            metaInfos.rnBlock = blockData.rnBlock;
        }
        result.metaInfos = metaInfos;
        result.senderBlock = blockData.senderPositionBlock;
        result.receiverBlock = blockData.receiverPositionBlock;
        result.waterTempBlock = waterTempBlock;

        result.ascanBlock = ascanBlock;
        result.ascanBlockRef = ascanBlockRef;
        result.dists = dists;
        result.distRefBlock = distRefBlock;
        result.waterTempRefBlock = waterTempRefBlock;
  
        // DetectResult detect = transmissionDetection(ascanBlock, ascanBlockRef, dists, distRefBlock, waterTempBlock, waterTempRefBlock, aExpectedSOSWater[0]);
        // result.attData = detect.att;
        // result.tofData = detect.tof;
        
        return result;
    }

}

void  getBlockOfTransmissionDataInThread(size_t aIndex, const Matrix& aMp, const Matrix& aMpRef, const Matrix& aSl, const Matrix& aSn, const Matrix& aRlList, const Matrix& aRnList,
                                         const TasTemps& aTasTemps, const Matrix& aExpectedSOSWater, GeometryInfo aGeom, GeometryInfo aGeomRef,
                                         const Matrix& aSnrRmsNoise, const Matrix& aSnrRmsNoiseRef, const MeasurementInfo& aExpInfo, const MeasurementInfo& aExpInfoRef,
                                         const PreComputes& aPreComputes, Parser* aParser, Parser* aParserRef)
{
    auto buffer = getBlockOfTransmissionData(aMp, aMpRef, aSl, aSn, aRlList, aRnList, aTasTemps, 
                                             aExpectedSOSWater, aGeom, aGeomRef, aSnrRmsNoise, aSnrRmsNoiseRef,
                                             aExpInfo, aExpInfoRef, aPreComputes, aParser, aParserRef);
    std::unique_lock<std::mutex> lock(CREATE_BUFFER_MUTEX); 
    BLOCK_OF_TRANSIMISSIONDARA_BUFFER[std::to_string(aIndex)] = buffer;
    std::cout<<"Add: "<<aIndex<<std::endl;
    lock.unlock();
    PROCESS_BUFFER_CONDITION.notify_one();
}

void  createThreadForGetBlockOfTransmissionData(const Matrix& aMotorPos, const Matrix& aMotoPosRef, const Matrix& aSlList, const Matrix& aSnList, const Matrix& aRlList, const Matrix& aRnList,
                                         const TasTemps& aTasTemps, const Matrix& aExpectedSOSWater, GeometryInfo aGeom, GeometryInfo aGeomRef,
                                         const Matrix& aSnrRmsNoise, const Matrix& aSnrRmsNoiseRef, const MeasurementInfo& aExpInfo, const MeasurementInfo& aExpInfoRef,
                                         const PreComputes& aPreComputes, Parser* aParser, Parser* aParserRef)
{
    size_t vectorSize = aMotorPos.getDataSize() * (aSlList.getDataSize() / transParams::senderTASSize) * (aSnList.getDataSize() / transParams::senderElementSize);
    std::thread speedUpThread[vectorSize];

    for(int i=0; i<aMotorPos.getDataSize(); ++i)
    {
        for(int j=0; j<aSlList.getDataSize() / transParams::senderTASSize; ++j)
        {
            for(int k=0; k<aSnList.getDataSize() / transParams::senderElementSize; ++k)
            {
                size_t index = i * (aSlList.getDataSize() / transParams::senderTASSize) * (aSnList.getDataSize() / transParams::senderElementSize) + 
                               j * (aSnList.getDataSize() / transParams::senderElementSize) + k;
                Matrix mp = aMotorPos(i).toMatrix();
                Matrix mpRef = aMotoPosRef(i).toMatrix();
                Matrix sl = aSlList.block(0, transParams::senderTASSize*j, transParams::senderTASSize*j+transParams::senderTASSize - 1);
                Matrix sn = aSnList.block(0, transParams::senderElementSize*k, transParams::senderElementSize*k+transParams::senderElementSize - 1);
                
                std::unique_lock<std::mutex> lock(CREATE_BUFFER_MUTEX); 
                CREATE_BUFFER_CONDITION.wait(lock, []{return BUFFER_COUNT<BUFFER_SIZE;});
                ++BUFFER_COUNT;
                lock.unlock();
                speedUpThread[index] =  std::thread(getBlockOfTransmissionDataInThread,index,mp,mpRef,sl,sn,aRlList,aRnList,aTasTemps,aExpectedSOSWater,aGeom,aGeomRef,aSnrRmsNoise,aSnrRmsNoiseRef,aExpInfo,aExpInfoRef,aPreComputes,aParser, aParserRef);
            }
        }
    }

    for(auto& t:speedUpThread)
    {
        t.join();
    }

}

TransmissionData Recon::getTransmissionData(const Aurora::Matrix& aMotorPos, const Aurora::Matrix& aMotoPosRef, const Aurora::Matrix& aSlList,
                                const Aurora::Matrix& aSnList, const Aurora::Matrix& aRlList, const Aurora::Matrix& aRnList,
                                const TempInfo& aTemp, const TempInfo& aTempRef, GeometryInfo& aGeom,
                                GeometryInfo& aGeomRef, const MeasurementInfo& aExpInfo, const MeasurementInfo& aExpInfoRef,
                                const PreComputes& aPreComputes, Parser* aParser, Parser* aParserRef)
{
    //推测是已经完成过透射重建并从完成的透射重建读取数据
    //暂不考虑此逻辑运行
    // if transParams.detection.forceRedetect == 0 && exist(transParams.pathSaveDetection, 'dir') && size(dir(transParams.pathSaveDetection), 1) > 2 % i.e. detection folder exists and is not empty
    //     % Load transmission detection data
    //     writeReconstructionLog('Loading transmission detection data. All available data from given motor positions are taken.', 1);
    // [tofDataTotal, attDataTotal, senderList, receiverList, waterTempList, dataInfo] = loadTransmissionDetectionData(transParams.pathSaveDetection, transParams.pathData, motorPos, expInfo.rootMeasUniqueID);
    TasTemps tasTemps;
    tasTemps.waterTempPreCalc_rl = extractTasTemperature(aTemp.tasTemperature, aRlList, aMotorPos, aTemp.jumoTemp, transParams::minTemperature, transParams::maxTemperature);
    tasTemps.waterTempPreCalc_sl = extractTasTemperature(aTemp.tasTemperature, aSlList, aMotorPos, aTemp.jumoTemp, transParams::minTemperature, transParams::maxTemperature);
    tasTemps.waterTempRefPreCalc_rl = extractTasTemperature(aTempRef.tasTemperature, aRlList, aMotoPosRef, aTempRef.jumoTemp, transParams::minTemperature, transParams::maxTemperature);
    tasTemps.waterTempRefPreCalc_sl = extractTasTemperature(aTempRef.tasTemperature, aSlList, aMotoPosRef, aTempRef.jumoTemp, transParams::minTemperature, transParams::maxTemperature);

    aGeom.sensData = precalcSensitivity(aSlList, aSnList, aRlList, aRnList, aMotorPos, aGeom);
    aGeomRef.sensData = aGeom.sensData;

    Matrix rmsNoise, rmsNoiseRef;
    if (transParams::applyCalib)
    {
        rmsNoise = estimateNoiseValueFromAScans(aSnList, aRnList, aGeom, aExpInfo, aParser);
        rmsNoiseRef = estimateNoiseValueFromAScans(aSnList, aRnList, aGeom, aExpInfo, aParserRef);
    }

    size_t numScans = aMotorPos.getDataSize() * aSlList.getDataSize() * aSnList.getDataSize() * aRlList.getDataSize() * aRnList.getDataSize();
    Matrix tofDataTotal = Matrix::fromRawData(new double[numScans], 1, numScans) + NAN;
    Matrix attDataTotal = Matrix::fromRawData(new double[numScans], 1, numScans) + NAN;
    Matrix waterTempList = zeros(1,numScans,1);
    Matrix senderList = zeros(3,numScans,1);
    Matrix receiverList = zeros(3,numScans,1);
    Matrix snrValues = zeros(1,numScans,1);
    Matrix snrValuesRef = zeros(1,numScans,1);

    Matrix mpBlockTotal;
    Matrix slBlockTotal;
    Matrix snBlockTotal;
    Matrix rlBlockTotal;
    Matrix rnBlockTotal;
    if(transParams::saveDetection || transParams::outlierOnTasDetection || transParams::saveDebugInfomation)
    {
        mpBlockTotal = zeros(1,numScans,1);
        slBlockTotal = zeros(1,numScans,1);
        snBlockTotal = zeros(1,numScans,1);
        rlBlockTotal = zeros(1,numScans,1);
        rnBlockTotal = zeros(1,numScans,1);
    }

    std::thread speedUpThread = std::thread(createThreadForGetBlockOfTransmissionData,aMotorPos,aMotoPosRef,aSlList,aSnList,aRlList,aRnList,tasTemps,aTemp.expectedSOSWater,aGeom,aGeomRef,rmsNoise,rmsNoiseRef,aExpInfo,aExpInfoRef,aPreComputes,aParser, aParserRef);
    int numData = 0;
    int numPossibleScans = 0;
    sched_param sch;
    int policy;
    pthread_getschedparam(pthread_self(), &policy, &sch);
    sch.sched_priority = 50;
    if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &sch))
    {
        std::cerr << "Failed to set thread priority" << std::endl;
    }

    for(int i=0; i<aMotorPos.getDataSize(); ++i)
    {
        for(int j=0; j<aSlList.getDataSize() / transParams::senderTASSize; ++j)
        {
            for(int k=0; k<aSnList.getDataSize() / transParams::senderElementSize; ++k)
            {
                size_t index = i * (aSlList.getDataSize() / transParams::senderTASSize) * (aSnList.getDataSize() / transParams::senderElementSize) + 
                                j * (aSnList.getDataSize() / transParams::senderElementSize) + k;
                std::unique_lock<std::mutex> lock(PROCESS_BUFFER_MUTEX);

                PROCESS_BUFFER_CONDITION.wait(lock, [index]{return BLOCK_OF_TRANSIMISSIONDARA_BUFFER.find(std::to_string(index)) != BLOCK_OF_TRANSIMISSIONDARA_BUFFER.end();});
                lock.unlock();                

                auto blockData = BLOCK_OF_TRANSIMISSIONDARA_BUFFER[std::to_string(index)];
                DetectResult detect = transmissionDetection( blockData.ascanBlock, blockData.ascanBlockRef, 
                                                             blockData.dists, blockData.distRefBlock, 
                                                             blockData.waterTempBlock, blockData.waterTempRefBlock,
                                                             aTemp.expectedSOSWater[0]);
                blockData.attData = detect.att;
                blockData.tofData = detect.tof;
                BlockOfTransmissionData transmissionBlock=blockData;
                size_t numUsedData = transmissionBlock.senderBlock.getDimSize(1);
                if(transParams::applyCalib)
                {
                    cblas_dcopy(numUsedData, transmissionBlock.metaInfos.snrValues.getData(), 1, snrValues.getData() + numData, 1);
                    cblas_dcopy(numUsedData, transmissionBlock.metaInfos.snrValuesRef.getData(), 1, snrValuesRef.getData() + numData, 1);
                }

                int rows = transmissionBlock.senderBlock.getDimSize(0);
                cblas_dcopy(numUsedData * rows, transmissionBlock.senderBlock.getData(), 1, senderList.getData() + numData*rows, 1);
                cblas_dcopy(numUsedData * rows, transmissionBlock.receiverBlock.getData(), 1, receiverList.getData() + numData*rows, 1);
                cblas_dcopy(numUsedData, transmissionBlock.tofData.getData(), 1, tofDataTotal.getData() + numData, 1);
                cblas_dcopy(numUsedData, transmissionBlock.attData.getData(), 1, attDataTotal.getData() + numData, 1);
                cblas_dcopy(numUsedData, transmissionBlock.waterTempBlock.getData(), 1, waterTempList.getData() + numData, 1);
                
                if(transParams::saveDetection || transParams::outlierOnTasDetection || transParams::saveDebugInfomation)
                {
                    cblas_dcopy(numUsedData, transmissionBlock.metaInfos.mpBlock.getData(), 1, mpBlockTotal.getData() + numData, 1);
                    cblas_dcopy(numUsedData, transmissionBlock.metaInfos.slBlock.getData(), 1, slBlockTotal.getData() + numData, 1);
                    cblas_dcopy(numUsedData, transmissionBlock.metaInfos.snBlock.getData(), 1, snBlockTotal.getData() + numData, 1);
                    cblas_dcopy(numUsedData, transmissionBlock.metaInfos.rlBlock.getData(), 1, rlBlockTotal.getData() + numData, 1);
                    cblas_dcopy(numUsedData, transmissionBlock.metaInfos.rnBlock.getData(), 1, rnBlockTotal.getData() + numData, 1);
                }
                numData += numUsedData;
                std::unique_lock<std::mutex> lockBufferCount(CREATE_BUFFER_MUTEX); 
                BLOCK_OF_TRANSIMISSIONDARA_BUFFER.erase(std::to_string(index));
                --BUFFER_COUNT;
                lockBufferCount.unlock();
                std::cout<<"Remove: "<<index<<std::endl;
                CREATE_BUFFER_CONDITION.notify_one();
            }
            Recon::notifyProgress(6+10*((i+1)*(j+1)/(aMotorPos.getDataSize()*(aSlList.getDataSize()/ transParams::senderTASSize))));   
        }
    }
    speedUpThread.join();

    double* filterData = Aurora::malloc(tofDataTotal.getDataSize());
    for(int i=0;i<tofDataTotal.getDataSize();++i)
    {
        if(tofDataTotal[i] != tofDataTotal[i])
        {
            filterData[i] = 0;
        }
        else
        {
            filterData[i] = 1;
        }
    }
    Matrix filter = Matrix::New(filterData, 1, tofDataTotal.getDataSize());
    if(transParams::applyCalib)
    {
        snrValues = removeDataFromArrays(snrValues, filter);
        snrValuesRef = removeDataFromArrays(snrValuesRef, filter);
    }
    senderList = removeDataFromArrays(senderList, filter);
    receiverList = removeDataFromArrays(receiverList, filter);
    tofDataTotal = removeDataFromArrays(tofDataTotal, filter);
    attDataTotal = removeDataFromArrays(attDataTotal, filter);
    waterTempList = removeDataFromArrays(waterTempList, filter);
    if(transParams::saveDebugInfomation || transParams::outlierOnTasDetection || transParams::saveDetection)
    {
        mpBlockTotal = removeDataFromArrays(mpBlockTotal, filter);
        slBlockTotal = removeDataFromArrays(slBlockTotal, filter);
        snBlockTotal = removeDataFromArrays(snBlockTotal, filter);
        rlBlockTotal = removeDataFromArrays(rlBlockTotal, filter);
        rnBlockTotal = removeDataFromArrays(rnBlockTotal, filter);
    }

    Matrix valid;
    if(transParams::applyCalib)
    {
        valid = findDefectTransmissionData(Matrix::copyFromRawData(snrValues.getData(), 1, numData), transParams::snrThreshold) * 
        findDefectTransmissionData(Matrix::copyFromRawData(snrValuesRef.getData(), 1, numData), transParams::snrThreshold);
    }
    else
    {
        valid = zeros(1,numData) + 1;
    }

    DataInfo dataInfno;
    double* findDefectData = Aurora::malloc(valid.getDataSize());
    int findDefectDataIndex = 0;
    for(int i=0; i<valid.getDataSize(); ++i)
    {
        if(valid[i] == 0)
        {
            findDefectData[findDefectDataIndex] = i + 1;
            ++findDefectDataIndex;
        }
    }
    dataInfno.findDefect = Matrix::New(findDefectData, 1, findDefectDataIndex);
    
    if(transParams::saveDebugInfomation)
    {
        dataInfno.sn = snBlockTotal;
        dataInfno.sl = slBlockTotal;
        dataInfno.rn = rnBlockTotal;
        dataInfno.rl = rlBlockTotal;
        dataInfno.mp = mpBlockTotal;
    }

    tofDataTotal = removeDataFromArrays(tofDataTotal, valid);
    attDataTotal = removeDataFromArrays(attDataTotal, valid);
    senderList = removeDataFromArrays(senderList, valid);
    receiverList = removeDataFromArrays(receiverList, valid);
    waterTempList = removeDataFromArrays(waterTempList, valid);
 
    dataInfno.numPossibleScans = numData;
    dataInfno.numValidScans = sum(valid);

    //以下逻辑config默认值不走。后续再实现todo
    // if(transParams.detection.outlierOnTasDetection)
    //     snBlockTotal = snBlockTotal(valid);
    //     slBlockTotal = slBlockTotal(valid);
    //     rnBlockTotal = rnBlockTotal(valid);
    //     rlBlockTotal = rlBlockTotal(valid);
        
    //     outliers = analyzeDetections(slBlockTotal, snBlockTotal, rlBlockTotal, rnBlockTotal, tofDataTotal);
        
        
    //     tofDataTotal = tofDataTotal(~outliers);
    //     attDataTotal = attDataTotal(:,~outliers);
    //     senderList = senderList(:,~outliers);
    //     receiverList = receiverList(:,~outliers);
    //     waterTempList = waterTempList(~outliers);
        
    //     writeReconstructionLog(sprintf('Removed additionally %i outliers. \n',sum(outliers)),1);
    //     writeReconstructionLog(sprintf('Total data after 3 filter steps: Taken %d/%d AScans for reconstruction (%.2f percent).', numel(tofDataTotal), numScans, (numel(tofDataTotal) / numScans)*100), 1)
    // end
    
    // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    
    // if transParams.detection.saveDetection % writing the detected values to file
    //     writeReconstructionLog('Save transmission detections.', 1);
        
    //     if(~transParams.detection.outlierOnTasDetection)
    //         snBlockTotal = snBlockTotal(valid);
    //         slBlockTotal = slBlockTotal(valid);
    //         rnBlockTotal = rnBlockTotal(valid);
    //         rlBlockTotal = rlBlockTotal(valid);
    //     end
    //     mpBlockTotal = mpBlockTotal(valid);
        
    //     if(transParams.detection.outlierOnTasDetection)
    //         snBlockTotal = snBlockTotal(~outliers);
    //         slBlockTotal = slBlockTotal(~outliers);
    //         rnBlockTotal = rnBlockTotal(~outliers);
    //         rlBlockTotal = rlBlockTotal(~outliers);
    //         mpBlockTotal = mpBlockTotal(~outliers);
    //     end
        
    //     saveTransmisssionDetectionData(transParams.pathSaveDetection, tofDataTotal, attDataTotal, senderList, receiverList, mpBlockTotal, slBlockTotal, snBlockTotal, rlBlockTotal, rnBlockTotal, waterTempList, expInfo.rootMeasUniqueID, dataInfo);
    // end

    TransmissionData result;
    result.attDataTotal = attDataTotal;
    result.tofDataTotal = tofDataTotal;
    result.receiverList = receiverList;
    result.senderList = senderList;
    result.dataInfo = dataInfno;
    result.waterTempList = waterTempList;
    return result;
}