
#include <stdio.h>
#include <string.h>
#include <malloc.h>
#include <math.h>

#define TB_HEAD "./tb_head.txt"
#define TB_TAIL "./tb_tail.txt"

#define MAX_LINE_LENGTH 256
#define MAX_OPCODE_LENGTH 8

#define PI_OVER_180 0.017453292519943295769236907684886f

int tb_length;

typedef enum {
	OP_UNKNOWN = 0,

	OP_SETMAT,
	OP_MULMAT,
	OP_RSTMAT,
	OP_XFOV,
	OP_YFOV,
	OP_PT3D,
	OP_LN3D,
	OP_COLOR,
	OP_CLR,
	OP_FLIP,
	OP_PT2D,
	OP_LN2D,

	POP_XROT,
	POP_YROT,
	POP_ZROT,
	POP_SCALE,
	POP_TRANS,
	POP_XYFOV,
} OPCODE;


OPCODE processOpcode(const char *opcode) {
	if(!stricmp(opcode, "setmat"))
		return OP_SETMAT;
	else if(!stricmp(opcode, "mulmat"))
		return OP_MULMAT;
	else if(!stricmp(opcode, "rstmat"))
		return OP_RSTMAT;
	else if(!stricmp(opcode, "xfov"))
		return OP_XFOV;
	else if(!stricmp(opcode, "yfov"))
		return OP_YFOV;
	else if(!stricmp(opcode, "pt3d"))
		return OP_PT3D;
	else if(!stricmp(opcode, "ln3d"))
		return OP_LN3D;
	else if(!stricmp(opcode, "color"))
		return OP_COLOR;
	else if(!stricmp(opcode, "clr"))
		return OP_CLR;
	else if(!stricmp(opcode, "flip"))
		return OP_FLIP;
	else if(!stricmp(opcode, "pt2d"))
		return OP_PT2D;
	else if(!stricmp(opcode, "ln2d"))
		return OP_LN2D;

	else if(!stricmp(opcode, "xrot"))
		return POP_XROT;
	else if(!stricmp(opcode, "yrot"))
		return POP_YROT;
	else if(!stricmp(opcode, "zrot"))
		return POP_ZROT;
	else if(!stricmp(opcode, "scale"))
		return POP_SCALE;
	else if(!stricmp(opcode, "trans"))
		return POP_TRANS;
	else if(!stricmp(opcode, "xyfov"))
		return POP_XYFOV;

	return OP_UNKNOWN;
}

void copyFile(const char *source, FILE *dest) {
	FILE *file;
	char line[MAX_LINE_LENGTH];

	file = fopen(source, "r");
	if(!file)
		return;

	while(!feof(file)) {
		//fscanf(file, "%[^\n]\n", line);
		fgets(line, MAX_LINE_LENGTH, file);
		fprintf(dest, "%s", line);
	}

	fprintf(dest, "\n");

	fclose(file);
}

char *appendStrings(const char *string, const char *suffix) {
	char *result;

	result = (char *)malloc((strlen(string) + strlen(suffix) + 1)*sizeof(char));
	strcpy(result, string);
	strcat(result, suffix);

	return result;
}

void outputFixedPoint(FILE *file, const float x) {
	char result[33];
	float z;
	float y;
	int i;

	if(x < 0) {
		result[0] = '1';
		z = -x;
	}
	else {
		result[0] = '0';
		z = x;
	}

	y = 16384;
	for(i = 1; i < 32; i++) {
		if(z >= y) {
			result[i] = '1';
			z -= y;
		}
		else
			result[i] = '0';

		y /= 2;
	}
	result[32] = '\0';

	if(result[0] == '1') {
		for(i = 1; i < 32; i++) {
			if(result[i] == '0')
				result[i] = '1';
			else
				result[i] = '0';
		}

		for(i = 31; i > 0; i--) {
			if(result[i] == '0') {
				result[i] = '1';
				break;
			}
			else
				result[i] = '0';
		}
	}

	fprintf(file, "%s", result);
}

void outputInteger(FILE *file, const int x) {
	int i;
	int z = x;
	int y = 128;

	for(i = 0; i < 8; i++) {
		if(z >= y) {
			fprintf(file, "1");
			z -= y;
		}
		else
			fprintf(file, "0");
		y = y >> 1;
	}
}


char *createSpaces(int count) {
	char *result;
	int i;

	result = (char *)malloc((count + 1)*sizeof(char));
	for(i = 0; i < count; i++)
		result[i] = ' ';
	result[count] = '\0';

	return result;
}

