// ------------------------------------------------------------
// matte.c
//
// Natural image matting using bayesian statistics
// From:
//
//
//
// 19.03.2005, J.Ahrensfeld
// ------------------------------------------------------------
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <sys/time.h>
#include "lapack_cwrap.h"
#include "mlio.h"
#include "mutil.h"
#include "clob.h"
#include "mtypes.h"
#include "imageio.h"
#include "pixel.h"
#include "tictoc.h"
#include "matte.h"

// ------------------------------------------------------------
//#define	MATTE_DEBUG
//#define	WITH_COND_COMPUTING
#define	USE_SLIDING_WINDOW
#define	WITH_SETSIZE_BY_MAJOR
//#define	WITH_ANIMATION


// ------------------------------------------------------------
#ifdef	USE_SLIDING_WINDOW
#undef	WITH_SETSIZE_BY_MAJOR
#endif

#define	SIGMA_C				0.3
#define	SIGMA_GF			8
#define	SQRT_2PI			2.506628274631000502415765284811
#define	LAMBDA_THRESH		0.6
#define	MAX_COLORS			16
#define	RADIUS_MIN			4.0
#define	RADIUS_MAX			50
#define	WEIGHTS_MIN			0.001
#define	ALPHA_CONV_THRESH	0.001
#define	ALPHA_TAU			0.33
#define	SOLVER_MAX_ITER		30
#define	SOLVER_EPS_COND		1E-12
#define	REPORT_INTERVAL		1
// ------------------------------------------------------------
void AlphaInit(set_data_t *pObj, double *pAlpha, UINT32 type)
{
	int i;
	pixel_t *pPixel = pObj->pPixel;

	if (type == pixel_state_foreground)
	{
		for (i=0; i < pObj->num_pixel; i++)
		{
			switch(pPixel[i].state)
			{
				case pixel_state_background:
				pAlpha[i] = 0.0;
				break;

				case pixel_state_foreground:
				pAlpha[i] = 1.0;
				break;

				case pixel_state_unknown:
				pAlpha[i] = 0.5;
				break;
			}
		}
	}
	else
	{

		for (i=0; i < pObj->num_pixel; i++)
		{
			switch(pPixel[i].state)
			{
				case pixel_state_background:
				pAlpha[i] = 1.0;
				break;

				case pixel_state_foreground:
				pAlpha[i] = 0.0;
				break;

				case pixel_state_unknown:
				pAlpha[i] = 0.5;
				break;
			}
		}
	}
}

