#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <sys/times.h>
#include <sys/time.h>

#define	CPU_FREQ_HZ	100000000
#include "libsys.h"
#include "irq.h"

UINT32 buffer[16384];

#define	T_SAMPLE		5000
#define	TIMEOUT			1000

typedef struct _bar_t
{
	UINT32 T;
	UINT32 black;
} bar_t;

enum
{
	state_idle = 0,
	state_sample,
	state_finish
};

bar_t bars[1024];

void bsort(float *pData, UINT32 len)
{
	float t;
	int i, j;

	for(i=len-1; i>=0; i--)
	{
		for(j=1; j<len; j++)
		{
			if (pData[j-1] > pData[j])
			{
				t = pData[j-1];
				pData[j-1] = pData[j];
				pData[j] = t;
			}
		}
	}
}

typedef struct _smedian_t
{
	UINT32 N;
	UINT32 is_dirty;
	float *pBuf, *pSorted;
	UINT32 w;
	float median;
} median_t;

void Median_init(median_t *pObj, UINT32 N, float init_val)
{
	int i;
	pObj->pBuf = (float*)malloc(N*sizeof(float));
	pObj->pSorted = (float*)malloc(N*sizeof(float));
	for (i=0; i < N; i++)
		pObj->pBuf[0] = init_val;
	pObj->median = init_val;
	pObj->is_dirty = 0;
	pObj->w = 0;
	pObj->N = N;
}

void Median_free(median_t *pObj)
{
	if (pObj->pBuf)
		free(pObj->pBuf);

	if (pObj->pSorted)
		free(pObj->pSorted);

	pObj->pBuf = NULL;
	pObj->pSorted = NULL;

}

void Median_value_add(median_t *pObj, float value)
{
	pObj->is_dirty = 1;
	pObj->pBuf[pObj->w++] = value;

	if (pObj->w == pObj->N)
		pObj->w = 0;

}

float Median_get(median_t *pObj)
{
	if (!pObj->is_dirty)
		return pObj->median;

	memcpy(pObj->pSorted, pObj->pBuf, pObj->N*sizeof(float));

	bsort(pObj->pSorted, pObj->N);

	if (pObj->N&1)
	{
		pObj->median = pObj->pSorted[(pObj->N-1)/2];
	}
	else
	{
		pObj->median = 0.5f*(pObj->pSorted[pObj->N/2] + pObj->pSorted[pObj->N/2+1]);
	}

	pObj->is_dirty = 0;

	return pObj->median;
}

// -------------------------------------------------------------
UINT32 detect_module(UINT32 T, UINT32 *pBC)
{
	UINT32 m, best_M;
	float err, kc, best_err;

	best_err = 4.f;
	best_M = 0;

	for (m=0; m < 4; m++)
	{
		kc = (float)T/pBC[m];
		err = fabs(kc - 1);

		if (err < best_err)
		{
			best_M = m+1;
			best_err = err;
		}
	}

	return best_M;
}

