#include "detection.h"

#include "Function1D.h"
#include "Function2D.h"
#include "Function3D.h"
#include "Matrix.h"
#include <sys/types.h>

using namespace Aurora;
namespace Recon {
    Matrix  analyzeDetections(const Matrix &slBlockTotal, const Matrix &snBlockTotal, const Matrix &rlBlockTotal, const Matrix &rnBlockTotal, const Matrix &tofDataTotal){
        //TODO: 待完成，暂时用不到
        return Matrix();
    }


    Matrix calculateSOSOffset(const Matrix &sosBlock, double referenceSOS, const Matrix &distBlock, double sampleRate){
        auto offset = (distBlock / sosBlock - distBlock / referenceSOS) * sampleRate;
        return offset;
    }


    Matrix calculateAttenuation(const Matrix &ascans, const Matrix &startPos, const Matrix &endPos, const Matrix &ascansRef, const Matrix &startPosRef, const Matrix &endPosRef)
    {
        auto ascans2 = zeros(ascans.getDimSize(0), ascans.getDimSize(1));
        auto ascansRef2 = zeros(ascansRef.getDimSize(0), ascansRef.getDimSize(1));
        #pragma omp parallel for 
        for (size_t i = 0; i < ascans.getDimSize(1); i++)
        {
            for (size_t j = startPos[i]-1; j < endPos[i]; j++)
            {
                ascans2(j, i) = ascans(j, i);
            }
            for (size_t j = startPosRef[i]-1; j < endPosRef[i]; j++)
            {
                ascansRef2(j, i) = ascansRef(j, i);
            }
        }

        auto pulseEnergy = sum(ascans2^2);
        auto pulseEnergyEmpty = sum(ascansRef2^2);

        return log(pulseEnergyEmpty/pulseEnergy);
    }

    SearchPosition calculateStarEndSearchPosition(const Matrix &aVDistBlock,
                                double minSpeedOfSound, double maxSpeedOfSound,
                                double sampleRate, double maxSample,
                                const Matrix &aVSosOffsetBlock,
                                double startOffset, double segmentLenOffset)
    {
        auto startSearch = floor((aVDistBlock / maxSpeedOfSound)*sampleRate+aVSosOffsetBlock+startOffset);
        for (size_t i = 0; i < startSearch.getDataSize(); i++)
        {
            startSearch[i] = (uint)(startSearch[i]);
            if(startSearch[i]<1) startSearch[i]=1;
            else if(startSearch[i]>maxSample) startSearch[i] = maxSample;
        }

        auto endSearch = ceil(aVDistBlock/minSpeedOfSound*sampleRate+aVSosOffsetBlock+startOffset+segmentLenOffset);
        for (size_t i = 0; i < endSearch.getDataSize(); i++)
        {
            endSearch[i] = (uint)(endSearch[i]);
            if(endSearch[i]<1) endSearch[i]=1;
            else if(endSearch[i]>maxSample) endSearch[i] = maxSample;
        }
        SearchPosition result;
        result.startSearch = startSearch;
        result.endSearch = endSearch;
        return result;
    }
    
    TimeWindowResult applyTimeWindowing(const Aurora::Matrix &AscanBlock, double sampleRate,
        const Aurora::Matrix &distBlock, const Aurora::Matrix &sosBlock,
        double expectedSOSWater, double startOffset, double segmentLenOffset,
        double minSpeedOfSound, double maxSpeedOfSound, bool gaussWindow)
    {
        auto sosOffset = calculateSOSOffset(sosBlock, expectedSOSWater, distBlock,  sampleRate);

        auto calcResult = calculateStarEndSearchPosition(distBlock, minSpeedOfSound, maxSpeedOfSound, sampleRate,AscanBlock.getDimSize(0), sosOffset, startOffset, segmentLenOffset);

        auto AscanBlockProcessed = zeros(AscanBlock.getDimSize(0),AscanBlock.getDimSize(1));

        if(gaussWindow)
        {
            auto expectedPosWater = (distBlock / expectedSOSWater) * sampleRate + startOffset;
            for (size_t i = 0; i < AscanBlock.getDimSize(1); i++)
            {
                auto windowWidth = calcResult.endSearch-calcResult.startSearch;
                auto t = linspace(-5,5,windowWidth[i]);
                auto gauss = exp( -0.1 * (transpose(t) ^ 2));
                
                for (size_t j = calcResult.startSearch[i]-1; j < calcResult.endSearch[i]; j++)
                {
                    AscanBlockProcessed(j, i) = AscanBlock(j, i);
                }
                auto temp = zeros(AscanBlockProcessed.getDimSize(0), 1);
                size_t end = std::round(expectedPosWater[i])-std::round(windowWidth[i]/2)+gauss.getDataSize()-1;
                size_t gIdx = 0;
                for (size_t k = std::round(expectedPosWater[i])- std::round(windowWidth[i]/2)-1; k < end; k++)
                {
                    temp[k] = gauss[gIdx++];
                }
                AscanBlockProcessed($,i) = AscanBlockProcessed($,i).toMatrix() * temp;
            }
        }
        else{
            for (size_t i = 0; i < AscanBlock.getDimSize(1); i++) {
                for (size_t j = calcResult.startSearch[i] - 1;
                     j < calcResult.endSearch[i]; j++) {
                    AscanBlockProcessed(j, i) = AscanBlock(j, i);
                }
            }
        }
        TimeWindowResult result;
        result.startSearch = calcResult.startSearch;
        result.AscanBlockProcessed = AscanBlockProcessed;
        return result;
    }
    
