/*
 *  ratlibgrid.c
 *  RatLib
 *
 *  Created by Curtis Jones on 2009.11.13.
 *  Copyright 2009 __MyCompanyName__. All rights reserved.
 *
 */

#include "ratlibgrid.h"
#include "ratlib.h"
#include "logger.h"
#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

/**
 *
 *
 */
int
ratlibgrid_init (ratlibgrid_t *grid, opool_t *pool, uint32_t width, uint32_t height)
{
	int error, bytes;
	
	if (unlikely(grid == NULL))
		LOG_ERROR_AND_RETURN(-1, "%s.. null ratlibgrid_t\n", __PRETTY_FUNCTION__);
	
	if (unlikely(width == 0))
		LOG_ERROR_AND_RETURN(-2, "%s.. invalid width (0)\n", __PRETTY_FUNCTION__);
	
	if (unlikely(height == 0))
		LOG_ERROR_AND_RETURN(-3, "%s.. invalid height (0)\n", __PRETTY_FUNCTION__);
	
	if (unlikely(0 != (error = cobject_init(&grid->cobject, (cobject_destroy_func)ratlibgrid_destroy, pool))))
		LOG_ERROR_AND_RETURN(-101, "%s.. failed to cobject_init, 0x%08X [%d]\n", __PRETTY_FUNCTION__, error, error);
	
	if (unlikely(0 != (error = slist_init(&grid->set1, &ratlib_pools()->slist_item))))
		LOG_ERROR_AND_RETURN(-102, "%s.. failed to slist_init(set1), 0x%08X [%d]\n", __PRETTY_FUNCTION__, error, error);
	
	if (unlikely(0 != (error = slist_init(&grid->set2, &ratlib_pools()->slist_item))))
		LOG_ERROR_AND_RETURN(-103, "%s.. failed to slist_init(set2), 0x%08X [%d]\n", __PRETTY_FUNCTION__, error, error);
	
	if (unlikely(0 != (error = slist_init(&grid->set3, &ratlib_pools()->slist_item))))
		LOG_ERROR_AND_RETURN(-104, "%s.. failed to slist_init(set3), 0x%08X [%d]\n", __PRETTY_FUNCTION__, error, error);
	
	bytes = width * height;
	grid->width = width;
	grid->height = height;
	
	if (unlikely(NULL == (grid->grid = malloc(bytes))))
		LOG_ERROR_AND_RETURN(-102, "%s.. failed to malloc(%d), %s\n", __PRETTY_FUNCTION__, bytes, strerror(errno));
	
	memset(grid->grid, 0, bytes);
	
	return 0;
}

/**
 *
 *
 */
inline int
ratlibgrid_destroy (ratlibgrid_t *grid)
{
	int error;
	
	if (unlikely(grid == NULL))
		LOG_ERROR_AND_RETURN(-1, "%s.. null ratlibgrid_t\n", __PRETTY_FUNCTION__);
	
	if (grid->grid != NULL) {
		free(grid->grid);
		grid->grid = NULL;
	}
	
	if (unlikely(0 != (error = slist_destroy(&grid->set1))))
		LOG_ERROR_AND_RETURN(-101, "%s.. failed to slist_destroy(set1), 0x%08X [%d]\n", __PRETTY_FUNCTION__, error, error);
	
	if (unlikely(0 != (error = slist_destroy(&grid->set2))))
		LOG_ERROR_AND_RETURN(-102, "%s.. failed to slist_destroy(set2), 0x%08X [%d]\n", __PRETTY_FUNCTION__, error, error);
	
	if (unlikely(0 != (error = slist_destroy(&grid->set3))))
		LOG_ERROR_AND_RETURN(-103, "%s.. failed to slist_destroy(set3), 0x%08X [%d]\n", __PRETTY_FUNCTION__, error, error);
	
	if (unlikely(0 != (error = cobject_destroy(&grid->cobject))))
		LOG_ERROR_AND_RETURN(-104, "%s.. failed to cobject_destroy, 0x%08X [%d]\n", __PRETTY_FUNCTION__, error, error);
	
	return 0;
}

/**
 *
 *
 */
inline int
ratlibgrid_isvisited (ratlibgrid_t *grid, ratlibpoint_t *point)
{
	return ratlibgrid_set_marked(grid, RATLIB_GRID_VISITED, point);
}

/**
 *
 *
 */