// ------------------------------------------------------------
void MatteInit(matte_t *pObj, image_t *pComposite, image_t *pTrimap, char *name, matte_settings_t *pSettings)
{
	double k_norm[2];

	int result, i, s;
	tictoc_t tictoc;
	char filename[1024];
	image_t im_dist_f, im_dist_b;
	int num_row, num_col, num_unknown;
	pxl_id_t t_f, t_b;
	pixel_t *pPixel;

	color_vector_t *pColor_dist_f, *pColor_dist_b;

	num_row = pComposite->num_row;
	num_col = pComposite->num_col;

	pObj->pWork = (double*)malloc(num_row*num_col*sizeof(double));

	// Copy settings
	memcpy(&pObj->settings, pSettings, sizeof(matte_settings_t));
	strcpy(pObj->name, name);

	SetInit(&pObj->comp_set, num_row, num_col);

	printf("Evaluating Trimap data...");
	Tic(&tictoc);
	PixelStateInit(&pObj->comp_set, pTrimap);
	ImageFree(pTrimap);

	// Init Alpha map
	ImageInit(&pObj->im_alpha_f, num_row, num_col, 1);
	AlphaInit(&pObj->comp_set, pObj->im_alpha_f.pColor_data, pixel_state_foreground);

	// Init 1-Alpha map
	ImageInit(&pObj->im_alpha_b, num_row, num_col, 1);
	AlphaInit(&pObj->comp_set, pObj->im_alpha_b.pColor_data, pixel_state_background);

	// Init probability map
	ImageInit(&pObj->im_P, num_row, num_col, 1);

	// Get number of unknown pixels
	num_unknown = GetSetByState(&pObj->comp_set, NULL, pixel_state_unknown);
	pObj->pCalcSet = (pxl_id_t*)malloc(num_unknown*sizeof(pxl_id_t));

	printf("done\n");
	Toc(&tictoc);

	// Create fore- and background images
	ImageInit(&pObj->im_f, num_row, num_col, 3);
	ImageInit(&pObj->im_b, num_row, num_col, 3);
	ImageCopy(pComposite, &pObj->im_f);
	ImageCopy(pComposite, &pObj->im_b);

	sprintf(filename, "%s_calcset.dat",name);
	result = LoadCalcSet(filename, &pObj->comp_set, pObj->pCalcSet, num_unknown);

	// If loading of calcset fails - compute a new one
	if (result < 0)
	{
		printf("Computing distance map...");
		Tic(&tictoc);

		// Get unknown pixels
		num_unknown = GetSetByState(&pObj->comp_set, pObj->pCalcSet, pixel_state_unknown);

		// Compute distance map for unknown pixel
		ComputeDistanceMap(&pObj->comp_set, pObj->pCalcSet, num_unknown);

		printf("done\n");
		Toc(&tictoc);

		printf("Computing calc set...");
		Tic(&tictoc);

		// Compute calcset base on distance map for unknown pixel
		ComputeCalcSet(&pObj->comp_set, pObj->pCalcSet, num_unknown);
		SaveCalcSet(filename, &pObj->comp_set, pObj->pCalcSet, num_unknown);

		printf("done\n");
		Toc(&tictoc);

	}

	ImageInit(&im_dist_f, num_row, num_col, 3);
	ImageInit(&im_dist_b, num_row, num_col, 3);

	printf("Computing images from distance maps for %d unknown pixels...",num_unknown);
	Tic(&tictoc);

	k_norm[0] = 1.0/num_col;
	k_norm[1] = 1.0/num_row;

	pPixel = pObj->comp_set.pPixel;
	pColor_dist_f = (color_vector_t*)im_dist_f.pColor_data;
	pColor_dist_b = (color_vector_t*)im_dist_b.pColor_data;

	for (i=0; i < num_unknown; i++)
	{
		s = pObj->pCalcSet[i];
		t_f = pPixel[s].id_closest_f;
		t_b = pPixel[s].id_closest_b;

		pColor_dist_f[s][0] = k_norm[0]*(double)pPixel[t_f].raster_coord[0];
		pColor_dist_f[s][2] = k_norm[1]*(double)pPixel[t_f].raster_coord[1];

		pColor_dist_b[s][0] = k_norm[0]*(double)pPixel[t_b].raster_coord[0];
		pColor_dist_b[s][2] = k_norm[1]*(double)pPixel[t_b].raster_coord[1];
	}

	printf("done\n");
	Toc(&tictoc);

	sprintf(filename, "%s_dist_f.tif",name);
	result = ImageSaveTiff(&im_dist_f, NULL, filename);
	sprintf(filename, "%s_dist_b.tif",name);
	result = ImageSaveTiff(&im_dist_b, NULL, filename);

	ImageFree(&im_dist_f);
	ImageFree(&im_dist_b);

}

// ------------------------------------------------------------
void MatteFree(matte_t *pObj)
{
	if (!pObj)
		return;

	if (pObj->pWork)
		free(pObj->pWork);

	if (pObj->pCalcSet)
		free(pObj->pCalcSet);

	SetFree(&pObj->comp_set);
}

// ------------------------------------------------------------
int MatteGetSet(set_data_t *pObj, pxl_id_t *pPid, pxl_id_t pid, double *pAlpha, double *radius, UINT32 types)
{

	int i, s, num_found;
	pos_t center;
	pixel_t *pPixel = pObj->pPixel;
	double err, errlast, errslope, mean_weight;
	double diff[2], a, d, s2_i, d_offset, sum_weight, w;

	s2_i  = 1.0/(SIGMA_GF*SIGMA_GF);

	memcpy(center, pPixel[pid].raster_coord, sizeof(pos_t));

	err = 0;
	errlast = 0;
	errslope = 1;
	num_found = 0;
	// A = r^2*pi
	// r = sqrt(A/pi)

	do
	{
		err = 1 - num_found;

		if (err < 0)
			*radius -= sqrt(-err/M_PI);
		else
			*radius += sqrt(err/M_PI);

		num_found = GetCircleSet(pObj, pPid, center, *radius, types);

		errslope = err - errlast;
		errlast = err;

	} while (num_found == 0);

	sum_weight = 0;
	do
	{

		sum_weight = 0;
		for (i=0; i < num_found; i++)
		{
			s = pPid[i];
			diff[0] = (double)(center[0] - pPixel[s].raster_coord[0]);
			diff[1] = (double)(center[1] - pPixel[s].raster_coord[1]);
			d = dnrm2(2, diff, 1);
			a = pAlpha[s];
//			w = a*a/(d*d);
			w = a*a*exp(-s2_i*d*d)*(1.0/SQRT_2PI);
			sum_weight += w;
		}
		mean_weight = sum_weight/num_found;

		if (sum_weight < WEIGHTS_MIN)
		{
			*radius += 1;
			num_found = GetCircleSet(pObj, pPid, center, *radius, types);
		}

	} while ((sum_weight < WEIGHTS_MIN) && (*radius < RADIUS_MAX));

//	printf("numfound = %d\n",num_found);
	return num_found;
}

