#include ".\anglewindow.h"

#include <iostream>

using namespace std;


AngleWindow::AngleWindow(int L)
{
	length = L;
	angleHist = new double[length];
	temp = new double[length];
	for (int i = 0; i < length; i++)
	{
		angleHist[i] = 0;
		temp[i] = 0;
	}

	currentIndex = 0;
	lastAngIndex = length-1;
}

AngleWindow::~AngleWindow(void)
{
	delete [] angleHist;
	delete [] temp;
}

void AngleWindow::ResetHist()
{
	currentIndex = 0;
	for (int i = 0; i < length; i++)
	{
		angleHist[i]= 0;
		temp[i]=0;
	}
}

void AngleWindow::addAngle(double angle)
{
	angleHist[currentIndex] = angle;
	currentIndex++;
	
	if (currentIndex > lastAngIndex)
	{
		currentIndex = 0;
	}
}
double AngleWindow::computeMedian()
{
	int j;
	double val;

		for(int i = 0; i<length; i++)
		{
			temp[i] = angleHist[i];
		}

		for (i = 0; i<length; i++)
		{
			val = angleHist[i];

			for (j = 0; j < i; j++)
			{
				if(temp[j]>val)
				{
					break;
				}
            }

			for (int k = i-1; k>=j; k--)
			{
				temp[k+1] = temp[k];
			}
			temp[j] = val;
		}

	return temp[length/2];
	
}