#include "Region.h"
#include <iostream>

#include "zerosearch.h"

using namespace std;

// searches for zero crossings in a given vector of floats
// for zero crossings in falling edge
// give edge = -1
//
//
// returns pointer to vetctor of ints

vector<Region> *zerosearch(
    vector<float> &input,
    int edge,               // search on rising edge=1, -1:falling
    unsigned int step,      // search in steps of step
    int VerbosityLevel
){

vector<Region> * ZeroPositions = new vector<Region>;
Region currentRegion = {0, 0, 0, 0.0, 0, 0.0, 0.0, 0};
    if (step < 1){
        if (VerbosityLevel > 0)
            cout << "zerosearch - stepsize=" << step
                << " is smaller than 1. returning." << endl;
        return ZeroPositions;
    }
    if (input.size() < step){
        if (VerbosityLevel > 0)
            cout << "zerosearch - input-vector.size()="<< input.size()
            << " is smaller than stepsize=" << step
            << "returning." << endl;
        return ZeroPositions;
    }

    if (edge > 0) // search for zero-x-ings on the rising edge
    {
        for (unsigned int sl = 0 ; sl < (input.size()-step) ; sl+=step)
        {
            if (input[sl] > 0.0) // can't be a rising 0-x-ing
            {
                continue;
            }
            if (input[sl] * input[sl+step] <= 0.0)  // 0-x-ing or both are 0
            {
                currentRegion.begin	= sl;
                currentRegion.end	= sl+step;
                ZeroPositions->push_back(currentRegion);
            }
        }
    }
    else if (edge < 0) // search for zero-x-ings on the falling edge
    {
        for (unsigned int sl = 0 ; sl < (input.size()-step) ; sl+=step)
        {
            if (input[sl] < 0.0) // can't be a falling 0-x-ing
            {
                continue;
            }
            if (input[sl] * input[sl+step] <= 0.0)  // 0-x-ing or both are 0
            {
                currentRegion.begin	= sl;
                currentRegion.end	= sl+step;
                ZeroPositions->push_back(currentRegion);
            }
        }

    }
    else // search for zero-x-ings on both esges
    {
        for (unsigned int sl = 0 ; sl < (input.size()-step) ; sl+=step)
        {
            if (input[sl] * input[sl+step] <= 0.0)  // 0-x-ing or both are 0
            {
                currentRegion.begin	= sl;
                currentRegion.end	= sl+step;
                ZeroPositions->push_back(currentRegion);
            }
        }
    }


    return ZeroPositions;
}

size_t ShiftRegionBy(vector<Region> &src,
    int Shift,
    int VerbosityLevel)
{
    vector<Region>::iterator it;
    // I copy code here ... not good, but I hope nobody is giving VL > 10 ever...
    if (VerbosityLevel > 10){
        for (it = src.begin() ; it < src.end() ; it++){
            cout << " it->begin="<< it->begin;
            it->begin += Shift;
            cout << "--> shifted="<< it->begin << endl;
            cout << " it->end="<< it->end;
            it->end += Shift;
            cout << "--> shifted="<< it->end << endl;
            cout << " it->maxPos="<< it->maxPos;
            it->maxPos += Shift;
            cout << "--> shifted="<< it->maxPos << endl;
        }
    return src.size();
    }

    for (it = src.begin() ; it < src.end() ; it++){
        it->begin += Shift;
        it->end += Shift;
        it->maxPos += Shift;
    }
    return src.size();
}

size_t EnlargeRegion(vector<Region> &src,
    int Left,
    int Right,
    int VerbosityLevel)
{
    vector<Region>::iterator it;
    // I copy code here ... not good, but I hope nobody is giving VL > 10 ever...
    if (VerbosityLevel > 10){
        for (it = src.begin() ; it < src.end() ; it++){
            cout << " it->begin="<< it->begin;
            it->begin -= Left;
            cout << "--> enlarged="<< it->begin << endl;
            cout << " it->end="<< it->end;
            it->end += Right;
            cout << "--> enlarged="<< it->end << endl;
        }
        return src.size();
    }


    for (it = src.begin() ; it < src.end() ; it++){
        it->begin -= Left;
        it->end += Right;
    }
    return src.size();

}