// ------------------------------------------------------------
int MatteGetColor(pxl_id_t *pPid, color_vector_t *pSrc, color_vector_t *pDst, int num_pixel)
{
	int i, s;

	for(i=0; i < num_pixel; i++)
	{
		s = pPid[i];
		pDst[i][0] = pSrc[s][0];
		pDst[i][1] = pSrc[s][1];
		pDst[i][2] = pSrc[s][2];
	}

	return i;
}

// ------------------------------------------------------------
int MatteGetData(pxl_id_t *pPid, double *pSrc, double *pDst, int num_pixel)
{
	int i, s;

	for(i=0; i < num_pixel; i++)
	{
		s = pPid[i];
		pDst[i] = pSrc[s];
	}

	return i;
}

// ------------------------------------------------------------
int MatteComputeWeights(set_data_t *pObj, pxl_id_t pid_u, pxl_id_t *pPid, double *pAlpha, double *pWeight, int num_pixel, double radius, UINT32 type)
{
	int i, s;
	pos_t center;
	double diff[2], a, d, s2_i, d_offset, sum_weight, alpha_mean;

	pixel_t *pPixel = pObj->pPixel;

	memcpy(center, pPixel[pid_u].raster_coord, sizeof(pos_t));

#ifndef	USE_SLIDING_WINDOW
	if (type == pixel_state_foreground)
	{
		d_offset = pPixel[pid_u].dist_closest_f;
	}
	else
	{
		d_offset = pPixel[pid_u].dist_closest_b;
	}

	d_offset = smax(d_offset-radius, 0);
#else
	d_offset = 0;
#endif

	s2_i  = 1.0/(SIGMA_GF*SIGMA_GF);

//	d = exp(-s2_i*radius*radius)*(1.0/SQRT_2PI);
//	printf("r=%f, min(d)=%f\n",radius,d);

	sum_weight = 0;
	alpha_mean = 0;
	for (i=0; i < num_pixel; i++)
	{
		s = pPid[i];
		diff[0] = (double)(center[0] - pPixel[s].raster_coord[0]);
		diff[1] = (double)(center[1] - pPixel[s].raster_coord[1]);
		d = dnrm2(2, diff, 1);
		d = smax(d - d_offset, 0);
		a = pAlpha[s];
		pWeight[i] = a*a*exp(-s2_i*d*d)*(1.0/SQRT_2PI);
//		pWeight[i] = 1.0;
		sum_weight += pWeight[i];
		alpha_mean += a;
	}

	alpha_mean /= num_pixel;
	if (alpha_mean < SOLVER_EPS_COND)
		fprintf(stderr, "ComputeWeights(): alpha_mean = %f\n", alpha_mean);

	if (sum_weight < SOLVER_EPS_COND)
		fprintf(stderr, "ComputeWeights(): Sum_i(w_i) = %f\n", sum_weight);

	return num_pixel;
}

// ------------------------------------------------------------
void ColorStatInit(color_stat_t *pObj, int max_pixel, int max_colors, UINT32 type)
{
	int info;
	double lwork_svd, lwork_inv;

	pObj->type = type;
	// Init SVD
	// Query work size
	info = dgesvd(/*JOBU*/'A', /*JOBVT*/'A', /*M*/3, /*N*/3, /*A*/NULL, /*LDA*/3, /*S*/NULL,
				/*U*/NULL, /*LDU*/3, /*VT*/NULL, /*LDVT*/3, /*WORK*/&lwork_svd, /*LWORK*/-1);

	if (info != 0)  fprintf (stderr, "Init SVD failure with error %d\n", info);

	// Matrix inversion
	// Query work size
	info = dgetri(3, NULL, 3, NULL, &lwork_inv, -1);
	if (info != 0)  fprintf (stderr, "Init dgetri failure with error %d\n", info);

	pObj->work_size = (int)smax(lwork_svd, lwork_inv);
	pObj->work_size_svd = (int)lwork_svd;
	pObj->work_size_inv = (int)lwork_inv;
	pObj->pWork = (double*)malloc(pObj->work_size*sizeof(double));
	pObj->pColor_mean = (color_vector_t*)malloc(max_colors*sizeof(color_vector_t));
	pObj->pCov_inv = (color_matrix_t*)malloc(max_colors*sizeof(color_matrix_t));
}

// ------------------------------------------------------------
void ColorStatFree(color_stat_t *pObj)
{

	if (!pObj)
		return;

	if (pObj->pWork)
		free(pObj->pWork);

	if (pObj->pColor_mean)
		free(pObj->pColor_mean);

	if (pObj->pCov_inv)
		free(pObj->pCov_inv);

}