void outputFixedPointOpcode(FILE *file, const char *opcode, const char *opcode_bin, const int elems, const int elems_per_row, const float *data) {
	int i, j;
	char *spaces;

	spaces = createSpaces(strlen(opcode) + 1);

	fprintf(file, "--%s ", opcode);
	for(i = 0, j = 0; i < elems; i++, j++) {
		if(j == elems_per_row) {
			fprintf(file, "\n--%s", spaces);
			j = 0;
		}
		fprintf(file, "%f ", data[i]);
	}
	fprintf(file, "\n");

	fprintf(file, "    OPCODE_IN <= \"%s\";\n", opcode_bin);

	for(i = 0; i < elems; i++) {
		fprintf(file, "    DATA_IN <= \"");
		outputFixedPoint(file, data[i]);
		fprintf(file, "\";\n");
    fprintf(file, "     while BUSY_OUT1 = '1' loop\n");
    fprintf(file, "       wait for 8 ns;\n");
    fprintf(file, "     end loop;\n");
		fprintf(file, "    wait for 8 ns;\n");
		fprintf(file, "    LOAD <= '1';\n");
		fprintf(file, "    wait for 8 ns;\n");
		fprintf(file, "    LOAD <= '0';\n");

		tb_length += 8;
	}

	if(elems == 0) {
    fprintf(file, "     while BUSY_OUT1 = '1' loop\n");
    fprintf(file, "       wait for 8 ns;\n");
    fprintf(file, "     end loop;\n");
		fprintf(file, "    wait for 8 ns;\n");
		fprintf(file, "    LOAD <= '1';\n");
		fprintf(file, "    wait for 8 ns;\n");
		fprintf(file, "    LOAD <= '0';\n");

		tb_length += 8;
	}
	fprintf(file, "\n");

	free(spaces);
}

