#include "SpikeRemoval.h"

#include <iostream>
using namespace std;

// compute the mean of the left and right neighbors of a channel
vector<float> * computeN1mean( vector<float> &src)
{
	vector<float> * dest = new vector<float>;

	dest->push_back(src[1]);

	for(unsigned int i = 1; i < src.size() - 1; i++){
/*        if (i == 0){ // use right sample es mean
			N1mean[i] = Ameas[i+1];
		}
		else if ( i == Samples-1 ){ //use left sample as mean
			N1mean[i] = Ameas[i-1];
		}
		else{
			N1mean[i] = ( Ameas[i-1] + Ameas[i+1] ) / 2.;
		}
*/
		dest->push_back( ( src[i-1] + src[i+1] ) / 2. );
	}

	dest->push_back(src[src.size() - 1]);
	return dest;
} // end of computeN1mean computation

void removeSpikes(
	vector<float> &src,
	vector<float> &dest,
	const float CandidateTHR,
	const float nextDiffTHR,
	const float nextNextDiffTHR
){
	vector<float> * NextNeighborMean = computeN1mean(src);

	if (src.size() != NextNeighborMean->size())
		//TODO .. if verbositylevel .. say something, use the return code to tell the caller..
	dest.clear();
	dest = src;

	float diff, nextDiff, nextNextDiff;

// find the spike and replace it by mean value of neighbors
	for (unsigned int i = 0; i < src.size(); i++) {

		diff = src[i] - (*NextNeighborMean)[i];

		if ( diff < CandidateTHR ){ // a spike candidate
			// check consistency with a single channel spike

			if ( src[i+2] - ( src[i] + src[i+3] )/2. > 10. )
			{
				dest[i+1] = ( src[i] + src[i+3] )/2.;
				dest[i+2] = ( src[i] + src[i+3] )/2.;
				i = i + 3;
			}
			else
			{
				nextDiff = src[i+1] - (*NextNeighborMean)[i+1];
				nextNextDiff = src[i+2] - (*NextNeighborMean)[i+2];

				if ( ( nextDiff > nextDiffTHR * diff ) && ( nextNextDiff < nextNextDiffTHR ) ){
					dest[i+1] = (*NextNeighborMean)[i+1];
					(*NextNeighborMean)[i+2] = (src[i+1] - src[i+3] / 2.);
					i = i + 2;//do not care about the next sample it was the spike
				}
				// treatment for the end of the pipeline must be added !!!
			}
		}
	} // end of spike search and correction
	delete NextNeighborMean;
	return;
}