// ------------------------------------------------------------
int ColorStatProcess(matte_t *pObj, color_stat_t *pStat, codebook_t *pCB, color_vector_t *pColor_set, int set_len, double *pWeight, double sigma_c)
{
	int i, j, num_colors, info, ipiv[3]={0};
	color_vector_t *pColor_data, *pColor_data_w, sv={0};
	color_vector_t color_mean={0};
	color_matrix_t cov={0}, cov1={0}, cov2={0}, cov_i={0}, U={0}, S={0}, VT={0}, T={0};
	double *pW, k_weight, sum_weight, s2, norm_cov, norm_cov_i, cond_cov;

	pW = (double*)malloc(set_len*sizeof(double));
	pColor_data = (color_vector_t*)malloc(set_len*sizeof(color_vector_t));

	s2 = sigma_c*sigma_c;

	// Loop over number of colors found
	pStat->num_colors = 0;
	for (i=0; i < pCB->num_colors; i++)
	{
		num_colors = pCB->size_C[i];

		// Get color values from Cluster i
		MatteGetColor((pxl_id_t*)pCB->ppC[i], pColor_set, pColor_data, num_colors);

		// Get corresponding weights of pixels from Cluster i
		MatteGetData((pxl_id_t*)pCB->ppC[i], pWeight, pW, num_colors);

		// k_weight = 1.0/sum_i(pW[i])
		sum_weight = dasum(num_colors, pW, 1);
		if (sum_weight == 0)
		{
//			for(j=0; j < num_colors; j++)
//				pW[j] = 1.0;

//			sum_weight = num_colors;
			continue;
		}

		k_weight = 1.0/sum_weight;
		if (isinf(k_weight))
			continue;

		// color_mean := k_weight*[sum_i(pW[i]*pColorData[i])]
		memset(color_mean, 0, sizeof(color_vector_t));
		dgemv('N', 3, num_colors, k_weight, (double*)pColor_data, 3, (double*)pW, 1, 0.0, (double*)color_mean, 1);

		// Remove mean from color data
		for (j=0; j < num_colors; j++)
		{
			pColor_data[j][0] -= color_mean[0];
			pColor_data[j][1] -= color_mean[1];
			pColor_data[j][2] -= color_mean[2];
		}

		// cov = k_weight*pColor_data*pColor_data'
		memset(cov, 0, sizeof(color_matrix_t));
		for (j=0; j < num_colors; j++)
		{
			dsyr('U', 3, pW[j], (double*)pColor_data[j], 1, (double*)cov, 3);
		}

		cov[0][1] = cov[1][0];
		cov[0][2] = cov[2][0];
		cov[1][2] = cov[2][1];

		// Scaling of cov
		for (j=0; j < 3; j++)
		{
			cov[j][0] *= k_weight;
			cov[j][1] *= k_weight;
			cov[j][2] *= k_weight;
		}

		// Do the Singular Value Decomposition
		memcpy(cov1, cov, sizeof(color_matrix_t));
		info = dgesvd(/*JOBU*/'A', /*JOBVT*/'A', /*M*/3, /*N*/3, /*A*/(double*)cov1, /*LDA*/3,
				/*S*/(double*)sv,	/*U*/(double*)U, /*LDU*/3,
				/*VT*/(double*)VT, /*LDVT*/3,
				/*WORK*/(double*)pStat->pWork, /*LWORK*/pStat->work_size_svd);

		if (info != 0)  fprintf (stderr, "SVD failure with error %d\n", info);

		// Add camera variance to singular value
		sv[0] += s2;
		sv[1] += s2;
		sv[2] += s2;

		// Compose new covariance matrix
		MakeDiag((double*)S, sv, 3);
		memset(T, 0, 3*sizeof(color_vector_t));
		memset(cov2, 0, 3*sizeof(color_vector_t));

		dgemm('N', 'N', 3, 3, 3, 1.0, (double*)U, 3, (double*)S, 3, 0.0, (double*)T, 3);
		dgemm('N', 'N', 3, 3, 3, 1.0, (double*)T, 3, (double*)VT, 3, 0.0, (double*)cov2, 3);

//#ifdef WITH_COND_COMPUTING
		norm_cov = dnrm(3, 3, (double*)cov2, '1');		// 1-norm
//#endif
		// Cov_inf = cov^-1;
		Eye((double*)cov_i, 1.0, 3);
		info = dgesv(3, 3, (double*)*cov2, 3, ipiv, (double*)cov_i, 3);
		if (info != 0)
		{
			fprintf (stderr, "Matrix inversion failure with error %d\n", info);
		}

//#ifdef WITH_COND_COMPUTING
		norm_cov_i = dnrm(3, 3, (double*)cov_i, '1');	// 1-norm
		cond_cov = 1.0/(norm_cov*norm_cov_i);

		if (cond_cov < SOLVER_EPS_COND)
			fprintf(stderr, "Condition(cov) = %f\n", cond_cov);
//#endif

		memcpy(pStat->pColor_mean[pStat->num_colors], color_mean, sizeof(color_vector_t));
		memcpy(pStat->pCov_inv[pStat->num_colors], cov_i, sizeof(color_matrix_t));
		pStat->num_colors++;
	}

	free(pColor_data);
	free(pW);

	return pStat->num_colors;
}

