#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "AS3.h"

typedef enum { FALSE, TRUE } boolean;

int resX, resY, nodeDistance, terrainWidth, terrainHeight;
float* buffer;
int* nodes;
int* path;
unsigned int* terrain;
int useTerrain = 0;

AS3_Val initializeBuffers(void* self, AS3_Val args)
{
	int i, cols, rows, alloc;
	AS3_ArrayValue(args, "IntType, IntType, IntType", &resX, &resY, &nodeDistance);
	buffer = malloc(resX*resY*sizeof(float)*3);
	cols = (int)floor(resX/nodeDistance);
	rows = (int)floor(resY/nodeDistance);
	alloc = cols*rows*sizeof(int)*4;
	nodes = malloc(alloc);
	for (i=0; i<cols*rows*4; i+=4) {
		nodes[i+1] = (int)floor((i/4)/cols);
		nodes[i] = (int)((i/4)-cols*nodes[i+1]);
		nodes[i+2] = 1;
		nodes[i+3] = 255;
	}
	AS3_Val retVal = AS3_Int(alloc);
	return retVal;
}

AS3_Val getBufferPointer(void* self, AS3_Val args)
{
	return AS3_Ptr(buffer);
}

AS3_Val getNodesPointer(void* self, AS3_Val args)
{
	return AS3_Ptr(nodes);
}

AS3_Val getPathPointer(void* self, AS3_Val args)
{
	return AS3_Ptr(path);
}

AS3_Val getTerrainPointer(void* self, AS3_Val args)
{
	AS3_ArrayValue(args, "IntType, IntType", &terrainWidth, &terrainHeight);
	if (useTerrain == 0) {
		terrain = malloc(terrainWidth*terrainHeight*sizeof(unsigned int));
	} else {
		realloc(terrain, terrainWidth*terrainHeight*sizeof(unsigned int));
	}
	useTerrain = 1;
	return AS3_Ptr(terrain);
}

static inline float getTerrainValue(int col, int row)
{
	int cols = (int)floor(resX/nodeDistance);
	int rows = (int)floor(resY/nodeDistance);
	int px = (int)round(terrainWidth*col/cols);
	int py = (int)round(terrainHeight*row/rows);
	int t = px+py*terrainWidth;
	float v = (float)(terrain[t]&16777215)/16777215+1;
	return v;
}

static inline int initNode(int col, int row, boolean accessible)
{
	int n = (int)(4*(col+row*(int)floor(resX/nodeDistance)));
	nodes[n+2] = accessible ? 1 : 0;
	return n;
}

static inline void remBitFlagAt(int position, int n)
{
	int p = (int)pow(2, position);
	nodes[n+3] -= ((nodes[n+3] & p) > 0) ? p : 0;
}

AS3_Val createNodes(void* self, AS3_Val args)
{
	int colMultiplier, rowMultiplier, i, i3, value, col, row, n, acc;
	int len = resX*resY;
	AS3_ArrayValue(args, "IntType, IntType", &colMultiplier, &rowMultiplier);
	for (i=0; i<len; i++) {
		i3 = 3*i;
		value = (int)buffer[i3];
		col = (int)buffer[i3+1]+colMultiplier;
		row = (int)buffer[i3+2]+rowMultiplier;
		if (value == -2) {
			n = initNode(col, row, FALSE);
		} else if ((value > 0) && (value < 32)) {
			n = initNode(col, row, TRUE);
			acc = nodes[n+3];
			switch (value) {
			case 1:
				//if (getBitFlagAt(6, n)) {
					remBitFlagAt(6, n);
					remBitFlagAt(2, initNode(col, (row+1), TRUE));
				//}
				break;
			case 2:
				//if (getBitFlagAt(4, n)) {
					remBitFlagAt(4, n);
					remBitFlagAt(0, initNode((col+1), row, TRUE));
				//}
				break;
			case 4:
				//if (getBitFlagAt(5, n)) {
					remBitFlagAt(5, n);
					remBitFlagAt(1, initNode((col+1), (row+1), TRUE));
				//}
				break;
			case 8:
				//if (getBitFlagAt(7, n)) {
					remBitFlagAt(7, n);
					remBitFlagAt(3, initNode((col-1), (row+1), TRUE));
				//}
				break;
			case 16:
				remBitFlagAt(5, n);
				remBitFlagAt(1, initNode((col+1), (row+1), TRUE));
				remBitFlagAt(7, initNode((col+1), row, TRUE));
				remBitFlagAt(3, initNode(col, (row+1), TRUE));
				break;
			}
		}
	}
	return 0;
}

