#include "determineOptimalPulse.h"
#include "Function1D.h"
#include "Function2D.h"
#include "Function3D.h"
#include "config/config.h"
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstring>
#include <omp.h>
namespace Recon {

    Aurora::Matrix determineOptimalPulse(double timeInterval, size_t expectedAScanLength)
    {
        double imgResolution = reflectParams::imageResolution;
        int optPulseFactor = reflectParams::optPulseFactor;
        auto minOptPulse = ceil(8*imgResolution*(1/timeInterval)/1500);

        if(optPulseFactor==-1)
        {
            optPulseFactor = minOptPulse;
            reflectParams::optPulseFactor = -1;
            printf("Optimal pulse automatically set to %f \n",minOptPulse);
        }
        else if(optPulseFactor< minOptPulse){
            printf("WARNING: optimal pulse too small for resolution");
        }

        auto desSamplingFreq = (1/timeInterval)*0.5;
        double sincFact = optPulseFactor;
        double sincLength = round(11*sincFact/16);
        sincLength = 3*sincLength; 
        double begin =-sincLength/2+(timeInterval)/2;
        double end = sincLength/2;
        int length = end - begin + 1;
        auto sincT = Aurora::zeros(1,length);
        for (int j = 0; begin <= end; begin++, j++)
        {
            sincT[j] = begin*timeInterval;
        }

        sincFact = sincFact/2;
        auto sincPeak = 2*sqrt(M_PI)*std::pow((desSamplingFreq/sincFact),3)*(1-2*((M_PI*desSamplingFreq/sincFact*sincT)^2))*exp(-((M_PI*desSamplingFreq/sincFact*sincT)^2));

        sincPeak = sincPeak/Aurora::max(sincPeak);
        auto sincPeak_len=sincPeak.getDataSize();

        Aurora::padding(sincPeak, expectedAScanLength-1, 0);
        // sincPeak = Aurora::transpose(sincPeak);
        size_t offset = floor((double)sincPeak_len/2);
        //cicshift
        // #pragma omp  parallel for
        for (size_t i = 0; i < sincPeak.getDimSize(1); i++)
        {
            double *temp = new double[offset]{0};
            double * beginPtr = sincPeak.getData()+i*sincPeak.getDimSize(0);
            double * endPtr = beginPtr + offset;
            double * col_end = beginPtr + (i+1)*sincPeak.getDimSize(0);
            std::copy(beginPtr,endPtr,temp);
            std::copy(endPtr,col_end,beginPtr);
            std::copy(temp,temp+offset,col_end-offset);
        }
        auto sincPeak_ft=fft(sincPeak);
        return sincPeak_ft;
    }
}