/**
 * \file sgm.c
 * \brief this file implements the fast and the research version of the SGM algorithm
 *
 *
 * \ingroup corefiles
 * \date Apr 14, 2009
 * \author Alexander Pimenov
 */


#include "global.h"
#include <stdlib.h>
#include <stdint.h>

#include <stdio.h>

#ifdef TRACE
#include <stdio.h>
#endif

#include "sgm.h"

typedef struct PathDescription_TAG {
	int dy;
	int dx;
	int isVertical;
	int pathClass;

} PathDescription;

#define PC_PATH_LIST_SIZE 12

PathDescription pathList[PC_PATH_LIST_SIZE] =
   {{ 1, 0, 1, PATH_TOP},
	{-1, 0, 1, PATH_BOTTOM},
	{ 0, 1, 0, PATH_LEFT},
	{ 0,-1, 0, PATH_RIGHT},

	{ 1, 1, 1, PATH_TOP_LEFT},
	{-1, 1, 1, PATH_BOTTOM_LEFT},
    { 1,-1, 1, PATH_TOP_RIGHT},
    {-1,-1, 1, PATH_BOTTOM_RIGHT},

	{ 1, 1, 0, PATH_TOP_LEFT},
	{-1, 1, 0, PATH_BOTTOM_LEFT},
    { 1,-1, 0, PATH_TOP_RIGHT},
	{-1,-1, 0, PATH_BOTTOM_RIGHT}
    };


#define ADDR(x,y,d) ((d) * I->h * I->w + (y) * I->w + (x))

/** @todo TODO: Use subpixel distance, or filter out the image or use MI*/
/**
 *
 */
static uint32_t C(G12Buffer *I, G12Buffer *J, int x, int y, int dx, int dy)
{
	int offsetI = (I->w * y + x);
	int newx = x + dx;
	if (newx < 0) newx = 0;
	if (newx > I->w - 1) newx = I->w - 1;

//	if (newx < 0) return 0x1FFFF;
//	if (newx > I->w - 1) return 0x1FFFF;


	int offsetJ = (J->w * y + newx);

	if (I->data[offsetI] > J->data[offsetJ])
		return (I->data[offsetI] - J->data[offsetJ]);
	else
		return (J->data[offsetJ] - I->data[offsetI]);
	//uint32_t diff = I->data[offsetI] - J->data[offsetJ];

	//return fabs( ((double)I->data[offsetI]) - ((double)J->data[offsetJ])) / 100.0;
}


/**
 * This function counts a minimum of the 4 unsigned 32 bit values.
 */
static inline uint32_t min4 (uint32_t a, uint32_t b, uint32_t c, uint32_t d) {
	if (a > b) a = b;
	if (c > d) c = d;
	if (a > c) return c;
	return a;
}

/**
 * This function counts a minimum of the 4 unsigned 16 bit values.
 */
static inline uint16_t min4_16 (uint16_t a, uint16_t b, uint16_t c, uint16_t d) {
	if (a > b) a = b;
	if (c > d) c = d;
	if (a > c) return c;
	return a;
}


DisparityBuffer *countDisparity( G12Buffer* I, G12Buffer* J, int dMax, int closePenlty, int longPenalty )
{
	G12Buffer *d1 = sgm_PC16(I, J,  dMax, closePenlty, longPenalty, NULL, NULL, NULL, 0xFFFF, NULL);
	G12Buffer *d2 = sgm_PC16(I, J, -dMax, closePenlty, longPenalty, NULL, NULL, NULL, 0xFFFF, NULL);
	DisparityBuffer *result = disparityBufferCreate(d1->h, d1->w);
	int i,j;
	for (i = 0; i <  d1->h; i++)
	{
		for (j = 0; j <  d1->w; j++)
		{
		    int leftDisparity = d1->data[i * d1->w + j] / (16.0 * 255.0) * dMax;
		    if (j + leftDisparity > d1->w)
		    {
		    	continue;
		    }
			int rightDisparity = d2->data[i * d1->w + j + leftDisparity] / (16.0 * 255.0) * dMax;
			if (leftDisparity == rightDisparity)
			{
				result->data[i * d1->w + j] = d1->data[i * d1->w + j];
			}
		}
	}
	result->maxValue = 16 * 255;

	return result;

}

/**
 * This function uses a lot of space. We should invent how not to use it
 *
 */