AS3_Val getOptimalPath(void* self, AS3_Val args) {
	int startNodeCol, startNodeRow, goalNodeCol, goalNodeRow, i, j, k, terrainCorrection, n, m;
	int cols = (int)floor(resX/nodeDistance);
	int rows = (int)floor(resY/nodeDistance);
	int len = cols*rows;
	int openSet[len];
	int closedSet[len];
	int newOpenSet[len];
	AS3_ArrayValue(args, "IntType, IntType, IntType, IntType", &startNodeCol, &startNodeRow, &goalNodeCol, &goalNodeRow);
	int dx = goalNodeCol-startNodeCol;
	int dy = goalNodeRow-startNodeRow;
	n = (int)(4*(startNodeCol+startNodeRow*(int)floor(resX/nodeDistance)));
	openSet[0] = n;
	int lnOpenSet = 1;
	int lnClosedSet = 0;
	int o = 4*len;
	const float SQRT2 = 1.4142135623730950488016887242097;
	float gx[o];
	float fx[o];
	float hx[o];
	float tv;
	int nNodes[8];
	int parents[o];
	gx[n] = 0;
	hx[n] = fx[n] = sqrt(dx*dx+dy*dy)*((useTerrain == 1) ? getTerrainValue(startNodeCol, startNodeRow) : 1);
	for (i=0; i<2*len; i++) {
		gx[i] = 0;
		hx[i] = 0;
		fx[i] = 0;
		parents[i] = -1;
	}
	while (lnOpenSet > 0) {
		float fxMin = -1;
		for (i=0; i<lnOpenSet; i++) {
			if ((fx[openSet[i]] < fxMin) || (fxMin < 0)) {
				n = openSet[i];
				fxMin = fx[n];
			}
		}
		if ((nodes[n] == goalNodeCol) && (nodes[n+1] == goalNodeRow)) {
			int p = 0;
			int q;
			int tmpPath[len];
			//parents[nodeIndex(startNodeCol, startNodeRow)] = -1;
			if (parents[n] >= 0) {
				q = (int)(n*.25);
				while ((q >= 0) && (parents[n] >= 0)) {
					tmpPath[(int)p++] = q;
					n = parents[n];
					q = (int)(n*.25);
				}
				q = (int)(4*(startNodeCol+startNodeRow*(int)floor(resX/nodeDistance))*.25);
				if (q >= 0) { tmpPath[(int)p++] = q; }
			}
			free(path);
			path = malloc((int)(p*sizeof(int)));
			float fxMax = 0;
			for (i=0; i<p; i++) {
				path[i] = tmpPath[i];
				if (fx[path[i]*4] > fxMax) {
					fxMax = fx[path[i]*4];
				}
			}
			AS3_Val retVal = AS3_Array("IntType, DoubleType", (int)(p*sizeof(int)), ((p < 3) ? fxMax*getTerrainValue(goalNodeCol, goalNodeRow) : fxMax));
			return retVal;
		}
		j = 0;
		for (i=0; i<lnOpenSet; i++) {
			if (openSet[i] != n) {
				newOpenSet[j++] = openSet[i];
			}
		}
		lnOpenSet = j;
		for (i=0; i<lnOpenSet; i++) {
			openSet[i] = newOpenSet[i];
		}
		closedSet[lnClosedSet++] = n;
		j = 0;
		for (i=0; i<4*len; i+=4) {
			dx = nodes[i]-nodes[n];
			dy = nodes[i+1]-nodes[n+1];
			const int a0 = nodes[n+3];
			if ((dx*dy >= -1) && (dx*dy <= 1) && (nodes[i+2] == 1)) {
				if      ((dx == -1) && (dy ==  0)) { if ((a0 & (int)pow(2, 0)) > 0) { nNodes[j++] = i; } }
				else if ((dx == -1) && (dy == -1)) { if ((a0 & (int)pow(2, 1)) > 0) { nNodes[j++] = i; } }
				else if ((dx ==  0) && (dy == -1)) { if ((a0 & (int)pow(2, 2)) > 0) { nNodes[j++] = i; } }
				else if ((dx ==  1) && (dy == -1)) { if ((a0 & (int)pow(2, 3)) > 0) { nNodes[j++] = i; } }
				else if ((dx ==  1) && (dy ==  0)) { if ((a0 & (int)pow(2, 4)) > 0) { nNodes[j++] = i; } }
				else if ((dx ==  1) && (dy ==  1)) { if ((a0 & (int)pow(2, 5)) > 0) { nNodes[j++] = i; } }
				else if ((dx ==  0) && (dy ==  1)) { if ((a0 & (int)pow(2, 6)) > 0) { nNodes[j++] = i; } }
				else if ((dx == -1) && (dy ==  1)) { if ((a0 & (int)pow(2, 7)) > 0) { nNodes[j++] = i; } }
			}
			if (j == 8) { break; }
		}
		for (i=0; i<j; i++) {
			m = nNodes[i];
			boolean inClosedSetFlag = FALSE;
			for (k=0; k<lnClosedSet; k++) {
				if (closedSet[k] == m) {
					inClosedSetFlag = TRUE;
					break;
				}
			}
			if (!inClosedSetFlag) {
				dx = nodes[m]-nodes[n];
				dy = nodes[m+1]-nodes[n+1];
				float tgx = gx[n]+sqrt(dx*dx+dy*dy);
				boolean tgxPlus = FALSE;
				boolean inOpenSetFlag = FALSE;
				for (k=0; k<lnOpenSet; k++) {
					if (openSet[k] == m) {
						inOpenSetFlag = TRUE;
						break;
					}
				}
				if (!inOpenSetFlag) {
					openSet[lnOpenSet++] = m;
					dx = goalNodeCol-nodes[m];
					dy = goalNodeRow-nodes[m+1];
					terrainCorrection = ((dx == 0) || (dy == 0)) ? 1 : SQRT2;
					if (useTerrain == 1) {
						tv = getTerrainValue(nodes[m], nodes[m+1]);
					} else { tv = 1;}
					hx[m] = sqrt(dx*dx+dy*dy)*terrainCorrection*tv;
					tgxPlus = TRUE;
				} else if (tgx < gx[m]) {
					tgxPlus = TRUE;
				}
				if (tgxPlus) {
					parents[m] = n;
					gx[m] = tgx;
					fx[m] = gx[m]+hx[m];
				}
			}
		}
	}
	AS3_Array("IntType, DoubleType", 0, 0);
}