// ------------------------------------------------------------
void InsertMatrix(double *A, int lda, double *B, int ldb, int subid)
{
	int i, j, k, m;

	m = lda/ldb;

	k = ldb*(subid%m);
	if (ldb*subid >= lda)
		k += lda*ldb;

	for (i=0; i < ldb; i++)
		for (j=0; j < ldb; j++)
			A[j+i*lda+k] = B[i*ldb+j];

}

// ------------------------------------------------------------
int	MatteEstimate(matte_sol_t *pObj, color_vector_t colorComp, color_stat_t *pStat_f, color_stat_t *pStat_b, double sigma_c)
{
	int i, j, k, iteration, info, ipiv[6], is_valid;
	double *pW_f, *pW_b, alpha, b0, a0;
	double a_s2, b_s2, ab_s2, s2_i, c[6], c_orig[6], c_last[6], x[6];
	color_matrix_t A[4] = {0}, A_orig[4], I, T, cov_f, cov_b;
	color_vector_t colorMean_f, colorMean_b, t, c1, c2;
	double a1, a2, delta_alpha, alpha_lt, L_f, L_b, L_sol;
	double norm_A, rcond_A, dwork[24];
	int iwork[6];

	// Find solution loop
	s2_i = 1.0/(sigma_c*sigma_c);

	// ----------------------------
	// Loop begin
	memset(pObj, 0, sizeof(matte_sol_t));
	pObj->L = -1.0/SOLVER_EPS_COND;

	if ((pStat_f->num_colors * pStat_b->num_colors) == 0)
		fprintf(stderr, "MatteEstimate(): No solution possible!\n");

	for (i=0; i < pStat_f->num_colors; i++)
	{
		memcpy(cov_f, pStat_f->pCov_inv[i], sizeof(color_matrix_t));
		memcpy(colorMean_f, pStat_f->pColor_mean[i], sizeof(color_vector_t));

		for (j=0; j < pStat_b->num_colors; j++)
		{

			memcpy(cov_b, pStat_b->pCov_inv[j], sizeof(color_matrix_t));
			memcpy(colorMean_b, pStat_b->pColor_mean[j], sizeof(color_vector_t));

			delta_alpha = 1;
			iteration = 0;
			alpha = 0.5;
			alpha_lt = 1.0;
			while((delta_alpha > ALPHA_CONV_THRESH) && (iteration < SOLVER_MAX_ITER))
			{
				memcpy(c_last, c, 6*sizeof(double));

				is_valid = 0;
				a0 = smin(smax(alpha,0),1);
				b0 = 1.0-a0;

				// alpha/sigma_c^2
				a_s2 = a0*s2_i;

				// (1-alpha)/sigma_c^2
				b_s2 = b0*s2_i;

				// A = [[inv_C_F+(I*a0^2)/sigma_c^2  (I*a0*(1-a0))/sigma_c^2]
				// ; [(I*a0*(1-a0))/sigma_c^2       inv_C_B+(I*(1-a0)^2)/sigma_c^2]];

				memcpy(T, cov_f, sizeof(color_matrix_t));
				for (k=0; k < 3; k++)
				{
					T[k][k] += (a0*a_s2);
				}
				InsertMatrix((double*)A, 6, (double*)T, 3, 0);

				Eye((double*)T, a0*b_s2, 3);
				InsertMatrix((double*)A, 6, (double*)T, 3, 1);
				InsertMatrix((double*)A, 6, (double*)T, 3, 2);

				memcpy(T, cov_b, sizeof(color_matrix_t));
				for (k=0; k < 3; k++)
				{
					T[k][k] += (b0*b_s2);
				}
				InsertMatrix((double*)A, 6, (double*)T, 3, 3);

#ifdef WITH_COND_COMPUTING
				// Checking condition of A
				norm_A = dnrm(6, 6, (double*)A, '1');		// 1-norm
				info = dgecon('1', 6, (double*)A, 6, norm_A, &rcond_A, dwork, iwork);
				if (info != 0)
				{
					fprintf (stderr, "Condition estimator failure with error %d\n", info);
					break;
				}

				if (rcond_A < SOLVER_EPS_COND)
				{
					fprintf(stderr, "Cond(A) = %g\n", rcond_A);
					break;
				}
#endif
				// c = [[inv_C_F*color_mean_F+(color_C*a0)/sigma_c^2]
				// ; [inv_C_B*color_mean_B+(color_C*(1-a0))/sigma_c^2]];
				memcpy(t, colorComp, sizeof(color_vector_t));
				dgemv('N', 3, 3, 1.0, (double*)cov_f, 3, (double*)colorMean_f, 1, a_s2, t, 1);
				memcpy(&c[0], t, sizeof(color_vector_t));

				memcpy(t, colorComp, sizeof(color_vector_t));
				dgemv('N', 3, 3, 1.0, (double*)cov_b, 3, (double*)colorMean_b, 1, b_s2, t, 1);
				memcpy(&c[3], t, sizeof(color_vector_t));

				// Solve
				// x = A/c
				memcpy(A_orig, A, 4*sizeof(color_matrix_t));
				memcpy(c_orig, c, 6*sizeof(double));
				info = dgesv(6, 1, (double*)A, 6, ipiv, (double*)c, 6);
				if (info != 0)
				{
					fprintf (stderr, "Solver failure with error %d\n", info);
#ifdef MATTE_DEBUG
					PrintMatrix("A=", (double*)A_orig, 6, 6);
					PrintMatrix("c=", (double*)c_orig, 1, 6);
					PrintMatrix("Cov_f=", (double*)cov_f, 3, 3);
					PrintMatrix("Cov_b=", (double*)cov_b, 3, 3);
					fprintf(stderr, "a0 = %f\n", a0);
#endif
				    // If solver fails just copy meanF into F and meanB into B
					memcpy(&c[0], colorMean_f, sizeof(color_vector_t));
					memcpy(&c[3], colorMean_b, sizeof(color_vector_t));
				}

				// C - B
				c1[0] = colorComp[0] - c[3];
				c1[1] = colorComp[1] - c[4];
				c1[2] = colorComp[2] - c[5];

				// F - B
				c2[0] = c[0] - c[3];
				c2[1] = c[1] - c[4];
				c2[2] = c[2] - c[5];

				a1 = ddot(3, (double*)c1, 1, (double*)c2, 1);
				a2 = dnrm2(3, c2, 1);
				alpha = a1/(a2*a2);
				alpha_lt = ALPHA_TAU*alpha + (1-ALPHA_TAU)*alpha_lt;
				delta_alpha = fabs(1 - alpha/alpha_lt);
				alpha = alpha_lt;
				is_valid = 1;
				iteration++;
			}

			if (!is_valid && !iteration)
				continue;

			// If we have a solution from previous iteration
			if (!is_valid && iteration)
				memcpy(c, c_last, 6*sizeof(double));

			c1[0] = c[0] - colorMean_f[0];
			c1[1] = c[1] - colorMean_f[1];
			c1[2] = c[2] - colorMean_f[2];
			c2[0] = c[3] - colorMean_b[0];
			c2[1] = c[4] - colorMean_b[1];
			c2[2] = c[5] - colorMean_b[2];

			// L_F = -(color_F-color_mean_F)'*inv_C_F*(color_F-color_mean_F)/2;
			dgemv('N', 3, 3, 0.5, (double*)cov_f, 3, (double*)c1, 1, 0.0, t, 1);
			L_f = -ddot(3, (double*)c1, 1, (double*)t, 1);

			// L_B = -(color_B-color_mean_B)'*inv_C_B*(color_B-color_mean_B)/2;
			dgemv('N', 3, 3, 0.5, (double*)cov_b, 3, (double*)c2, 1, 0.0, t, 1);
			L_b = -ddot(3, (double*)c2, 1, (double*)t, 1);

			c1[0] = colorComp[0] - (alpha*c[0] + (1-alpha)*c[3]);
			c1[1] = colorComp[1] - (alpha*c[1] + (1-alpha)*c[4]);
			c1[2] = colorComp[2] - (alpha*c[2] + (1-alpha)*c[5]);

			// Likelihood(k) = -norm(color_C-a*color_F-(1-a)*color_B)^2/sigma_c^2 + L_F + L_B;
			L_sol = dnrm2(3, c1, 1);
			L_sol = -(L_sol*L_sol)*s2_i + L_f + L_b;

			if (L_sol > pObj->L)
			{
				pObj->L = L_sol;
				pObj->P = pow(10,L_sol);
				pObj->alpha = alpha;
				memcpy(pObj->color_f, &c[0], sizeof(color_vector_t));
				memcpy(pObj->color_b, &c[3], sizeof(color_vector_t));

				pObj->alpha_c = smin(smax(alpha,0),1);

				pObj->color_f_c[0] = smin(smax(pObj->color_f[0],0),1);
				pObj->color_f_c[1] = smin(smax(pObj->color_f[1],0),1);
				pObj->color_f_c[2] = smin(smax(pObj->color_f[2],0),1);

				pObj->color_b_c[0] = smin(smax(pObj->color_b[0],0),1);
				pObj->color_b_c[1] = smin(smax(pObj->color_b[1],0),1);
				pObj->color_b_c[2] = smin(smax(pObj->color_b[2],0),1);

				pObj->solver_rounds = iteration;
			}

#ifdef MATTE_DEBUG
			PrintMatrix("Mean_f=", (double*)colorMean_f, 1, 3);
			PrintMatrix("color_f=", (double*)&c[0], 1, 3);
			PrintMatrix("Mean_b=", (double*)colorMean_b, 1, 3);
			PrintMatrix("color_b=", (double*)&c[3], 1, 3);
			printf("Alpha=%g\n", alpha);
			printf("P=%g\n", pow(10,L_sol));
			printf("\n");
#endif

		}
	}
	// Loop end
	// ----------------------------
	if (pObj->solver_rounds == 0)
		fprintf(stderr, "MatteEstimate(): No solution found!\n");
}