#include <limits>
size_t findAbsMaxInRegions(
    vector<Region> &regions,
    vector<float> &data,
    int VerbosityLevel)
{
    vector<Region>::iterator reg;
    for (reg = regions.begin() ; reg < regions.end() ; reg++){
        reg->maxVal=-numeric_limits<float>::max();
        // 1st check if both ends of the region are in the data-vector
        if (reg->begin < 0) reg->begin=0;
        if ((unsigned int)reg->end > data.size()-1) reg->end=data.size()-1;

        // TODO or TOTHINKOF:
        // Region might overlap ... I don't care .. I just search
        // in a later step Maxima at the same Positions can be removed...
        // of course this means, some searches were needless ...
        //cout << "searching from " << reg->begin << " to " << reg->end << endl;
        for (unsigned int pos=reg->begin; pos <= (unsigned int)reg->end; pos++){
            if (data[pos] > reg->maxVal){
                reg->maxVal = data[pos];
                reg->maxPos = pos;
            }
        }
        if (VerbosityLevel > 3) {
            cout << "findAbsMaxInRegions - found Max at:"
                << reg->maxPos << " with Value:" << reg->maxVal << endl;
        }
    }
    return regions.size();
}

#include <algorithm>
bool compMaxPosOfRegions (Region a, Region b) {
  return (a.maxPos == b.maxPos);
}

size_t removeEqualMaxima(
    vector<Region> &regions,
    int VerbosityLevel)
{
    if (VerbosityLevel > 2){
        cout << "removeEqualMaxima:" << endl;
        cout << "region before contains:" << endl;
        for (unsigned int i=0; i<regions.size(); i++){
            cout << i <<"\t";
            cout << regions[i].begin <<"\t";
            cout << regions[i].end <<"\t";
            cout << regions[i].maxPos <<"\t";
            cout << regions[i].maxVal <<"\t";
            cout << endl;
        }
    }

    vector<Region>::iterator it;
    it = unique (regions.begin(), regions.end() , compMaxPosOfRegions);
    regions.resize( it - regions.begin() );

    if (VerbosityLevel > 1){
        cout << "region now contains:" << endl;
        for (unsigned int i=0; i<regions.size(); i++){
            cout << i <<"\t";
            cout << regions[i].begin <<"\t";
            cout << regions[i].end <<"\t";
            cout << regions[i].maxPos <<"\t";
            cout << regions[i].maxVal <<"\t";
            cout << endl;
        }
    }
    return regions.size();
}

size_t removeRegionOnFallingEdge(
    vector<Region> &regions,
    unsigned int FallingEdgeWidth,
    int VerbosityLevel)
{
if ( FallingEdgeWidth < 1){
    if (VerbosityLevel > 0){
      cout << " removeRegionOnFallingEdge: FallingEdgeWidth  < 1" << endl;
      cout << " FallingEdgeWidth =" << FallingEdgeWidth << endl;
      cout << "returning." << endl;
    return regions.size();
    }
  }
    if (regions.size() < 1){
        if (VerbosityLevel > 3)
            cout << "removeRegionOnFallingEdge: source vector empty." << endl;
        return 0;
    }

 vector<Region>::reverse_iterator it = regions.rbegin();
  while( it != regions.rend()-1 )
  {
  if (VerbosityLevel >10)
        cout << it->maxPos << "\t" << (it+1)->maxPos << "\t" << it->maxPos - (it+1)->maxPos << endl;

    if ( it->maxPos - (it+1)->maxPos < (int)FallingEdgeWidth) {
      if (VerbosityLevel > 3)
        cout << "erasing Region @ " << it->maxPos << endl;
       regions.erase( --it.base() ) ;
      ++it;
    }else
      ++it;
  }

  return regions.size();
}