static uint32_t *computeCostArray(G12Buffer* I, G12Buffer* J, int dMax, int isInverted)
{
	uint32_t *Carray = (uint32_t *)dv_malloc(dMax * I->h * I->w * sizeof(uint32_t));
	int d,i,j;
	// Fill the cost array and Sarray
	for (d = 0; d < dMax; d ++)
	{
		for (i = 0; i  < I->h; i++)
		{
			for (j = 0; j  < I->w; j++)
			{
				int offset = ADDR(j,i,d);
				if (!isInverted)
				{
					Carray[offset] = C(I,J, j,i, d ,0);
				}
				else
				{
					Carray[offset] = C(J,I, j,i,-d ,0);
				}
			}
		}
	}
	return Carray;
}

/**
 * This function implements the classical SGM algorithm (slow).
 *
 **/
G12Buffer* sgm_PC16 (
		G12Buffer* I,
		G12Buffer* J,
		int dMax,
		int closePenlty,
		int longPenalty,
		uint32_t **sArrayTest,
		uint32_t **cArrayTest,
		uint32_t **pArrayTest,
		uint32_t mask,
		SGM_Progress_Callback callback)
{
    uint32_t *Carray;
    uint32_t *Sarray;
    uint32_t *Parray;

	int i,j,d;

	int isInverted = dMax < 0;
	dMax = isInverted ? -dMax : dMax;


	uint32_t *Lcost = (uint32_t *) dv_malloc (dMax * sizeof(uint32_t *));
	uint32_t *Lcost1 = (uint32_t *) dv_malloc (dMax * sizeof(uint32_t *));
	uint32_t *costs;
	uint32_t *costs1;
	uint32_t *tmp;

	uint32_t min;
	uint32_t oldmin;

	int dx, dy;
	int x,y;


	int arraySize = dMax * I->h * I->w * sizeof(uint32_t);

	if (pArrayTest != NULL)
	{

		int storageAlloc = arraySize * PATH_LIMIT;
#ifdef ASSERTS
		if (storageAlloc / PATH_LIMIT != arraySize)
			DOASSERT("An integer overflow selecting the debug buffer. Sorry.");
#endif

		printf("%d bits will be used to store the debug data.\n", storageAlloc);
		Parray = (uint32_t *) dv_malloc (storageAlloc);
	}

	Carray = computeCostArray(I, J, dMax, isInverted);
	Sarray = (uint32_t *) dv_calloc (arraySize, 1);


	// Traverse all the paths types

	int pathcount;
	for (pathcount = 0; pathcount < PATH_LIMIT; pathcount++)
	{
		if (!(pathList[pathcount].pathClass & mask))
			continue;

		dy = pathList[pathcount].dy;
		dx = pathList[pathcount].dx;
		int isVertical = pathList[pathcount].isVertical;
#ifdef TRACE
		printf("Path Group %d [%d;%d]\n", pathcount, dy, dx);
#endif

		int startx = 0;
		int starty = 0;
		if (isVertical)
		{
			if (dy < 0) starty = I->h - 1;
		}
		else
		{
			if (dx < 0) startx = I->w - 1;
		}

		if (callback != NULL)
			callback(pathcount, 0);
		// For every path in the group
		while (1)
		{
			x = startx;
			y = starty;

			min = UINT32_MAX;
			for (d = 0; d < dMax; d++)
			{
				uint32_t result = Carray[ADDR(x,y,d)];
				if (result < min)
					min = result;
				Lcost[d] = result;
				Lcost1[d] = 0;
			}

			costs = Lcost;
			costs1 = Lcost1;
			oldmin = min;

			while (x < I->w && x >= 0 && y < I->h && y >= 0)
			{

				min = UINT32_MAX;
				for (d = 0; d < dMax; d++)
				{
					uint32_t cost = Carray[ADDR(x,y,d)];
					uint32_t l1 = (d > 0        ? (costs[d - 1] + closePenlty) : UINT32_MAX);
					uint32_t l2 = (d < dMax - 1 ? (costs[d + 1] + closePenlty) : UINT32_MAX);
					uint32_t l3 = oldmin + longPenalty;
					uint32_t l4 = costs[d];
#ifdef ASSERTS
					uint32_t sum =  cost + min4 (l1, l2, l3, l4);
					if (sum < cost || sum < min4 (l1, l2, l3, l4))
						DOASSERT("TROUBLE: OVERFLOW IN SGM");

					if (oldmin > sum )
						DOASSERT("TROUBLE: INTERNAL BUG IN SGM");
#endif
					uint32_t result =
						cost + min4 (l1, l2, l3, l4) - oldmin;

					if (result < min) {
						min = result;
					}
					costs1[d] = result;
#ifdef ASSERTS
					sum = Sarray[ADDR(x,y,d)] + result;
					if (sum < Sarray[ADDR(x,y,d)] || sum < result)
						DOASSERT("TROUBLE: OVERFLOW IN SGM");
#endif
					if (pArrayTest != NULL) {
						Parray[(ADDR(x,y,d)) * PATH_LIMIT + pathcount] = result;
					}
					Sarray[ADDR(x,y,d)] += result;
					//Sarray[ADDR(x,y,d)]++;
				}

				oldmin = min;
				tmp = costs;
				costs = costs1;
				costs1 = tmp;
				x += dx;
				y += dy;
			}

			if (isVertical)
			{
				startx++;
				if (startx >=  I->w) {
					break;
				}
			}
			else
			{
				starty++;
				if (starty >=  I->h) {
					break;
				}
			}

		}
	}
#ifdef TRACE
	printf("Arrays done %d\n", Sarray[56]);
#endif
#ifdef ASSERTS
	uint32_t globalMax = 0;
#endif

	G12Buffer *toReturn = g12BufferCreate(I->h, I->w);

	for (i = 0; i < I->h; i++ )
	{
		for (j = 0; j < I->w; j++ )
		{
			double min = 2 * (double)UINT32_MAX;
			double minpos = -1;
			double pos;
			for (d = 1; d < dMax - 1; d++)
			{
#ifdef ASSERTS
				if ((uint32_t)Sarray[ADDR(j,i,d)] >= (1 << 16))
				{
					char s[100];
					snprintf(s, 100, "Overflow of 2^16 in SGM global cost buffer at (%d,%d) disp = %d value=%d", j, i, d, (uint32_t)Sarray[ADDR(j,i,d)] );
					int p;
/*					for (p = 0; p < PATH_LIMIT; p++)
						printf( "Path %d cost: %d\n", p, Parray[(ADDR(j,i,d)) * PATH_LIMIT + p]);*/
					printf("%s\n", s);
					//DOASSERT(s);

				}

#endif


//#define SIMPLE_DIST
#ifdef SIMPLE_DIST
				uint32_t result = Sarray[ADDR(j,i,d)];
				if (result < min)
				{
					min = result;
					minpos = d;
				}
#else
				// Parbolic Interpolation
				int E = Sarray[ADDR(j,i,d - 1)];
				int F = Sarray[ADDR(j,i,d)];
				int G = Sarray[ADDR(j,i,d + 1)];

				double c = F;
				double a = (E + G - 2 * F) / 2;
				double b = (G - E) / 2;
				double result;

				// Parabola goes down. minimum is on the ends
				do {
					if (a > 0) {
						pos = - b / (2 * a);
						if ( pos >= -1 && pos <= 1) {
							result = a * pos * pos + b * pos + c;
							pos += d;
							break;
						}
					}

					if ( E > G ) {
						result = G;
						pos = d + 1;
					}
					else
					{
						result = E;
						pos = d - 1;
					}

				} while (0);

				if (result < min)
				{
					min = result;
					minpos = pos;
				}
#endif
			}
#ifdef ASSERTS
			if (minpos < 0 || minpos > dMax) {
				DOASSERT("INTERNAL ERROR");
			}
#endif

			toReturn->data[toReturn->w * i + j] = ((minpos / dMax) * 255.0) * 16.0;
//			toReturn->data[(toReturn->w * i + j) * toReturn->bpp] = ((255.0 / (minpos + 1)));
		}
	}

	if (cArrayTest != NULL) {
		*cArrayTest = Carray;
	} else {
		dv_free(Carray);
	}
	if (sArrayTest != NULL) {
		*sArrayTest = Sarray;
	} else {
		dv_free(Sarray);
	}
	if (pArrayTest != NULL) {
			*pArrayTest = Parray;
	}
	dv_free(Lcost);
	dv_free(Lcost1);
	return toReturn;

}