inline int
ratlibgrid_markasvisited (ratlibgrid_t *grid, ratlibpoint_t *point)
{
	uint8_t *unit;
	
	if (unlikely(grid == NULL))
		LOG_ERROR_AND_RETURN(-1, "%s.. null ratlibgrid_t\n", __PRETTY_FUNCTION__);
	
	if (unlikely(point == NULL))
		LOG_ERROR_AND_RETURN(-2, "%s.. null ratlibpoint_t\n", __PRETTY_FUNCTION__);
	
	unit = (grid->grid + (point->x + (point->y * grid->width)));
	*unit |= RATLIB_GRID_VISITED;
	
	return 0;
}

/**
 *
 *
 */
inline int
ratlibgrid_set_clear (ratlibgrid_t *grid, uint32_t set)
{
	uint8_t *unit;
	slist_t *list;
	ratlibpoint_t *point;
	
	if (unlikely(grid == NULL))
		LOG_ERROR_AND_RETURN(-1, "%s.. null ratlibgrid_t\n", __PRETTY_FUNCTION__);
	
	if (RATLIB_GRID_SET1 == set)
		list = &grid->set1;
	else if (RATLIB_GRID_SET2 == set)
		list = &grid->set2;
	else if (RATLIB_GRID_SET3 == set)
		list = &grid->set3;
	else
		LOG_ERROR_AND_RETURN(-2, "%s.. invalid set (%d)\n", __PRETTY_FUNCTION__, set);
	
	while (0 == slist_pop(list,(cobject_t**)&point) && point != NULL) {
		unit = (grid->grid + (point->x + (point->y * grid->width)));
		*unit &= ~set;
		ratlibpoint_release(point);
	}
	
	return 0;
}

/**
 *
 *
 */
inline int
ratlibgrid_set_mark (ratlibgrid_t *grid, uint32_t set, ratlibpoint_t *point)
{
	uint8_t *unit;
	slist_t *list;
	
	if (unlikely(grid == NULL))
		LOG_ERROR_AND_RETURN(-1, "%s.. null ratlibgrid_t\n", __PRETTY_FUNCTION__);
	
	if (unlikely(point == NULL))
		LOG_ERROR_AND_RETURN(-2, "%s.. null ratlibpoint_t\n", __PRETTY_FUNCTION__);
	
	if (RATLIB_GRID_SET1 == set)
		list = &grid->set1;
	else if (RATLIB_GRID_SET2 == set)
		list = &grid->set2;
	else if (RATLIB_GRID_SET3 == set)
		list = &grid->set3;
	else
		LOG_ERROR_AND_RETURN(-3, "%s.. invalid set (%d)\n", __PRETTY_FUNCTION__, set);
	
	unit = (grid->grid + (point->x + (point->y * grid->width)));
	*unit |= set;
	
	slist_push(list, (cobject_t*)point);
	
	return 0;
}

/**
 *
 *
 */
inline int
ratlibgrid_set_unmark (ratlibgrid_t *grid, uint32_t set, ratlibpoint_t *point)
{
	uint8_t *unit;
	
	if (unlikely(grid == NULL))
		LOG_ERROR_AND_RETURN(-1, "%s.. null ratlibgrid_t\n", __PRETTY_FUNCTION__);
	
	if (unlikely(point == NULL))
		LOG_ERROR_AND_RETURN(-2, "%s.. null ratlibpoint_t\n", __PRETTY_FUNCTION__);
	
	unit = (grid->grid + (point->x + (point->y * grid->width)));
	*unit &= ~set;
	
	return 0;
}

/**
 *
 *
 */
inline int
ratlibgrid_set_marked (ratlibgrid_t *grid, uint32_t set, ratlibpoint_t *point)
{
	if (unlikely(grid == NULL))
		LOG_ERROR_AND_RETURN(-1, "%s.. null ratlibgrid_t\n", __PRETTY_FUNCTION__);
	
	if (unlikely(point == NULL))
		LOG_ERROR_AND_RETURN(-2, "%s.. null ratlibpoint_t\n", __PRETTY_FUNCTION__);
	
	return *(grid->grid + (point->x + (point->y * grid->width))) & set;
}

/**
 *
 *
 */
