#include <string.h>
#include <limits>
#include "../include/data_filters.h"
#include <cmath>

pavo_response_scan_t* DataFilter::Filter(pavo_response_scan_t* data_buffer, int count)
 {return data_buffer;}

pavo_response_pcd_t* DataFilter::Filter(pavo_response_pcd_t* data_buffer, int count)
{return data_buffer;}

DataFilter::~DataFilter() {}


MADataFilter::MADataFilter(int scale, int distance):
      _point_scale(scale),
      _delta_distance(distance)
    {
	   _distance_diff=50;
	   _intensity_diff=10;
	   _window_size=5;
	    enable_tailfilter=false;
	   _window_vector.resize(_window_size);
    }


pavo_response_scan_t* MADataFilter::Filter(pavo_response_scan_t* data_buffer, int count, bool m_first)
{
    if(count < this->_point_scale)
        return data_buffer;

    if(enable_tailfilter)
	{
      HandlTialFilter(data_buffer, count);
      return data_buffer;
	}

    pavo_response_scan_t* copy_ptr = new pavo_response_scan_t[count];
    memcpy(copy_ptr, data_buffer, sizeof(pavo_response_scan_t)*count);
    for(int i=0; i<count; i++)
    {
        uint16_t left_lpp = LeftLpp(data_buffer, i, count);
	    uint16_t middle_lpp = MiddleLpp(data_buffer, i, count);
        uint16_t right_lpp = RightLpp(data_buffer, i, count);
	
        int choice = 0;
	if(m_first)
		choice = ChooseLppM(left_lpp, middle_lpp, right_lpp );
	else
		choice = ChooseLpp(left_lpp, middle_lpp, right_lpp );
        switch(choice)
        {
        case 1:
	    copy_ptr[i].distance = this->_avg_left;
            break;
        case 2:
            copy_ptr[i].distance = this->_avg_middle;
            break;
        case 3:
            copy_ptr[i].distance = this->_avg_right;
            break;
        }
    }
    memcpy(data_buffer, copy_ptr, sizeof(pavo_response_scan_t)*count);
    delete[] copy_ptr;
    return data_buffer;
}

pavo_response_pcd_t* MADataFilter::Filter(pavo_response_pcd_t* data_buffer, int count, bool m_first)
{
    return data_buffer;
}


int MADataFilter::GetScale()
{
    return _point_scale;
}
void MADataFilter::SetScale(int scale)
{
    _point_scale = scale;
}

uint16_t MADataFilter::GetDistance()
{
    return _delta_distance;
}

void MADataFilter::SetDistance(uint16_t distance)
{
   _delta_distance = distance;
}


int MADataFilter::ChooseLpp(uint16_t left_lpp, uint16_t middle_lpp, uint16_t right_lpp)
{
    int choice = 0;
    uint16_t min = _delta_distance;
    if(left_lpp < min)
    {
        choice = 1;
        min = left_lpp;
    }

    if(middle_lpp < min)
    {
        choice = 2;
        min = middle_lpp;
    }

    if(right_lpp < min)
    {
        choice = 3;
        min = right_lpp;
    }

    return choice;
         
}

int MADataFilter::ChooseLppM(uint16_t left_lpp, uint16_t middle_lpp, uint16_t right_lpp)
{
    int choice = 0;
    uint16_t min = _delta_distance;

    if(middle_lpp < min)
    {
        choice = 2;
        min = middle_lpp;
	return choice;
    }

    if(left_lpp < min)
    {
        choice = 1;
        min = left_lpp;
    }



    if(right_lpp < min)
    {
        choice = 3;
        min = right_lpp;
    }

    return choice;
         
}

uint16_t MADataFilter::LeftLpp(pavo_response_scan_t* data_buffer, int idx, int capacity)
{
    uint16_t min = std::numeric_limits<uint16_t>::max();
    uint16_t max = 0;
    if(idx < _point_scale - 1)
        return _delta_distance + 1;

    this->_avg_left = 0;

    for(int i=0; i<_point_scale;i++)
    {
        if(data_buffer[idx-i].distance==0)
            return _delta_distance + 1;
            
        this->_avg_left += data_buffer[idx-i].distance;
        if(data_buffer[idx-i].distance < min)
        {
            min = data_buffer[idx-i].distance;
        }
        if(data_buffer[idx-i].distance > max)
        {
            max = data_buffer[idx-i].distance;
        }
    } 
    this->_avg_left /= _point_scale; 
    return max - min;
       
}