    //TODO:need test
    Aurora::Matrix detectAttVectorized(const Aurora::Matrix &Ascan, const Aurora::Matrix &AscanRef,
                        const Aurora::Matrix &distRef,
                        const Aurora::Matrix &sosWaterRef,
                        const Aurora::Matrix &tof, int aScanReconstructionFrequency,
                        double offsetElectronic, int detectionWindowATT)
    {
        auto sizeAscan = size(Ascan);

        auto sampleRate = aScanReconstructionFrequency;
        double offsetElectronicSamples = offsetElectronic * sampleRate;

        auto envelopeOfAScan = abs(hilbert(Ascan));
        auto envelopeOfReferenceAScan = abs(hilbert(AscanRef));
        auto tof2 = (distRef / sosWaterRef) * sampleRate + offsetElectronicSamples;

        auto startPos = zeros(Ascan.getDimSize(1), 1);
        auto endPos = zeros(Ascan.getDimSize(1), 1);
        auto startPosRef = zeros(Ascan.getDimSize(1), 1);
        auto endPosRef = zeros(Ascan.getDimSize(1), 1);
        for (size_t i = 0; i < Ascan.getDimSize(1); i++)
        {
            startPos(i) = floor(max(tof(i).toMatrix()*sampleRate+offsetElectronicSamples,1));
            endPos(i) = ceil(min(sizeAscan(1).toMatrix(), tof(i).toMatrix()*sampleRate+offsetElectronicSamples+detectionWindowATT));
            startPosRef(i) =  floor(max( tof2(i).toMatrix(),1));
            endPosRef(i) = ceil(min(sizeAscan(1).toMatrix(), tof2(i).toMatrix()+detectionWindowATT));
        }
        return calculateAttenuation(envelopeOfAScan,startPos,endPos,envelopeOfReferenceAScan,startPosRef,endPosRef);
    }

    int nextpow2(unsigned int value){
        unsigned int compareValue = 2;
        int result = 1;    
        while(compareValue<=value){
            compareValue=compareValue<<1;
            result++;
        }
        return result;
    }
    //TODO:need test
    Aurora::Matrix detectTofVectorized(
        const Aurora::Matrix &AscanBlock, const Aurora::Matrix &AscanRefBlock,
        const Aurora::Matrix &distBlock, const Aurora::Matrix &distBlockRef,
        const Aurora::Matrix &sosWaterBlock,
        const Aurora::Matrix &sosWaterRefBlock, double expectedSOSWater,
        int useTimeWindowing, int aScanReconstructionFrequency,
        double offsetElectronic, int detectionWindowSOS, double minSpeedOfSound,
        double maxSpeedOfSound, bool gaussWindow) 
        {
            auto sampleRate = aScanReconstructionFrequency;
            double offsetElectronicSamples = offsetElectronic * sampleRate; 
            Matrix diffStartSearch;
            if (useTimeWindowing == 1)
            {
                auto timeResult1 = applyTimeWindowing(AscanBlock, sampleRate, distBlock, sosWaterBlock, expectedSOSWater,  offsetElectronicSamples, detectionWindowSOS, minSpeedOfSound, maxSpeedOfSound, gaussWindow);
                auto timeResult2= applyTimeWindowing(AscanRefBlock, sampleRate, distBlockRef, sosWaterRefBlock, expectedSOSWater,  offsetElectronicSamples, detectionWindowSOS, minSpeedOfSound, maxSpeedOfSound, gaussWindow);
                
                diffStartSearch = timeResult1.startSearch - timeResult1.startSearch;
            }

            auto m = std::max(size(AscanBlock, 1), size(AscanRefBlock, 1));
            auto maxlag = std::max(size(AscanBlock, 1), size(AscanRefBlock, 1)) - 1;
            auto mxl = std::min(maxlag, m-1);
            auto ceilLog2 = nextpow2(2*m-1);
            auto m2 = pow(2,ceilLog2);

            auto x = fft(AscanBlock, m2);
            auto y = fft(AscanRefBlock, m2);
            auto c1 = ifft(x*conj(y));
            auto c = complex(zeros(mxl+mxl+1,c1.getDimSize(1)));
            for (size_t i = 0; i < mxl; i++)
            {
                c(i,$) = c1(m2-mxl+i, $);
                c(i+mxl,$) = c1(i, $);
            }
            c(mxl+mxl,$) = c1(mxl, $);

            auto shiftInSamples =zeros(1,c1.getDimSize(1));
            for (size_t i = 0; i < c1.getDimSize(1); i++)
            {
                long rowID=0,colID=0;
                max(c(i,$).toMatrix(),All,rowID,colID);
                shiftInSamples[i]=-maxlag+colID;
            }
            if (useTimeWindowing)
            {
                shiftInSamples = shiftInSamples - diffStartSearch;
            }
            auto tof = shiftInSamples / sampleRate + distBlock / sosWaterBlock;
            auto sosValue = distBlock / tof;
        }
}