#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 "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>

using namespace Aurora;
using namespace Recon;

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;
        }
    }
    printf("   - optimal pulse");
    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();
    printf("   - blocking");
    Matrix Env =  Aurora::zeros((int)reflectParams::imageXYZ[0],(int)reflectParams::imageXYZ[1],(int)reflectParams::imageXYZ[2]);

    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);
                
                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 channelBlockSize = ind.getDataSize();
                double* 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);
                auto preprocessData = preprocessAScanBlockForReflection(blockData.ascanBlockPreprocessed, blockData.mpBlock, blockData.slBlock,
                                                                        blockData.snBlock, blockData.rlBlock, blockData.rnBlock, blockData.senderPositionBlock,
                                                                        blockData.receiverPositionBlock, blockData.gainBlock, channelBlock, aExpInfo, aPreComputes);
                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);
                RECON_INFO("Reflection Reconstructon: " + std::to_string(j));
            }
        }
    }

    return Env;
}