#include "startReflectionReconstruction.h"
#include "Function.h"
#include "Function2D.h"
#include "Function3D.h"
#include "Matrix.h"
#include "MatlabWriter.h"

#include "common/getGeometryInfo.h"
#include "common/precalculateChannelList.h"
#include "common/dataBlockCreation/getAScanBlockPreprocessed.h"
#include "common/dataBlockCreation/removeDataFromArrays.h"
#include "log/notify.h"
#include "reflectionReconstruction/preprocessData/determineOptimalPulse.h"
#include "reflectionReconstruction/reconstructionSAFT/reconstructionSAFT.h"
#include "src/reflectionReconstruction/preprocessData/preprocessAScanBlockForReflection.h"
#include "config/config.h"
#include "log/log.h"

#include "CudaEnvInit.h"

#include <cstdio>
#include <iostream>
#include <queue>
#include <thread>
#include <vector>

using namespace Aurora;
using namespace Recon;

namespace
{
    std::queue<preprocessAScanRResult> PRODUCER_PROCESSDATAS;
    std::queue<AscanBlockPreprocessed> PRODUCER_BLOCKDATAS;
    std::mutex PRODUCER_MUTEX;
    std::condition_variable PRODUCER_CONDITION;
    std::mutex CUSTOMER_MUTEX;
    std::condition_variable CUSTOMER_CONDITION;
}

void producerThread( Parser* aParser, const Aurora::Matrix& aMotorPos,
                     const Aurora::Matrix& aSlList, const Aurora::Matrix& aSnList,
                     const Aurora::Matrix& aRlList, const Aurora::Matrix& aRnList,
                     GeometryInfo& aGeom, MeasurementInfo& aExpInfo, PreComputes& aPreComputes)
{
    if(reflectParams::useOptPulse==1 && reflectParams::runReflectionReco)
    {
        aPreComputes.sincPeak_ft = determineOptimalPulse(aPreComputes.timeInterval, aExpInfo.expectedAScanLength);
    }
    printf("   - channel list");
    auto channelList = precalculateChannelList(aRlList, aRnList, aExpInfo, aPreComputes);
    size_t numScans = aMotorPos.getDataSize() * aSlList.getDataSize() *
                      aSnList.getDataSize() * aRlList.getDataSize() *
                      aRnList.getDataSize();
    int numTakenScans = 0,numProcessedScans = 0,numPossibleScans = 0;
    for(int i=0; i<aMotorPos.getDataSize(); ++i)
    {
        //#pragma omp parallel for num_threads(24)
        for(int j=0; j<aSlList.getDataSize() / transParams::senderTASSize; ++j)
        {
            for(int k=0; k<aSnList.getDataSize() / transParams::senderElementSize; ++k)
            {
                Matrix mp = aMotorPos(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);
                auto blockData = getAscanBlockPreprocessed(aParser, mp, sl, sn, aRlList, aRnList, aGeom, aExpInfo, true, false);

                float* 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 channelBlockSize = ind.getDataSize();
                float* channelBlockData = Aurora::malloc(channelBlockSize);
                for(size_t i=0; i<channelBlockSize; ++i)
                {
                    channelBlockData[i] = channelList[ind[i] - 1];
                }
                Matrix channelBlock = Matrix::New(channelBlockData, 1, channelBlockSize);
                RECON_INFO("start cpu---------preprocessAScanBlockForReflection");
                auto preprocessData = preprocessAScanBlockForReflection(blockData.ascanBlockPreprocessed, blockData.mpBlock, blockData.slBlock,
                                      blockData.snBlock, blockData.rlBlock, blockData.rnBlock, blockData.senderPositionBlock,
                                      blockData.receiverPositionBlock, blockData.gainBlock, channelBlock, aExpInfo, aPreComputes);
                PRODUCER_BLOCKDATAS.push(blockData);
                PRODUCER_PROCESSDATAS.push(preprocessData);
            }
        }
    }
}

Aurora::Matrix Recon::startReflectionReconstruction( Parser* aParser, int aSAFT_mode, const Aurora::Matrix& aMotorPos,
        const Aurora::Matrix& aSlList, const Aurora::Matrix& aSnList,
        const Aurora::Matrix& aRlList, const Aurora::Matrix& aRnList,
        GeometryInfo& aGeom, TransRecos& aTransRecos,
        MeasurementInfo& aExpInfo, PreComputes& aPreComputes)
{
    printf("Reflection reconstruction is carried out.");

    printf("Preperations for reconstructions.");

    printf("   - reset GPUs");
    for (size_t i = 0; i < reflectParams::gpuSelectionList.getDataSize(); i++)
    {
        std::string msg;
        if (!resetGPUDevice((int)reflectParams::gpuSelectionList[i],msg))
        {
            std::cerr<<msg<<std::endl;
        }
    }
    std::thread thread = std::thread(producerThread, aParser, aMotorPos, aSlList, aSnList, aRlList, aRnList, aGeom, aExpInfo, aPreComputes);

                                     Matrix Env =  Aurora::zeros((int)reflectParams::imageXYZ[0],(int)reflectParams::imageXYZ[1],(int)reflectParams::imageXYZ[2]);




                                     for(int i=0; i<aMotorPos.getDataSize(); ++i)
{
    //#pragma omp parallel for num_threads(24)
    for(int j=0; j<aSlList.getDataSize() / transParams::senderTASSize; ++j)
        {
            for(int k=0; k<aSnList.getDataSize() / transParams::senderElementSize; ++k)
            {
                RECON_INFO("start gpu---------recontructSAFT");
                Env = recontructSAFT(removeDataFromArrays(preprocessData.AscanBlock, preprocessData.usedData),
                                     removeDataFromArrays(blockData.senderPositionBlock, preprocessData.usedData),
                                     removeDataFromArrays(blockData.receiverPositionBlock, preprocessData.usedData),
                                     removeDataFromArrays(blockData.mpBlock, preprocessData.usedData),
                                     aSAFT_mode, aTransRecos, Env);
                std::cout<<Env[0]<<"-" << Env[1] <<"-" << Env[2] <<"-" << Env[3]<<std::endl;
                RECON_INFO("Reflection Reconstructon: " + std::to_string(j));
            }
            //Recon::notifyProgress(25+73*((j*i)/(aMotorPos.getDataSize() * aSlList.getDataSize())));
        }
    }

    return Env;
}