#include <stdio.h>
#include <stdlib.h>
#include <cstring>
#include <algorithm>
#include <cmath>

#include "mathMethods.hpp"

using namespace std; 

#define PI 3.141592653589793

/*
 * calculates energy sum form energy move and energy pot
 *
 */
void energyEstimation(double* partData, int numElements, double* energySum) {

	//calculate energy sum
	energySum[0] = partData[0];
	diff(partData, numElements, energySum);

	numElements--; // get last element index

	for (int i = 0; i < numElements; i++) {
		energySum[i] += energySum[i + 1];
		energySum[i] = (energySum[i] * energySum[i]) * 0.5;
		energySum[i] += (partData[i] * partData[i]);
	}

	energySum[numElements] = (energySum[numElements] * energySum[numElements]) * 0.5;
	energySum[numElements] += partData[numElements] * partData[numElements];
}

/*
 * detection and deletion of transmission signal in given window (defined by start, endPos)
 *
 */
void deleteTransmissionSignal(double* aScan, int numSamples, int windowLength, int startPos, int endPos) {
    
    // variable definitions

	// counter
	int i, j;
    
    int index, index2;
    int l_partData;
    
	l_partData = endPos - startPos; // num Data
	l_partData++;

	double partData[l_partData], energySum[l_partData];
    
    // cutting relevant part from aScan and weightning over range
	j = startPos;	
	for (i = 0; i < l_partData; i++) {
		partData[i] = aScan[j] * (0.75 - 0.25 * cos((2 * PI * i / (l_partData - 1))));
		j++;
	}

	// calculate energy
	energyEstimation(partData, l_partData, energySum);

	// calculate mean of energy along dimension
	double mean_energySum[l_partData + windowLength] = { 0 };
	for (i = 0; i < windowLength; i++) {
		for (j = 0; j < l_partData; j++) {
			mean_energySum[i + j] += energySum[j];
		}
	}

	// windowing of mean energy
	int num = (l_partData + windowLength - round((double)(windowLength - 0.001) / 2.0)) - windowLength / 2 + 1;
	double mean_energySum2[num];
	for (i = 0; i < num; i++) {
		mean_energySum2[i] = mean_energySum[windowLength / 2 + i - 1];

	}

	// calculate g
	double g[num];
	g[0] = 0;
	diff(mean_energySum2, num, g);
	addFollower(g, num);

	// calculate f
	double f[num];
	f[0] = g[0];
	diff(g, num, f);
	addFollower(f, num);

	for (i = 0; i < num; i++) {
		if (f[i] < 0 || g[i] < 0) {
			f[i] = 0;
		}
	}

    int peak = distance(g, max_element(g, g + num));
	index = distance(f, max_element(f, f + peak));
	double medianMeanEnergySum = findMedian(mean_energySum2, num);
	
    // SEEK for MAX == begin
	if (mean_energySum2[index] > medianMeanEnergySum) {
		while (mean_energySum2[index] >= medianMeanEnergySum && index > 0) {
			--index;
		}
	}

	// SEEK for NEXT MAX == end
	index2 = peak;
	while (mean_energySum2[index2] >= medianMeanEnergySum && index2 < num) {
		index2++;
	}
	if (index2 >= num) {
		index2 = num - 2 ;// take second last element
	}

    // absolute start/end psoitions
	endPos = index2 + startPos;
	startPos += index;

	// remove transmission signal    
	memset(&aScan[startPos], 0, sizeof(double)*(endPos - startPos + 1));

	return;

}