#include "v3norm.h"

#ifdef NIOS2_TARGET

void debug_v3norm() {
	fix16_t value_buffer[192];
	for (int i = 0; i < 192; i++) {
		alt_irq_context status;
		uint32_t time;
		status = alt_irq_disable_all();
		IOWR_ALTERA_AVALON_TIMER_SNAPL(SYS_TIMER_BASE,0); //freeze timestamp
		time = IORD_ALTERA_AVALON_TIMER_SNAPL(SYS_TIMER_BASE); // read lower 16 bit of time stamp
		time |= (IORD_ALTERA_AVALON_TIMER_SNAPH(SYS_TIMER_BASE) << 16); //load upper 16 bits
		alt_irq_enable_all (status);
		time = (time << 16) | (time >> 16);
		value_buffer[i] = time;
	}
	v3norm(value_buffer, 64);
}

void ci_mul_tests()
{
	const int N = 5;
	fix16_t tests[5] = {
		0x00010000, // one
		0x11110000, // -one
		0xfff842ab, // x
		0xffe2859a, // y
		0x001e3999, // y
	};
	int failed = 0;
	printf("running mul tests\n");
	for (int i = 0; i < N; i++) {
		for (int j = i; j < N; j++) {
			fix16_t a = tests[i];
			fix16_t b = tests[j];
			fix16_t hw_res = (fix16_t)ALT_CI_CI_MUL_0(a, b);
			fix16_t sw_res = fix16_mul(a, b);
			if (hw_res != sw_res) {
				printf("test %2d: %ld [%8.lx] * %ld [%8.lx] = %ld [%8.lx] (actual %ld [%8.lx])\n", i * 4 + j, a, a, b, b, hw_res, hw_res, sw_res, sw_res);
				failed++;
			}
		}
	}
	printf("failed tests: %d out of %d\n", failed, N * (N + 1) / 2);
}

void ci_div_tests()
{
	const int EPSILON = 0;
	const int N = 5;
	fix16_t tests[5] = {
		0x00010000, // one
		0x11110000, // -one
		0xfff842ab, // x
		0xffe2859a, // y
		0x001e3999, // y
	};
	int failed = 0;
	printf("running div tests\n");
	for (int i = 0; i < N; i++) {
		for (int j = 0; j < N; j++) {
			fix16_t a = tests[i];
			fix16_t b = tests[j];
			//(void)ALT_CI_CI_DIV_0(DIV_ENQUEUE, a, b);
			HW_DIV_WRITE(a, b);
			fix16_t sw_res = fix16_div(a, b);
			//fix16_t hw_res = (fix16_t)ALT_CI_CI_DIV_0(DIV_DEQUEUE, 0, 0);
			fix16_t hw_res = HW_DIV_READ();
			if (fix16_abs(hw_res - sw_res) > EPSILON) {
				printf("test %2d: %ld [%8.lx] / %ld [%8.lx] = %ld [%8.lx] (actual %ld [%8.lx])\n", i * 4 + j, a, a, b, b, hw_res, hw_res, sw_res, sw_res);
				failed++;
			}
		}
	}
	printf("failed tests: %d out of %d\n", failed, N * N);
}

void ci_sqrt_tests()
{
	const int EPSILON = 0;
	const int N = 5;
	fix16_t tests[5] = {
		0x00010000, // one
		0x11110000, // -one
		0xfff842ab, // x
		0xffe2859a, // y
		0x001e3999, // y
	};
	int failed = 0;
	printf("running sqrt tests\n");
	for (int i = 0; i < N; i++) {
		for (int j = i; j < N; j++) {
			fix16_t a = tests[i];
			fix16_t b = tests[j];
			fix16_t r = fix16_mul(a, b);
			if (r < 0)
				continue;
			fix16_t sw_res = fix16_sqrt(r);
			IOWR(AVALON_MM_SQRT_0_BASE, 0, r);
			// wait for sqrt result
			while (IORD(AVALON_MM_SQRT_0_BASE, 0));
			fix16_t hw_res = IORD(AVALON_MM_SQRT_0_BASE, 1);
			if (fix16_abs(hw_res - sw_res) > EPSILON) {
				printf("test %2d: sqrt(%ld) [sqrt(%8.lx)] = %ld [%8.lx] (actual %ld [%8.lx])\n", i * 4 + j, r, r, hw_res, hw_res, sw_res, sw_res);
				failed++;
			}
		}
	}
	printf("failed tests: %d out of %d\n", failed, N * (N + 1) / 2);
}

