#include "ceMatchedFilterHandling.h"
#include "Function.h"
#include "Function1D.h"
#include "Function2D.h"
#include "Function3D.h"

Aurora::Matrix Recon::adaptFrequency(Aurora::Matrix &aCE, double ceSampleFrequency, double requiredFrequency)
{
    if (requiredFrequency < ceSampleFrequency){
        double stride = ceSampleFrequency / (aCE.getDataSize() - 1);
        double *f2Data = Aurora::malloc(aCE.getDataSize());
        for (size_t i = 0; i < aCE.getDataSize(); i++)
        {
            f2Data[i] = i*stride;
        }
        auto f2 = Aurora::Matrix::New(f2Data, aCE.getDataSize(), 1, 1);
        auto df = fft(aCE);
        df = df * (f2 < (requiredFrequency / 2));
        int halfdfLength = (int)std::ceil(((double)aCE.getDataSize()) * 0.5);
        df.forceReshape(halfdfLength, 1, 1);
        aCE = Aurora::ifft_symmetric(df, aCE.getDataSize());
    }
    double *xData = Aurora::malloc(aCE.getDataSize());
    for (size_t i = 0; i < aCE.getDataSize(); i++)
    {
        xData[i] = (double)(i+1.0);
    }
    auto xMatrix = Aurora::Matrix::New(xData,aCE.getDataSize(),1,1); 
    double stride = ceSampleFrequency /requiredFrequency;
    double value = 1.0;
    size_t length = std::ceil(((double)(aCE.getDataSize()-1))/stride);
    double *x1Data = Aurora::malloc(length);
    for (size_t i = 0; i < length; i++)
    {
        x1Data[i] = value;
        value+=stride;
    }
    auto x1Matrix = Aurora::Matrix::New(x1Data,length,1,1); 
    
    aCE = Aurora::interp1(xMatrix,aCE,x1Matrix,Aurora::InterpnMethod::Spline);
    return aCE;
}

Aurora::Matrix Recon::preprocessCE(Aurora::Matrix &aCE, double ceSampleFrequency, double requiredFrequency, double expectedLength)
{
    adaptFrequency(aCE,ceSampleFrequency,requiredFrequency);
    aCE = aCE - Aurora::mean(aCE).getScalar();
    aCE = aCE / Aurora::max(Aurora::abs(aCE)).getScalar(); 
    aCE = aCE / Aurora::sum(Aurora::abs(aCE)).getScalar(); 
    
    Aurora::padding(aCE,expectedLength-1,0.0);

    if (expectedLength<aCE.getDataSize()){
        aCE.forceReshape(expectedLength,1,1);
    }
    return aCE;
}

Aurora::Matrix Recon::reviseMatchedFilter(const Aurora::Matrix &aMFTime,
                                          bool aRemoveOutliersFromCEMeasured)
{
    if (aRemoveOutliersFromCEMeasured)
    {
        auto normSTD = Aurora::std(Aurora::abs(aMFTime));
        Aurora::nantoval(normSTD, 0.0);
        auto sortSTD = Aurora::sort(normSTD);
        int t = (int)std::round(0.4 * aMFTime.getDimSize(1)) - 1;
        t = t <= 0 ? 1.0 : t;
        auto absFTime = abs(aMFTime);
        auto maxAbsFTime = max(absFTime);
        auto maxFlag = maxAbsFTime < (0.1 * max(maxAbsFTime));
        auto lessFlag = normSTD < sortSTD(0, t).toMatrix();
        long maxCol, maxRow;
        max(normSTD, Aurora::Column, maxRow, maxCol);
        for (int j = 0; j < aMFTime.getDimSize(1); ++j)
        {
            if ((bool)(lessFlag.getData()[j]) || (bool)(maxFlag.getData()[j]))
            {
                aMFTime(Aurora::$, j) = aMFTime(Aurora::$, maxCol);
            }
        }
    }
    auto matchedFilter = fft(aMFTime);
    auto sumDiff = Aurora::zeros(1, matchedFilter.getDimSize(1));
    long minCol = 0, row;
    {
        auto absMatchedFilter = Aurora::abs(matchedFilter);
        auto highNoiseScore = Aurora::mean(absMatchedFilter) * Aurora::std(absMatchedFilter);
        auto medianNoise = Aurora::median(highNoiseScore);
        Aurora::min(Aurora::abs(highNoiseScore - Aurora::median(highNoiseScore)), Aurora::Column, row, minCol);
    }
    auto maxMatchFilter = matchedFilter(Aurora::$, minCol).toMatrix();
    for (int k = 0; k < matchedFilter.getDimSize(1); ++k)
    {
        sumDiff.getData()[k] = Aurora::sum(Aurora::abs(matchedFilter(Aurora::$, k).toMatrix() - maxMatchFilter)).getData()[0];
    }

    double meansumDiff = Aurora::mean(sumDiff).getScalar();
    double stdSumDiff = Aurora::std(sumDiff).getScalar();
    double sumDiffJ = meansumDiff + 2.596 * stdSumDiff;
    for (int l = 0; l < sumDiff.getDataSize(); ++l)
    {
        if (sumDiff.getData()[l] > sumDiffJ)
        {
            matchedFilter(Aurora::$, l) = matchedFilter(Aurora::$, minCol);
        }
    }
    return matchedFilter;
}
Aurora::Matrix Recon::createMatchedFilter(const Aurora::Matrix &aCE,
                                          bool measuredCEused, bool findDefects,
                                          bool aRemoveOutliersFromCEMeasured, std::string aHardwareVersion)
{
    Aurora::Matrix mFTime = Aurora::Matrix::copyFromRawData(aCE.getData(), aCE.getDimSize(0), aCE.getDimSize(1));
    Aurora::Matrix matchedFilter;
    if (measuredCEused && findDefects){
        matchedFilter = reviseMatchedFilter(mFTime, aRemoveOutliersFromCEMeasured);
    }
    else{
        matchedFilter = Aurora::fft(mFTime);
    }
    if (measuredCEused)
    {
        auto mFTime2 = Aurora::real(Aurora::ifft(aHardwareVersion == "USCT3dv3" ? -matchedFilter : matchedFilter));
        mFTime2 = mFTime2 / Aurora::repmat(Aurora::max(Aurora::abs(mFTime2)), mFTime2.getDimSize(0), 1);
        mFTime2 = mFTime2 / Aurora::repmat(Aurora::sum(Aurora::abs(mFTime2)), mFTime2.getDimSize(0), 1);
        matchedFilter = Aurora::fft(mFTime2);
    }
    return matchedFilter;
}