// ------------------------------------------------------------
// pixel.c
//
// Retrieving pixels in image and maintaining pixel attributes
//
// 13.03.2005, J.Ahrensfeld
// ------------------------------------------------------------
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <sys/time.h>
#include "bintree.h"
#include "mutil.h"
#include "lapack_cwrap.h"
#include "clob.h"
#include "pixel.h"

// ------------------------------------------------------------
#define	CALCSET_RADIUS_INITIAL		5
#define	CALCSET_RADIUS_INCREMENT	1.0
#define	CALCSET_DISTMIN_INITIAL		1E12

// ------------------------------------------------------------
void SetInit(set_data_t *pObj, int num_row, int num_col)
{
	int i, x, y;

	pObj->num_row = num_row;
	pObj->num_col = num_col;
	pObj->num_pixel = num_row*num_col;

	pObj->pPixel = (pixel_t*)malloc(pObj->num_pixel*sizeof(pixel_t));

	x = y = 0;
	for (i=0; i < pObj->num_pixel; i++)
	{
		pObj->pPixel[i].id_this = i;
		pObj->pPixel[i].state = 0;
		pObj->pPixel[i].raster_coord[0] = x++;
		pObj->pPixel[i].raster_coord[1] = y;
		if (x >= num_col)
		{
			x = 0;
			y++;
		}
	}
}

// ------------------------------------------------------------
void SetFree(set_data_t *pObj)
{
	if (!pObj)
		return;

	if (pObj->pPixel)
		free(pObj->pPixel);
}

// ------------------------------------------------------------
void PixelStateInit(set_data_t *pObj, image_t *pTrimap_image)
{
	int i, k;
	double *pTrimap = (double*)pTrimap_image->pColor_data;

	k = pTrimap_image->num_ch;

	pObj->num_fore = 0;
	pObj->num_back = 0;
	pObj->num_unknown = 0;

	for (i=0; i < pObj->num_pixel; i++)
	{
		pObj->pPixel[i].state = pixel_state_background;
		if(pTrimap[k*i] > 0.7)
		{
			pObj->pPixel[i].state = pixel_state_foreground;
			pObj->num_fore++;
		}
		else
		{
			if(pTrimap[k*i] > 0.2)
			{
				pObj->pPixel[i].state = pixel_state_unknown;
				pObj->num_unknown++;
			}
		}
	}
	pObj->num_back = pObj->num_pixel - (pObj->num_fore + pObj->num_unknown);
}

// ------------------------------------------------------------
int GetCropSet(set_data_t *pObj, int *pCrop, pos_t center, pos_t rect_size, UINT32 select_state)
{

	int i, j, x, y, min_x, min_y, max_x, max_y;

	if (!(rect_size[COORD_X]%2))
		rect_size[COORD_X]++;

	if (!(rect_size[COORD_Y]%2))
		rect_size[COORD_Y]++;

	min_x = center[COORD_X]-(rect_size[COORD_X]-1)/2;
	if (min_x < 0)
		min_x = 0;

	max_x = center[COORD_X]+(rect_size[COORD_X]-1)/2;
	if (max_x >= pObj->num_col)
		max_x = pObj->num_col-1;

	min_y = center[COORD_Y]-(rect_size[COORD_Y]-1)/2;
	if (min_y < 0)
		min_y = 0;

	max_y = center[COORD_Y]+(rect_size[COORD_Y]-1)/2;
	if (max_y >= pObj->num_row)
		max_y = pObj->num_row-1;


	i = 0;
	if (select_state == pixel_state_all)
	{
		for (y=min_y; y <= max_y; y++)
		{
			for (x=min_x; x <= max_x; x++)
			{
				pCrop[i] = y*pObj->num_col + x;
				i++;
			}
		}
	}
	else
	{
		for (y=min_y; y <= max_y; y++)
		{
			for (x=min_x; x <= max_x; x++)
			{
				j = y*pObj->num_col + x;
				if (pObj->pPixel[j].state & select_state)
				{
					pCrop[i] = j;
					i++;
				}
			}
		}
	}

	return i;
}

// ------------------------------------------------------------
int GetCircleSet(set_data_t *pObj, int *pCrop, pos_t center, double radius, UINT32 select_state)
{
	int i, j, num_found;

	double distance, diff[2];
	pos_t rect_size;

	rect_size[0] = 2*(int)radius+1;
	rect_size[1] = 2*(int)radius+1;

	num_found = GetCropSet(pObj, pCrop, center, rect_size, select_state);

	j=0;
	for (i=0; i < num_found; i++)
	{
		diff[0] = (double)(center[0] - pObj->pPixel[pCrop[i]].raster_coord[0]);
		diff[1] = (double)(center[1] - pObj->pPixel[pCrop[i]].raster_coord[1]);
		distance = dnrm2(2, diff, 1);

		if(distance < radius)
		{
			pCrop[j++] = pCrop[i];
		}
	}

	return j;
}

// ------------------------------------------------------------
int GetSetByState(set_data_t *pObj, pxl_id_t *pSet, UINT32 select_state)
{

	UINT32 i, j;

	j=0;
	if (pSet)
	{
		for (i=0; i < pObj->num_pixel; i++)
		{

			if (pObj->pPixel[i].state & select_state)
			{
				pSet[j] = pObj->pPixel[i].id_this;
				j++;
			}
		}
	}
	else	// Query only
	{
		for (i=0; i < pObj->num_pixel; i++)
		{

			if (pObj->pPixel[i].state & select_state)
			{
				j++;
			}
		}
	}
	return j;
}