uint16_t MADataFilter::MiddleLpp(pavo_response_scan_t* data_buffer, int idx, int capacity)
{
    uint16_t min = std::numeric_limits<uint16_t>::max();
    uint16_t max = 0;
    if(idx < _point_scale/2 || idx + _point_scale/2 > capacity )
        return _delta_distance + 1;

    int start = idx - _point_scale/2;
    this->_avg_middle = 0;
    for(int i=0; i<_point_scale;i++)
    {
        if(data_buffer[start+i].distance==0)
            return _delta_distance + 1;
        this->_avg_middle += data_buffer[start+i].distance;
        if(data_buffer[start+i].distance < min)
        {
            min = data_buffer[start+i].distance;
        }
        if(data_buffer[start+i].distance > max)
        {
            max = data_buffer[start+i].distance;
        }
    } 
    this->_avg_middle /= _point_scale;
    return max - min;
}

uint16_t MADataFilter::RightLpp(pavo_response_scan_t* data_buffer, int idx, int capacity)
{
    uint16_t min = std::numeric_limits<uint16_t>::max();
    uint16_t max = 0;
    if(idx + _point_scale > capacity - 1 )
        return _delta_distance + 1;

    this->_avg_right = 0;
    for(int i=0; i<_point_scale;i++)
    {
        if(data_buffer[idx+i].distance==0)
            return _delta_distance + 1;
         this->_avg_right += data_buffer[idx+i].distance;
        if(data_buffer[idx+i].distance < min)
        {
            min = data_buffer[idx+i].distance;
        }
        if(data_buffer[idx+i].distance > max)
        {
            max = data_buffer[idx+i].distance;
        }
    } 
    this->_avg_right /= _point_scale;
    return max - min;
}

pavo_response_scan_t*  MADataFilter::HandlTialFilter(pavo_response_scan_t* data_buffer, int count)
{
    int start,end,ret;
    pavo_response_scan_t* copy_ptr = new pavo_response_scan_t[count];
    pavo_response_scan_t* copy_ptr_org = new pavo_response_scan_t[count];
    memcpy(copy_ptr, data_buffer, sizeof(pavo_response_scan_t)*count);
    memcpy(copy_ptr_org, data_buffer, sizeof(pavo_response_scan_t)*count);

    SetWindow(5);
    if(count>3500)
    {
      SetDistance(50.0);
    }
    if(count<=3500&& count>2500)
    {
      SetDistance(100.0);
    }

    for(int i=0; i<count;)
    {
        start = SearchSkipPoint(copy_ptr_org,i, count);
        if(start>=count) break;
	    end = SearchContinuityPoint(copy_ptr_org, start, count);
        ret = HandlePoint(copy_ptr, start,end, count);
        if(ret>=count) break;
        i=ret;
    }
    memcpy(data_buffer, copy_ptr, sizeof(pavo_response_scan_t)*count);
    delete[] copy_ptr;
    delete[] copy_ptr_org;
    return data_buffer;
}