void handler7(void)
{
	UINT32 volatile *pTim_ctrl = (UINT32*)SYS_ITIM_CTRL;
	UINT32 volatile *pTim_stat = (UINT32*)SYS_ITIM_STAT;
	UINT32 volatile *pPS20_stat = (UINT32*)SYS_PS2_0_STAT;
	UINT32 volatile *pPS21_stat = (UINT32*)SYS_PS2_1_STAT;
	static UINT32 state, bar_count;
	static UINT32 timeout_count, timeout_reload;
	static UINT32 white, pin_last, skip, start;
	static UINT32 module_cnt_w, module_cnt_b, Tmod0_w, Tmod0_b, M, T0;
	float ratio, error, kc;
	UINT32 i, j;
	static median_t med_b, med_w;
	static UINT32 bc_b[4], bc_w[4];

	if (*pTim_stat & 1)
	{
		*pTim_stat = 1;
		white = ((*pPS20_stat & SYS_PS2_BIT_PIN_DATA) == 0);

		switch (state)
		{
			case state_idle:
			timeout_reload = TIMEOUT;
			if (white != pin_last)
			{
				if (white)
					break;

				state = state_sample;
				module_cnt_b = 0;
				module_cnt_w = 0;
				Tmod0_w	= 0xFFFFFFFF;
				Tmod0_b	= 0xFFFFFFFF;
				bar_count = 0;
				Median_init(&med_b, 3, 1);
				Median_init(&med_w, 3, 1);
			}
			break;

			case state_sample:
			if (!timeout_count)
			{
				state = state_finish;
				break;
			}

			if (white)
			{
				module_cnt_w++;
			}
			else
			{
				module_cnt_b++;
			}
			if (white != pin_last)
			{
				if (pin_last)
				{
					if (bar_count < 10)
					{
						if (1.5*module_cnt_w < Tmod0_w)
						{
							Tmod0_w = module_cnt_w;
						}
					}
					timeout_reload = 10*Tmod0_w;
					bars[bar_count].T = module_cnt_w;
					bars[bar_count].black = 0;
					module_cnt_w = 0;

				}
				else
				{
					if (bar_count < 10)
					{
						if (1.5*module_cnt_b < Tmod0_b)
						{
							Tmod0_b = module_cnt_b;
						}
					}
					timeout_reload = 10*Tmod0_b;
					bars[bar_count].T = module_cnt_b;
					bars[bar_count].black = 1;
					module_cnt_b = 0;
				}
				bar_count++;

			}
			break;

			case state_finish:
			printf("------------------------------------------------\n");
			printf("          Black                    White        \n");
			skip = 0;
			start = 1;
			for (i=0; i < 4; i++)
			{
				bc_b[i] = (i+1)*Tmod0_b;
				bc_w[i] = (i+1)*Tmod0_w;
			}
			for (i=0; i < bar_count; i++)
			{
				if (bars[i].black)
				{
					M = detect_module(bars[i].T, bc_b);
					if (M)
					{
						kc = (float)bars[i].T/bc_b[M-1];
						error = kc - 1;
						Median_value_add(&med_b, kc);
						printf("M=%d, R=%2.2f, E=%+2.2f [%04d]", M, kc, error, bc_b[M-1]);
						kc = Median_get(&med_b);
						bc_b[M-1] = (UINT32)(kc*bc_b[M-1]);
					}
//					printf("%d", M);

				}
				else
				{
					M = detect_module(bars[i].T, bc_w);
					if (M)
					{
						kc = (float)bars[i].T/bc_w[M-1];
						error = kc - 1;
						Median_value_add(&med_w, kc);
						printf("          M=%d, R=%2.2f, E=%+2.2f [%04d]", M, kc, error, bc_w[M-1]);
						kc = Median_get(&med_w);
						bc_w[M-1] = (UINT32)(kc*bc_w[M-1]);

					}
//					printf("%d", M);
					printf("\n");
				}
			}
			Median_free(&med_b);
			Median_free(&med_w);
			if (bar_count)
				printf("\n");

			state = state_idle;
			break;

		}
		if (white != pin_last)
			timeout_count = timeout_reload;

		pin_last = white;
		if (timeout_count)
			timeout_count--;

	}
}

int main(void)
{

	int i;
	UINT32 volatile *pTim_ctrl = (UINT32*)SYS_ITIM_CTRL;
	UINT32 volatile *pTim_stat = (UINT32*)SYS_ITIM_STAT;
	UINT32 volatile *pTim0_cnt = (UINT32*)SYS_ITIM0_CNT;
	UINT32 volatile *pTim0_cmp = (UINT32*)SYS_ITIM0_CMP;

	*pTim0_cnt = 0;
	*pTim0_cmp = (UINT32)T_SAMPLE;
	*pTim_stat = 1;
	*pTim_ctrl = 3;

	interrupt_register(7, (void*)handler7);

	printf("Jenners BarCoder\n");

	interrupt_enable(7);

	while(1)
	{
	}
	return 0;
}