size_t removeRegionWithMaxOnEdge(
    vector<Region> &regions,
    unsigned int EdgeWidth,
    int VerbosityLevel)
{
    if (EdgeWidth < 1){
        if (VerbosityLevel > 0){
            cout << "removeReginWithMaximaOnEdge: EdgeWidth < 1" << endl;
            cout << "EdgeWidth=" << EdgeWidth << endl;
            cout << "returning." << endl;
        return regions.size();
        }
    }

    vector<Region>::iterator it = regions.begin();
    while( it != regions.end() )
    {
//	cout << it->begin << "\t";
//	cout << it->maxPos << "\t";
//	cout << it->end << "\t";
//	cout << it->maxVal << endl;

        if (it->maxPos < (int)(it->begin+EdgeWidth)) {
            if (VerbosityLevel > 3)
                cout << "erasing Region(max@left edge) " << it->maxPos << endl;
            it = regions.erase( it ) ;
            //++it;
        }else if (it->maxPos > (int)(it->end-EdgeWidth)) {
            if (VerbosityLevel > 3)
                cout << "erasing Region(max@right edge) " << it->maxPos << endl;
            it = regions.erase( it ) ;
            //++it;
        }else
            ++it;

    }

    return regions.size();
}

size_t removeMaximaBelow(
    vector<Region> &regions,
    float threshold,
    int VerbosityLevel)
{
//	if (threshold < 0){
//		if (VerbosityLevel > 0)
//			cout << "removeMaximaBelow: threshold < 0" << endl;
//			cout << "threshold=" << threshold << endl;
//			cout << "returning." << endl;
//		return regions.size();
//	}

    vector<Region>::iterator it = regions.begin();
    while( it != regions.end() )
    {
        if (it->maxVal < threshold ) {
            if (VerbosityLevel > 3){
                cout << "removing max " << it->maxVal << "\t";
                cout << " @ " << it->maxPos << "\t";
                cout << endl;
            }
            it = regions.erase( it ) ;
        }else
            ++it;
    }

    return regions.size();
}

size_t removeMaximaAbove(
    vector<Region> &regions,
    float threshold,
    int VerbosityLevel)
{
    if (threshold < 0){
        if (VerbosityLevel > 0)
            cout << "removeMaximaAbove: threshold < 0" << endl;
            cout << "threshold=" << threshold << endl;
            cout << "returning." << endl;
        return regions.size();
    }

    vector<Region>::iterator it = regions.begin();
    while( it != regions.end() )
    {
        if (it->maxVal > threshold ) {
            if (VerbosityLevel > 3){
                cout << "removing max " << it->maxVal << "\t";
                cout << " @ " << it->maxPos << "\t";
                cout << endl;
            }
            it = regions.erase( it ) ;
        }else
            ++it;
    }

    return regions.size();
}

Region FindAbsMax(
    vector<Region> &regions,
    int VerbosityLevel)
{
    Region AbsMax;
    AbsMax.begin = -1;
    AbsMax.maxPos = -1;
    AbsMax.end = -1;
    AbsMax.maxVal=-numeric_limits<float>::max();
    if (regions.size() < 1)
        return AbsMax;

    for (vector<Region>::iterator it = regions.begin(); it < regions.end(); ++it)
    {
        if (it->maxVal > AbsMax.maxVal ) {
            AbsMax = *it;
        }
    }
    if (VerbosityLevel > 5){
        AbsMax.begin +=0;
    }
    return AbsMax;
}

////////////////
////////////// old Version of the code
/*
    for (unsigned int sl = step; sl < input.size(); sl+=step){
        if (input[sl] * input[sl-] < 0 || input[sl]==0.0 ){ // sign change --> zero crossing OR really zero

            if ( (input[sl-1] - input[sl]) * edge < 0){ // this is the crossing the user wanted

                // check if we go lower than a certain limit in the next few slices
                for ( int lala=0; lala<post; lala++){
                    if (input[sl+lala] > 1.5) {
                        zeroPositions->push_back(sl);
                        sl += lala;
                        break;
                    }
                }

            } else if ( (input[sl-1] - input[sl]) * edge > 0){ // this is the zero x-ing the user did not want

                // do nothing

            } else { // sl and sl-1 are equal .. don't know waht do to...

            }

        }

    } // end of loop over slices - between pre and post

*/


