#include <complex.h>
#include "fftw3.h"
#include <stdio.h>
#include <iostream>
#include <math.h>
#include <iomanip>
#include <cmath>
#include <stdlib.h>
#include <omp.h>
#include <algorithm>
#include <cstring>
#include "structDef.hpp"
#include "deleteTransmissionSignal.hpp"

using namespace std;

#define REAL 0
#define IMAG 1
#define FFTW_WISDOM_TYPE FFTW_MEASURE//FFTW_PATIENT //FFTW_MEASURE //FFTW_ESTIMATE

/* 
 * C implementation of preprocessing of AScan for reflection reconstruction 
 * TODO: memory alloc
 */
void performSignalProcessingC(double* aScanArray, int numScans, int numSamples, double* matchedFilter_r, double* matchedFilter_i, int* channelList, double* offsetFour_r, double* offsetFour_i, double* sincPeakFour_r, double* sincPeakFour_i, int* startPos, int* endPos, struct Params *paramStruct, double* aScanArrayProcessed) {

 fftw_make_planner_thread_safe();
 fftw_plan plan_fftAScanAScanComplex, plan_ifftAScanComplex, plan_fftHelpAScanComplex, plan_ifftAScanComplexHelp;
 char filenameFftwWisdom[200] = "";
 sprintf(filenameFftwWisdom, "fftw_wisdom_reflectionPreprocessing_%d.wis", FFTW_WISDOM_TYPE);
 int loadedWisdomUsed = 0;
 // loading

#pragma omp parallel default(none)  num_threads(paramStruct->numThreads) shared(loadedWisdomUsed, filenameFftwWisdom, plan_fftAScanAScanComplex, plan_ifftAScanComplex, plan_fftHelpAScanComplex, plan_ifftAScanComplexHelp, numScans, numSamples, aScanArray, matchedFilter_i, matchedFilter_r, channelList, offsetFour_r, offsetFour_i, paramStruct,  sincPeakFour_r, sincPeakFour_i, startPos, endPos, aScanArrayProcessed)
	{

    	double *help, *aScan;
        help = (double *) fftw_malloc(sizeof(double) * numSamples);
        aScan = (double *) fftw_malloc(sizeof(double) * numSamples);

        fftw_complex *aScanComplex;
        aScanComplex = (fftw_complex *) fftw_malloc(sizeof(fftw_complex)* numSamples);
        
        
		/* plans */
#pragma omp critical (createPlan) 
        {
            // load wisom if exist
            if ( loadedWisdomUsed == 0){
                loadedWisdomUsed = fftw_import_wisdom_from_filename(filenameFftwWisdom);
            }
            
            if(plan_fftAScanAScanComplex==NULL){
                plan_fftAScanAScanComplex = fftw_plan_dft_r2c_1d((int)numSamples, aScan, aScanComplex, FFTW_WISDOM_TYPE);
                if (plan_fftAScanAScanComplex == NULL) { throw "plan creation failed."; }
            }
            
            if(plan_ifftAScanComplex==NULL){
                plan_ifftAScanComplex = fftw_plan_dft_1d((int)numSamples, aScanComplex, aScanComplex, FFTW_BACKWARD, FFTW_WISDOM_TYPE);
                if (plan_ifftAScanComplex == NULL) { throw "plan creation failed."; }
            }
            
            if (paramStruct->useOptPulse == 1 && plan_fftHelpAScanComplex==NULL) {
                plan_fftHelpAScanComplex = fftw_plan_dft_r2c_1d((int)numSamples, help, aScanComplex, FFTW_WISDOM_TYPE);
                if (plan_fftHelpAScanComplex == NULL) { throw "plan creation failed."; }
            }
            
            if(plan_ifftAScanComplexHelp==NULL){
                plan_ifftAScanComplexHelp = fftw_plan_dft_c2r_1d((int)numSamples, aScanComplex, help, FFTW_WISDOM_TYPE);
                if (plan_ifftAScanComplexHelp == NULL) { throw "plan creation failed."; }
            }
        }

#pragma omp for
		for (int numScan = 0; numScan < numScans; numScan++) {
            
            double helpReal, helpImag;
			int i, iScanStart, iMatchedFilter;
			
            iScanStart = numScan * numSamples;
			iMatchedFilter = (int)(channelList[numScan] - 1)*numSamples;

			memcpy(aScan, &aScanArray[iScanStart], sizeof(double) * numSamples);
            
            // Change to foutier domain 
            fftw_execute_dft_r2c(plan_fftAScanAScanComplex, aScan, aScanComplex);
            
			for (i = 0; i < numSamples / 2 + 1; i++) {

				if ((paramStruct->useCorrelation == 1) && (paramStruct->matchedFilterCeAScan == 1)) {
					// matchedFiltering
					helpReal = (aScanComplex[i][REAL] * matchedFilter_r[iMatchedFilter + i] + aScanComplex[i][IMAG] * matchedFilter_i[iMatchedFilter + i]) / numSamples;
					helpImag = (aScanComplex[i][IMAG] * matchedFilter_r[iMatchedFilter + i] - aScanComplex[i][REAL] * matchedFilter_i[iMatchedFilter + i]) / numSamples;

				}
				else {
					helpReal = aScanComplex[i][REAL] / numSamples;
					helpImag = aScanComplex[i][IMAG] / numSamples;
				}

				//remove offset
				aScanComplex[i][REAL] = helpReal * offsetFour_r[i] - helpImag *  offsetFour_i[i];
				aScanComplex[i][IMAG] = helpReal * offsetFour_i[i] + offsetFour_r[i] * helpImag;

			}

			if (paramStruct->useOptPulse == 1) {
				// analytic signal and optimal pulse...
				
                aScanComplex[0][REAL] = 0.0;
				aScanComplex[0][IMAG] = 0.0;

				for (i = numSamples / 2; i < numSamples; i++) {
					aScanComplex[i][REAL] = 0.0;
					aScanComplex[i][IMAG] = 0.0;
				}

				for (i = 1; i < numSamples / 2; i++) {
					aScanComplex[i][REAL] *= 2;
					aScanComplex[i][IMAG] *= 2;
				}

				// back transform for analytic signal computation
                fftw_execute_dft(plan_ifftAScanComplex, aScanComplex, aScanComplex);

				// abs, cut to original A-Scan length
				for (i = 0; i < numSamples; i++) {
					aScan[i] = sqrt(aScanComplex[i][REAL] * aScanComplex[i][REAL] + aScanComplex[i][IMAG] * aScanComplex[i][IMAG]);
				}
                
                // set aScan = 0, where no max point (gradient change)
				help[0] = 0;
				for (i = 1; i < numSamples; i++) {
					help[i] = aScan[i - 1] - aScan[i];		
                    if (!(help[i - 1] < 0.0 && help[i]>0.0)) {
						aScan[i - 1] = 0.0;
					}
				}
				aScan[numSamples - 1] = 0.0;
                
                // help = AScan with negative values set to 0
				for (i = 0; i < numSamples; i++) {
					if (aScan[i] < 0.0) {
						help[i] = 0.0;
					}
					else {
						help[i] = aScan[i];
					}
				}

				// reduce number of peaks to largest peaks
				if ((paramStruct->limitNumPulsesTo > 0) && (numSamples > paramStruct->limitNumPulsesTo)) {
					double helpSort[numSamples];
					memcpy(helpSort, help, sizeof(helpSort));
					sort(helpSort, helpSort + numSamples);
					for (i = 0; i < numSamples; i++) {
						if (help[i] < helpSort[numSamples - paramStruct->limitNumPulsesTo]) {
							help[i] = 0.0;
						}
					}
				}
                
                // change to foutier domain again
                fftw_execute_dft_r2c(plan_fftHelpAScanComplex, help, aScanComplex);

				for (i = 0; i < numSamples / 2 + 1; i++) {
					helpReal = aScanComplex[i][REAL];
					helpImag = aScanComplex[i][IMAG];
					aScanComplex[i][REAL] = (helpReal* sincPeakFour_r[i] - helpImag *  sincPeakFour_i[i]) / numSamples;
					aScanComplex[i][IMAG] = (helpReal * sincPeakFour_i[i] + sincPeakFour_r[i] * helpImag) / numSamples;
				}
                
                // back transform
                fftw_execute_dft_c2r(plan_ifftAScanComplexHelp, aScanComplex, help);

				for (i = 0; i < numSamples; i++) {
					if (aScan[i] >= 0) {
						aScanArrayProcessed[iScanStart + i] = help[i];
					}
					else {
						aScanArrayProcessed[iScanStart + i] = aScan[i];
					}
				}

			}
			else {
                // back transform
               fftw_execute_dft_c2r(plan_ifftAScanComplexHelp, aScanComplex, help);

                memcpy(&aScanArrayProcessed[iScanStart], help, sizeof(help));
			}

			if (paramStruct->useOptPulse == 1) {
				deleteTransmissionSignal(&aScanArrayProcessed[iScanStart], numSamples, paramStruct->windowLength, --startPos[numScan], --endPos[numScan]);
			}

		}
        
        if (aScanComplex != NULL) fftw_free(aScanComplex);
        if (aScan != NULL) fftw_free(aScan);
        if (help != NULL) fftw_free(help);


	}

    // clean
    if (plan_fftAScanAScanComplex != NULL) fftw_destroy_plan(plan_fftAScanAScanComplex);
    if (plan_ifftAScanComplex != NULL) fftw_destroy_plan(plan_ifftAScanComplex);
    if (paramStruct->useOptPulse == 1 && plan_fftHelpAScanComplex != NULL) fftw_destroy_plan(plan_fftHelpAScanComplex);
    if(plan_ifftAScanComplexHelp!=NULL) fftw_destroy_plan(plan_ifftAScanComplexHelp);

 
    if (loadedWisdomUsed == 0 && fftw_export_wisdom_to_filename(filenameFftwWisdom)){
        printf("Wisdom saved.\n");
    }

    fftw_forget_wisdom();
//     fftw_cleanup();

	return;
}