inline int
ratlibgrid_set_any (ratlibgrid_t *grid, uint32_t set, ratlibpoint_t **point)
{
	int error;
	slist_t *list;
	ratlibpoint_t *tmppoint;
	
	if (unlikely(grid == NULL))
		LOG_ERROR_AND_RETURN(-1, "%s.. null ratlibgrid_t\n", __PRETTY_FUNCTION__);
	
	if (unlikely(point == NULL))
		LOG_ERROR_AND_RETURN(-2, "%s.. null ratlibpoint_t\n", __PRETTY_FUNCTION__);
	
	*point = NULL;
	
	if (RATLIB_GRID_SET1 == set)
		list = &grid->set1;
	else if (RATLIB_GRID_SET2 == set)
		list = &grid->set2;
	else if (RATLIB_GRID_SET3 == set)
		list = &grid->set3;
	else
		LOG_ERROR_AND_RETURN(-3, "%s.. invalid set (%d)\n", __PRETTY_FUNCTION__, set);
	
	while (1) {
		if (unlikely(0 != (error = slist_pop(list, (cobject_t**)&tmppoint))))
			LOG_ERROR_AND_RETURN(-101, "%s.. failed to slist_pop, 0x%08X [%d]\n", __PRETTY_FUNCTION__, error, error);
		
		if (tmppoint == NULL)
			break;
		
		if (!ratlibgrid_set_marked(grid, set, tmppoint)) {
			ratlibpoint_release(tmppoint);
			continue;
		}
		
		ratlibgrid_set_unmark(grid, set, tmppoint);
		*point = tmppoint;
		break;
	}
	
	return 0;
}

/**
 *
 *
 */
inline int
ratlibgrid_set_count (ratlibgrid_t *grid, uint32_t set)
{
	if (unlikely(grid == NULL))
		LOG_ERROR_AND_RETURN(-1, "%s.. null ratlibgrid_t\n", __PRETTY_FUNCTION__);
	
	if (RATLIB_GRID_SET1 == set)
		return grid->set1.count;
	else if (RATLIB_GRID_SET2 == set)
		return grid->set2.count;
	else if (RATLIB_GRID_SET3 == set)
		return grid->set3.count;
	else
		LOG_ERROR_AND_RETURN(-3, "%s.. invalid set (%d)\n", __PRETTY_FUNCTION__, set);
}

/**
 *
 *
 */
inline int
ratlibgrid_set_aups (ratlibgrid_t *grid, ratlibpoint_t *point, uint32_t set_src, uint32_t set_dst)
{
	ratlibpoint_t *pointN;
	ratlibpoint_t *pointE;
	ratlibpoint_t *pointS;
	ratlibpoint_t *pointW;
	ratlibpoint_t *pointNE;
	ratlibpoint_t *pointSE;
	ratlibpoint_t *pointSW;
	ratlibpoint_t *pointNW;
	
	if (unlikely(grid == NULL))
		LOG_ERROR_AND_RETURN(-1, "%s.. null ratlibgrid_t\n", __PRETTY_FUNCTION__);
	
	if (unlikely(point == NULL))
		LOG_ERROR_AND_RETURN(-2, "%s.. null ratlibpoint_t\n", __PRETTY_FUNCTION__);
	
	pointN  = ratlib_point(point->x,   point->y-1);
	pointE  = ratlib_point(point->x+1, point->y  );
	pointS  = ratlib_point(point->x,   point->y+1);
	pointW  = ratlib_point(point->x-1, point->y  );
	pointNE = ratlib_point(point->x+1, point->y-1);
	pointSE = ratlib_point(point->x+1, point->y+1);
	pointSW = ratlib_point(point->x-1, point->y+1);
	pointNW = ratlib_point(point->x-1, point->y-1);
	
	ratlibgrid_set_clear(grid, set_dst);
	
	if (point->y > 0)
		if (!ratlibgrid_set_marked(grid, set_src, pointN)) ratlibgrid_set_mark(grid, set_dst, pointN);
	
	if (point->x < grid->width-1)
		if (!ratlibgrid_set_marked(grid, set_src, pointE)) ratlibgrid_set_mark(grid, set_dst, pointE);
	
	if (point->y < grid->height - 1)
		if (!ratlibgrid_set_marked(grid, set_src, pointS)) ratlibgrid_set_mark(grid, set_dst, pointS);
	
	if (point->x > 0)
		if (!ratlibgrid_set_marked(grid, set_src, pointW)) ratlibgrid_set_mark(grid, set_dst, pointW);
	
	if (point->y > 0 && point->x < grid->width-1)
		if (!ratlibgrid_set_marked(grid, set_src, pointNE)) ratlibgrid_set_mark(grid, set_dst, pointNE);
	
	if (point->y < grid->height - 1 && point->x < grid->width-1)
		if (!ratlibgrid_set_marked(grid, set_src, pointSE)) ratlibgrid_set_mark(grid, set_dst, pointSE);
	
	if (point->y < grid->height - 1 && point->x > 0)
		if (!ratlibgrid_set_marked(grid, set_src, pointSW)) ratlibgrid_set_mark(grid, set_dst, pointSW);
	
	if (point->y > 0 && point->x > 0)
		if (!ratlibgrid_set_marked(grid, set_src, pointNW)) ratlibgrid_set_mark(grid, set_dst, pointNW);
	
	ratlibpoint_release(pointN);
	ratlibpoint_release(pointE);
	ratlibpoint_release(pointS);
	ratlibpoint_release(pointW);
	ratlibpoint_release(pointNE);
	ratlibpoint_release(pointSE);
	ratlibpoint_release(pointSW);
	ratlibpoint_release(pointNW);
	
	return 0;
}