uint16_t MADataFilter::SearchSkipPoint(pavo_response_scan_t* data_buffer, int idx, int capacity)
{
    int start,count;
    int window_count;
    start=idx;
    count=capacity;
    int i,ret;
    double distance1,distance2;
    bool is_skip=true;
    double distance_diff=0;
    double sum,avg;

    //_window_vector.clear();
    //init the window data
    window_count=0;
    sum=avg=0;
    if(start<_window_size)
    {
    	for(int j=0;window_count<_window_size &&j<count;j++)
    	{
    		ret=j;
    		_window_vector[window_count%_window_size]=j;
    		if(j>=start && j!=0)
    		{
    			distance1=data_buffer[j].distance;
    			distance_diff=std::abs(distance1-avg);
    			if(distance_diff>_distance_diff)
    			{
    			   return ret;
    			}
    		}
    		sum+=data_buffer[j].distance;
    		avg=sum/(window_count+1);
    		window_count++;
    	}
    }
    else
    {
    	for(int j=start-_window_size;window_count<_window_size &&j<count;j++)
    	{
    		ret=j;
    		_window_vector[window_count%_window_size]=j;
       		sum+=data_buffer[j].distance;
        	avg=sum/(window_count+1);
        	window_count++;
    	}
    }
    if(window_count<_window_size) return ret;
    window_count=0;
    for(i=ret+1;i<count;i++)
    {
    	ret=i;
    	distance1=data_buffer[i].distance;
    	distance_diff=std::abs(distance1-avg);
		if(distance_diff>_distance_diff)
		{
		   return ret;
		}
		sum=sum-data_buffer[_window_vector[window_count]].distance;
		sum=sum+data_buffer[i].distance;
		avg=sum/(window_count+1);
		_window_vector[window_count]=i;
		window_count++;
		window_count%=_window_size;
    }
	return ret;

}

uint16_t MADataFilter::SearchContinuityPoint(pavo_response_scan_t* data_buffer, int idx, int capacity)
{
    int start,count;
    int window_count;
    int i,ret;
    double distance1,distance2;
    double distance_diff=0;
    double sum,avg;
    bool is_continuity=true;

    start=idx;
    count=capacity;
    //_window_vector.clear();
    //init the window data
    window_count=0;
    sum=avg=0;
    if(start<_window_size)
    {
    	for(int j=0;window_count<_window_size &&j<count;j++)
    	{
    		ret=j;
    		_window_vector[window_count%_window_size]=j;
    		if(j>=start && j!=0)
    		{
    			distance1=data_buffer[j].distance;
    			distance_diff=std::abs(distance1-avg);
    			if(distance_diff<_distance_diff)
    			{
    			   return ret;
    			}
    		}
    		sum+=data_buffer[j].distance;
    		avg=sum/(window_count+1);
    		window_count++;
    	}
    }
    else
    {
    	for(int j=start-_window_size;window_count<_window_size &&j<count;j++)
    	{
    		ret=j;
    		_window_vector[window_count%_window_size]=j;
       		sum+=data_buffer[j].distance;
        	avg=sum/(window_count+1);
        	window_count++;
    	}
    }
    if(window_count<_window_size) return ret;
    window_count=0;
    for(i=ret+1;i<count;i++)
    {
    	ret=i;
    	distance1=data_buffer[i].distance;
    	distance_diff=std::abs(distance1-avg);
		if(distance_diff<_distance_diff)
		{
		   return ret;
		}
		sum=sum-data_buffer[_window_vector[window_count]].distance;
		sum=sum+data_buffer[i].distance;
		avg=sum/(window_count+1);
		_window_vector[window_count]=i;
		window_count++;
		window_count%=_window_size;
    }
	return ret;
}

uint16_t MADataFilter::HandlePoint(pavo_response_scan_t* data_buffer, int start,int end, int capacity)
{
	int i;
	int count=capacity;
	int half=(start+end)/2;
	for(i=start;i<end && i<count;i++)
	{
		if(data_buffer[i].distance>=1500)
		{
			continue;
		}
		if(i<half)
		{
			if(start>=1)
			{
				data_buffer[i].distance=data_buffer[start-1].distance;
			}
			else
			{
				data_buffer[i].distance=0;
			}
		}
		else
		{
			if(end>=1)
			{
				data_buffer[i].distance=data_buffer[end].distance;
			}
			else
			{
				data_buffer[i].distance=0;
			}
		}
	}
	return end+1;

}

void MADataFilter::SetWindow(int size)
{
	if(size)
	{
		_window_size=size;
		_window_vector.resize(size);
	}
	else
	{
		_window_size=5;
		_window_vector.resize(5);
	}

}

void MADataFilter::SetDistance(double size)
{
	if(size)
	{
		_distance_diff=size;
	}
	else
	{
		_distance_diff=5.0;
	}

}

void MADataFilter::SetIntensity(double size)
{
	_intensity_diff=size;
}

void MADataFilter::EnableTailFilter(bool en)
{
	enable_tailfilter=en;
}