// ------------------------------------------------------------
// Calcset set stuff
// Creates optimal computing order for unknown pixels
// ------------------------------------------------------------
int ComputeDistanceMap(set_data_t *pObj, pxl_id_t *pSet_unknown, int num_unknown)
{
	int i, j, s, t, num_fore, num_back;
	pxl_id_t *pSet_f, *pSet_b, id_min;
	double radius, distance, distance_min, diff[2];
	pos_t curr_pos;
	node_t *node;
	bintree_t bst;

	void *data;

	pSet_f = (pxl_id_t*)malloc(pObj->num_fore*sizeof(pxl_id_t));
	pSet_b = (pxl_id_t*)malloc(pObj->num_back*sizeof(pxl_id_t));

	for (i=0; i < num_unknown; i++)
	{
		s = pSet_unknown[i];
		curr_pos[0] = pObj->pPixel[s].raster_coord[0];
		curr_pos[1] = pObj->pPixel[s].raster_coord[1];

		radius = CALCSET_RADIUS_INITIAL;
		do
		{
			num_fore = GetCircleSet(pObj, pSet_f, curr_pos, radius, pixel_state_foreground);
			radius += CALCSET_RADIUS_INCREMENT;

		} while (num_fore == 0);

		radius = CALCSET_RADIUS_INITIAL;
		do
		{
			num_back = GetCircleSet(pObj, pSet_b, curr_pos, radius, pixel_state_background);
			radius += CALCSET_RADIUS_INCREMENT;

		} while (num_back == 0);

		bintree_init(&bst, num_fore);
		distance_min = CALCSET_DISTMIN_INITIAL;
		for (j=0; j < num_fore; j++)
		{
			t = pSet_f[j];
			diff[0] = (double)(curr_pos[0] - pObj->pPixel[t].raster_coord[0]);
			diff[1] = (double)(curr_pos[1] - pObj->pPixel[t].raster_coord[1]);

			distance = diff[0]*diff[0] + diff[1]*diff[1];

			data = (void*)t;
			bintree_insert(&bst, distance, data);

		}
		node = bintree_GetNodeMin(&bst);
		id_min = (int)node->pData;
		distance_min = node->value;
		pObj->pPixel[s].id_closest_f = id_min;
		pObj->pPixel[s].dist_closest_f = sqrt(distance_min);
		bintree_free(&bst);

		bintree_init(&bst, num_back);
		distance_min = CALCSET_DISTMIN_INITIAL;
		for (j=0; j < num_back; j++)
		{
			t = pSet_b[j];
			diff[0] = (double)(curr_pos[0] - pObj->pPixel[t].raster_coord[0]);
			diff[1] = (double)(curr_pos[1] - pObj->pPixel[t].raster_coord[1]);

			distance = diff[0]*diff[0] + diff[1]*diff[1];

			data = (void*)t;
			bintree_insert(&bst, distance, data);

		}
		node = bintree_GetNodeMin(&bst);

		id_min = (int)node->pData;
		distance_min = node->value;
		pObj->pPixel[s].id_closest_b = id_min;
		pObj->pPixel[s].dist_closest_b = sqrt(distance_min);
		bintree_free(&bst);
	}

	free(pSet_f);
	free(pSet_b);

	return num_unknown;
}

// ------------------------------------------------------------
int ComputeCalcSet(set_data_t *pObj, pxl_id_t *pSet_unknown, int num_unknown)
{
	int i, s;
	void *data;

	double v[2], distance;
	bintree_t bst;
	node_t *node;

	bintree_init(&bst, num_unknown);

	for (i=0; i < num_unknown; i++)
	{
		s = pSet_unknown[i];
		v[0] = pObj->pPixel[s].dist_closest_f;
		v[1] = pObj->pPixel[s].dist_closest_b;

		if (v[0] < v[1])
			distance = v[0];
		else
			distance = v[1];

		data = (void*)s;
		bintree_insert(&bst, distance, data);
	}

	i=0;
	do
	{
		node = bintree_GetNodeMin(&bst);
		s = (int)node->pData;
		pSet_unknown[i] = s;
		i++;

	} while(bintree_node_remove(&bst, node));

	bintree_free(&bst);

	return i;
}
// ------------------------------------------------------------
int SaveCalcSet(char *filename, set_data_t *pObj, pxl_id_t *pSet_unknown, int num_unknown)
{
	int i;
	pxl_id_t s;
	FILE *pFile;

	pFile = fopen(filename, "wb");
	if (!pFile)
	{
		fprintf(stderr, "Unable to open %s\n",filename);
		return -1;
	}

	fwrite(&num_unknown, sizeof(int), 1, pFile);
	fwrite(pSet_unknown, sizeof(pxl_id_t), num_unknown, pFile);

	for(i=0; i < num_unknown; i++)
	{
		s = pSet_unknown[i];
		fwrite(&pObj->pPixel[s], sizeof(pixel_t), 1, pFile);
	}


	fclose(pFile);

	return 0;
}

// ------------------------------------------------------------
int LoadCalcSet(char *filename, set_data_t *pObj, pxl_id_t *pSet_unknown, int num_unknown)
{
	int i, N;
	pxl_id_t s;
	FILE *pFile;

	pFile = fopen(filename, "rb");
	if (!pFile)
	{
		fprintf(stderr, "Unable to open %s\n",filename);
		return -1;
	}

	fread(&N, sizeof(int), 1, pFile);
	if (N != num_unknown)
	{
		fclose(pFile);
		return -1;
	}

	fread(pSet_unknown, sizeof(pxl_id_t), num_unknown, pFile);

	for(i=0; i < num_unknown; i++)
	{
		s = pSet_unknown[i];
		fread(&pObj->pPixel[s], sizeof(pixel_t), 1, pFile);
	}

	fclose(pFile);

	for(i=0; i < num_unknown; i++)
	{
		s = pSet_unknown[i];
		if(pObj->pPixel[s].state != pixel_state_unknown)
			return -1;
	}

	return 0;
}