/**
 * Adjacent, marked, point set
 *
 */
inline int
ratlibgrid_set_amps (ratlibgrid_t *grid, ratlibpoint_t *point, uint32_t set_src, uint32_t set_dst)
{
	ratlibpoint_t *pointN;
	ratlibpoint_t *pointE;
	ratlibpoint_t *pointS;
	ratlibpoint_t *pointW;
	ratlibpoint_t *pointNE;
	ratlibpoint_t *pointSE;
	ratlibpoint_t *pointSW;
	ratlibpoint_t *pointNW;
	
	if (unlikely(grid == NULL))
		LOG_ERROR_AND_RETURN(-1, "%s.. null ratlibgrid_t\n", __PRETTY_FUNCTION__);
	
	if (unlikely(point == NULL))
		LOG_ERROR_AND_RETURN(-2, "%s.. null ratlibpoint_t\n", __PRETTY_FUNCTION__);
	
	pointN  = ratlib_point(point->x,   point->y-1);
	pointE  = ratlib_point(point->x+1, point->y  );
	pointS  = ratlib_point(point->x,   point->y+1);
	pointW  = ratlib_point(point->x-1, point->y  );
	pointNE = ratlib_point(point->x+1, point->y-1);
	pointSE = ratlib_point(point->x+1, point->y+1);
	pointSW = ratlib_point(point->x-1, point->y+1);
	pointNW = ratlib_point(point->x-1, point->y-1);
	
	ratlibgrid_set_clear(grid, set_dst);
	
	if (point->y > 0)
		if (ratlibgrid_set_marked(grid, set_src, pointN)) ratlibgrid_set_mark(grid, set_dst, pointN);
	
	if (point->x < grid->width-1)
		if (ratlibgrid_set_marked(grid, set_src, pointE)) ratlibgrid_set_mark(grid, set_dst, pointE);
	
	if (point->y < grid->height - 1)
		if (ratlibgrid_set_marked(grid, set_src, pointS)) ratlibgrid_set_mark(grid, set_dst, pointS);
	
	if (point->x > 0)
		if (ratlibgrid_set_marked(grid, set_src, pointW)) ratlibgrid_set_mark(grid, set_dst, pointW);
	
	if (point->y > 0 && point->x < grid->width-1)
		if (ratlibgrid_set_marked(grid, set_src, pointNE)) ratlibgrid_set_mark(grid, set_dst, pointNE);
	
	if (point->y < grid->height - 1 && point->x < grid->width-1)
		if (ratlibgrid_set_marked(grid, set_src, pointSE)) ratlibgrid_set_mark(grid, set_dst, pointSE);
	
	if (point->y < grid->height - 1 && point->x > 0)
		if (ratlibgrid_set_marked(grid, set_src, pointSW)) ratlibgrid_set_mark(grid, set_dst, pointSW);
	
	if (point->y > 0 && point->x > 0)
		if (ratlibgrid_set_marked(grid, set_src, pointNW)) ratlibgrid_set_mark(grid, set_dst, pointNW);
	
	ratlibpoint_release(pointN);
	ratlibpoint_release(pointE);
	ratlibpoint_release(pointS);
	ratlibpoint_release(pointW);
	ratlibpoint_release(pointNE);
	ratlibpoint_release(pointSE);
	ratlibpoint_release(pointSW);
	ratlibpoint_release(pointNW);
	
	return 0;
}

/**
 *
 *
 */
inline ratlibgrid_t*
ratlibgrid_retain (ratlibgrid_t *grid)
{
	if (unlikely(grid == NULL))
		LOG_ERROR_AND_RETURN(NULL, "%s.. null ratlibgrid_t\n", __PRETTY_FUNCTION__);
	
	return (ratlibgrid_t*)cobject_retain(&grid->cobject);
}

/**
 *
 *
 */
inline void
ratlibgrid_release (ratlibgrid_t *grid)
{
	if (unlikely(grid == NULL))
		LOG_ERROR_AND_RETURN(, "%s.. null ratlibgrid_t\n", __PRETTY_FUNCTION__);
	
	cobject_release(&grid->cobject);
}