#define FAST_SGM
#ifdef FAST_SGM
G12Buffer* sgm_fast (G12Buffer* left, G12Buffer* right, int dMax, int closePenlty, int longPenalty)
{

    uint16_t *Carray;

	int i,j,d;
	int h = left->h;
	int w = left->w;
	int isInverted = dMax < 0;
	dMax = isInverted ? -dMax : dMax;

	long mem_profile = 0;

	int arraySize = dMax * left->h * left->w * sizeof(uint16_t);
	Carray = (uint16_t *) dv_malloc (arraySize);
	mem_profile += arraySize;

	// Fill the cost array. In fact we don't need this
	for (d = 0; d < dMax; d ++)
	{
		for (i = 0; i < h; i++)
		{
			for (j = 0; j < w; j++)
			{
				int offset = (d * h * w + i * w + j);
				if (isInverted)
				{
					Carray[offset] = C(right, left, j,i, -d ,0);
				}
				else
				{
					Carray[offset] = C(left,right, j,i, d ,0);
				}
			}
		}
	}

	/*Allocate memory for the process*/
	uint32_t *Sline = (uint32_t *) dv_malloc (dMax * w * sizeof(uint32_t));

	uint16_t **(Ltop[3]);
	for (i = 0; i < 3; i++)
		Ltop[i] = (uint16_t **) dv_malloc (w * sizeof(uint16_t *));

	for (i = 0; i < 3; i++)
		for (j = 0; j < w; j++ )
			Ltop[i][j] = (uint16_t *)dv_malloc (dMax * sizeof(uint16_t));

	uint16_t *(Ltnew[5]);
	for (i = 0; i < 5; i++)
		Ltnew[i] = (uint16_t *) dv_malloc (dMax * sizeof(uint16_t));

	uint16_t *(Ltmin[3]);
	for (i = 0; i < 3; i++)
	Ltmin[i] = (uint16_t *) dv_malloc (w * sizeof(uint16_t));

	uint16_t *Lleft  = (uint16_t *) dv_malloc(sizeof(uint16_t) * dMax);
	uint16_t *Lright = (uint16_t *) dv_malloc(sizeof(uint16_t) * dMax);

	G12Buffer *toReturn = g12BufferCreate(h, w);

	for (j = 0; j < w ; j++)
	{
		Ltmin[0][j] = UINT16_MAX;
		Ltmin[1][j] = UINT16_MAX;
		Ltmin[2][j] = UINT16_MAX;

		for (d = 0; d < dMax; d++)
		{
			Ltop[0][j][d] = Carray[d * h * w + 0 * w + j];
			Ltop[1][j][d] = Carray[d * h * w + 0 * w + j];
			Ltop[2][j][d] = Carray[d * h * w + 0 * w + j];
			if (Ltmin[0][j] > Ltop[0][j][d])
				Ltmin[0][j] = Ltop[0][j][d];

			if (Ltmin[1][j] > Ltop[1][j][d])
				Ltmin[1][j] = Ltop[1][j][d];

			if (Ltmin[2][j] > Ltop[2][j][d])
				Ltmin[2][j] = Ltop[2][j][d];
		}
	}

	for (i = 1; i < h; i++)
	{
		uint16_t l1, l2, l3, l4, result;
		uint16_t *tmp;

		uint16_t LleftMin = UINT16_MAX;
		uint16_t LrightMin = UINT16_MAX;
		for (j = 0; j < dMax * w; j++)
			Sline[j] = 0;

		for (d = 0; d < dMax; d++) {
			Lleft [d] = Carray[d * h * w + i * w + 0];
			if (Lleft [d] < LleftMin)
				LleftMin = Lleft[d];
			Lright[d] = Carray[d * h * w + i * w + w - 1];
			if (Lright [d] < LrightMin)
				LrightMin = Lright[d];
		}

		// Fill the line
		for (j = 1; j < w - 1; j++) {
			uint16_t minTop = UINT16_MAX;
			uint16_t minLU = UINT16_MAX;
			uint16_t minRU = UINT16_MAX;
			uint16_t minL = UINT16_MAX;

			uint16_t oldminTop = Ltmin[0][j];
			uint16_t oldminLU  = Ltmin[1][j - 1];
			uint16_t oldminRU  = Ltmin[2][j + 1];

			for (d = 0; d < dMax; d++) {
				uint16_t cost = Carray[d * h * w + i * w + j];

				// Vertical
				l1 = (d > 0        ? (Ltop[0][j][d - 1] + closePenlty) : UINT16_MAX);
				l2 = (d < dMax - 1 ? (Ltop[0][j][d + 1] + closePenlty) : UINT16_MAX);
				l3 = Ltop[0][j][d];
				l4 = oldminTop + longPenalty;
				result =  min4_16(l1, l2, l3, l4) - oldminTop + cost;

				if (result < minTop) {
					minTop = result;
				}
				Ltnew[0][d] = result;
				Sline[j * dMax +  d] += result;
#ifdef ASSERTS
				if (Sline[j * dMax + d] >= (1 << 16))
					DOASSERT("Fast SGM overflow1");
#endif


				// LU
				l1 = (d > 0        ? (Ltop[1][j - 1][d - 1] + closePenlty) : UINT16_MAX);
				l2 = (d < dMax - 1 ? (Ltop[1][j - 1][d + 1] + closePenlty) : UINT16_MAX);
				l3 = Ltop[1][j - 1][d];
				l4 = oldminLU + longPenalty;
				result = min4_16 (l1, l2, l3, l4) - oldminLU + cost;

				if (result < minLU) {
					minLU = result;
				}
				Ltnew[1][d] = result;
				Sline[j * dMax +  d] += result;
#ifdef ASSERTS
				if (Sline[j * dMax + d] >= (1 << 16))
					DOASSERT("Fast SGM overflow2");
#endif


				// RU
				l1 = (d > 0        ? (Ltop[2][j + 1][d - 1] + closePenlty) : UINT16_MAX);
				l2 = (d < dMax - 1 ? (Ltop[2][j + 1][d + 1] + closePenlty) : UINT16_MAX);
				l3 = Ltop[2][j + 1][d];
				l4 = oldminRU + longPenalty;
				result = min4_16 (l1, l2, l3, l4) - oldminRU + cost;

				if (result < minRU) {
					minRU = result;
				}
				Ltnew[2][d] = result;
				Sline[j * dMax +  d] += result;
#ifdef ASSERTS
				if (Sline[j * dMax + d] >= (1 << 16))
					DOASSERT("Fast SGM overflow3");
#endif

				// Also From Left
				l1 = (d > 0        ? (Lleft[d - 1] + closePenlty) : UINT16_MAX);
				l2 = (d < dMax - 1 ? (Lleft[d + 1] + closePenlty) : UINT16_MAX);
				l3 = Lleft[d];
				l4 = LleftMin + longPenalty;
				result = min4_16 (l1, l2, l3, l4) - LleftMin + cost;

				if (result < minL) {
					minL = result;
				}
				Ltnew[3][d] = result;
				Sline[j * dMax +  d] += result;
#ifdef ASSERTS
				if (Sline[j * dMax + d] >= (1 << 16))
					DOASSERT("Fast SGM overflow4");
#endif
			}

			tmp = Ltop[0][j];
			Ltop[0][j] = Ltnew[0];
			Ltnew[0] = tmp;

			tmp = Ltop[1][j];
			Ltop[1][j] = Ltnew[1];
			Ltnew[1] = tmp;

			tmp = Ltop[2][j];
			Ltop[2][j] = Ltnew[2];
			Ltnew[2] = tmp;

			tmp = Lleft;
			Lleft = Ltnew[3];
			Ltnew[3] = tmp;

			Ltmin[0][j] = minTop;
			Ltmin[1][j] = minLU;
			Ltmin[2][j] = minRU;
			LleftMin = minL;
		}

		// Also From Right
		for (j = w - 1; j >= 1; j--) {
			uint16_t minR = UINT16_MAX;

			for (d = 0; d < dMax; d++) {
				uint16_t cost = Carray[d * h * w + i * w + j];

				l1 = (d > 0        ? (Lright[d - 1] + closePenlty) : UINT16_MAX);
				l2 = (d < dMax - 1 ? (Lright[d + 1] + closePenlty) : UINT16_MAX);
				l3 = Lright[d];
				l4 = LrightMin + longPenalty;
				result = min4_16 (l1, l2, l3, l4) - LrightMin + cost;

				if (result < minR) {
					minR = result;
				}
				Ltnew[4][d] = result;
				Sline[j * dMax +  d] += result;
#ifdef ASSERTS
				if (Sline[j * dMax + d] >= (1 << 16))
					DOASSERT("Fast SGM overflow5");
#endif
			}
			tmp = Lright;
			Lright = Ltnew[4];
			Ltnew[4] = tmp;
			LrightMin = minR;
		}



		for (j = 1; j < w - 1; j++) {
			double min = 2 * (double)UINT32_MAX;
			double minpos = -1;
			for (d = 1; d < dMax - 2; d++)
			{
#ifdef ASSERTS
				if (Sline[j * dMax + d] >= (1 << 16))
					DOASSERT("Fast SGM overflow5");
#endif

				uint16_t result = Sline[j * dMax + d];
				if (result < min)
				{
					min = result;
					minpos = d;
				}
			}
#ifdef ASSERTS
			if (minpos < 0 || minpos > dMax)
				DOASSERT("INTERNAL ERROR");
#endif
			toReturn->data[w * i + j] = ((minpos / dMax) * 255.0) * 16.0;
		}


	}

	dv_free(Carray);


	/*Allocate memory for the process*/
	dv_free (Sline);

	for (i = 0; i < 3; i++)
	{
		for (j = 0; j < w; j++ )
			dv_free(Ltop[i][j]);
		dv_free(Ltop[i]);
	}

	for (i = 0; i < 5; i++)
		dv_free(Ltnew[i]);

	for (i = 0; i < 3; i++)
		dv_free(Ltmin[i]);

	dv_free(Lleft);
	dv_free(Lright);
	return toReturn;
}
#endif