size_t findTimeOfHalfMaxLeft(
    vector<Region> &regions,
    vector<float> &data,
    float baseline,
    int beginRisingEdge,
    int endRisingEdge,
    int VerbosityLevel)
{
    float pulse_size = 0;     //
    float thr = 0;

    //int begin = beginRisingEdge;
    int end = endRisingEdge;
    int counter = 0;
    // local vars for line fitting
    float xmean = 0.0;
    float ymean = 0.0;
    float cov = 0.0;          // empiric covarianve between x and y
    float var = 0.0;          // empiric variance of x
    // result of line fitting
    float slope = 0.0;
    float intercept = 0.0;
    // result of this function -- the position of the half rising edge
    float pos_of_thr_Xing = 0.0;
    int result = 0;

    vector<Region>::iterator reg;
    for (reg = regions.begin() ; reg < regions.end() ; reg++){
        counter = 0;
        // check if linear falling edge is completely in pipeline
        // if not --> delete
        if ( reg->maxPos - end < 0 )
        {
            // delete this region from vector<Region>
            reg = regions.erase( reg ) ;
            --reg;
            continue;
        }

        // The maximum was probably found using smoothed data,
        // so I read the value at the position of the maximum from the data
        // again. So I rely on a well defined maxPos.
        if (VerbosityLevel > 1) cout << "## baseline: " << baseline << endl;
        pulse_size = data[reg->maxPos] - baseline;
        if (VerbosityLevel > 1) cout << "## pulse_size: " << pulse_size << endl;
        thr = pulse_size / 2. + baseline;
        if (VerbosityLevel > 1) cout << "## thr: " << thr << endl;

        // I think 5 slices left of the max there begins the rising edge
        // and I think the rising edge is about 6 slices long.
        // but these numbers are input as parameters
        // beginRisingEdge &
        // endRisingEdge

        // I fit a linear function to the falling edge
        // ALARM check for out of range values...


        for (int slice=reg->maxPos - beginRisingEdge;
                    slice > reg->maxPos - endRisingEdge; --slice)
        {
            xmean += slice;
            ymean += data[slice];
            counter++;
        }
//        xmean /= beginRisingEdge - endRisingEdge;
//        ymean /= beginRisingEdge - endRisingEdge;
        xmean /= counter;
        ymean /= counter;
        if (VerbosityLevel > 2) cout << "## xmean: " << xmean << endl;
        if (VerbosityLevel > 2) cout << "## ymean: " << ymean << endl;

        for (int slice=reg->maxPos - beginRisingEdge;
                    slice > reg->maxPos - endRisingEdge; --slice)
        {
            cov = (data[slice] - ymean) * (slice - xmean);
            var = (slice - xmean) * (slice - xmean);
        }
        if (VerbosityLevel > 2) cout << "## cov: " << cov << endl;
        if (VerbosityLevel > 2) cout << "## var: " << var << endl;

        slope = cov / var;
        intercept = ymean - slope * xmean;
        // now calculate, where the fittet line crosses the threshold
        pos_of_thr_Xing = (thr - intercept) / slope;
        result = (int)(pos_of_thr_Xing + 0.5);


        if (VerbosityLevel > 2)
        {
            cout << "findTimeOfHalfMaxLeft() is still in debugging phase:" << endl;
            cout << "please edit the code in oder to suppress this output:" << endl;
            cout << "threshold: " << thr << endl;
            cout << "slope: " << slope << " [mV/timeslice]" << endl;
            cout << "intercept: " << intercept << "[mV]" << endl;
            cout << "position where line crosses threshold " << pos_of_thr_Xing << endl;
            cout << "converted to slice number " << result << endl;

        }

        reg->halfRisingEdgePos = result;

        reg->distanceEdgeToMax = reg->maxPos - result;
        reg->interceptRisingEdge = intercept;
        reg->slopeOfRisingEdge = slope;
    }
    return regions.size();
}

size_t removeRegionWithToFlatSlope(
    vector<Region> &regions,
    float minSlope,
    int VerbosityLevel)
{
    vector<Region>::iterator it = regions.begin();
    while( it != regions.end() )
    {
        if (it->slopeOfRisingEdge < minSlope ) {
            if (VerbosityLevel > 3){
                cout << "removing max " << it->maxVal << "\t";
                cout << " @ " << it->maxPos << "\t";
                cout << endl;
            }
            it = regions.erase( it ) ;
        }else
            ++it;
    }

    return regions.size();
}