int main()
{
	AS3_Val initializeBuffersVal = AS3_Function(NULL, initializeBuffers);
	AS3_Val getBufferPointerVal = AS3_Function(NULL, getBufferPointer);
	AS3_Val getNodesPointerVal = AS3_Function(NULL, getNodesPointer);
	AS3_Val getPathPointerVal = AS3_Function(NULL, getPathPointer);
	AS3_Val getTerrainPointerVal = AS3_Function(NULL, getTerrainPointer);
	AS3_Val createNodesVal = AS3_Function(NULL, createNodes);
	AS3_Val getOptimalPathVal = AS3_Function(NULL, getOptimalPath);
    AS3_Val result = AS3_Object(
    	"createNodes: AS3ValType, initializeBuffers: AS3ValType, getBufferPointer: AS3ValType, getNodesPointer: AS3ValType, getPathPointer: AS3ValType, getTerrainPointer: AS3ValType, getOptimalPath: AS3ValType",
    	createNodesVal, initializeBuffersVal, getBufferPointerVal, getNodesPointerVal, getPathPointerVal, getTerrainPointerVal, getOptimalPathVal);
    AS3_Release(initializeBuffersVal);
    AS3_Release(getBufferPointerVal);
    AS3_Release(getNodesPointerVal);
    AS3_Release(getPathPointerVal);
    AS3_Release(getTerrainPointerVal);
    AS3_Release(createNodesVal);
    AS3_Release(getOptimalPathVal);
    AS3_LibInit(result);
    return 0;
}