int main(int argc, char **argv) {
	FILE *file;
	char *temp;
	FILE *out;
	char opcode[MAX_OPCODE_LENGTH];
	OPCODE op;
	float fbuf[19];
	int ibuf[4];

	if(argc != 2) {
		printf("Please supply a source file!\n");
		return 1;
	}

	file = fopen(argv[1], "r");
	if(!file) {
		printf("Please supply a VALID source file!\n");
		return 2;
	}

	tb_length = 12;

	temp = appendStrings(argv[1], ".vhd");
	out = fopen(temp, "w");
	free(temp);
	copyFile(TB_HEAD, out);

	while(!feof(file)) {
		fscanf(file, "%s", opcode);
		op = processOpcode(opcode);

		switch(op) {
		case OP_SETMAT:
			fscanf(file, "\t%f %f %f %f\n\t%f %f %f %f\n\t%f %f %f %f\n\t%f %f %f %f\n",
				&fbuf[0],  &fbuf[1],  &fbuf[2],  &fbuf[3],
				&fbuf[4],  &fbuf[5],  &fbuf[6],  &fbuf[7],
				&fbuf[8],  &fbuf[9],  &fbuf[10], &fbuf[11],
				&fbuf[12], &fbuf[13], &fbuf[14], &fbuf[15]);

			outputFixedPointOpcode(out, "setmat", "0000", 16, 4, fbuf);
			break;
		case OP_MULMAT:
			fscanf(file, "\t%f %f %f %f\n\t%f %f %f %f\n\t%f %f %f %f\n\t%f %f %f %f\n",
				&fbuf[0],  &fbuf[1],  &fbuf[2],  &fbuf[3],
				&fbuf[4],  &fbuf[5],  &fbuf[6],  &fbuf[7],
				&fbuf[8],  &fbuf[9],  &fbuf[10], &fbuf[11],
				&fbuf[12], &fbuf[13], &fbuf[14], &fbuf[15]);

			outputFixedPointOpcode(out, "mulmat", "0001", 16, 4, fbuf);
			break;
		case OP_RSTMAT:
			fscanf(file, "\n");

			outputFixedPointOpcode(out, "rstmat", "0010", 0, 0, fbuf);
			break;
		case OP_XFOV:
			fscanf(file, "\t%f\n",
				&fbuf[0]);

			fbuf[0] = tanf(PI_OVER_180 * fbuf[0]/2.0f);

			outputFixedPointOpcode(out, "xfov", "0011", 1, 1, fbuf);
			break;
		case OP_YFOV:
			fscanf(file, "\t%f\n",
				&fbuf[0]);

			fbuf[0] = tanf(PI_OVER_180 * fbuf[0]/2.0f);

			outputFixedPointOpcode(out, "yfov", "0100", 1, 1, fbuf);
			break;
		case OP_PT3D:
			fscanf(file, "\t%f %f %f\n",
				&fbuf[0], &fbuf[1], &fbuf[2]);

			outputFixedPointOpcode(out, "pt3d", "0101", 3, 3, fbuf);
			break;
		case OP_LN3D:
			fscanf(file, "\t%f %f %f\n\t%f %f %f\n",
				&fbuf[0], &fbuf[1], &fbuf[2],
				&fbuf[3], &fbuf[4], &fbuf[5]);

			outputFixedPointOpcode(out, "ln3d", "0110", 6, 3, fbuf);
			break;
		case OP_COLOR:
			fscanf(file, "\t%d\n",
				&ibuf[0]);

			fprintf(out, "--color %d\n", ibuf[0]);
			fprintf(out, "    OPCODE_IN <= \"0111\";\n");
			fprintf(out, "    DATA_IN <= \"000000000000000000000000");
			outputInteger(out, ibuf[0]);
			fprintf(out, "\";\n");
      fprintf(out, "    while BUSY_OUT1 = '1' loop\n");
      fprintf(out, "      wait for 8 ns;\n");
      fprintf(out, "    end loop;\n");
			fprintf(out, "    wait for 8 ns;\n");
			fprintf(out, "    LOAD <= '1';\n");
			fprintf(out, "    wait for 8 ns;\n");
			fprintf(out, "    LOAD <= '0';\n");
			fprintf(out, "\n");

			tb_length += 8;
			break;
		case OP_CLR:
			fscanf(file, "\n");

			outputFixedPointOpcode(out, "clr", "1000", 0, 0, fbuf);
			break;
		case OP_FLIP:
			fscanf(file, "\n");

			outputFixedPointOpcode(out, "flip", "1001", 0, 0, fbuf);
			break;
		case OP_PT2D:
			fscanf(file, "\t%d %d\n",
				&ibuf[0], &ibuf[1]);

			fprintf(out, "--pt2d %d %d\n", ibuf[0], ibuf[1]);
			fprintf(out, "    OPCODE_IN <= \"1010\";\n");
			fprintf(out, "    DATA_IN <= \"0000000000000000");
			outputInteger(out, ibuf[0]);
			outputInteger(out, ibuf[1]);
			fprintf(out, "\";\n");
      fprintf(out, "    while BUSY_OUT1 = '1' loop\n");
      fprintf(out, "      wait for 8 ns;\n");
      fprintf(out, "    end loop;\n");
			fprintf(out, "    wait for 8 ns;\n");
			fprintf(out, "    LOAD <= '1';\n");
			fprintf(out, "    wait for 8 ns;\n");
			fprintf(out, "    LOAD <= '0';\n");
			fprintf(out, "\n");

			tb_length += 8;
			break;
		case OP_LN2D:
			fscanf(file, "\t%d %d\n\t%d %d\n",
				&ibuf[0], &ibuf[1],
				&ibuf[2], &ibuf[3]);

			fprintf(out, "--ln2d %d %d\n--     %d %d\n",
				ibuf[0], ibuf[1],
				ibuf[2], ibuf[3]);
			fprintf(out, "    OPCODE_IN <= \"1011\";\n");
			fprintf(out, "    DATA_IN <= \"");
			outputInteger(out, ibuf[0]);
			outputInteger(out, ibuf[1]);
			outputInteger(out, ibuf[2]);
			outputInteger(out, ibuf[3]);
			fprintf(out, "\";\n");
      fprintf(out, "    while BUSY_OUT1 = '1' loop\n");
      fprintf(out, "      wait for 8 ns;\n");
      fprintf(out, "    end loop;\n");
			fprintf(out, "    wait for 8 ns;\n");
			fprintf(out, "    LOAD <= '1';\n");
			fprintf(out, "    wait for 8 ns;\n");
			fprintf(out, "    LOAD <= '0';\n");
			fprintf(out, "\n");
			break;

			tb_length += 8;

		case POP_XROT:
			fscanf(file, "\t%f\n",
				&fbuf[16]);

			fbuf[17] = sinf(PI_OVER_180 * fbuf[16]);
			fbuf[18] = cosf(PI_OVER_180 * fbuf[16]);

			fbuf[0] = 1.0f;
			fbuf[1] = 0.0f;
			fbuf[2] = 0.0f;
			fbuf[3] = 0.0f;
			
			fbuf[4] = 0.0f;
			fbuf[5] = fbuf[18];
			fbuf[6] = fbuf[17];
			fbuf[7] = 0.0f;
			
			fbuf[8] = 0.0f;
			fbuf[9] = -fbuf[17];
			fbuf[10] = fbuf[18];
			fbuf[11] = 0.0f;
			
			fbuf[12] = 0.0f;
			fbuf[13] = 0.0f;
			fbuf[14] = 0.0f;
			fbuf[15] = 1.0f;

			outputFixedPointOpcode(out, "mulmat", "0001", 16, 4, fbuf);
			break;
		case POP_YROT:
			fscanf(file, "\t%f\n",
				&fbuf[16]);

			fbuf[17] = sinf(PI_OVER_180 * fbuf[16]);
			fbuf[18] = cosf(PI_OVER_180 * fbuf[16]);

			fbuf[0] = fbuf[18];
			fbuf[1] = 0.0f;
			fbuf[2] = -fbuf[17];
			fbuf[3] = 0.0f;
			
			fbuf[4] = 0.0f;
			fbuf[5] = 1.0f;
			fbuf[6] = 0.0f;
			fbuf[7] = 0.0f;
			
			fbuf[8] = fbuf[17];
			fbuf[9] = 0.0f;
			fbuf[10] = fbuf[18];
			fbuf[11] = 0.0f;
			
			fbuf[12] = 0.0f;
			fbuf[13] = 0.0f;
			fbuf[14] = 0.0f;
			fbuf[15] = 1.0f;

			outputFixedPointOpcode(out, "mulmat", "0001", 16, 4, fbuf);
			break;
		case POP_ZROT:
			fscanf(file, "\t%f\n",
				&fbuf[16]);

			fbuf[17] = sinf(PI_OVER_180 * fbuf[16]);
			fbuf[18] = cosf(PI_OVER_180 * fbuf[16]);

			fbuf[0] = fbuf[18];
			fbuf[1] = fbuf[17];
			fbuf[2] = 0.0f;
			fbuf[3] = 0.0f;
			
			fbuf[4] = -fbuf[17];
			fbuf[5] = fbuf[18];
			fbuf[6] = 0.0f;
			fbuf[7] = 0.0f;
			
			fbuf[8] = 0.0f;
			fbuf[9] = 0.0f;
			fbuf[10] = 1.0f;
			fbuf[11] = 0.0f;
			
			fbuf[12] = 0.0f;
			fbuf[13] = 0.0f;
			fbuf[14] = 0.0f;
			fbuf[15] = 1.0f;

			outputFixedPointOpcode(out, "mulmat", "0001", 16, 4, fbuf);
			break;
		case POP_SCALE:
			fscanf(file, "\t%f %f %f\n",
				&fbuf[16], &fbuf[17], &fbuf[18]);

			fbuf[0] = fbuf[16];
			fbuf[1] = 0.0f;
			fbuf[2] = 0.0f;
			fbuf[3] = 0.0f;
			
			fbuf[4] = 0.0f;
			fbuf[5] = fbuf[17];
			fbuf[6] = 0.0f;
			fbuf[7] = 0.0f;
			
			fbuf[8] = 0.0f;
			fbuf[9] = 0.0f;
			fbuf[10] = fbuf[18];
			fbuf[11] = 0.0f;
			
			fbuf[12] = 0.0f;
			fbuf[13] = 0.0f;
			fbuf[14] = 0.0f;
			fbuf[15] = 1.0f;

			outputFixedPointOpcode(out, "mulmat", "0001", 16, 4, fbuf);
			break;
		case POP_TRANS:
			fscanf(file, "\t%f %f %f\n",
				&fbuf[16], &fbuf[17], &fbuf[18]);

			fbuf[0] = 1.0f;
			fbuf[1] = 0.0f;
			fbuf[2] = 0.0f;
			fbuf[3] = fbuf[16];
			
			fbuf[4] = 0.0f;
			fbuf[5] = 1.0f;
			fbuf[6] = 0.0f;
			fbuf[7] = fbuf[17];
			
			fbuf[8] = 0.0f;
			fbuf[9] = 0.0f;
			fbuf[10] = 1.0f;
			fbuf[11] = fbuf[18];
			
			fbuf[12] = 0.0f;
			fbuf[13] = 0.0f;
			fbuf[14] = 0.0f;
			fbuf[15] = 1.0f;

			outputFixedPointOpcode(out, "mulmat", "0001", 16, 4, fbuf);
			break;
		case POP_XYFOV:
			fscanf(file, "\t%f\n",
				&fbuf[0]);

			fbuf[0] = tanf(PI_OVER_180 * fbuf[0]/2);

			outputFixedPointOpcode(out, "xfov", "0011", 1, 1, fbuf);
			outputFixedPointOpcode(out, "yfov", "0100", 1, 1, fbuf);
			break;
		};
	}

	copyFile(TB_TAIL, out);

	printf("Test Bench is %i ns long.\n", tb_length);

	return 0;
}