/*
void v3norm_original(fix16_t* values, uint32_t count)
{
	#ifdef SW_MUL
	#define mul(a, b) fix16_mul((a), (b))
	#else
	#define mul(a, b) HW_MUL((a), (b))
	#endif

	for (uint32_t i = 0; i < count; i++) {
	
		fix16_t x = values[3 * i];
		fix16_t y = values[3 * i + 1];
		fix16_t z = values[3 * i + 2];

		fix16_t sum = fix16_add(fix16_add(mul(x, x), mul(y, y)), mul(z, z));
		#ifdef SW_SQRT
		fix16_t len = fix16_sqrt(sum);
		#else
		IOWR(AVALON_MM_SQRT_0_BASE, 0, sum);
		// wait for sqrt result
		while (IORD(AVALON_MM_SQRT_0_BASE, 0));
		fix16_t len = IORD(AVALON_MM_SQRT_0_BASE, 1);
		#endif

		#ifndef CALCULATION_METHOD 
			#error "CALCULATION_METHOD must be set to either MUL (0) or DIV (1)"
		#endif

		#if CALCULATION_METHOD == DIV
		#ifdef SW_DIV
		fix16_t x_norm = fix16_div(x,len);
		fix16_t y_norm = fix16_div(y,len);
		fix16_t z_norm = fix16_div(z,len);
		#else
		HW_DIV_WRITE(x, len);
		HW_DIV_WRITE(y, len);
		HW_DIV_WRITE(z, len);
		fix16_t x_norm = HW_DIV_READ();
		fix16_t y_norm = HW_DIV_READ();
		fix16_t z_norm = HW_DIV_READ();
		#endif
		#elif CALCULATION_METHOD == MUL
		#ifdef SW_DIV
		fix16_t len_inv = fix16_div(fix16_one, len);
		#else
		HW_DIV_WRITE(fix16_one, len);
		fix16_t len_inv = HW_DIV_READ();
		#endif
		fix16_t x_norm = mul(x, len_inv);
		fix16_t y_norm = mul(y, len_inv);
		fix16_t z_norm = mul(z, len_inv);
		#endif
		
		values[3 * i]     = x_norm;
		values[3 * i + 1] = y_norm;
		values[3 * i + 2] = z_norm;
	}
}
*/

void v3norm(fix16_t* values, uint32_t count)
{
	const int N = 4;
	uint32_t rest = (count % N);
	uint32_t split = count - rest;
	fix16_t pos[N];
	for (uint32_t i = 0; i < split; i += N) {
		for (uint32_t j = 0; j < N; j++) {
			pos[j] = 3 * (i + j);
		}
		for (uint32_t j = 0; j < N; j++) {
			fix16_t x = values[pos[j]];
			fix16_t y = values[pos[j] + 1];
			fix16_t z = values[pos[j] + 2];
			fix16_t len = HW_MUL(x, x) + HW_MUL(y, y) + HW_MUL(z, z);
			IOWR(AVALON_MM_SQRT_0_BASE, 0, len);
		}
		for (uint32_t j = 0; j < N; j++) {
			while (IORD(AVALON_MM_SQRT_0_BASE, 0));
			fix16_t len = IORD(AVALON_MM_SQRT_0_BASE, 1);
			HW_DIV_WRITE(fix16_one, len);
		}
		for (uint32_t j = 0; j < N; j++) {
			fix16_t len_inv = HW_DIV_READ();
			fix16_t x_norm = HW_MUL(values[pos[j]],     len_inv);
			fix16_t y_norm = HW_MUL(values[pos[j] + 1], len_inv);
			fix16_t z_norm = HW_MUL(values[pos[j] + 2], len_inv);
			values[pos[j]]     = x_norm;
			values[pos[j] + 1] = y_norm;
			values[pos[j] + 2] = z_norm;
		}
	}

	//printf("processed %ld elements\n", split);
	//printf("processing remaining %ld elements\n", rest);

	for (uint32_t i = split; i < count; i++) {
		fix16_t x = values[3 * i];
		fix16_t y = values[3 * i + 1];
		fix16_t z = values[3 * i + 2];
		fix16_t sum = fix16_add(fix16_add(HW_MUL(x, x), HW_MUL(y, y)), HW_MUL(z, z));
		IOWR(AVALON_MM_SQRT_0_BASE, 0, sum);
		while (IORD(AVALON_MM_SQRT_0_BASE, 0));
		fix16_t len = IORD(AVALON_MM_SQRT_0_BASE, 1);
		HW_DIV_WRITE(fix16_one, len);
		fix16_t len_inv = HW_DIV_READ();
		fix16_t x_norm = HW_MUL(x, len_inv);
		fix16_t y_norm = HW_MUL(y, len_inv);
		fix16_t z_norm = HW_MUL(z, len_inv);
		values[3 * i]     = x_norm;
		values[3 * i + 1] = y_norm;
		values[3 * i + 2] = z_norm;
	}
}

#endif
