#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 SingleCandidateTHR,
	const float DoubleCandidateTHR,
	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;
        int checkDouble;
	 
// 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];
                checkDouble = 0;
		if ( diff < SingleCandidateTHR ){ // a single spike candidate
			// check consistency with a single channel spike
                        nextDiff = src[i+1] - (*NextNeighborMean)[i+1];	
			if ( nextDiff > -1.6 * diff )
			{
				dest[i+1] = ( src[i] + src[i+3] )/2.;
				i = i + 2;
				printf("single spike\n");
			}
			else{
			  // do notthing - not really a single spike, but check if it is a double
			  checkDouble = 1;
			}
		}
		if (diff < DoubleCandidateTHR || checkDouble == 1) { // a double spike candidate
			nextDiff = src[i+1] - (*NextNeighborMean)[i+1];
			nextNextDiff = src[i+2] - (*NextNeighborMean)[i+2];
			//printf("double spike candidate\n");
			//		printf("%d %f %f %f\n", i, diff, nextDiff, nextNextDiff);
			//		printf("%f %f %f\n", DoubleCandidateTHR, nextDiffTHR, nextNextDiffTHR);
					// check the consistency with a double spike
			if ( ( nextDiff > nextDiffTHR * DoubleCandidateTHR * -1. ) && ( nextNextDiff > nextDiffTHR * DoubleCandidateTHR * -1. ) ){
		                        dest[i+1] = (src[i] + src[i+3]) / 2. ;
		                        dest[i+2] = (src[i] + src[i+3]) / 2.;
					  //(*NextNeighborMean)[i+2] = (src[i+1] - src[i+3] / 2.);
					i = i + 3; //do not care about the next sample it was the spike
					printf("double spike\n");
				}
				// treatment for the end of the pipeline must be added !!!
		   }
	} // end of spike search and correction
	delete NextNeighborMean;
	return;
}