// ------------------------------------------------------------
int MatteProcess(matte_t *pObj)
{

	int i, num_fore, num_back, num_colors_f, num_colors_b;
	int max_npixel, num_computed;
	pxl_id_t pid_u, pid_closest_f, pid_closest_b, *pPid_f, *pPid_b;
	pxl_id_t *pCalcSet = pObj->pCalcSet;
	set_data_t *pCompSet = &pObj->comp_set;
	pixel_t *pPixel = pCompSet->pPixel;
	color_vector_t *pColor_set;
	double *pW_f, *pW_b, *pAlpha_f, *pAlpha_b, *pLikely, radius_f, radius_b;
	color_vector_t colorComp, *pColorComp, *pColor_f, *pColor_b;
	matte_sol_t sol;
	char filename[1024];
	double progress, last_progress;

	last_progress = -REPORT_INTERVAL;

	if(pCompSet->num_fore > pCompSet->num_back)

		max_npixel = pCompSet->num_fore + pCompSet->num_unknown;
	else
		max_npixel = pCompSet->num_back + pCompSet->num_unknown;


	colorquant_t clob_f, clob_b;
	codebook_t cb_f, cb_b;
	color_stat_t colorStat_f, colorStat_b;

	ColorQuant_Init(&clob_f, max_npixel, MAX_COLORS);
	ColorQuant_Init(&clob_b, max_npixel, MAX_COLORS);
	CodebookAlloc(&cb_f, max_npixel, MAX_COLORS);
	CodebookAlloc(&cb_b, max_npixel, MAX_COLORS);

	pPid_f = (pxl_id_t*)malloc(max_npixel*sizeof(pxl_id_t));
	pPid_b = (pxl_id_t*)malloc(max_npixel*sizeof(pxl_id_t));

	pW_f = (double*)malloc(max_npixel*sizeof(double));
	pW_b = (double*)malloc(max_npixel*sizeof(double));

	pColor_set = (color_vector_t*)malloc(max_npixel*sizeof(color_vector_t));

	ColorStatInit(&colorStat_f, max_npixel, MAX_COLORS, pixel_state_foreground);
	ColorStatInit(&colorStat_b, max_npixel, MAX_COLORS, pixel_state_background);

	pColor_f = (color_vector_t*)pObj->im_f.pColor_data;
	pColor_b = (color_vector_t*)pObj->im_b.pColor_data;
	pColorComp = pColor_f;
	pAlpha_f = (double*)pObj->im_alpha_f.pColor_data;
	pAlpha_b = (double*)pObj->im_alpha_b.pColor_data;
	pLikely = (double*)pObj->im_P.pColor_data;

	num_computed = 0;
	for(i=0; i < pCompSet->num_unknown; i++)
	{

		pid_u = pCalcSet[i];

		if(pixel_state_unknown != pObj->comp_set.pPixel[pid_u].state)
		{
			fprintf(stderr,"Pixel #%d is not unknown\n", i);
		}

		memcpy(colorComp, pColorComp[pid_u], sizeof(color_vector_t));

		pid_closest_f = pPixel[pid_u].id_closest_f;
		pid_closest_b = pPixel[pid_u].id_closest_b;

		// Get foreground set and quantize
		radius_f = RADIUS_MIN;

#ifdef USE_SLIDING_WINDOW
		num_fore = MatteGetSet(pCompSet, pPid_f, pid_u, pAlpha_f, &radius_f, pixel_state_foreground | pixel_state_computed);
#else
		num_fore = MatteGetSet(pCompSet, pPid_f, pid_closest_f, pAlpha_f, &radius_f, pixel_state_foreground | pixel_state_computed);
#endif
		MatteGetColor(pPid_f, pColor_f, pColor_set, num_fore);

		num_colors_f = ColorQuant(&clob_f, &cb_f, pColor_set, 3, num_fore, MAX_COLORS, LAMBDA_THRESH);

		MatteComputeWeights(pCompSet, pid_u, pPid_f, pAlpha_f, pW_f, num_fore, radius_f, pixel_state_foreground);
		ColorStatProcess(pObj, &colorStat_f, &cb_f, pColor_set, num_fore, pW_f, SIGMA_C);

		// Get background set and quantize
		radius_b = RADIUS_MIN;

#ifdef USE_SLIDING_WINDOW
		num_back = MatteGetSet(pCompSet, pPid_b, pid_u, pAlpha_b, &radius_b, pixel_state_background | pixel_state_computed);
#else
		num_back = MatteGetSet(pCompSet, pPid_b, pid_closest_b, pAlpha_b, &radius_b, pixel_state_background | pixel_state_computed);
#endif
		MatteGetColor(pPid_b, pColor_b, pColor_set, num_back);

		num_colors_b = ColorQuant(&clob_b, &cb_b, pColor_set, 3, num_back, MAX_COLORS, LAMBDA_THRESH);

		MatteComputeWeights(pCompSet, pid_u, pPid_b, pAlpha_b, pW_b, num_back, radius_b, pixel_state_background);
		ColorStatProcess(pObj, &colorStat_b, &cb_b, pColor_set, num_back, pW_b, SIGMA_C);

		// Find solution
		MatteEstimate(&sol, colorComp, &colorStat_f, &colorStat_b, SIGMA_C);

#ifdef	MATTE_DEBUG
		printf("\n------------------------------------------\n");
		printf("Unknown Pixel #%d\n", i);
		PrintMatrix("color_c=", (double*)colorComp, 3, 1);
		PrintMatrix("color_f=", (double*)sol.color_f, 3, 1);
		PrintMatrix("color_b=", (double*)sol.color_b, 3, 1);
		printf("Alpha= %f\n", sol.alpha);
		printf("P_sol=%f\n", sol.P);
		printf("Number of solver round=%d\n",sol.solver_rounds);
		printf("Number of colors int foreground statistic=%d\n",colorStat_f.num_colors);
		printf("Number of colors int background statistic=%d\n",colorStat_b.num_colors);
#endif
		memcpy(pColor_f[pid_u], sol.color_f_c, sizeof(color_vector_t));
		memcpy(pColor_b[pid_u], sol.color_b_c, sizeof(color_vector_t));
		pAlpha_f[pid_u] = sol.alpha_c;
		pAlpha_b[pid_u] = 1-sol.alpha_c;
		pLikely[pid_u] = sol.P;

		// Label unknown pixel as computed
		if (sol.solver_rounds > 0)
		{
			pObj->comp_set.pPixel[pid_u].state = pixel_state_computed;
			num_computed++;
		}
		else
		{
			fprintf(stderr, "Unknown Pixel #%d(ID=%d) not computed.\n", i, pid_u);
		}


#ifdef WITH_ANIMATION
		sprintf(filename, "ani/%s_alpha_%5.5d.tif",pObj->name,i);
		ImageSaveTiff(&pObj->im_alpha_f, NULL, filename);
		sprintf(filename, "ani/%s_est_fore_%5.5d.tif",pObj->name, i);
		ImageSaveTiff(&pObj->im_f, NULL, filename);
		sprintf(filename, "ani/%s_est_back_%5.5d.tif",pObj->name, i);
		ImageSaveTiff(&pObj->im_b, NULL, filename);
#endif
		// Output progress
		progress = 100*(double)num_computed/(pCompSet->num_unknown-1);

		if ((progress-last_progress) >= REPORT_INTERVAL)
		{
			printf("\r%d %% computed...", (int)progress);
			last_progress = progress;
		}
	}


	printf("\r%d %% computed.   \n", (int)progress);
	printf("%d of %d pixels computed.\n\n", num_computed, pCompSet->num_unknown);

	sprintf(filename, "%s_alpha.tif", pObj->name);
	ImageSaveTiff(&pObj->im_alpha_f, NULL, filename);
	sprintf(filename, "%s_fore.tif",pObj->name);
	ImageSaveTiff(&pObj->im_f, pAlpha_f, filename);
	sprintf(filename, "%s_back.tif",pObj->name);
	ImageSaveTiff(&pObj->im_b, pAlpha_b, filename);
	sprintf(filename, "%s_est_fore.tif",pObj->name);
	ImageSaveTiff(&pObj->im_f, NULL, filename);
	sprintf(filename, "%s_est_back.tif",pObj->name);
	ImageSaveTiff(&pObj->im_b, NULL, filename);
	sprintf(filename, "%s_p.tif",pObj->name);
	ImageSaveTiff(&pObj->im_P, NULL, filename);

	ImageFree(&pObj->im_f);
	ImageFree(&pObj->im_b);
	ImageFree(&pObj->im_alpha_f);
	ImageFree(&pObj->im_alpha_b);
	ImageFree(&pObj->im_P);

	CodebookFree(&cb_f);
	CodebookFree(&cb_b);

	ColorQuant_Free(&clob_f);
	ColorQuant_Free(&clob_b);

	ColorStatFree(&colorStat_f);
	ColorStatFree(&colorStat_b);

	free(pPid_f);
	free(pPid_b);
	free(pW_f);
	free(pW_b);
	free(pColor_set);

}
// ------------------------------------------------------------