// Kernel_Optimization.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "time.h"






void		initialize_psi_vector(long** psi_vector, long** N_psi_vector, long **gamma_vector);


long			proj_optim_l1_norm_for_edges(double *f_opt,double *Z_opt, double *h, double *new_he, double *new_ve, double *new_de, double *new_dre, long N, long M, long k01, long k02, long k03, long k04, double * Z, double *Kappa, double lambda,double b0, double b1, double epsilon0, double *rho1, double *rho2, double *rho3, double *rho4,double slope, double mu);
long			proj_optim_l1_norm_for_edges2(double *f_opt,double *Z_opt, double *h, double *new_he, double *new_ve, double *new_de, double *new_dre, long N, long M, long k01, long k02, long k03, long k04, double * Z, double *Kappa, double lambda,double b0, double b1, double epsilon0, double *rho1, double *rho2, double *rho3, double *rho4,double slope, double mu,double mu2);
long			proj_optim_l1_norm(double *f_opt,double *Z_opt, double *h, long N, long M, double *Z, double *Kappa, double lambda,double b0, double b1, double epsilon0);
long			proj_optim_l1_l2_norm_semi_parametric(/*outputs*/double *f_opt,double *Z_opt, double *h, double *b_psi, /*inputs*/long *psi, double *gamma, short *psi_used, double *Z, double *Kappa, long N, long N_psi, double lambda,double b0, double b1, double epsilon0, double mu, double mu2,double E1, double E2);
long			proj_optim_l1_l2_norm_semi_parametric(/*outputs*/double *f_opt,double *Z_opt, double *h, double *b_psi, /*inputs*/long *psi, double *gamma, short *psi_used, double *Z, double *Kappa, long N, long N_psi, double lambda,double b0, double b1, double epsilon0, double mu, double mu2,double E1, double E2);
long			proj_optim_l1_l1_norm_semi_parametric(/*outputs*/double *f_opt,double *Z_opt, double *h, double *b_psi, /*inputs*/long *psi, double *gamma, short *psi_used, double *Z, double *Kappa, long N, long N_psi, double lambda,double b0, double b1, double epsilon0, double mu, double mu2,double E1, double E2);

long			progressive_proj_optim_l1_l2_norm_semi_parametric(/*outputs*/double *f_opt,double *Z_opt, double *h, double *b_psi, /*inputs*/long *psi, double *gamma, short *psi_used, double *Z, double *Kappa, long N, long N_psi, double lambda,double b0, double b1, double epsilon0, double mu, double mu2, double E1, double E2,short keep);



void			Kernel_Denoising(double *return_pic, double *ori_pic, long M0, long N0, long n, long steps, double lambda_min, double lambda_max, double sigma_min, double sigma_max,double b0, double b1, double epsilon0, double initial_mu, double *mu_vector, long LEN_mu, double *per_vector, long *n_vector, long *method_vector, long *iter_pixel_step);
void			Kernel_Denoising_semi_parametric(/*output*/double *return_pic, /*inputs*/double *ori_pic, long M0, long N0, long n, long steps, double lambda, double sigma,double b0, double b1, double epsilon0, double initial_mu, double *mu_vector, double *mu2_vector, double *calc_type, long LEN_mu, double *per_vector, long *n_vector, long *method_vector, long *iter_pixel_step, long *psi_vector, long *N_psi_vector, long *gamma_vector);
void			fix_edge_values(double *pic, double* edge_value, int M0, int N0, int n, int k_max,double b0, double b1);

void		replace_with_min_in_window(double* buffer, int M0, int N0, int n);
void		image_extend(double *pic,double *ori_pic, long M0, long N0, long m, long n);
void		window_median_grad_calc(double *med_grad, double *im, long M, long N, long n1, long n2);
void		window_max_grad_calc(double *med_grad, double *im, long M, long N, long n1, long n2);
void		window_norm_grad_calc(double *med_grad, double *im, long M, long N, long n1, long n2);
void		norm_grad_calc(double *grad, double *im, long M, long N);
double		PSNR_calc(double *Im1, double *Im2,long N,double R);
long		doubles_compare(const double * x, const double *y);
void		RKHS_eval(double *f_val, double *f, double *Kappa, long M, long N);
double		RKHS_norm(double *f, double *Kappa, long N, long M);
double		mean_value2D(double *Z, long N, long M);
void		zero_filling(double *mem, long N);
void		constant_filling_short(short *mem, short c, long N);
void		zero_filling_long(long *mem, long N);
double		square_sum1D(double *A, long N);
double		sum1D(double *A, long N);
double		find_max(double *vector, long N);
long		find_max_long(long *vector, long N);
double		sign(double x);
double		signbit(double x);
double		erf1(double x);
double		erf(double x);
double		erfc(double x);
double		round(double);





void initialize_psi_vector(long** psi_vector, long** N_psi_vector, long **gamma_vector){
	int			V=6;
	int			l0,l1,l,l2;
	long		i,j,k,v,n;
	double*		*psi;
	double		*gamma;
	double		x[20],y[20],slope;
	
	*psi_vector=(long*)malloc(V*sizeof(long));
	*N_psi_vector=(long*)malloc(V*sizeof(long));
	*gamma_vector=(long*)malloc(V*sizeof(long));

	/*for v=0*/
	v=0;
	n=3;
	(*N_psi_vector)[v]=10;
	psi=(double**)malloc( (*N_psi_vector)[v]*sizeof(long) );
	gamma=(double*)malloc( (*N_psi_vector)[v]*sizeof(double) );
	(*psi_vector)[v]=psi;
	(*gamma_vector)[v]=gamma;
	/*Construct the 3x3 functions*/
	psi[0]=(double*)malloc( n*n*sizeof(double) );
	gamma[0]=1;
	psi[0][0]=-1;  psi[0][1]=1;   psi[0][2]=1;
	psi[0][3]=-1;  psi[0][4]=1;   psi[0][5]=1;
	psi[0][6]=-1;  psi[0][7]=1;   psi[0][8]=1;

	psi[1]=(double*)malloc( n*n*sizeof(double) );
	gamma[1]=1;
	psi[1][0]=-1;  psi[1][1]=-1;   psi[1][2]=1;
	psi[1][3]=-1;  psi[1][4]=-1;   psi[1][5]=1;
	psi[1][6]=-1;  psi[1][7]=-1;   psi[1][8]=1;

	psi[2]=(double*)malloc( n*n*sizeof(double) );
	gamma[2]=1;
	psi[2][0]=-1;  psi[2][1]=-1;   psi[2][2]=-1;
	psi[2][3]=1;  psi[2][4]=1;   psi[2][5]=1;
	psi[2][6]=1;  psi[2][7]=1;   psi[2][8]=1;

	psi[3]=(double*)malloc( n*n*sizeof(double) );
	gamma[3]=1;
	psi[3][0]=-1;  psi[3][1]=-1;   psi[3][2]=-1;
	psi[3][3]=-1;  psi[3][4]=-1;   psi[3][5]=-1;
	psi[3][6]=1;  psi[3][7]=1;   psi[3][8]=1;

	psi[4]=(double*)malloc( n*n*sizeof(double) );
	gamma[4]=1;
	psi[4][0]=1;  psi[4][1]=-1;   psi[4][2]=-1;
	psi[4][3]=1;  psi[4][4]=1;   psi[4][5]=-1;
	psi[4][6]=1;  psi[4][7]=1;   psi[4][8]=1;

	
	psi[5]=(double*)malloc( n*n*sizeof(double) );
	gamma[5]=1;
	psi[5][0]=-1;  psi[5][1]=-1;   psi[5][2]=-1;
	psi[5][3]=1;  psi[5][4]=-1;   psi[5][5]=-1;
	psi[5][6]=1;  psi[5][7]=1;   psi[5][8]=-1;


	psi[6]=(double*)malloc( n*n*sizeof(double) );
	gamma[6]=1;
	psi[6][0]=-1;  psi[6][1]=-1;   psi[6][2]=1;
	psi[6][3]=-1;  psi[6][4]=1;   psi[6][5]=1;
	psi[6][6]=1;  psi[6][7]=1;   psi[6][8]=1;


	psi[7]=(double*)malloc( n*n*sizeof(double) );
	gamma[7]=1;
	psi[7][0]=-1;  psi[7][1]=-1;   psi[7][2]=-1;
	psi[7][3]=-1;  psi[7][4]=-1;   psi[7][5]=1;
	psi[7][6]=-1;  psi[7][7]=1;   psi[7][8]=1;

	psi[8]=(double*)malloc( n*n*sizeof(double) );
	gamma[8]=1;
	psi[8][0]=0;  psi[8][1]=2;   psi[8][2]=0;
	psi[8][3]=0;  psi[8][4]=2;   psi[8][5]=0;
	psi[8][6]=0;  psi[8][7]=2;   psi[8][8]=0;


	psi[9]=(double*)malloc( n*n*sizeof(double) );
	gamma[9]=1;
	psi[9][0]=0;  psi[9][1]=0;   psi[9][2]=0;
	psi[9][3]=2;  psi[9][4]=2;   psi[9][5]=2;
	psi[9][6]=0;  psi[9][7]=0;   psi[9][8]=0;


	/********************************************************************/
	/*for v=1*/
	/*********************************************************************/
	v=1;
	n=5;
	(*N_psi_vector)[v]=44;
	psi=(double**)malloc( (*N_psi_vector)[v]*sizeof(long) );
	gamma=(double*)malloc( (*N_psi_vector)[v]*sizeof(double) );
	(*psi_vector)[v]=psi;
	(*gamma_vector)[v]=gamma;
	for(i=0;i<n;i++)
		x[i]=((double)i)/(n-1);
	for(j=0;j<n;j++)
		y[j]=((double)j)/(n-1);

	slope=(n-1);
	k=0;/*vertical edges*/
	l0=2;
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf(slope*2*(x[i] - 1/2.0 + l/(2*slope)) );
			}
		k++;
	}

/*horizontal edges*/
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf(slope*2*(y[j] - 1/2.0 + l/(2*slope)) );
			}
		k++;
	}


	/*diagonal edges  / */
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf(slope*2*(x[i] - y[j] + l/(2*slope)) );
			}
		k++;
	}


	/*diagonal edges  \ */
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf(slope*2*(x[i] + y[j] - 1 + l/(2*slope)) );
			}
		k++;
	}

	/*diagonal edges 2    slope x- 2*slope y*/
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf( slope*(x[i]-2*y[j] + 1/2.0 + l/slope) );
			}
		k++;
	}


	/*diagonal edges 2    2*slope x- slope y*/
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf( slope*(2*x[i]-y[j] - 1/2.0 + l/slope) );
			}
		k++;
	}


	/*diagonal edges 2    slope x + 2*slope y*/
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf( slope*(2*x[i] + y[j] - 3/2.0 + l/slope ) );
			}
		k++;
	}

	/*diagonal edges 2    slope x + 2*slope y*/
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf( slope*(x[i] + 2*y[j] - 3/2.0 + l/slope) );
			}
		k++;
	}



	/**horizontal peak**/
	l2=0;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(x[i]-0.5 + l/slope)*(x[i]-0.5 + l/slope) );
			}
		k++;
	}


	/*vertical peak*/
	l2=0;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(y[j]-0.5 + l/slope)*(y[j]-0.5 + l/slope) );
			}
		k++;
	}


	/**diagonal peak  /  **/
	l2=2;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(x[i]-y[j] + l/slope)*(x[i]-y[j] + l/slope) );
			}
		k++;
	}

	/**diagonal peak  \  **/
	l2=2;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(x[i]+y[j] -1 + l/slope)*(x[i]+y[j]-1 + l/slope) );
			}
		k++;
	}
	

	/**diagonal peak  6x-12y   **/
	l2=2;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(x[i]-2*y[j] +1/2.0 + 2*l/slope)*(x[i]-2*y[j] +1/2.0 + 2*l/slope) );
			}
		k++;
	}

	/**diagonal peak  12x-6y  **/
	l2=2;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(2*x[i]-y[j] - 1/2.0 + 2*l/slope)*(2*x[i]-y[j] - 1/2.0 + 2*l/slope) );
			}
		k++;
	}

	/**diagonal peak  6x+12y  **/
	l2=2;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(x[i]+2*y[j] - 3/2.0 + 2*l/slope)*(x[i]+2*y[j] - 3/2.0 + 2*l/slope) );
			}
		k++;
	}

	/**diagonal peak  12x+6y  **/
	l2=2;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(2*x[i]+y[j] - 3/2.0 + 2*l/slope)*(2*x[i]+y[j] - 3/2.0 + 2*l/slope) );
			}
		k++;
	}

	


	/***************************************************************/
	/*for v=2*/
	/***************************************************************/
	v=2;
	n=7;
	(*N_psi_vector)[v]=52;
	psi=(double**)malloc( (*N_psi_vector)[v]*sizeof(long) );
	gamma=(double*)malloc( (*N_psi_vector)[v]*sizeof(double) );
	(*psi_vector)[v]=psi;
	(*gamma_vector)[v]=gamma;
	for(i=0;i<n;i++)
		x[i]=((double)i)/(n-1);
	for(j=0;j<n;j++)
		y[j]=((double)j)/(n-1);

	slope=(n-1);
	k=0;/*vertical edges*/
	l0=4;
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf(slope*2*(x[i] - 1/2.0 + l/(2*slope)) );
			}
		k++;
	}

/*horizontal edges*/
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf(slope*2*(y[j] - 1/2.0 + l/(2*slope)) );
			}
		k++;
	}


	/*diagonal edges  / */
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf(slope*2*(x[i] - y[j] + l/(2*slope)) );
			}
		k++;
	}


	/*diagonal edges  \ */
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf(slope*2*(x[i] + y[j] - 1 + l/(2*slope)) );
			}
		k++;
	}

	/*diagonal edges 2    slope x- 2*slope y*/
	l0=2;
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf( slope*(x[i]-2*y[j] + 1/2.0 + l/slope) );
			}
		k++;
	}


	/*diagonal edges 2    2*slope x- slope y*/
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf( slope*(2*x[i]-y[j] - 1/2.0 + l/slope) );
			}
		k++;
	}


	/*diagonal edges 2    slope x + 2*slope y*/
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf( slope*(2*x[i] + y[j] - 3/2.0 + l/slope ) );
			}
		k++;
	}

	/*diagonal edges 2    slope x + 2*slope y*/
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf( slope*(x[i] + 2*y[j] - 3/2.0 + l/slope) );
			}
		k++;
	}



	/**horizontal peak**/
	l2=0;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(x[i]-0.5 + l/slope)*(x[i]-0.5 + l/slope) );
			}
		k++;
	}


	/*vertical peak*/
	l2=0;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(y[j]-0.5 + l/slope)*(y[j]-0.5 + l/slope) );
			}
		k++;
	}


	/**diagonal peak  /  **/
	l2=2;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(x[i]-y[j] + l/slope)*(x[i]-y[j] + l/slope) );
			}
		k++;
	}

	/**diagonal peak  \  **/
	l2=2;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(x[i]+y[j] -1 + l/slope)*(x[i]+y[j]-1 + l/slope) );
			}
		k++;
	}
	

	/**diagonal peak  6x-12y   **/
	l2=2;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(x[i]-2*y[j] +1/2.0 + 2*l/slope)*(x[i]-2*y[j] +1/2.0 + 2*l/slope) );
			}
		k++;
	}

	/**diagonal peak  12x-6y  **/
	l2=2;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(2*x[i]-y[j] - 1/2.0 + 2*l/slope)*(2*x[i]-y[j] - 1/2.0 + 2*l/slope) );
			}
		k++;
	}

	/**diagonal peak  6x+12y  **/
	l2=2;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(x[i]+2*y[j] - 3/2.0 + 2*l/slope)*(x[i]+2*y[j] - 3/2.0 + 2*l/slope) );
			}
		k++;
	}

	/**diagonal peak  12x+6y  **/
	l2=2;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(2*x[i]+y[j] - 3/2.0 + 2*l/slope)*(2*x[i]+y[j] - 3/2.0 + 2*l/slope) );
			}
		k++;
	}

	


	/***************************************************************/
	/*for v=3*/
	/***************************************************************/
	v=3;
	n=9;
	(*N_psi_vector)[v]=76;
	psi=(double**)malloc( (*N_psi_vector)[v]*sizeof(long) );
	gamma=(double*)malloc( (*N_psi_vector)[v]*sizeof(double) );
	(*psi_vector)[v]=psi;
	(*gamma_vector)[v]=gamma;
	for(i=0;i<n;i++)
		x[i]=((double)i)/(n-1);
	for(j=0;j<n;j++)
		y[j]=((double)j)/(n-1);

	slope=(n-1);
	k=0;/*vertical edges*/
	l0=4;
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf(slope*2*(x[i] - 1/2.0 + l/(2*slope)) );
			}
		k++;
	}

/*horizontal edges*/
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf(slope*2*(y[j] - 1/2.0 + l/(2*slope)) );
			}
		k++;
	}


	/*diagonal edges  / */
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf(slope*2*(x[i] - y[j] + l/(2*slope)) );
			}
		k++;
	}


	/*diagonal edges  \ */
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf(slope*2*(x[i] + y[j] - 1 + l/(2*slope)) );
			}
		k++;
	}

	/*diagonal edges 2    slope x- 2*slope y*/
	l0=4;
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf( slope*(x[i]-2*y[j] + 1/2.0 + l/slope) );
			}
		k++;
	}


	/*diagonal edges 2    2*slope x- slope y*/
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf( slope*(2*x[i]-y[j] - 1/2.0 + l/slope) );
			}
		k++;
	}


	/*diagonal edges 2    slope x + 2*slope y*/
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf( slope*(2*x[i] + y[j] - 3/2.0 + l/slope ) );
			}
		k++;
	}

	/*diagonal edges 2    slope x + 2*slope y*/
	for(l=-l0;l<=+l0;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=erf( slope*(x[i] + 2*y[j] - 3/2.0 + l/slope) );
			}
		k++;
	}



	/**horizontal peak**/
	l2=2;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(x[i]-0.5 + l/slope)*(x[i]-0.5 + l/slope) );
			}
		k++;
	}


	/*vertical peak*/
	l2=2;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(y[j]-0.5 + l/slope)*(y[j]-0.5 + l/slope) );
			}
		k++;
	}


	/**diagonal peak  /  **/
	l2=4;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(x[i]-y[j] + l/slope)*(x[i]-y[j] + l/slope) );
			}
		k++;
	}

	/**diagonal peak  \  **/
	l2=4;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(x[i]+y[j] -1 + l/slope)*(x[i]+y[j]-1 + l/slope) );
			}
		k++;
	}
	

	/**diagonal peak  6x-12y   **/
	l2=4;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(x[i]-2*y[j] +1/2.0 + 2*l/slope)*(x[i]-2*y[j] +1/2.0 + 2*l/slope) );
			}
		k++;
	}

	/**diagonal peak  12x-6y  **/
	l2=4;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(2*x[i]-y[j] - 1/2.0 + 2*l/slope)*(2*x[i]-y[j] - 1/2.0 + 2*l/slope) );
			}
		k++;
	}

	/**diagonal peak  6x+12y  **/
	l2=4;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(x[i]+2*y[j] - 3/2.0 + 2*l/slope)*(x[i]+2*y[j] - 3/2.0 + 2*l/slope) );
			}
		k++;
	}

	/**diagonal peak  12x+6y  **/
	l2=4;
	for(l=-l2;l<=+l2;l+=2){
		gamma[k]=1;
		psi[k]=(double*)malloc( n*n*sizeof(double) );
		for (i=0;i<n;i++)
			for(j=0;j<n;j++){
				psi[k][j*n+i]=2*exp( -slope*(2*x[i]+y[j] - 3/2.0 + 2*l/slope)*(2*x[i]+y[j] - 3/2.0 + 2*l/slope) );
			}
		k++;
	}

}




long		doubles_compare(const double * x, const double *y){
	if (*x==*y)
		return 0;
	if (*x>*y)
		return 1;
	else
		return -1;
}


double		round(double x){
	if (ceil(x)-x>x-floor(x))
		return	floor(x);
	else
		return ceil(x);
}


double	find_max(double *vector, long N)
{
	double	max;
	long		i;

	max=vector[0];
	for(i=1;i<N;i++){
		if (vector[i]>max)
			max=vector[i];
	}
	return max;
}

double	find_min(double *vector, long N)
{
	double	min;
	long		i;

	min=vector[0];
	for(i=1;i<N;i++){
		if (vector[i]<min)
			min=vector[i];
	}
	return min;
}

long	find_max_long(long *vector, long N)
{
	long	max;
	long		i;

	max=vector[0];
	for(i=1;i<N;i++){
		if (vector[i]>max)
			max=vector[i];
	}
	return max;
}



void   zero_filling(double *mem, long N){
	long  i;
	for (i=0;i<N;i++){
		mem[i]=0;
	}
}

void   constant_filling_short(short *mem, short c, long N){
	long  i;
	for (i=0;i<N;i++){
		mem[i]=c;
	}
}

void   zero_filling_long(long *mem, long N){
	long  i;
	for (i=0;i<N;i++){
		mem[i]=0;
	}
}

/*************************** 
*   erf.cpp 
*   author:  Steve Strand 
*   written: 29-Jan-04 
***************************/ 


static const double rel_error= 1E-12;        //calculate 12 significant figures 
//you can adjust rel_error to trade off between accuracy and speed 
//but don't ask for > 15 figures (assuming usual 52 bit mantissa in a double) 

double signbit(double x){
	if (x<0)
		return 1;
	else 
		return 0;
}


double erf(double x) {
	if (x>0)
		return erf1(x);
	else
		return -erf1(-x);
}

double erf1(double x) 
//erf(x) = 2/sqrt(pi)*integral(exp(-t^2),t,0,x) 
//       = 2/sqrt(pi)*[x - x^3/3 + x^5/5*2! - x^7/7*3! + ...] 
//       = 1-erfc(x) 
{ 
    static const double two_sqrtpi=  1.128379167095512574;        // 2/sqrt(pi) 
	double sum= x, term= x, xsqr= x*x; 
	long j= 1;
    if (fabs(x) > 2.2) { 
        return 1.0 - erfc(x);        //use continued fraction when fabs(x) > 2.2 
    } 
    
    do { 
        term*= xsqr/j; 
        sum-= term/(2*j+1); 
        ++j; 
        term*= xsqr/j; 
        sum+= term/(2*j+1); 
        ++j; 
    } while (fabs(term)/sum > rel_error); 
    return two_sqrtpi*sum; 
} 


double erfc(double x) 
//erfc(x) = 2/sqrt(pi)*integral(exp(-t^2),t,x,inf) 
//        = exp(-x^2)/sqrt(pi) * [1/x+ (1/2)/x+ (2/2)/x+ (3/2)/x+ (4/2)/x+ ...] 
//        = 1-erf(x) 
//expression inside [] is a continued fraction so '+' means add to denominator only 
{ 
    static const double one_sqrtpi=  0.564189583547756287;        // 1/sqrt(pi) 
	double a=1, b=x;                //last two convergent numerators 
    double c=x, d=x*x+0.5;          //last two convergent denominators 
    double q1, q2= b/d;             //last two convergents (a/c and b/d) 
    double n= 1.0, t; 
    if (fabs(x) < 2.2) { 
        return 1.0 - erf1(x);        //use series when fabs(x) < 2.2 
    } 
    if (signbit(x)) {              //continued fraction only valid for x>0 
        return 2.0 - erfc(-x); 
    } 
    
    do { 
        t= a*n+b*x; 
        a= b; 
        b= t; 
        t= c*n+d*x; 
        c= d; 
        d= t; 
        n+= 0.5; 
        q1= q2; 
        q2= b/d; 
      } while (fabs(q1-q2)/q2 > rel_error); 
    return one_sqrtpi*exp(-x*x)*q2; 
} 
/***************************************************************/




double sign(double x){
	if (x>=0){
		if (x==0)
			return 0;
		else
			return +1;
	}
	else
		return -1;
}

double square_sum1D(double *A, long N){
	long	i;
	double sum=0;

	for(i=0;i<N;i++){
		sum+=A[i]*A[i];
	}
	return sum;
}

double sum1D(double *A, long N){
	long	i;
	double sum=0;

	for(i=0;i<N;i++){
		sum+=A[i];
	}
	return sum;
}





double PSNR_calc(double *Im1, double *Im2,long N,double R){
	long		i;
	double	MSE;

	MSE=0;
	for(i=0;i<N;i++){
		MSE+=(Im1[i]-Im2[i])*(Im1[i]-Im2[i])/N;
	}
	return 10*log10(R*R/MSE);
}



double mean_value2D(double *Z, long N, long M)
{
	long i;
	double sum=0;
	for (i=0;i<N*M;i++){
		sum+=Z[i];
	}
	return sum/N/M;
}


double RKHS_norm(double *f, double *Kappa, long N, long M){

	double norm=0;
	long i,j,i1,j1;

	for(j=1;j<=N;j++){
		for (i=1;i<=M;i++){
			for (j1=1;j1<=N;j1++){
				for (i1=1;i1<=M;i1++){
					norm=norm + f[(j-1)*M+i-1]*f[(j1-1)*M+i1-1]*Kappa[(j-1)*M*N*M+(i-1)*N*M + (j1-1)*N +i1-1];
				}
			}
		}
	}
	return norm;
}


void RKHS_eval(double *f_val, double *f, double *Kappa, long M, long N){

	double  sum;
	long		i,j,i1,j1;

	for (j=1;j<=N;j++){
		for (i=1;i<=M;i++){
			sum=0;
			for (j1=1;j1<=N;j1++){
				for (i1=1;i1<=M;i1++){
					sum=sum+f[(j1-1)*M+i1-1]*Kappa[(j-1)*M*N*M+(i-1)*N*M+(j1-1)*M+i1-1];
				}
			}
			(f_val)[(j-1)*M+i-1]=sum;
		}
	}
}









/*for image*/
void	image_extend(double *extended_im,double *im, long M0, long N0, long m, long n){

	long	i,j,i0,j0,M,N;
	
	M=M0+m;
	N=N0+n;
	/*-----------------*/
	/*     |-----------|
	       |-----------|
		   |-----------|
		   |-----------|
		   *************
		   */
	for (i=1;i<=m/2;i++)
		for (j=1;j<=N0;j++)
			/*extended_im(i,j+n/2)=im(1,j);*/
			extended_im[(i-1)*N+j+n/2-1]=im[j-1];

	/*extended_im(m/2+1:M+m/2,1+n/2:N+n/2)=im(1:M,1:N);*/
	i0=1;
	for (i=m/2+1;i<=M0+m/2;i++){
		j0=1;
		for (j=1+n/2;j<=N0+n/2;j++){
			extended_im[(i-1)*N+j-1]=im[(i0-1)*N0+j0-1];
			j0++;
		}
		i0++;
	}

	for (i=M0+m/2+1;i<=M0+m;i++)
		for (j=1;j<=N0;j++)
			/*extended_im(i,j+n/2)=im(M,j);*/
			extended_im[(i-1)*N+j-1+n/2]=im[(M0-1)*N0+j-1];


	for (i=1;i<=M;i++)
		for (j=1;j<=n/2;j++)
			/*extended_im(i,j)=extended_im(i,n/2+1);*/
			extended_im[(i-1)*N+j-1]=extended_im[(i-1)*N+n/2];
    

	for (i=1;i<=M;i++)
		for (j=N-n/2+1;j<=N;j++)
			/*extended_im(i,j)=extended_im(i,N-n/2-1);*/
			extended_im[(i-1)*N+j-1]=extended_im[(i-1)*N+N-n/2-1];

}






void norm_grad_calc(double *grad, double *im, long M, long N){
	long	 i,j;

	/*grad(1,1)= abs(im(1,1)-im(1,2))  + abs(im(1,1)-im(2,1))  + abs(im(1,1)-im(2,2)) ;*/
	grad[0]=fabs(im[0]-im[1]) + fabs(im[0]-im[N]) + fabs(im[0]-im[N+1]);

	for (j=2;j<=N-1;j++)
		grad[j-1]= fabs(im[j-1]-im[j-2])  + fabs(im[j-1]-im[N+j-2])  + fabs(im[j-1]-im[N+j-1]) + fabs(im[j-1]-im[N+j]) + fabs(im[j-1]-im[j]) ;
	
	/*for j=N*/
	grad[N-1]=fabs(im[N-1]-im[N-2])  + fabs(im[N-1]-im[N+N-2])  + fabs(im[N-1]-im[N+N-1]);

	for(i=2;i<=M-1;i++){
		/*for j=1*/
		grad[(i-1)*N]=fabs(im[(i-1)*N]-im[i*N]) + fabs(im[(i-1)*N]-im[i*N+1]) + fabs(im[(i-1)*N]-im[(i-1)*N+1]) + fabs(im[(i-1)*N]-im[(i-2)*N+1]) + fabs(im[(i-1)*N]-im[(i-2)*N]) ;
		for(j=2;j<=N-1;j++)
			grad[(i-1)*N+j-1]=fabs(im[(i-1)*N+j-1]-im[(i-2)*N+j-1]) + fabs(im[(i-1)*N+j-1]-im[(i-2)*N+j-2]) + fabs(im[(i-1)*N+j-1]-im[(i-1)*N+j-2]) + fabs(im[(i-1)*N+j-1]-im[i*N+j-2])  + fabs(im[(i-1)*N+j-1]-im[i*N+j-1])  +  fabs(im[(i-1)*N+j-1]-im[i*N+j]) + fabs(im[(i-1)*N+j-1]-im[(i-1)*N+j]) + fabs(im[(i-1)*N+j-1]-im[(i-2)*N+j]) ;
		
		/*for j=M*/
		grad[(i-1)*N+j-1]=fabs(im[(i-1)*N+N-1]-im[(i-2)*N+N-1]) + fabs(im[(i-1)*N+N-1]-im[(i-2)*N+N-2]) + fabs(im[(i-1)*N+N-1]-im[(i-1)*N+N-2])  + fabs(im[(i-1)*N+N-1]-im[i*N+N-2]) + fabs(im[(i-1)*N+N-1]-im[i*N+N-1]);
	}

	/*for i=M*/
	grad[(M-1)*N]=fabs(im[(M-1)*N]-im[(M-2)*N])  + fabs(im[(M-1)*N]-im[(M-2)*N+1])  + fabs(im[(M-1)*N]-im[(M-1)*N+1])  ;
	for (j=2;j<=N-1;j++)
		grad[(M-1)*N+j-1]= fabs(im[(M-1)*N+j-1]-im[(M-1)*N+j-2]) + fabs(im[(M-1)*N+j-1]-im[(M-2)*N+j-2])  + fabs(im[(M-1)*N+j-1]-im[(M-2)*N+j-1])  + fabs(im[(M-1)*N+j-1]-im[(M-2)*N+j])  + fabs(im[(M-1)*N+j-1]-im[(M-1)*N+j]) ;
	
	grad[(M-1)*N+N-1]=fabs(im[(M-1)*N+N-1]-im[(M-2)*N+N-1]) + fabs(im[(M-1)*N+N-1]-im[(M-2)*N+N-2])  + fabs(im[(M-1)*N+N-1]-im[(M-1)*N+N-2])  ;

}



void  window_median_grad_calc(double *med_grad, double *im, long M, long N, long n1, long n2){

	double	*grad=(double*)malloc(M*N*sizeof(double));
	long		i,j,k,l,i0,j0,N0=N-n1+1;
	double	sum;

	norm_grad_calc(grad,  im,  M,  N);

	i0=0;
	for (i=1+(long)floor(n1/2);i<=M-(long)floor(n1/2);i++){
		j0=0;
		for(j=1+(long)floor(n1/2);j<=N-(long)floor(n1/2);j++){
			sum=0;
			for (k=-(long)floor(n2/2);k<=(long)floor(n2/2);k++)
				for (l=-(long)floor(n2/2);l<=(long)floor(n2/2);l++)
					sum=sum+grad[(i+k-1)*N+j+l-1];
			med_grad[(i0)*N0+j0]=sum/n2/n2;
			j0++;
		}
		i0++;
	}
	free(grad);
}




void  window_max_grad_calc(double *med_grad, double *im, long M, long N, long n1, long n2){

	double	*grad=(double*)malloc(M*N*sizeof(double));
	long		i,j,k,l,i0,j0,N0=N-n1+1;
	double	max;

	norm_grad_calc(grad,  im,  M,  N);

	i0=0;
	for (i=1+(long)floor(n1/2);i<=M-(long)floor(n1/2);i++){
		j0=0;
		for(j=1+(long)floor(n1/2);j<=N-(long)floor(n1/2);j++){
			max=-1;
			for (k=-(long)floor(n2/2);k<=(long)floor(n2/2);k++)
				for (l=-(long)floor(n2/2);l<=(long)floor(n2/2);l++)
					if (grad[(i+k-1)*N+j+l-1]>max)
						max=grad[(i+k-1)*N+j+l-1];
			med_grad[(i0)*N0+j0]=max;
			j0++;
		}
		i0++;
	}
	free(grad);
}





void  window_norm_grad_calc(double *med_grad, double *im, long M, long N, long n1, long n2){

	double	*grad=(double*)malloc(M*N*sizeof(double));
	long		i,j,k,l,i0,j0,N0=N-n1+1;
	double	max;

	norm_grad_calc(grad,  im,  M,  N);

	i0=0;
	for (i=1+(long)floor(n1/2);i<=M-(long)floor(n1/2);i++){
		j0=0;
		for(j=1+(long)floor(n1/2);j<=N-(long)floor(n1/2);j++){
			med_grad[(i0)*N0+j0]=grad[(i-1)*N+j-1];
			j0++;
		}
		i0++;
	}
	free(grad);
}










/*optimization methods*/
long proj_optim_l1_norm_for_edges(double *f_opt,double *Z_opt, double *h, double *new_he, double *new_ve, double *new_de, double *new_dre, long N, long M, long k01, long k02, long k03, long k04, double *Z, double *Kappa, double lambda,double b0, double b1, double epsilon0, double *rho1, double *rho2, double *rho3, double *rho4,double slope, double mu)
{
	double E1=b1/400;
	double E2=0.1;
	double	new_h,sum,norm_diff;
	double	*f, *f_new, *u, *he, *Erfh, *ve, *Erfv, *de, *Erfd, *dre, *Erfdr, *u_val, *f_new_val, *temp, *Z_new_opt;
	long stop_criterion;

	double	*sigma, *x, *y, *f_val, *error_Z,*diff;
	long		r,step,k,i,j;
	double  grad_cf_h, grad_lf_h, cf, lf, normf, normB, norm_grad_lf, tempv;
	double	*grad_cf, *grad_lf, *grad_cf_he, *grad_cf_ve, *grad_cf_de, *grad_cf_dre;
	double	*grad_lf_he, *grad_lf_ve, *grad_lf_de, *grad_lf_dre;
	double	curve[1001],combined_norm;



	/*[N,M]=size(Z);*/
	/*k01=length(rho1);*/
	/*k02=length(rho2);*/
	/*k03=length(rho3);*/
	/*k04=length(rho4);*/

	/*f=zeros(N,M);*/
	f=(double *) malloc(N*M*sizeof(double));
	zero_filling(f,N*M);
	f_new_val=(double *) malloc(N*M*sizeof(double));
	zero_filling(f_new_val,N*M);
	u=(double *) malloc(N*M*sizeof(double));
	zero_filling(u,N*M);
	u_val=(double *) malloc(N*M*sizeof(double));
	zero_filling(u_val,N*M);
	temp=(double *) malloc(N*M*sizeof(double));
	zero_filling(temp,N*M);
	Z_new_opt=(double *) malloc(N*M*sizeof(double));
	zero_filling(Z_new_opt,N*M);
	diff=(double *) malloc(N*M*sizeof(double));
	zero_filling(diff,N*M);
	/*f_new=zeros(N,M);*/
	f_new=(double *)malloc(N*M*sizeof(double));
	zero_filling(f_new,N*M);
	/*h(1)=sum(sum(Z))/N/M;*/
	*h=mean_value2D(Z,N,M);
	if (k01!=0){
		/*he=zeros(k01,1);*/
		he=(double *)malloc(k01*sizeof(double));
		zero_filling(he,k01);
		/*Erfh=zeros(k01,N,M);*/
		Erfh=(double *)malloc(k01*N*M*sizeof(double));
		zero_filling(Erfh,k01*N*M);
		grad_lf_he=(double *)malloc(k01*sizeof(double));
		zero_filling(grad_lf_he,k01);
		grad_cf_he=(double *)malloc(k01*sizeof(double));
		zero_filling(grad_cf_he,k01);
	}
	else{
		/*he=[];*/
		he=NULL;
		/*Erfh=[];*/
		Erfh=NULL;
		grad_lf_he=NULL;
		grad_cf_he=NULL;
	}

	if (k02!=0){
		/*ve=zeros(k02,1);*/
		ve=(double *)malloc(k02*sizeof(double));
		zero_filling(ve,k02);
		/*Erfv=zeros(k02,N,M);*/
		Erfv=(double *)malloc(k02*N*M*sizeof(double));
		zero_filling(Erfv,k02*N*M);
		grad_lf_ve=(double *)malloc(k02*sizeof(double));
		zero_filling(grad_lf_ve,k02);
		grad_cf_ve=(double *)malloc(k02*sizeof(double));
		zero_filling(grad_cf_ve,k02);
	}
	else{
		/*ve=[];*/
		ve=NULL;
		/*Erfv=[];*/
		Erfv=NULL;
		grad_lf_ve=NULL;
		grad_cf_ve=NULL;
	}

	if (k03!=0){
		/*de=zeros(k03,1);*/
		de=(double *)malloc(k03*sizeof(double));
		zero_filling(de,k03);
		/*Erfd=zeros(k03,N,M);*/
		Erfd=(double *)malloc(k03*N*M*sizeof(double));
		zero_filling(Erfd,k03*N*M);
		grad_lf_de=(double *)malloc(k03*sizeof(double));
		zero_filling(grad_lf_de,k03);
		grad_cf_de=(double *)malloc(k03*sizeof(double));
		zero_filling(grad_cf_de,k03);
	}
	else{
		/*de=[];*/
		de=NULL;
		/*Erfd=[];*/
		Erfd=NULL;
		grad_lf_de=NULL;
		grad_cf_de=NULL;
	}

	if (k04!=0){
		/*dre=zeros(k04,1);*/
		dre=(double *)malloc(k04*sizeof(double));
		zero_filling(dre,k04);
		/*Erfdr=zeros(k04,N,M);*/
		Erfdr=(double *)malloc(k04*N*M*sizeof(double));
		zero_filling(Erfdr,k04*N*M);
		grad_lf_dre=(double *)malloc(k04*sizeof(double));
		zero_filling(grad_lf_dre,k04);
		grad_cf_dre=(double *)malloc(k04*sizeof(double));
		zero_filling(grad_cf_dre,k04);
	}
	else{
		/*dre=[];*/
		dre=NULL;
		/*Erfdr=[];*/
		Erfdr=NULL;
		grad_lf_dre=NULL;
		grad_cf_dre=NULL;
	}
	stop_criterion=0;

	
	sigma=(double *)malloc(1001*sizeof(double));
	zero_filling(sigma,1001);

	for (r=1;r<=10;r++){
		sigma[r-1]=11-r;
	}

	for (r=11;r<=1001;r++){
		sigma[r-1]=10/((double)r);
	}

	/*x=0:1/(M-1):1;*/
	x=(double *)malloc(N*sizeof(double));
	for (i=0;i<N;i++){
		x[i]=(double)i/(N-1);
	}
	/*y=0:1/(N-1):1;*/
	y=(double *)malloc(M*sizeof(double));
	for (i=0;i<M;i++){
		y[i]=(double)i/(M-1);
	}
	f_val=(double*)malloc(N*M*sizeof(double));
	zero_filling(f_val,N*M);
	RKHS_eval(f_val,f,Kappa,N,M);
	step=0;
	
	/*grad_cf=zeros(N,M);*/
	grad_cf=(double *)malloc(N*M*sizeof(double));
	zero_filling(grad_cf,N*M);
	grad_lf=(double *)malloc(N*M*sizeof(double));
	zero_filling(grad_lf,N*M);

	
	/*initial step. Construct the erf matrices*/
	/*1. the horizontal edges - Erfh.*/
	for (k=1;k<=k01;k++){
		/*Erfh(k,:,:)=erf(slope*x'*ones(1,M) + (-slope/2 + rho1(k))*ones(N,M));*/
		for(j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				Erfh[(k-1)*N*M+(j-1)*M+i-1]=erf(slope*y[j-1] + (-slope/2 +rho1[k-1]));
			}
		}
	}

	/*%2 the vertical edges - Erfv.*/
	for (k=1;k<=k02;k++){
		/*Erfv(k,:,:)=erf(slope*ones(N,1)*y + (-slope/2 + rho2(k))*ones(N,M));*/
		for(j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				Erfv[(k-1)*N*M+(j-1)*M+i-1]=erf(slope*x[i-1] + (-slope/2 + rho2[k-1]));
			}
		}
	}

	/*%3 the diagonal edges (from left to right) - Erfd*/
	for (k=1;k<=k03;k++){
		/*Erfd(k,:,:)=erf(slope*x'*ones(1,M)+slope*ones(N,1)*y + (-slope + rho3(k))*ones(N,M));*/
		for(j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				Erfd[(k-1)*N*M+(j-1)*M+i-1]=erf(slope*x[i-1] + slope*y[j-1] + (-slope + rho3[k-1]));
			}
		}
	}

	/*%4 the diagonal edges (from right to left) - Erfd*/
	for (k=1;k<=k04;k++){
		/*Erfdr(k,:,:)=erf(slope*x'*ones(1,M)-slope*ones(N,1)*y + rho4(k)*ones(N,M));*/
		for(j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				Erfdr[(k-1)*N*M+(j-1)*M+i-1]=erf(slope*x[i-1] - slope*y[j-1] + rho4[k-1]);
			}
		}
	}

	/*Z_opt=f_val+h(1)*ones(N,M);*/
	for (j=1;j<=N;j++){
		for(i=1;i<=M;i++){
			Z_opt[(j-1)*M+i-1]=f_val[(j-1)*M+i-1]+*h;
		}
	}
	
	error_Z=(double *)malloc(N*M*sizeof(double));
	while (stop_criterion==0){
		step=step+1;
		/*error_Z=Z_opt-Z;*/
		for (j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				error_Z[(j-1)*M+i-1]=Z_opt[(j-1)*M+i-1]-Z[(j-1)*M+i-1];
			}
		}

		/*phase 1. Computing the subgradient of c(f) in respect with f and h*/
		
		grad_cf_h=0;
		if (k01!=0)
			for(k=0;k<k01;k++){
				grad_cf_he[k]=0;}
		if (k02!=0)
			for(k=0;k<k02;k++){
				grad_cf_ve[k]=0;}
		if (k03!=0)
			for(k=0;k<k03;k++){
				grad_cf_de[k]=0;}
		if (k04!=0)
			for(k=0;k<k04;k++){
				grad_cf_dre[k]=0;}
		
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				if (fabs(error_Z[(j-1)*M+i-1])<=epsilon0)
					grad_cf[(j-1)*M+i-1]=0;
				else{
					grad_cf[(j-1)*M+i-1]=sign(error_Z[(j-1)*M+i-1]);
					grad_cf_h=grad_cf_h+sign(error_Z[(j-1)*M+i-1]);
					if (k01!=0){
						/*grad_cf_he=grad_cf_he+sign(error_Z(j,i))*Erfh(:,j,i);*/
						for(k=1;k<=k01;k++){
							grad_cf_he[k-1]=grad_cf_he[k-1]+sign(error_Z[(j-1)*M+i-1])*Erfh[(k-1)*N*M+(j-1)*M+i-1];
						}
					}
					if (k02!=0){
						for(k=1;k<=k02;k++){
							grad_cf_ve[k-1]=grad_cf_ve[k-1]+sign(error_Z[(j-1)*M+i-1])*Erfv[(k-1)*N*M+(j-1)*M+i-1];
						}
					}

					if (k03!=0){
						for(k=1;k<=k03;k++){
							grad_cf_de[k-1]=grad_cf_de[k-1]+sign(error_Z[(j-1)*M+i-1])*Erfd[(k-1)*N*M+(j-1)*M+i-1];
						}
					}
					if (k04!=0){
						for(k=1;k<=k04;k++){
							grad_cf_dre[k-1]=grad_cf_dre[k-1]+sign(error_Z[(j-1)*M+i-1])*Erfdr[(k-1)*N*M+(j-1)*M+i-1];
						}
					}
					
				}
				grad_cf[(j-1)*M+i-1]=grad_cf[(j-1)*M+i-1]/N/M;
			}
		}
		grad_cf_h=grad_cf_h/N/M;
		for(k=1;k<=k01;k++){
			grad_cf_he[k-1]=grad_cf_he[k-1]/N/M;}
		for(k=1;k<=k02;k++){
			grad_cf_ve[k-1]=grad_cf_ve[k-1]/N/M;}
		for(k=1;k<=k03;k++){
			grad_cf_de[k-1]=grad_cf_de[k-1]/N/M;}
		for(k=1;k<=k04;k++){
			grad_cf_dre[k-1]=grad_cf_dre[k-1]/N/M;}


		/*phase 2. Computing the subgradient of l(f)*/
		/*grad_lf=grad_cf + lambda*f;*/
		for(j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				grad_lf[(j-1)*M+i-1]=grad_cf[(j-1)*M+i-1]+lambda*f[(j-1)*M+i-1];
			}
		}
		grad_lf_h=grad_cf_h;
		/*grad_lf_he=grad_cf_he + mu*he;*/
		for(k=0;k<k01;k++){
			grad_lf_he[k]=grad_cf_he[k] + mu*he[k];}
		/*grad_lf_ve=grad_cf_ve + mu*ve;*/
		for(k=0;k<k02;k++){
			grad_lf_ve[k]=grad_cf_ve[k] + mu*ve[k];}
		/*grad_lf_de=grad_cf_de + mu*de;*/
		for(k=0;k<k03;k++){
			grad_lf_de[k]=grad_cf_de[k] + mu*de[k];}
		/*grad_lf_dre=grad_cf_dre + mu*dre;*/
		for(k=0;k<k04;k++){
			grad_lf_dre[k]=grad_cf_dre[k] + mu*dre[k];}


		/*phase 3. Computing l(f) and L(f)*/
		cf=0;
		normf=RKHS_norm(f,Kappa,N,M);
		/*normB=sum(he.^2) + sum(ve.^2) + sum(de.^2) + sum(dre.^2);*/
		normB=square_sum1D(he,k01)+square_sum1D(ve,k02)+square_sum1D(de,k03) + square_sum1D(dre,k04);

		for(j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				if ( fabs(error_Z[(j-1)*M+i-1])<=epsilon0 )
					cf=cf+0;
				else
					cf=cf+fabs(error_Z[(j-1)*M+i-1])-epsilon0;
			}
		}
		cf=cf/N/M;
		lf=cf + lambda*normf/2 + mu/2*normB;

		/*phase 4. Computing norm of the subgradient L(f) (in respect with f)*/
		norm_grad_lf=RKHS_norm(grad_lf,Kappa,N,M);

		/*phase 4.5. Computing the  norm of the combined gradient*/
		combined_norm=norm_grad_lf+ grad_lf_h*grad_lf_h + square_sum1D(grad_lf_he,k01)+square_sum1D(grad_lf_ve,k02)+square_sum1D(grad_lf_de,k03) + square_sum1D(grad_lf_dre,k04);

		/*phase 5. Computing u*/

		if (combined_norm!=0){
			/*u=f-sigma(step)*grad_lf/sqrt(norm_grad_lf);*/
			for (j=1;j<=N;j++){
				for(i=1;i<=M;i++){
					u[(j-1)*M+i-1]=f[(j-1)*M+i-1] - sigma[step]*grad_lf[(j-1)*M+i-1]/sqrt(combined_norm);
				}
			} 
			/*new_h(1)=h(1) - sigma(step)*sign(grad_lf_h);*/
			new_h=*h- sigma[step]*grad_lf_h/sqrt(combined_norm);

			/*new_he=he - sigma(step)*grad_lf_he/sqrt(sum(grad_lf_he.^2));*/
			for(k=0;k<k01;k++){
				new_he[k]=he[k] - sigma[step] * grad_lf_he[k]/sqrt(combined_norm);
			}

			/*new_ve=ve - sigma(step)*grad_lf_ve/sqrt(sum(grad_lf_ve.^2));*/
			for(k=0;k<k02;k++){
				new_ve[k]=ve[k] - sigma[step] * grad_lf_ve[k]/sqrt(combined_norm);
			}

			/*new_de=de - sigma(step)*grad_lf_de/sqrt(sum(grad_lf_de.^2));*/
			for(k=0;k<k03;k++){
				new_de[k]=de[k] - sigma[step] * grad_lf_de[k]/sqrt(combined_norm);
			}

			/*new_dre=dre - sigma(step)*grad_lf_dre/sqrt(sum(grad_lf_dre.^2));*/
			for(k=0;k<k04;k++){
				new_dre[k]=dre[k] - sigma[step] * grad_lf_dre[k]/sqrt(combined_norm);
			}
		}
		else{
			/*u=f;*/
			for (j=1;j<=N;j++){
				for(i=1;i<=M;i++){
					u[(j-1)*M+i-1]=f[(j-1)*M+i-1];
				}
			}
			/*new_h(1)=h(1);*/
			new_h=*h;

			/*new_he=he;*/
			for(k=0;k<k01;k++){
				new_he[k]=he[k];
			}

			for(k=0;k<k02;k++){
				new_ve[k]=ve[k];
			}

			for(k=0;k<k03;k++){
				new_de[k]=de[k];
			}

			/*new_dre=dre;*/
			for(k=0;k<k04;k++){
				new_dre[k]=dre[k];
			}
		}

		
		/*construct u_val*/
		/*u_val=RKHS_eval(u,Kappa);*/
		RKHS_eval(u_val,u,Kappa,N,M);
		/*f_new=u;*/
		for (j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				f_new[(j-1)*M+i-1]=u[(j-1)*M+i-1];
			}
		}



		/*phase 7. Compute the distance ||f_new-f||^2*/
		RKHS_eval(f_new_val,f_new,Kappa,N,M);
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				temp[(j-1)*M+i-1]=0;
				if (k01!=0){
					sum=0;
					for(k=0;k<k01;k++){
						sum+=new_he[k]*Erfh[k*M*N+(j-1)*M+i-1];
					}
					temp[(j-1)*M+i-1]=temp[(j-1)*M+i-1]+sum;
				}
				
				if (k02!=0){
					sum=0;
					for(k=0;k<k02;k++){
						sum+=new_ve[k]*Erfv[k*M*N+(j-1)*M+i-1];
					}
					temp[(j-1)*M+i-1]=temp[(j-1)*M+i-1]+sum;
				}

				if (k03!=0){
					sum=0;
					for(k=0;k<k03;k++){
						sum+=new_de[k]*Erfd[k*M*N+(j-1)*M+i-1];
					}
					temp[(j-1)*M+i-1]=temp[(j-1)*M+i-1]+sum;
				}

				if (k04!=0){
					sum=0;
					for(k=0;k<k04;k++){
						sum+=new_dre[k]*Erfdr[k*M*N+(j-1)*M+i-1];
					}
					temp[(j-1)*M+i-1]=temp[(j-1)*M+i-1]+sum;
				}
			}
		}
		/*Z_new_opt=f_new_val + new_h(1)*ones(N,M) + temp;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				Z_new_opt[(j-1)*M+i-1]=f_new_val[(j-1)*M+i-1] + new_h + temp[(j-1)*M+i-1];
			}
		}
		/*diff=Z_new_opt - Z_opt;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				diff[(j-1)*M+i-1]=Z_new_opt[(j-1)*M+i-1] - Z_opt[(j-1)*M+i-1];
			}
		}
		/*norm_diff=sqrt(sum(sum(diff.^2))/N/M);*/
		norm_diff=sqrt(square_sum1D(diff,N*M)/N/M);
		
		/*phase 9. Update curves*/
		curve[step]=norm_diff;
		/*objective[step]=lf;*/

		/*phase 10. Check stopping criterion*/
		if (step>1){
			if ( curve[step]==0 )
				stop_criterion=1;
			else
				if ( (fabs(curve[step]-curve[step-1])/curve[step]<E2) && (fabs(curve[step])<E1) )
					stop_criterion=1;
		}

		/*f=f_new;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				f[(j-1)*M+i-1]=f_new[(j-1)*M+i-1];
			}
		}
		*h=new_h;
		/*he=new_he;*/
		for (k=0;k<k01;k++){
			he[k]=new_he[k];
		}
		/*ve=new_ve;*/
		for (k=0;k<k02;k++){
			ve[k]=new_ve[k];
		}
		/*de=new_de;*/
		for (k=0;k<k03;k++){
			de[k]=new_de[k];
		}
		/*dre=new_dre;*/
		for (k=0;k<k04;k++){
			dre[k]=new_dre[k];
		}
		/*f_val=f_new_val;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				f_val[(j-1)*M+i-1]=f_new_val[(j-1)*M+i-1];
			}
		}
		/*Z_opt=Z_new_opt;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				Z_opt[(j-1)*M+i-1]=Z_new_opt[(j-1)*M+i-1];
			}
		}
		if (step==1000)
			break;

	}/*Stoping criterion*/
	/*f_opt=f;*/
	for (j=1;j<=N;j++){
		for (i=1;i<=M;i++){
			f_opt[(j-1)*M+i-1]=f[(j-1)*M+i-1];
		}
	}

	free(f);
	free(f_new_val);
	free(u);
	free(u_val);
	free(temp);
	free(Z_new_opt);
	free(diff);
	free(f_new);
	if (k01!=0){
		free(he);
		free(Erfh);
		free(grad_lf_he);
		free(grad_cf_he);
	}
	if (k02!=0){
		free(ve);
		free(Erfv);
		free(grad_lf_ve);
		free(grad_cf_ve);
	}

	if(k03!=0){
		free(de);
		free(Erfd);
		free(grad_lf_de);
		free(grad_cf_de);
	}

	if(k04!=0){
		free(dre);
		free(Erfdr);
		free(grad_lf_dre);
		free(grad_cf_dre);
	}

	free(sigma);
	free(x);
	free(y);
	free(f_val);
	free(grad_cf);
	free(grad_lf);
	free(error_Z);
	return step;

}











long proj_optim_l1_norm_for_edges2(double *f_opt,double *Z_opt, double *h, double *new_he, double *new_ve, double *new_de, double *new_dre, long N, long M, long k01, long k02, long k03, long k04, double *Z, double *Kappa, double lambda,double b0, double b1, double epsilon0, double *rho1, double *rho2, double *rho3, double *rho4,double slope, double mu, double mu2)
{
	double	E1=b1/400;
	double	E2=0.001;
	double	*new_h,sum,norm_diff;
	double	*f, *f_new, *u, *he, *Erfh, *ve, *Erfv, *de, *Erfd, *dre, *Erfdr, *u_val, *f_new_val, *temp, *Z_new_opt;
	long stop_criterion;

	double	*sigma, *x, *y, *f_val, *error_Z,*diff;
	long		r,step,k,i,j;
	double  *grad_cf_h, *grad_lf_h, cf, lf, normf, normB, norm_grad_lf, tempv;
	double	*grad_cf, *grad_lf, *grad_cf_he, *grad_cf_ve, *grad_cf_de, *grad_cf_dre;
	double	*grad_lf_he, *grad_lf_ve, *grad_lf_de, *grad_lf_dre;
	double	curve[1001],combined_norm;


	/*[N,M]=size(Z);*/
	/*k01=length(rho1);*/
	/*k02=length(rho2);*/
	/*k03=length(rho3);*/
	/*k04=length(rho4);*/

	/*f=zeros(N,M);*/
    new_h=(double *)malloc(4*sizeof(double));
	grad_cf_h=(double *)malloc(4*sizeof(double));
	grad_lf_h=(double *)malloc(4*sizeof(double));
	f=(double *) malloc(N*M*sizeof(double));
	zero_filling(f,N*M);
	f_new_val=(double *) malloc(N*M*sizeof(double));
	zero_filling(f_new_val,N*M);
	u=(double *) malloc(N*M*sizeof(double));
	zero_filling(u,N*M);
	u_val=(double *) malloc(N*M*sizeof(double));
	zero_filling(u_val,N*M);
	temp=(double *) malloc(N*M*sizeof(double));
	zero_filling(temp,N*M);
	Z_new_opt=(double *) malloc(N*M*sizeof(double));
	zero_filling(Z_new_opt,N*M);
	diff=(double *) malloc(N*M*sizeof(double));
	zero_filling(diff,N*M);
	/*f_new=zeros(N,M);*/
	f_new=(double *)malloc(N*M*sizeof(double));
	zero_filling(f_new,N*M);
	/*h(1)=sum(sum(Z))/N/M;*/
	h[0]=mean_value2D(Z,N,M);
    h[1]=0;
    h[2]=0;
    h[3]=0;
	if (k01!=0){
		/*he=zeros(k01,1);*/
		he=(double *)malloc(k01*sizeof(double));
		zero_filling(he,k01);
		/*Erfh=zeros(k01,N,M);*/
		Erfh=(double *)malloc(k01*N*M*sizeof(double));
		zero_filling(Erfh,k01*N*M);
		grad_lf_he=(double *)malloc(k01*sizeof(double));
		zero_filling(grad_lf_he,k01);
		grad_cf_he=(double *)malloc(k01*sizeof(double));
		zero_filling(grad_cf_he,k01);
	}
	else{
		/*he=[];*/
		he=NULL;
		/*Erfh=[];*/
		Erfh=NULL;
		grad_lf_he=NULL;
		grad_cf_he=NULL;
	}

	if (k02!=0){
		/*ve=zeros(k02,1);*/
		ve=(double *)malloc(k02*sizeof(double));
		zero_filling(ve,k02);
		/*Erfv=zeros(k02,N,M);*/
		Erfv=(double *)malloc(k02*N*M*sizeof(double));
		zero_filling(Erfv,k02*N*M);
		grad_lf_ve=(double *)malloc(k02*sizeof(double));
		zero_filling(grad_lf_ve,k02);
		grad_cf_ve=(double *)malloc(k02*sizeof(double));
		zero_filling(grad_cf_ve,k02);
	}
	else{
		/*ve=[];*/
		ve=NULL;
		/*Erfv=[];*/
		Erfv=NULL;
		grad_lf_ve=NULL;
		grad_cf_ve=NULL;
	}

	if (k03!=0){
		/*de=zeros(k03,1);*/
		de=(double *)malloc(k03*sizeof(double));
		zero_filling(de,k03);
		/*Erfd=zeros(k03,N,M);*/
		Erfd=(double *)malloc(k03*N*M*sizeof(double));
		zero_filling(Erfd,k03*N*M);
		grad_lf_de=(double *)malloc(k03*sizeof(double));
		zero_filling(grad_lf_de,k03);
		grad_cf_de=(double *)malloc(k03*sizeof(double));
		zero_filling(grad_cf_de,k03);
	}
	else{
		/*de=[];*/
		de=NULL;
		/*Erfd=[];*/
		Erfd=NULL;
		grad_lf_de=NULL;
		grad_cf_de=NULL;
	}

	if (k04!=0){
		/*dre=zeros(k04,1);*/
		dre=(double *)malloc(k04*sizeof(double));
		zero_filling(dre,k04);
		/*Erfdr=zeros(k04,N,M);*/
		Erfdr=(double *)malloc(k04*N*M*sizeof(double));
		zero_filling(Erfdr,k04*N*M);
		grad_lf_dre=(double *)malloc(k04*sizeof(double));
		zero_filling(grad_lf_dre,k04);
		grad_cf_dre=(double *)malloc(k04*sizeof(double));
		zero_filling(grad_cf_dre,k04);
	}
	else{
		/*dre=[];*/
		dre=NULL;
		/*Erfdr=[];*/
		Erfdr=NULL;
		grad_lf_dre=NULL;
		grad_cf_dre=NULL;
	}
	stop_criterion=0;

	
	sigma=(double *)malloc(1001*sizeof(double));
	zero_filling(sigma,1001);

	for (r=1;r<=10;r++){
		sigma[r-1]=11-r;
	}

	for (r=11;r<=1001;r++){
		sigma[r-1]=10/((double)r);
	}

	/*x=0:1/(M-1):1;*/
	x=(double *)malloc(N*sizeof(double));
	for (i=0;i<M;i++){
		x[i]=(double)i/(M-1);
	}
	/*y=0:1/(N-1):1;*/
	y=(double *)malloc(M*sizeof(double));
	for (i=0;i<N;i++){
		y[i]=(double)i/(N-1);
	}
	f_val=(double*)malloc(N*M*sizeof(double));
	zero_filling(f_val,N*M);
	RKHS_eval(f_val,f,Kappa,N,M);
	step=0;
	
	/*grad_cf=zeros(N,M);*/
	grad_cf=(double *)malloc(N*M*sizeof(double));
	zero_filling(grad_cf,N*M);
	grad_lf=(double *)malloc(N*M*sizeof(double));
	zero_filling(grad_lf,N*M);

	
	/*initial step. Construct the erf matrices*/
	/*1. the horizontal edges - Erfh.*/
	for (k=1;k<=k01;k++){
		/*Erfh(k,:,:)=erf(slope*x'*ones(1,M) + (-slope/2 + rho1(k))*ones(N,M));*/
		for(j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				Erfh[(k-1)*N*M+(j-1)*M+i-1]=erf(slope*y[j-1] + (-slope/2 +rho1[k-1]));
			}
		}
	}

	/*%2 the vertical edges - Erfv.*/
	for (k=1;k<=k02;k++){
		/*Erfv(k,:,:)=erf(slope*ones(N,1)*y + (-slope/2 + rho2(k))*ones(N,M));*/
		for(j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				Erfv[(k-1)*N*M+(j-1)*M+i-1]=erf(slope*x[i-1] + (-slope/2 + rho2[k-1]));
			}
		}
	}

	/*%3 the diagonal edges (from left to right) - Erfd*/
	for (k=1;k<=k03;k++){
		/*Erfd(k,:,:)=erf(slope*x'*ones(1,M)+slope*ones(N,1)*y + (-slope + rho3(k))*ones(N,M));*/
		for(j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				Erfd[(k-1)*N*M+(j-1)*M+i-1]=erf(slope*x[i-1] + slope*y[j-1] + (-slope + rho3[k-1]));
			}
		}
	}

	/*%4 the diagonal edges (from right to left) - Erfd*/
	for (k=1;k<=k04;k++){
		/*Erfdr(k,:,:)=erf(slope*x'*ones(1,M)-slope*ones(N,1)*y + rho4(k)*ones(N,M));*/
		for(j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				Erfdr[(k-1)*N*M+(j-1)*M+i-1]=erf(slope*x[i-1] - slope*y[j-1] + rho4[k-1]);
			}
		}
	}

	/*Z_opt=f_val+h(1)*ones(N,M);*/
	for (j=1;j<=N;j++){
		for(i=1;i<=M;i++){
			Z_opt[(j-1)*M+i-1]=f_val[(j-1)*M+i-1]+h[0];
		}
	}
	
	error_Z=(double *)malloc(N*M*sizeof(double));
	while (stop_criterion==0){
		step=step+1;
		/*error_Z=Z_opt-Z;*/
		for (j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				error_Z[(j-1)*M+i-1]=Z_opt[(j-1)*M+i-1]-Z[(j-1)*M+i-1];
			}
		}

		/*phase 1. Computing the subgradient of c(f) in respect with f and h*/
		
		grad_cf_h[0]=0;
		grad_cf_h[1]=0;
		grad_cf_h[2]=0;
		grad_cf_h[3]=0;
		if (k01!=0)
			for(k=0;k<k01;k++){
				grad_cf_he[k]=0;}
		if (k02!=0)
			for(k=0;k<k02;k++){
				grad_cf_ve[k]=0;}
		if (k03!=0)
			for(k=0;k<k03;k++){
				grad_cf_de[k]=0;}
		if (k04!=0)
			for(k=0;k<k04;k++){
				grad_cf_dre[k]=0;}
		
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				if (fabs(error_Z[(j-1)*M+i-1])<=epsilon0){
					grad_cf[(j-1)*M+i-1]=0;
				}
				else{
					grad_cf[(j-1)*M+i-1]=sign(error_Z[(j-1)*M+i-1]);
					grad_cf_h[0]=grad_cf_h[0]+sign(error_Z[(j-1)*M+i-1]);
					grad_cf_h[1]=grad_cf_h[1]+sign(error_Z[(j-1)*M+i-1])*x[i-1];
					grad_cf_h[2]=grad_cf_h[2]+sign(error_Z[(j-1)*M+i-1])*y[j-1];
					grad_cf_h[3]=grad_cf_h[3]+sign(error_Z[(j-1)*M+i-1])*x[i-1]*y[j-1];
					if (k01!=0){
						/*grad_cf_he=grad_cf_he+sign(error_Z(j,i))*Erfh(:,j,i);*/
						for(k=1;k<=k01;k++){
							grad_cf_he[k-1]=grad_cf_he[k-1]+sign(error_Z[(j-1)*M+i-1])*Erfh[(k-1)*N*M+(j-1)*M+i-1];
						}
					}
					if (k02!=0){
						for(k=1;k<=k02;k++){
							grad_cf_ve[k-1]=grad_cf_ve[k-1]+sign(error_Z[(j-1)*M+i-1])*Erfv[(k-1)*N*M+(j-1)*M+i-1];
						}
					}

					if (k03!=0){
						for(k=1;k<=k03;k++){
							grad_cf_de[k-1]=grad_cf_de[k-1]+sign(error_Z[(j-1)*M+i-1])*Erfd[(k-1)*N*M+(j-1)*M+i-1];
						}
					}
					if (k04!=0){
						for(k=1;k<=k04;k++){
							grad_cf_dre[k-1]=grad_cf_dre[k-1]+sign(error_Z[(j-1)*M+i-1])*Erfdr[(k-1)*N*M+(j-1)*M+i-1];
						}
					}
					
				}
				grad_cf[(j-1)*M+i-1]=grad_cf[(j-1)*M+i-1]/N/M;
			}
		}
		grad_cf_h[0]=grad_cf_h[0]/N/M;
		grad_cf_h[1]=grad_cf_h[1]/N/M;
		grad_cf_h[2]=grad_cf_h[2]/N/M;
		grad_cf_h[3]=grad_cf_h[3]/N/M;
		for(k=1;k<=k01;k++){
			grad_cf_he[k-1]=grad_cf_he[k-1]/N/M;}
		for(k=1;k<=k02;k++){
			grad_cf_ve[k-1]=grad_cf_ve[k-1]/N/M;}
		for(k=1;k<=k03;k++){
			grad_cf_de[k-1]=grad_cf_de[k-1]/N/M;}
		for(k=1;k<=k04;k++){
			grad_cf_dre[k-1]=grad_cf_dre[k-1]/N/M;}


		/*phase 2. Computing the subgradient of l(f)*/
		/*grad_lf=grad_cf + lambda*f;*/
		for(j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				grad_lf[(j-1)*M+i-1]=grad_cf[(j-1)*M+i-1]+lambda*f[(j-1)*M+i-1];
			}
		}
		grad_lf_h[0]=grad_cf_h[0];
		grad_lf_h[1]=grad_cf_h[1]+mu2*h[1];
		grad_lf_h[2]=grad_cf_h[2]+mu2*h[2];
		grad_lf_h[3]=grad_cf_h[3]+mu2*h[3];
		/*grad_lf_he=grad_cf_he + mu*he;*/
		for(k=0;k<k01;k++){
			grad_lf_he[k]=grad_cf_he[k] + mu*he[k];}
		/*grad_lf_ve=grad_cf_ve + mu*ve;*/
		for(k=0;k<k02;k++){
			grad_lf_ve[k]=grad_cf_ve[k] + mu*ve[k];}
		/*grad_lf_de=grad_cf_de + mu*de;*/
		for(k=0;k<k03;k++){
			grad_lf_de[k]=grad_cf_de[k] + mu*de[k];}
		/*grad_lf_dre=grad_cf_dre + mu*dre;*/
		for(k=0;k<k04;k++){
			grad_lf_dre[k]=grad_cf_dre[k] + mu*dre[k];}


		/*phase 3. Computing l(f) and L(f)*/
		cf=0;
		normf=RKHS_norm(f,Kappa,N,M);
		/*normB=sum(he.^2) + sum(ve.^2) + sum(de.^2) + sum(dre.^2);*/
		normB=square_sum1D(he,k01)+square_sum1D(ve,k02)+square_sum1D(de,k03) + square_sum1D(dre,k04);

		for(j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				if ( fabs(error_Z[(j-1)*M+i-1])<=epsilon0 )
					cf=cf+0;
				else
					cf=cf+fabs(error_Z[(j-1)*M+i-1])-epsilon0;
			}
		}
		cf=cf/N/M;
		lf=cf + lambda*normf/2 + mu/2*normB;

		/*phase 4. Computing norm of the subgradient L(f) (in respect with f)*/
		norm_grad_lf=RKHS_norm(grad_lf,Kappa,N,M);

		/*phase 4.5. Computing the  norm of the combined gradient*/
		combined_norm=norm_grad_lf+ square_sum1D(grad_lf_h,4) -grad_lf_h[0]*grad_lf_h[0] + square_sum1D(grad_lf_he,k01)+square_sum1D(grad_lf_ve,k02)+square_sum1D(grad_lf_de,k03) + square_sum1D(grad_lf_dre,k04);

		/*phase 5. Computing u*/

		if (combined_norm!=0){
			/*u=f-sigma(step)*grad_lf/sqrt(norm_grad_lf);*/
			for (j=1;j<=N;j++){
				for(i=1;i<=M;i++){
					u[(j-1)*M+i-1]=f[(j-1)*M+i-1] - sigma[step]*grad_lf[(j-1)*M+i-1]/sqrt(combined_norm);
				}
			} 
			/*new_h(1)=h(1) - sigma(step)*sign(grad_lf_h);*/
			new_h[0]=h[0]- sigma[step]*grad_lf_h[0]/sqrt(combined_norm);
			new_h[1]=h[1]- sigma[step]*grad_lf_h[1]/sqrt(combined_norm);
			new_h[2]=h[2]- sigma[step]*grad_lf_h[2]/sqrt(combined_norm);
			new_h[3]=h[3]- sigma[step]*grad_lf_h[3]/sqrt(combined_norm);

			/*new_he=he - sigma(step)*grad_lf_he/sqrt(sum(grad_lf_he.^2));*/
			for(k=0;k<k01;k++){
				new_he[k]=he[k] - sigma[step] * grad_lf_he[k]/sqrt(combined_norm);
			}

			/*new_ve=ve - sigma(step)*grad_lf_ve/sqrt(sum(grad_lf_ve.^2));*/
			for(k=0;k<k02;k++){
				new_ve[k]=ve[k] - sigma[step] * grad_lf_ve[k]/sqrt(combined_norm);
			}

			/*new_de=de - sigma(step)*grad_lf_de/sqrt(sum(grad_lf_de.^2));*/
			for(k=0;k<k03;k++){
				new_de[k]=de[k] - sigma[step] * grad_lf_de[k]/sqrt(combined_norm);
			}

			/*new_dre=dre - sigma(step)*grad_lf_dre/sqrt(sum(grad_lf_dre.^2));*/
			for(k=0;k<k04;k++){
				new_dre[k]=dre[k] - sigma[step] * grad_lf_dre[k]/sqrt(combined_norm);
			}
		}
		else{
			/*u=f;*/
			for (j=1;j<=N;j++){
				for(i=1;i<=M;i++){
					u[(j-1)*M+i-1]=f[(j-1)*M+i-1];
				}
			}
			/*new_h(1)=h(1);*/
			new_h[0]=h[0];
			new_h[1]=h[1];
			new_h[2]=h[2];
			new_h[3]=h[3];

			/*new_he=he;*/
			for(k=0;k<k01;k++){
				new_he[k]=he[k];
			}

			for(k=0;k<k02;k++){
				new_ve[k]=ve[k];
			}

			for(k=0;k<k03;k++){
				new_de[k]=de[k];
			}

			/*new_dre=dre;*/
			for(k=0;k<k04;k++){
				new_dre[k]=dre[k];
			}
		}
		
		/*construct u_val*/
		/*u_val=RKHS_eval(u,Kappa);*/
		RKHS_eval(u_val,u,Kappa,N,M);
		/*f_new=u;*/
		for (j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				f_new[(j-1)*M+i-1]=u[(j-1)*M+i-1];
			}
		}



		/*phase 7. Compute the distance ||f_new-f||^2*/
		RKHS_eval(f_new_val,f_new,Kappa,N,M);
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				temp[(j-1)*M+i-1]=0;
				if (k01!=0){
					sum=0;
					for(k=0;k<k01;k++){
						sum+=new_he[k]*Erfh[k*M*N+(j-1)*M+i-1];
					}
					temp[(j-1)*M+i-1]=temp[(j-1)*M+i-1]+sum;
				}
				
				if (k02!=0){
					sum=0;
					for(k=0;k<k02;k++){
						sum+=new_ve[k]*Erfv[k*M*N+(j-1)*M+i-1];
					}
					temp[(j-1)*M+i-1]=temp[(j-1)*M+i-1]+sum;
				}

				if (k03!=0){
					sum=0;
					for(k=0;k<k03;k++){
						sum+=new_de[k]*Erfd[k*M*N+(j-1)*M+i-1];
					}
					temp[(j-1)*M+i-1]=temp[(j-1)*M+i-1]+sum;
				}

				if (k04!=0){
					sum=0;
					for(k=0;k<k04;k++){
						sum+=new_dre[k]*Erfdr[k*M*N+(j-1)*M+i-1];
					}
					temp[(j-1)*M+i-1]=temp[(j-1)*M+i-1]+sum;
				}
			}
		}
		/*Z_new_opt=f_new_val + new_h(1)*ones(N,M) + temp;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				Z_new_opt[(j-1)*M+i-1]=f_new_val[(j-1)*M+i-1] + new_h[0] +new_h[1]*x[i-1] + new_h[2]*y[j-1] +new_h[3]*x[i-1]*y[j-1] + temp[(j-1)*M+i-1];
			}
		}
		/*diff=Z_new_opt - Z_opt;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				diff[(j-1)*M+i-1]=Z_new_opt[(j-1)*M+i-1] - Z_opt[(j-1)*M+i-1];
			}
		}
		/*norm_diff=sqrt(sum(sum(diff.^2))/N/M);*/
		norm_diff=sqrt(square_sum1D(diff,N*M)/N/M);
		
		/*phase 9. Update curves*/
		curve[step]=norm_diff;
		/*objective[step]=lf;*/

		/*phase 10. Check stopping criterion*/
		if (step>1){
			if ( curve[step]==0 )
				stop_criterion=1;
			else
				if ( (fabs(curve[step]-curve[step-1])/curve[step]<E2) && (fabs(curve[step])<E1) )
					stop_criterion=1;
		}

		/*f=f_new;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				f[(j-1)*M+i-1]=f_new[(j-1)*M+i-1];
			}
		}
		h[0]=new_h[0];
        h[1]=new_h[1];
        h[2]=new_h[2];
        h[3]=new_h[3];
		/*he=new_he;*/
		for (k=0;k<k01;k++){
			he[k]=new_he[k];
		}
		/*ve=new_ve;*/
		for (k=0;k<k02;k++){
			ve[k]=new_ve[k];
		}
		/*de=new_de;*/
		for (k=0;k<k03;k++){
			de[k]=new_de[k];
		}
		/*dre=new_dre;*/
		for (k=0;k<k04;k++){
			dre[k]=new_dre[k];
		}
		/*f_val=f_new_val;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				f_val[(j-1)*M+i-1]=f_new_val[(j-1)*M+i-1];
			}
		}
		/*Z_opt=Z_new_opt;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				Z_opt[(j-1)*M+i-1]=Z_new_opt[(j-1)*M+i-1];
			}
		}
		if (step==1000)
			break;

	}/*Stoping criterion*/
	/*f_opt=f;*/
	for (j=1;j<=N;j++){
		for (i=1;i<=M;i++){
			f_opt[(j-1)*M+i-1]=f[(j-1)*M+i-1];
		}
	}

	free(f);
	free(f_new_val);
	free(u);
	free(u_val);
	free(temp);
	free(Z_new_opt);
	free(diff);
	free(f_new);
	if (k01!=0){
		free(he);
		free(Erfh);
		free(grad_lf_he);
		free(grad_cf_he);
	}
	if (k02!=0){
		free(ve);
		free(Erfv);
		free(grad_lf_ve);
		free(grad_cf_ve);
	}

	if(k03!=0){
		free(de);
		free(Erfd);
		free(grad_lf_de);
		free(grad_cf_de);
	}

	if(k04!=0){
		free(dre);
		free(Erfdr);
		free(grad_lf_dre);
		free(grad_cf_dre);
	}

	free(sigma);
	free(x);
	free(y);
	free(f_val);
	free(grad_cf);
	free(grad_lf);
	free(error_Z);
	return step;

}






int proj_optim_l1_norm(double *f_opt,double *Z_opt, double *h, long N, long M, double *Z, double *Kappa, double lambda,double b0, double b1, double epsilon0)
{
	double E1=b1/400;
	double E2=0.1;
	double	new_h,norm_diff;
	double	*f, *f_new, *u, *u_val, *f_new_val, *temp, *Z_new_opt;
	int stop_criterion;

	double	*sigma, *x, *y, *f_val, *error_Z,*diff;
	int		r,step,i,j;
	double  grad_cf_h, grad_lf_h, cf, lf, normf, norm_grad_lf;
	double	*grad_cf, *grad_lf;
	double	curve[1001],combined_norm;


	/*[N,M]=size(Z);*/

	/*f=zeros(N,M);*/
	f=(double *) malloc(N*M*sizeof(double));
	zero_filling(f,N*M);
	f_new_val=(double *) malloc(N*M*sizeof(double));
	zero_filling(f_new_val,N*M);
	u=(double *) malloc(N*M*sizeof(double));
	zero_filling(u,N*M);
	u_val=(double *) malloc(N*M*sizeof(double));
	zero_filling(u_val,N*M);
	temp=(double *) malloc(N*M*sizeof(double));
	zero_filling(temp,N*M);
	Z_new_opt=(double *) malloc(N*M*sizeof(double));
	zero_filling(Z_new_opt,N*M);
	diff=(double *) malloc(N*M*sizeof(double));
	zero_filling(diff,N*M);
	/*f_new=zeros(N,M);*/
	f_new=(double *)malloc(N*M*sizeof(double));
	zero_filling(f_new,N*M);
	/*h(1)=sum(sum(Z))/N/M;*/
	*h=mean_value2D(Z,N,M);
	
	stop_criterion=0;

	
	sigma=(double *)malloc(1001*sizeof(double));
	zero_filling(sigma,1001);

	for (r=1;r<=10;r++){
		sigma[r-1]=11-r;
	}

	for (r=11;r<=1001;r++){
		sigma[r-1]=10/((double)r);
	}

	/*x=0:1/(M-1):1;*/
	x=(double *)malloc(N*sizeof(double));
	for (i=0;i<N;i++){
		x[i]=(double)i/(N-1);
	}
	/*y=0:1/(N-1):1;*/
	y=(double *)malloc(M*sizeof(double));
	for (i=0;i<M;i++){
		y[i]=(double)i/(M-1);
	}
	f_val=(double*)malloc(N*M*sizeof(double));
	zero_filling(f_val,N*M);
	RKHS_eval(f_val,f,Kappa,N,M);
	step=0;
	
	/*grad_cf=zeros(N,M);*/
	grad_cf=(double *)malloc(N*M*sizeof(double));
	zero_filling(grad_cf,N*M);
	grad_lf=(double *)malloc(N*M*sizeof(double));
	zero_filling(grad_lf,N*M);

	
	

	/*Z_opt=f_val+h(1)*ones(N,M);*/
	for (j=1;j<=N;j++){
		for(i=1;i<=M;i++){
			Z_opt[(j-1)*M+i-1]=f_val[(j-1)*M+i-1]+*h;
		}
	}
	
	error_Z=(double *)malloc(N*M*sizeof(double));
	while (stop_criterion==0){
		step=step+1;
		/*error_Z=Z_opt-Z;*/
		for (j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				error_Z[(j-1)*M+i-1]=Z_opt[(j-1)*M+i-1]-Z[(j-1)*M+i-1];
			}
		}

		/*phase 1. Computing the subgradient of c(f) in respect with f and h*/
		
		grad_cf_h=0;
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				if (fabs(error_Z[(j-1)*M+i-1])<=epsilon0)
					grad_cf[(j-1)*M+i-1]=0;
				else{
					grad_cf[(j-1)*M+i-1]=sign(error_Z[(j-1)*M+i-1]);
					grad_cf_h=grad_cf_h+sign(error_Z[(j-1)*M+i-1]);
				}
				grad_cf[(j-1)*M+i-1]=grad_cf[(j-1)*M+i-1]/N/M;
			}
		}
		grad_cf_h=grad_cf_h/N/M;


		/*phase 2. Computing the subgradient of l(f)*/
		/*grad_lf=grad_cf + lambda*f;*/
		for(j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				grad_lf[(j-1)*M+i-1]=grad_cf[(j-1)*M+i-1]+lambda*f[(j-1)*M+i-1];
			}
		}
		grad_lf_h=grad_cf_h;


		/*phase 3. Computing l(f) and L(f)*/
		cf=0;
		normf=RKHS_norm(f,Kappa,N,M);

		for(j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				if ( fabs(error_Z[(j-1)*M+i-1])<=epsilon0 )
					cf=cf+0;
				else
					cf=cf+fabs(error_Z[(j-1)*M+i-1])-epsilon0;
			}
		}
		cf=cf/N/M;
		lf=cf + lambda*normf/2;

		/*phase 4. Computing norm of the subgradient L(f) (in respect with f)*/
		norm_grad_lf=RKHS_norm(grad_lf,Kappa,N,M);

		/*phase 4.5. Computing the  norm of the combined gradient*/
		combined_norm=norm_grad_lf;

		/*phase 5. Computing u*/

		if (combined_norm!=0){
			/*u=f-sigma(step)*grad_lf/sqrt(norm_grad_lf);*/
			for (j=1;j<=N;j++){
				for(i=1;i<=M;i++){
					u[(j-1)*M+i-1]=f[(j-1)*M+i-1] - sigma[step]*grad_lf[(j-1)*M+i-1]/sqrt(combined_norm);
				}
			} 
			/*new_h(1)=h(1) - sigma(step)*sign(grad_lf_h);*/
			new_h=*h- sigma[step]*grad_lf_h/sqrt(combined_norm);
		
		}
		else{
			/*u=f;*/
			for (j=1;j<=N;j++){
				for(i=1;i<=M;i++){
					u[(j-1)*M+i-1]=f[(j-1)*M+i-1];
				}
			}
			/*new_h(1)=h(1);*/
			new_h=*h;
		}


		/*phase 5. Computing u*/
		if (norm_grad_lf!=0){
			/*u=f-sigma(step)*grad_lf/sqrt(norm_grad_lf);*/
			for (j=1;j<=N;j++){
				for(i=1;i<=M;i++){
					u[(j-1)*M+i-1]=f[(j-1)*M+i-1] - sigma[step]*grad_lf[(j-1)*M+i-1]/sqrt(norm_grad_lf);
				}
			} 
		}
		else{
			/*u=f;*/
			for (j=1;j<=N;j++){
				for(i=1;i<=M;i++){
					u[(j-1)*M+i-1]=f[(j-1)*M+i-1];
				}
			}
		}

		if (grad_lf_h!=0)
			/*new_h(1)=h(1) - sigma(step)*sign(grad_lf_h);*/
			new_h=*h- sigma[step]*sign(grad_lf_h);
		else
			/*new_h(1)=h(1);*/
			new_h=*h;
		
		
		
		/*construct u_val*/
		/*u_val=RKHS_eval(u,Kappa);*/
		RKHS_eval(u_val,u,Kappa,N,M);
		/*f_new=u;*/
		for (j=1;j<=N;j++){
			for(i=1;i<=M;i++){
				f_new[(j-1)*M+i-1]=u[(j-1)*M+i-1];
			}
		}



		/*phase 7. Compute the distance ||f_new-f||^2*/
		RKHS_eval(f_new_val,f_new,Kappa,N,M);
		/*Z_new_opt=f_new_val + new_h(1)*ones(N,M) + temp;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				Z_new_opt[(j-1)*M+i-1]=f_new_val[(j-1)*M+i-1] + new_h;
			}
		}
		/*diff=Z_new_opt - Z_opt;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				diff[(j-1)*M+i-1]=Z_new_opt[(j-1)*M+i-1] - Z_opt[(j-1)*M+i-1];
			}
		}
		/*norm_diff=sqrt(sum(sum(diff.^2))/N/M);*/
		norm_diff=sqrt(square_sum1D(diff,N*M)/N/M);
		
		/*phase 9. Update curves*/
		curve[step]=norm_diff;
		/*objective[step]=lf;*/

		/*phase 10. Check stopping criterion*/
		if (step>1){
			if ( curve[step]==0 )
				stop_criterion=1;
			else
				if ( (fabs(curve[step]-curve[step-1])/curve[step]<E2) && (fabs(curve[step])<E1) )
					stop_criterion=1;
		}

		/*f=f_new;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				f[(j-1)*M+i-1]=f_new[(j-1)*M+i-1];
			}
		}
		*h=new_h;
		/*f_val=f_new_val;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				f_val[(j-1)*M+i-1]=f_new_val[(j-1)*M+i-1];
			}
		}
		/*Z_opt=Z_new_opt;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=M;i++){
				Z_opt[(j-1)*M+i-1]=Z_new_opt[(j-1)*M+i-1];
			}
		}
		if (step==1000)
			break;

	}/*Stoping criterion*/
	/*f_opt=f;*/
	for (j=1;j<=N;j++){
		for (i=1;i<=M;i++){
			f_opt[(j-1)*M+i-1]=f[(j-1)*M+i-1];
		}
	}


	free(f);
	free(f_new_val);
	free(u);
	free(u_val);
	free(temp);
	free(Z_new_opt);
	free(diff);
	free(f_new);
	free(sigma);
	free(x);
	free(y);
	free(f_val);
	free(grad_cf);
	free(grad_lf);
	free(error_Z);

return step;


}























long progressive_proj_optim_l1_l2_norm_semi_parametric(/*outputs*/double *f_opt,double *Z_opt, double *h, double *b_psi, /*inputs*/long *psi, double *gamma, short *psi_used, double *Z, double *Kappa, long N, long N_psi, double lambda,double b0, double b1, double epsilon0, double mu, double mu2, double E1, double E2, short	keep){
	short		stop=0;
	double		*sorted_b_psi,threshold;
	long		total_psi_used=N_psi,i,how_many_steps;
	long			(*compare)(const double*,const double*);

	compare=&doubles_compare;

	sorted_b_psi=(double*)malloc(N_psi*sizeof(double));
	how_many_steps=proj_optim_l1_l2_norm_semi_parametric(f_opt, Z_opt, h, b_psi, psi, gamma, psi_used, Z, Kappa, N, N_psi, lambda, b0, b1, epsilon0,  mu,  mu2,E1,E2);
	while (keep<total_psi_used){
		/*reduce psi_used. Keep half of them with the highest |b_psi| values*/
		for(i=0;i<N_psi;i++){
			sorted_b_psi[i]=fabs(b_psi[i]);
		}
		qsort(sorted_b_psi,N_psi,sizeof(double),compare);
		total_psi_used=total_psi_used/2;
		threshold=sorted_b_psi[N_psi-total_psi_used];

		for(i=0;i<N_psi;i++){
			if (fabs(b_psi[i])>=threshold)
				psi_used[i]=1;
			else
				psi_used[i]=0;
		} 
		mu=2*mu;
		how_many_steps=proj_optim_l1_l2_norm_semi_parametric(f_opt, Z_opt, h, b_psi, psi, gamma, psi_used, Z, Kappa, N, N_psi, lambda, b0, b1, epsilon0,  mu,  mu2, E1, E2);
	}
	free(sorted_b_psi);

	
}







long proj_optim_l1_l2_norm_semi_parametric(/*outputs*/double *f_opt,double *Z_opt, double *h, double *b_psi, /*inputs*/long *psi, double *gamma, short *psi_used, double *Z, double *Kappa, long N, long N_psi, double lambda,double b0, double b1, double epsilon0, double mu, double mu2,double E1, double E2)
{
	double	*new_h,sum,norm_diff;
	double	*f, *f_new, *u, *he, *u_val, *f_new_val, *temp, *Z_new_opt;
	long	stop_criterion;

	double	*sigma, *sigma2,*x, *y, *f_val, *error_Z,*diff;
	long	r,step,k,i,j;
	double  *grad_c_h, *grad_l_h, *grad_c_b, *grad_l_b, cf, lf, normf, normB, normH, norm_grad_l_f, tempv;
	double	*grad_c_f, *grad_l_f;
	double	curve[10001],combined_norm;
	double	*new_b_psi,L_function0, L_function1,norm_grad_l_b;
	long	total_psi_used;

	total_psi_used=0;
	for(i=0;i<N_psi;i++){
		if (psi_used[i]==1)
			total_psi_used++;
	}


	/*[N,M]=size(Z);*/
	/*k01=length(rho1);*/
	/*k02=length(rho2);*/
	/*k03=length(rho3);*/
	/*k04=length(rho4);*/

	/*f=zeros(N,M);*/
    new_h=(double *)malloc(4*sizeof(double));
	grad_c_h=(double *)malloc(4*sizeof(double));
	grad_l_h=(double *)malloc(4*sizeof(double));
	f=(double *) malloc(N*N*sizeof(double));
	zero_filling(f,N*N);
	zero_filling(b_psi,N_psi);
	f_new_val=(double *) malloc(N*N*sizeof(double));
	zero_filling(f_new_val,N*N);
	u=(double *) malloc(N*N*sizeof(double));
	zero_filling(u,N*N);
	u_val=(double *) malloc(N*N*sizeof(double));
	zero_filling(u_val,N*N);
	temp=(double *) malloc(N*N*sizeof(double));
	zero_filling(temp,N*N);
	Z_new_opt=(double *) malloc(N*N*sizeof(double));
	zero_filling(Z_new_opt,N*N);
	diff=(double *) malloc(N*N*sizeof(double));
	zero_filling(diff,N*N);
	/*f_new=zeros(N,M);*/
	f_new=(double *)malloc(N*N*sizeof(double));
	zero_filling(f_new,N*N);
	/*h(1)=sum(sum(Z))/N/M;*/
	h[0]=mean_value2D(Z,N,N);
    h[1]=0;
    h[2]=0;
    h[3]=0;
	
	new_b_psi=(double*)malloc(N_psi*sizeof(double));
	zero_filling(new_b_psi,N_psi);
	grad_c_b=(double*)malloc(N_psi*sizeof(double));
	zero_filling(grad_c_b,N_psi);
	grad_l_b=(double*)malloc(N_psi*sizeof(double));
	zero_filling(grad_l_b,N_psi);

	stop_criterion=0;

	
	sigma=(double *)malloc(10001*sizeof(double));
	zero_filling(sigma,10001);
	sigma2=(double *)malloc(10001*sizeof(double));
	zero_filling(sigma2,10001);

	for (r=1;r<=100;r++){
		sigma2[r-1]=1/2;
	}
	for (r=1;r<=10001;r++){
		sigma2[r-1]=10.0/(10+r);
	}
	for (r=1;r<=10001;r++){
		sigma[r-1]=1.0/r;
	}

	/*x=0:1/(M-1):1;*/
	x=(double *)malloc(N*sizeof(double));
	for (i=0;i<N;i++){
		x[i]=(double)i/(N-1);
	}
	/*y=0:1/(N-1):1;*/
	y=(double *)malloc(N*sizeof(double));
	for (j=0;j<N;j++){
		y[j]=(double)j/(N-1);
	}
	f_val=(double*)malloc(N*N*sizeof(double));
	zero_filling(f_val,N*N);
	RKHS_eval(f_val,f,Kappa,N,N);
	step=0;
	
	/*grad_cf=zeros(N,M);*/
	grad_c_f=(double *)malloc(N*N*sizeof(double));
	zero_filling(grad_c_f,N*N);
	grad_l_f=(double *)malloc(N*N*sizeof(double));
	zero_filling(grad_l_f,N*N);

	/*Z_opt=f_val+h(1)*ones(N,M);*/
	for (j=1;j<=N;j++){
		for(i=1;i<=N;i++){
			Z_opt[(j-1)*N+i-1]=h[0];
		}
	}
	
	error_Z=(double *)malloc(N*N*sizeof(double));
	for (j=1;j<=N;j++){
		for(i=1;i<=N;i++){
			error_Z[(j-1)*N+i-1]=Z_opt[(j-1)*N+i-1]-Z[(j-1)*N+i-1];
		}
	}
	cf=0;
	normf=RKHS_norm(f,Kappa,N,N);
	normB=0;
	for(k=0;k<N_psi;k++){
		if (psi_used[k]==1)
			normB=normB + gamma[k]*b_psi[k]*b_psi[k];
	}

	normH=h[1]*h[1]+h[2]*h[2]+h[3]*h[3];

	for(j=1;j<=N;j++){
		for(i=1;i<=N;i++){
			if ( fabs(error_Z[(j-1)*N+i-1])< epsilon0 )
				cf=cf+0;
			else
				cf=cf + fabs(error_Z[(j-1)*N+i-1])-epsilon0;
		}
	}
	cf=cf/N/N;
	L_function0=cf   +   lambda*normf/2/N/N   +   mu/2/total_psi_used*normB   +   mu2/2*normH;
	while (stop_criterion==0){
		step=step+1;
		/*phase 1. Computing the subgradient of c(f) in respect with f, h and b*/
		grad_c_h[0]=0;
		grad_c_h[1]=0;
		grad_c_h[2]=0;
		grad_c_h[3]=0;
		zero_filling(grad_c_b,N_psi);
		
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				if (fabs(error_Z[(j-1)*N+i-1])<=epsilon0)
					grad_c_f[(j-1)*N+i-1]=0;
				else{
					grad_c_f[(j-1)*N+i-1]=sign(error_Z[(j-1)*N+i-1]);
					grad_c_h[0]=grad_c_h[0]+sign(error_Z[(j-1)*N+i-1]);
					grad_c_h[1]=grad_c_h[1]+sign(error_Z[(j-1)*N+i-1])*x[i-1];
					grad_c_h[2]=grad_c_h[2]+sign(error_Z[(j-1)*N+i-1])*y[j-1];
					grad_c_h[3]=grad_c_h[3]+sign(error_Z[(j-1)*N+i-1])*x[i-1]*y[j-1];
					for(k=1;k<=N_psi;k++){
						if (psi_used[k-1]==1)
							grad_c_b[k-1]=grad_c_b[k-1]+sign(error_Z[(j-1)*N+i-1])*((double*)psi[k-1])[(j-1)*N+i-1];
						else
							grad_c_b[k-1]=0;
					}	
				}
				grad_c_f[(j-1)*N+i-1]=grad_c_f[(j-1)*N+i-1]/N/N;
			}
		}
		grad_c_h[0]=grad_c_h[0]/N/N;
		grad_c_h[1]=grad_c_h[1]/N/N;
		grad_c_h[2]=grad_c_h[2]/N/N;
		grad_c_h[3]=grad_c_h[3]/N/N;
		for(k=1;k<=N_psi;k++){
			grad_c_b[k-1]=grad_c_b[k-1]/N/N;}


		/*phase 2. Computing the subgradient of l(f)*/
		/*grad_lf=grad_cf + lambda*f;*/
		for(j=1;j<=N;j++){
			for(i=1;i<=N;i++){
				grad_l_f[(j-1)*N+i-1]=grad_c_f[(j-1)*N+i-1]   +   lambda/N/N * f[(j-1)*N+i-1];
			}
		}
		grad_l_h[0]=grad_c_h[0];
		grad_l_h[1]=grad_c_h[1]+mu2*h[1];
		grad_l_h[2]=grad_c_h[2]+mu2*h[2];
		grad_l_h[3]=grad_c_h[3]+mu2*h[3];
		/*grad_l_b=grad_c_b + mu*b;*/
		for(k=0;k<N_psi;k++){
			if (psi_used[k]==1)
				grad_l_b[k]=grad_c_b[k] + mu/total_psi_used*gamma[k]*b_psi[k];
			else
				grad_l_b[k]=0;
		}
		
		/*phase 4. Computing norm of the subgradient L(f) (in respect with f)*/
		norm_grad_l_f=RKHS_norm(grad_l_f,Kappa,N,N);


		/*phase 4.5. Computing the  norm of the combined gradient*/
		norm_grad_l_b=square_sum1D(grad_l_b,N_psi);
		combined_norm = norm_grad_l_f + square_sum1D(grad_l_h, 4) + norm_grad_l_b;
		if (norm_grad_l_f<0)
			printf("error1\n");
		if (norm_grad_l_b<0)
			printf("error2\n");

		/*phase 5. Computing u*/

		if (norm_grad_l_f!=0){
			/*u=f-sigma(step)*grad_lf/sqrt(norm_grad_lf);*/
			for (j=1;j<=N;j++){
				for(i=1;i<=N;i++){
					u[(j-1)*N+i-1]=f[(j-1)*N+i-1] - sigma[step]*grad_l_f[(j-1)*N+i-1]/sqrt(norm_grad_l_f);
				}
			} 
		}
		else{
			for (j=1;j<=N;j++){
				for(i=1;i<=N;i++){
					u[(j-1)*N+i-1]=f[(j-1)*N+i-1];
				}
			} 
		}

		if (grad_l_h[0]!=0)
			new_h[0]=h[0]- sigma[step]*sign(grad_l_h[0]);
		else
			new_h[0]=h[0];
		if (grad_l_h[1]!=0)
			new_h[1]=h[1]- sigma[step]*sign(grad_l_h[1]);
		else
			new_h[1]=h[1];
		if (grad_l_h[2]!=0)
			new_h[2]=h[2]- sigma[step]*sign(grad_l_h[2]);
		else
			new_h[2]=h[2];
		if (grad_l_h[3]!=0)
			new_h[3]=h[3]- sigma[step]*sign(grad_l_h[3]);
		else
			new_h[3]=h[3];
		for(k=0;k<N_psi;k++){
			if ((psi_used[k]==1)&&(grad_l_b[k]!=0))
				new_b_psi[k]=b_psi[k] - sigma2[step] * grad_l_b[k]/sqrt(norm_grad_l_b);
			else
				new_b_psi[k]=b_psi[k];
		}
		
		/*construct u_val*/
		/*u_val=RKHS_eval(u,Kappa);*/
		RKHS_eval(u_val,u,Kappa,N,N);
		/*f_new=u;*/
		for (j=1;j<=N;j++){
			for(i=1;i<=N;i++){
				f_new[(j-1)*N+i-1]=u[(j-1)*N+i-1];
			}
		}

		/*phase 7. Compute the distance ||L_new-L||*/
		RKHS_eval(f_new_val,f_new,Kappa,N,N);
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				temp[(j-1)*N+i-1]=0;
				sum=0;
				for(k=0;k<N_psi;k++){
					if (psi_used[k]==1)
						sum+=new_b_psi[k]*((double*)psi[k])[(j-1)*N+i-1];
				}
				temp[(j-1)*N+i-1]=temp[(j-1)*N+i-1]+sum;
			}
		}
		/*Z_new_opt=f_new_val + new_h(1)*ones(N,M) + temp;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				Z_new_opt[(j-1)*N+i-1]=f_new_val[(j-1)*N+i-1] + new_h[0] +new_h[1]*x[i-1] + new_h[2]*y[j-1] +new_h[3]*x[i-1]*y[j-1] + temp[(j-1)*N+i-1];
			}
		}

		
		normf=RKHS_norm(f_new,Kappa,N,N);
		normB=0;
		for(k=0;k<N_psi;k++){
			if (psi_used[k]==1)
				normB=normB + gamma[k]*new_b_psi[k]*new_b_psi[k];
		}
		normH=new_h[1]*new_h[1]+new_h[2]*new_h[2]+new_h[3]*new_h[3];

		for (j=1;j<=N;j++){
			for(i=1;i<=N;i++){
				error_Z[(j-1)*N+i-1]=Z_new_opt[(j-1)*N+i-1]-Z[(j-1)*N+i-1];
			}
		}
		cf=0;
		for(j=1;j<=N;j++){
			for(i=1;i<=N;i++){
				if ( fabs(error_Z[(j-1)*N+i-1])< epsilon0 )
					cf=cf+0;
				else
					cf=cf + fabs(error_Z[(j-1)*N+i-1])-epsilon0;
			}
		}
		cf=cf/N/N;
		L_function1=cf   +   lambda*normf/2/N/N   +   mu/2/total_psi_used*normB   +   mu2/2*normH;

		
		/*phase 9. Update curves*/
		//curve[step]=fabs(L_function1-L_function0);
		for (i=0;i<N*N;i++)
			temp[i]=fabs(Z_new_opt[i]-Z_opt[i]);
		curve[step]=sum1D(temp,N*N)/N/N;
		L_function0=L_function1;
		/*objective[step]=lf;*/

		/*phase 10. Check stopping criterion*/
		if (step>1){
			if ( curve[step]==0 )
				stop_criterion=1;
			else
				if ( (fabs(curve[step])<E1) &&(sqrt(combined_norm)<E2))
					stop_criterion=1;
		}

		/*f=f_new;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				f[(j-1)*N+i-1]=f_new[(j-1)*N+i-1];
			}
		}
		h[0]=new_h[0];
        h[1]=new_h[1];
        h[2]=new_h[2];
        h[3]=new_h[3];
		/*b=new_b;*/
		for (k=0;k<N_psi;k++){
			b_psi[k]=new_b_psi[k];
		}
		/*f_val=f_new_val;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				f_val[(j-1)*N+i-1]=f_new_val[(j-1)*N+i-1];
			}
		}
		/*Z_opt=Z_new_opt;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				Z_opt[(j-1)*N+i-1]=Z_new_opt[(j-1)*N+i-1];
			}
		}
		if (step==10000)
			break;

	}
	/*Stoping criterion*/
	/*f_opt=f;*/
	for (j=1;j<=N;j++){
		for (i=1;i<=N;i++){
			f_opt[(j-1)*N+i-1]=f[(j-1)*N+i-1];
		}
	}

	for (i=0;i<N*N;i++){
		if ((Z_opt[i]>=0)&&(Z_opt[i]<=255))
			i=i;
		else
			i=i;
	}
	free(f);
	free(f_new_val);
	free(u);
	free(u_val);
	free(temp);
	free(Z_new_opt);
	free(diff);
	free(f_new);
	free(new_b_psi);
	free(grad_l_b);
	free(grad_c_b);
	free(sigma);
	free(sigma2);
	free(x);
	free(y);
	free(f_val);
	free(grad_c_f);
	free(grad_l_f);
	free(error_Z);
	return step;

}










long proj_optim_l1_l2_norm_semi_parametric2(/*outputs*/double *f_opt,double *Z_opt, double *h, double *b_psi, /*inputs*/long *psi, double *gamma, short *psi_used, double *Z, double *Kappa, long N, long N_psi, double lambda,double b0, double b1, double epsilon0, double mu, double mu2,double E1, double E2)
{
	double	*new_h,sum,norm_diff;
	double	*f, *f_new, *u, *he, *u_val, *f_new_val, *temp, *Z_new_opt;
	long	stop_criterion;

	double	*sigma, *sigma2,*x, *y, *f_val, *error_Z,*diff;
	long	r,step,k,i,j;
	double  *grad_c_h, *grad_l_h, *grad_c_b, *grad_l_b, cf, lf, normf, normB, normH, norm_grad_l_f, tempv;
	double	*grad_c_f, *grad_l_f;
	double	curve[10001],combined_norm;
	double	*new_b_psi,L_function0, L_function1,norm_grad_l_b;
	long	total_psi_used;

	total_psi_used=0;
	for(i=0;i<N_psi;i++){
		if (psi_used[i]==1)
			total_psi_used++;
	}


	/*[N,M]=size(Z);*/
	/*k01=length(rho1);*/
	/*k02=length(rho2);*/
	/*k03=length(rho3);*/
	/*k04=length(rho4);*/

	/*f=zeros(N,M);*/
    new_h=(double *)malloc(4*sizeof(double));
	grad_c_h=(double *)malloc(4*sizeof(double));
	grad_l_h=(double *)malloc(4*sizeof(double));
	f=(double *) malloc(N*N*sizeof(double));
	zero_filling(f,N*N);
	zero_filling(b_psi,N_psi);
	f_new_val=(double *) malloc(N*N*sizeof(double));
	zero_filling(f_new_val,N*N);
	u=(double *) malloc(N*N*sizeof(double));
	zero_filling(u,N*N);
	u_val=(double *) malloc(N*N*sizeof(double));
	zero_filling(u_val,N*N);
	temp=(double *) malloc(N*N*sizeof(double));
	zero_filling(temp,N*N);
	Z_new_opt=(double *) malloc(N*N*sizeof(double));
	zero_filling(Z_new_opt,N*N);
	diff=(double *) malloc(N*N*sizeof(double));
	zero_filling(diff,N*N);
	/*f_new=zeros(N,M);*/
	f_new=(double *)malloc(N*N*sizeof(double));
	zero_filling(f_new,N*N);
	/*h(1)=sum(sum(Z))/N/M;*/
	h[0]=mean_value2D(Z,N,N);
    h[1]=0;
    h[2]=0;
    h[3]=0;
	
	new_b_psi=(double*)malloc(N_psi*sizeof(double));
	zero_filling(new_b_psi,N_psi);
	grad_c_b=(double*)malloc(N_psi*sizeof(double));
	zero_filling(grad_c_b,N_psi);
	grad_l_b=(double*)malloc(N_psi*sizeof(double));
	zero_filling(grad_l_b,N_psi);

	stop_criterion=0;

	
	sigma=(double *)malloc(10001*sizeof(double));
	zero_filling(sigma,10001);
	sigma2=(double *)malloc(10001*sizeof(double));
	zero_filling(sigma2,10001);

	for (r=1;r<=100;r++){
		sigma2[r-1]=1/2;
	}
	for (r=1;r<=10001;r++){
		sigma2[r-1]=10.0/(10+r);
	}
	for (r=1;r<=10001;r++){
		sigma[r-1]=1.0/r;
	}

	/*x=0:1/(M-1):1;*/
	x=(double *)malloc(N*sizeof(double));
	for (i=0;i<N;i++){
		x[i]=(double)i/(N-1);
	}
	/*y=0:1/(N-1):1;*/
	y=(double *)malloc(N*sizeof(double));
	for (j=0;j<N;j++){
		y[j]=(double)j/(N-1);
	}
	f_val=(double*)malloc(N*N*sizeof(double));
	zero_filling(f_val,N*N);
	RKHS_eval(f_val,f,Kappa,N,N);
	step=0;
	
	/*grad_cf=zeros(N,M);*/
	grad_c_f=(double *)malloc(N*N*sizeof(double));
	zero_filling(grad_c_f,N*N);
	grad_l_f=(double *)malloc(N*N*sizeof(double));
	zero_filling(grad_l_f,N*N);

	/*Z_opt=f_val+h(1)*ones(N,M);*/
	for (j=1;j<=N;j++){
		for(i=1;i<=N;i++){
			Z_opt[(j-1)*N+i-1]=h[0];
		}
	}
	
	error_Z=(double *)malloc(N*N*sizeof(double));
	for (j=1;j<=N;j++){
		for(i=1;i<=N;i++){
			error_Z[(j-1)*N+i-1]=Z_opt[(j-1)*N+i-1]-Z[(j-1)*N+i-1];
		}
	}
	cf=0;
	normf=RKHS_norm(f,Kappa,N,N);
	normB=0;
	for(k=0;k<N_psi;k++){
		if (psi_used[k]==1)
			normB=normB + gamma[k]*b_psi[k]*b_psi[k];
	}

	normH=h[1]*h[1]+h[2]*h[2]+h[3]*h[3];

	for(j=1;j<=N;j++){
		for(i=1;i<=N;i++){
			if ( fabs(error_Z[(j-1)*N+i-1])< epsilon0 )
				cf=cf+0;
			else
				cf=cf + fabs(error_Z[(j-1)*N+i-1])-epsilon0;
		}
	}
	cf=cf/N/N;
	L_function0=cf   +   lambda*normf/2/N/N   +   mu/2/total_psi_used*normB   +   mu2/2*normH;
	while (stop_criterion==0){
		step=step+1;
		/*phase 1. Computing the subgradient of c(f) in respect with f, h and b*/
		grad_c_h[0]=0;
		grad_c_h[1]=0;
		grad_c_h[2]=0;
		grad_c_h[3]=0;
		zero_filling(grad_c_b,N_psi);
		
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				if (fabs(error_Z[(j-1)*N+i-1])<=epsilon0)
					grad_c_f[(j-1)*N+i-1]=0;
				else{
					grad_c_f[(j-1)*N+i-1]=sign(error_Z[(j-1)*N+i-1]);
					grad_c_h[0]=grad_c_h[0]+sign(error_Z[(j-1)*N+i-1]);
					grad_c_h[1]=grad_c_h[1]+sign(error_Z[(j-1)*N+i-1])*x[i-1];
					grad_c_h[2]=grad_c_h[2]+sign(error_Z[(j-1)*N+i-1])*y[j-1];
					grad_c_h[3]=grad_c_h[3]+sign(error_Z[(j-1)*N+i-1])*x[i-1]*y[j-1];
					for(k=1;k<=N_psi;k++){
						if (psi_used[k-1]==1)
							grad_c_b[k-1]=grad_c_b[k-1]+sign(error_Z[(j-1)*N+i-1])*((double*)psi[k-1])[(j-1)*N+i-1];
						else
							grad_c_b[k-1]=0;
					}	
				}
				grad_c_f[(j-1)*N+i-1]=grad_c_f[(j-1)*N+i-1]/N/N;
			}
		}
		grad_c_h[0]=grad_c_h[0]/N/N;
		grad_c_h[1]=grad_c_h[1]/N/N;
		grad_c_h[2]=grad_c_h[2]/N/N;
		grad_c_h[3]=grad_c_h[3]/N/N;
		for(k=1;k<=N_psi;k++){
			grad_c_b[k-1]=grad_c_b[k-1]/N/N;}


		/*phase 2. Computing the subgradient of l(f)*/
		/*grad_lf=grad_cf + lambda*f;*/
		for(j=1;j<=N;j++){
			for(i=1;i<=N;i++){
				grad_l_f[(j-1)*N+i-1]=grad_c_f[(j-1)*N+i-1]   +   lambda/N/N * f[(j-1)*N+i-1];
			}
		}
		grad_l_h[0]=grad_c_h[0];
		grad_l_h[1]=grad_c_h[1]+mu2*h[1];
		grad_l_h[2]=grad_c_h[2]+mu2*h[2];
		grad_l_h[3]=grad_c_h[3]+mu2*h[3];
		/*grad_l_b=grad_c_b + mu*b;*/
		for(k=0;k<N_psi;k++){
			if (psi_used[k]==1)
				grad_l_b[k]=grad_c_b[k] + mu/total_psi_used*sum1D(b_psi,N_psi);
			else
				grad_l_b[k]=0;
		}
		
		/*phase 4. Computing norm of the subgradient L(f) (in respect with f)*/
		norm_grad_l_f=RKHS_norm(grad_l_f,Kappa,N,N);


		/*phase 4.5. Computing the  norm of the combined gradient*/
		norm_grad_l_b=square_sum1D(grad_l_b,N_psi);
		combined_norm = norm_grad_l_f + square_sum1D(grad_l_h, 4) + norm_grad_l_b;
		if (norm_grad_l_f<0)
			printf("error1\n");
		if (norm_grad_l_b<0)
			printf("error2\n");

		/*phase 5. Computing u*/

		if (norm_grad_l_f!=0){
			/*u=f-sigma(step)*grad_lf/sqrt(norm_grad_lf);*/
			for (j=1;j<=N;j++){
				for(i=1;i<=N;i++){
					u[(j-1)*N+i-1]=f[(j-1)*N+i-1] - sigma[step]*grad_l_f[(j-1)*N+i-1]/sqrt(norm_grad_l_f);
				}
			} 
		}
		else{
			for (j=1;j<=N;j++){
				for(i=1;i<=N;i++){
					u[(j-1)*N+i-1]=f[(j-1)*N+i-1];
				}
			} 
		}

		if (grad_l_h[0]!=0)
			new_h[0]=h[0]- sigma[step]*sign(grad_l_h[0]);
		else
			new_h[0]=h[0];
		if (grad_l_h[1]!=0)
			new_h[1]=h[1]- sigma[step]*sign(grad_l_h[1]);
		else
			new_h[1]=h[1];
		if (grad_l_h[2]!=0)
			new_h[2]=h[2]- sigma[step]*sign(grad_l_h[2]);
		else
			new_h[2]=h[2];
		if (grad_l_h[3]!=0)
			new_h[3]=h[3]- sigma[step]*sign(grad_l_h[3]);
		else
			new_h[3]=h[3];
		for(k=0;k<N_psi;k++){
			if ((psi_used[k]==1)&&(grad_l_b[k]!=0))
				new_b_psi[k]=b_psi[k] - sigma2[step] * grad_l_b[k]/sqrt(norm_grad_l_b);
			else
				new_b_psi[k]=b_psi[k];
		}
		
		/*construct u_val*/
		/*u_val=RKHS_eval(u,Kappa);*/
		RKHS_eval(u_val,u,Kappa,N,N);
		/*f_new=u;*/
		for (j=1;j<=N;j++){
			for(i=1;i<=N;i++){
				f_new[(j-1)*N+i-1]=u[(j-1)*N+i-1];
			}
		}

		/*phase 7. Compute the distance ||L_new-L||*/
		RKHS_eval(f_new_val,f_new,Kappa,N,N);
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				temp[(j-1)*N+i-1]=0;
				sum=0;
				for(k=0;k<N_psi;k++){
					if (psi_used[k]==1)
						sum+=new_b_psi[k]*((double*)psi[k])[(j-1)*N+i-1];
				}
				temp[(j-1)*N+i-1]=temp[(j-1)*N+i-1]+sum;
			}
		}
		/*Z_new_opt=f_new_val + new_h(1)*ones(N,M) + temp;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				Z_new_opt[(j-1)*N+i-1]=f_new_val[(j-1)*N+i-1] + new_h[0] +new_h[1]*x[i-1] + new_h[2]*y[j-1] +new_h[3]*x[i-1]*y[j-1] + temp[(j-1)*N+i-1];
			}
		}

		
		normf=RKHS_norm(f_new,Kappa,N,N);
		normH=new_h[1]*new_h[1]+new_h[2]*new_h[2]+new_h[3]*new_h[3];

		for (j=1;j<=N;j++){
			for(i=1;i<=N;i++){
				error_Z[(j-1)*N+i-1]=Z_new_opt[(j-1)*N+i-1]-Z[(j-1)*N+i-1];
			}
		}
		cf=0;
		for(j=1;j<=N;j++){
			for(i=1;i<=N;i++){
				if ( fabs(error_Z[(j-1)*N+i-1])< epsilon0 )
					cf=cf+0;
				else
					cf=cf + fabs(error_Z[(j-1)*N+i-1])-epsilon0;
			}
		}
		cf=cf/N/N;
		L_function1=cf   +   lambda*normf/2/N/N   +   mu/2/total_psi_used*sum1D(b_psi,N_psi)   +   mu2/2*normH;

		
		/*phase 9. Update curves*/
		//curve[step]=fabs(L_function1-L_function0);
		for (i=0;i<N*N;i++)
			temp[i]=fabs(Z_new_opt[i]-Z_opt[i]);
		curve[step]=sum1D(temp,N*N)/N/N;
		L_function0=L_function1;
		/*objective[step]=lf;*/

		/*phase 10. Check stopping criterion*/
		if (step>1){
			if ( curve[step]==0 )
				stop_criterion=1;
			else
				if ( (fabs(curve[step])<E1) &&(sqrt(combined_norm)<E2))
					stop_criterion=1;
		}

		/*f=f_new;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				f[(j-1)*N+i-1]=f_new[(j-1)*N+i-1];
			}
		}
		h[0]=new_h[0];
        h[1]=new_h[1];
        h[2]=new_h[2];
        h[3]=new_h[3];
		/*b=new_b;*/
		for (k=0;k<N_psi;k++){
			b_psi[k]=new_b_psi[k];
		}
		/*f_val=f_new_val;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				f_val[(j-1)*N+i-1]=f_new_val[(j-1)*N+i-1];
			}
		}
		/*Z_opt=Z_new_opt;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				Z_opt[(j-1)*N+i-1]=Z_new_opt[(j-1)*N+i-1];
			}
		}
		if (step==10000)
			break;

	}
	/*Stoping criterion*/
	/*f_opt=f;*/
	for (j=1;j<=N;j++){
		for (i=1;i<=N;i++){
			f_opt[(j-1)*N+i-1]=f[(j-1)*N+i-1];
		}
	}

	for (i=0;i<N*N;i++){
		if ((Z_opt[i]>=0)&&(Z_opt[i]<=255))
			i=i;
		else
			i=i;
	}
	free(f);
	free(f_new_val);
	free(u);
	free(u_val);
	free(temp);
	free(Z_new_opt);
	free(diff);
	free(f_new);
	free(new_b_psi);
	free(grad_l_b);
	free(grad_c_b);
	free(sigma);
	free(sigma2);
	free(x);
	free(y);
	free(f_val);
	free(grad_c_f);
	free(grad_l_f);
	free(error_Z);
	return step;

}









long proj_optim_l1_l1_norm_semi_parametric(/*outputs*/double *f_opt,double *Z_opt, double *h, double *b_psi, /*inputs*/long *psi, double *gamma, short *psi_used, double *Z, double *Kappa, long N, long N_psi, double lambda,double b0, double b1, double epsilon0, double mu, double mu2,double E1, double E2)
{
	double	*new_h,sum,norm_diff;
	double	*f, *f_new, *u, *he, *u_val, *f_new_val, *temp, *Z_new_opt;
	long	stop_criterion;

	double	*sigma, *sigma2,*x, *y, *f_val, *error_Z,*diff;
	long	r,step,k,i,j;
	double  *grad_c_h, *grad_l_h, *grad_c_b, *grad_l_b, cf, lf, normf, normB, normH, norm_grad_l_f, tempv;
	double	*grad_c_f, *grad_l_f;
	double	curve[10001],combined_norm;
	double	*new_b_psi,L_function0, L_function1,norm_grad_l_b;
	long	total_psi_used;

	total_psi_used=0;
	for(i=0;i<N_psi;i++){
		if (psi_used[i]==1)
			total_psi_used++;
	}


	/*[N,M]=size(Z);*/
	/*k01=length(rho1);*/
	/*k02=length(rho2);*/
	/*k03=length(rho3);*/
	/*k04=length(rho4);*/

	/*f=zeros(N,M);*/
    new_h=(double *)malloc(4*sizeof(double));
	grad_c_h=(double *)malloc(4*sizeof(double));
	grad_l_h=(double *)malloc(4*sizeof(double));
	f=(double *) malloc(N*N*sizeof(double));
	zero_filling(f,N*N);
	zero_filling(b_psi,N_psi);
	f_new_val=(double *) malloc(N*N*sizeof(double));
	zero_filling(f_new_val,N*N);
	u=(double *) malloc(N*N*sizeof(double));
	zero_filling(u,N*N);
	u_val=(double *) malloc(N*N*sizeof(double));
	zero_filling(u_val,N*N);
	temp=(double *) malloc(N*N*sizeof(double));
	zero_filling(temp,N*N);
	Z_new_opt=(double *) malloc(N*N*sizeof(double));
	zero_filling(Z_new_opt,N*N);
	diff=(double *) malloc(N*N*sizeof(double));
	zero_filling(diff,N*N);
	/*f_new=zeros(N,M);*/
	f_new=(double *)malloc(N*N*sizeof(double));
	zero_filling(f_new,N*N);
	/*h(1)=sum(sum(Z))/N/M;*/
	h[0]=mean_value2D(Z,N,N);
    h[1]=0;
    h[2]=0;
    h[3]=0;
	
	new_b_psi=(double*)malloc(N_psi*sizeof(double));
	zero_filling(new_b_psi,N_psi);
	grad_c_b=(double*)malloc(N_psi*sizeof(double));
	zero_filling(grad_c_b,N_psi);
	grad_l_b=(double*)malloc(N_psi*sizeof(double));
	zero_filling(grad_l_b,N_psi);

	stop_criterion=0;

	
	sigma=(double *)malloc(10001*sizeof(double));
	zero_filling(sigma,10001);
	sigma2=(double *)malloc(10001*sizeof(double));
	zero_filling(sigma2,10001);

	for (r=1;r<=100;r++){
		sigma2[r-1]=1/2;
	}
	for (r=1;r<=10001;r++){
		sigma2[r-1]=10.0/(10+r);
	}
	for (r=1;r<=10001;r++){
		sigma[r-1]=1.0/r;
	}

	/*x=0:1/(M-1):1;*/
	x=(double *)malloc(N*sizeof(double));
	for (i=0;i<N;i++){
		x[i]=(double)i/(N-1);
	}
	/*y=0:1/(N-1):1;*/
	y=(double *)malloc(N*sizeof(double));
	for (j=0;j<N;j++){
		y[j]=(double)j/(N-1);
	}
	f_val=(double*)malloc(N*N*sizeof(double));
	zero_filling(f_val,N*N);
	RKHS_eval(f_val,f,Kappa,N,N);
	step=0;
	
	/*grad_cf=zeros(N,M);*/
	grad_c_f=(double *)malloc(N*N*sizeof(double));
	zero_filling(grad_c_f,N*N);
	grad_l_f=(double *)malloc(N*N*sizeof(double));
	zero_filling(grad_l_f,N*N);

	/*Z_opt=f_val+h(1)*ones(N,M);*/
	for (j=1;j<=N;j++){
		for(i=1;i<=N;i++){
			Z_opt[(j-1)*N+i-1]=h[0];
		}
	}
	
	error_Z=(double *)malloc(N*N*sizeof(double));
	for (j=1;j<=N;j++){
		for(i=1;i<=N;i++){
			error_Z[(j-1)*N+i-1]=Z_opt[(j-1)*N+i-1]-Z[(j-1)*N+i-1];
		}
	}
	cf=0;
	normf=RKHS_norm(f,Kappa,N,N);
	normB=0;
	for(k=0;k<N_psi;k++){
		if (psi_used[k]==1)
			normB=normB + gamma[k]*fabs(b_psi[k]);
	}

	normH=fabs(h[1])+fabs(h[2])+fabs(h[3]);

	for(j=1;j<=N;j++){
		for(i=1;i<=N;i++){
			if ( fabs(error_Z[(j-1)*N+i-1])< epsilon0 )
				cf=cf+0;
			else
				cf=cf + fabs(error_Z[(j-1)*N+i-1])-epsilon0;
		}
	}
	cf=cf/N/N;
	L_function0=cf   +   lambda*normf/N/N   +   mu/total_psi_used*normB   +   mu2*normH;
	while (stop_criterion==0){
		step=step+1;
		/*phase 1. Computing the subgradient of c(f) in respect with f, h and b*/
		grad_c_h[0]=0;
		grad_c_h[1]=0;
		grad_c_h[2]=0;
		grad_c_h[3]=0;
		zero_filling(grad_c_b,N_psi);
		
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				if (fabs(error_Z[(j-1)*N+i-1])<=epsilon0)
					grad_c_f[(j-1)*N+i-1]=0;
				else{
					grad_c_f[(j-1)*N+i-1]=sign(error_Z[(j-1)*N+i-1]);
					grad_c_h[0]=grad_c_h[0]+sign(error_Z[(j-1)*N+i-1]);
					grad_c_h[1]=grad_c_h[1]+sign(error_Z[(j-1)*N+i-1])*x[i-1];
					grad_c_h[2]=grad_c_h[2]+sign(error_Z[(j-1)*N+i-1])*y[j-1];
					grad_c_h[3]=grad_c_h[3]+sign(error_Z[(j-1)*N+i-1])*x[i-1]*y[j-1];
					for(k=1;k<=N_psi;k++){
						if (psi_used[k-1]==1)
							grad_c_b[k-1]=grad_c_b[k-1]+sign(error_Z[(j-1)*N+i-1])*((double*)psi[k-1])[(j-1)*N+i-1];
						else
							grad_c_b[k-1]=0;
					}	
				}
				grad_c_f[(j-1)*N+i-1]=grad_c_f[(j-1)*N+i-1]/N/N;
			}
		}
		grad_c_h[0]=grad_c_h[0]/N/N;
		grad_c_h[1]=grad_c_h[1]/N/N;
		grad_c_h[2]=grad_c_h[2]/N/N;
		grad_c_h[3]=grad_c_h[3]/N/N;
		for(k=1;k<=N_psi;k++){
			grad_c_b[k-1]=grad_c_b[k-1]/N/N;}


		/*phase 2. Computing the subgradient of l(f)*/
		/*grad_lf=grad_cf + lambda*f;*/
		for(j=1;j<=N;j++){
			for(i=1;i<=N;i++){
				grad_l_f[(j-1)*N+i-1]=grad_c_f[(j-1)*N+i-1]   +   lambda/N/N * f[(j-1)*N+i-1];
			}
		}
		grad_l_h[0]=grad_c_h[0];
		grad_l_h[1]=grad_c_h[1]+mu2*sign(h[1]);
		grad_l_h[2]=grad_c_h[2]+mu2*sign(h[2]);
		grad_l_h[3]=grad_c_h[3]+mu2*sign(h[3]);
		/*grad_l_b=grad_c_b + mu*b;*/
		for(k=0;k<N_psi;k++){
			if (psi_used[k]==1)
				grad_l_b[k]=grad_c_b[k] + mu/total_psi_used*gamma[k]*sign(b_psi[k]);
			else
				grad_l_b[k]=0;
		}
		
		/*phase 4. Computing norm of the subgradient L(f) (in respect with f)*/
		norm_grad_l_f=RKHS_norm(grad_l_f,Kappa,N,N);


		/*phase 4.5. Computing the  norm of the combined gradient*/
		norm_grad_l_b=square_sum1D(grad_l_b,N_psi);
		combined_norm = norm_grad_l_f + square_sum1D(grad_l_h, 4) + norm_grad_l_b;
		if (norm_grad_l_f<0)
			printf("error1\n");
		if (norm_grad_l_b<0)
			printf("error2\n");

		/*phase 5. Computing u*/

		if (norm_grad_l_f!=0){
			/*u=f-sigma(step)*grad_lf/sqrt(norm_grad_lf);*/
			for (j=1;j<=N;j++){
				for(i=1;i<=N;i++){
					u[(j-1)*N+i-1]=f[(j-1)*N+i-1] - sigma[step]*grad_l_f[(j-1)*N+i-1]/sqrt(norm_grad_l_f);
				}
			} 
		}
		else{
			for (j=1;j<=N;j++){
				for(i=1;i<=N;i++){
					u[(j-1)*N+i-1]=f[(j-1)*N+i-1];
				}
			} 
		}

		if (grad_l_h[0]!=0)
			new_h[0]=h[0]- sigma[step]*sign(grad_l_h[0]);
		else
			new_h[0]=h[0];
		if (grad_l_h[1]!=0)
			new_h[1]=h[1]- sigma[step]*sign(grad_l_h[1]);
		else
			new_h[1]=h[1];
		if (grad_l_h[2]!=0)
			new_h[2]=h[2]- sigma[step]*sign(grad_l_h[2]);
		else
			new_h[2]=h[2];
		if (grad_l_h[3]!=0)
			new_h[3]=h[3]- sigma[step]*sign(grad_l_h[3]);
		else
			new_h[3]=h[3];
		for(k=0;k<N_psi;k++){
			if ((psi_used[k]==1)&&(grad_l_b[k]!=0))
				new_b_psi[k]=b_psi[k] - sigma2[step] * grad_l_b[k]/sqrt(norm_grad_l_b);
			else
				new_b_psi[k]=b_psi[k];
		}
		
		/*construct u_val*/
		/*u_val=RKHS_eval(u,Kappa);*/
		RKHS_eval(u_val,u,Kappa,N,N);
		/*f_new=u;*/
		for (j=1;j<=N;j++){
			for(i=1;i<=N;i++){
				f_new[(j-1)*N+i-1]=u[(j-1)*N+i-1];
			}
		}

		/*phase 7. Compute the distance ||L_new-L||*/
		RKHS_eval(f_new_val,f_new,Kappa,N,N);
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				temp[(j-1)*N+i-1]=0;
				sum=0;
				for(k=0;k<N_psi;k++){
					if (psi_used[k]==1)
						sum+=new_b_psi[k]*((double*)psi[k])[(j-1)*N+i-1];
				}
				temp[(j-1)*N+i-1]=temp[(j-1)*N+i-1]+sum;
			}
		}
		/*Z_new_opt=f_new_val + new_h(1)*ones(N,M) + temp;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				Z_new_opt[(j-1)*N+i-1]=f_new_val[(j-1)*N+i-1] + new_h[0] +new_h[1]*x[i-1] + new_h[2]*y[j-1] +new_h[3]*x[i-1]*y[j-1] + temp[(j-1)*N+i-1];
			}
		}

		
		normf=RKHS_norm(f_new,Kappa,N,N);
		normB=0;
		for(k=0;k<N_psi;k++){
			if (psi_used[k]==1)
				normB=normB + gamma[k]*fabs(new_b_psi[k]);
		}
		normH=fabs(new_h[1])+fabs(new_h[2])+fabs(new_h[3]);

		for (j=1;j<=N;j++){
			for(i=1;i<=N;i++){
				error_Z[(j-1)*N+i-1]=Z_new_opt[(j-1)*N+i-1]-Z[(j-1)*N+i-1];
			}
		}
		cf=0;
		for(j=1;j<=N;j++){
			for(i=1;i<=N;i++){
				if ( fabs(error_Z[(j-1)*N+i-1])< epsilon0 )
					cf=cf+0;
				else
					cf=cf + fabs(error_Z[(j-1)*N+i-1])-epsilon0;
			}
		}
		cf=cf/N/N;
		L_function1=cf   +   lambda*normf/2/N/N   +   mu/total_psi_used*normB   +   mu2*normH;

		
		/*phase 9. Update curves*/
		//curve[step]=fabs(L_function1-L_function0);
		for (i=0;i<N*N;i++)
			temp[i]=fabs(Z_new_opt[i]-Z_opt[i]);
		curve[step]=sum1D(temp,N*N)/N/N;
		L_function0=L_function1;
		/*objective[step]=lf;*/

		/*phase 10. Check stopping criterion*/
		if (step>1){
			if ( curve[step]==0 )
				stop_criterion=1;
			else
				if ( (fabs(curve[step])<E1) &&(sqrt(combined_norm)<100*E2))
					stop_criterion=1;
		}

		/*f=f_new;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				f[(j-1)*N+i-1]=f_new[(j-1)*N+i-1];
			}
		}
		h[0]=new_h[0];
        h[1]=new_h[1];
        h[2]=new_h[2];
        h[3]=new_h[3];
		/*b=new_b;*/
		for (k=0;k<N_psi;k++){
			b_psi[k]=new_b_psi[k];
		}
		/*f_val=f_new_val;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				f_val[(j-1)*N+i-1]=f_new_val[(j-1)*N+i-1];
			}
		}
		/*Z_opt=Z_new_opt;*/
		for (j=1;j<=N;j++){
			for (i=1;i<=N;i++){
				Z_opt[(j-1)*N+i-1]=Z_new_opt[(j-1)*N+i-1];
			}
		}
		if (step==10000)
			break;

	}
	/*Stoping criterion*/
	/*f_opt=f;*/
	for (j=1;j<=N;j++){
		for (i=1;i<=N;i++){
			f_opt[(j-1)*N+i-1]=f[(j-1)*N+i-1];
		}
	}

	for (i=0;i<N*N;i++){
		if ((Z_opt[i]>=0)&&(Z_opt[i]<=255))
			i=i;
		else
			i=i;
	}
	free(f);
	free(f_new_val);
	free(u);
	free(u_val);
	free(temp);
	free(Z_new_opt);
	free(diff);
	free(f_new);
	free(new_b_psi);
	free(grad_l_b);
	free(grad_c_b);
	free(sigma);
	free(sigma2);
	free(x);
	free(y);
	free(f_val);
	free(grad_c_f);
	free(grad_l_f);
	free(error_Z);
	return step;

}



















void		Kernel_Denoising(double *return_pic, double *ori_pic, long M0, long N0, long n, long steps, double lambda_min, double lambda_max, double sigma_min, double sigma_max,double b0, double b1, double epsilon0, double initial_mu, double *mu_vector, long LEN_mu, double *per_vector, long *n_vector, long *method_vector, long *iter_pixel_step){
	
	long			maxn,step,metritis;
	long			pixel_step,m,M,N,i,j,i0,j0,i1,j1,k,l;
	double			*new_pic,sigma,lambda,mu;
	long			*index_pic;
	double			*Kappa1;
	unsigned long	*Kappa;
	double			*x,*y;
	double			percentage,percent,grad;
	long			print=0,k0,l0;
	double			*pic;

	/*double		slope=4;
	double			rho1[3]={-1, 0, 1}, l1=3;
	long			k01=3;
	double			rho2[3]={-1, 0, 1}, l2=3;
	long			k02=3;
	double			rho3[3]={-1, 0, 1}, l3=3;
	long			k03=3;
	double			rho4[3]={-1, 0, 1}, l4=3;
	long			k04=3;*/

	double		slope=8;
	double		rho1[7]={-3, -2, -1, 0, 1, 2, 3}, l1=7;
	long			k01=7;
	double		rho2[7]={-3, -2, -1, 0, 1, 2, 3}, l2=7;
	long			k02=7;
	double		rho3[5]={-2, -1, 0, 1, 2}, l3=5;
	long			k03=5;
	double		rho4[5]={-2, -1, 0, 1, 2 }, l4=5;
	long			k04=5;

	double		*region;
	double		*new_region;
	double		*med_grad;
	double		*temp_array;
	double		*thres=(double*)malloc(LEN_mu*sizeof(double));
	double		*metr=(double*)malloc(LEN_mu*sizeof(double));

	double		*f_opt,max_grad,min_grad;
	long			how_many_steps,method,k1;
	long			(*compare)(const double*,const double*);

	double		h2;
	double		*h=(double*)malloc(k01*sizeof(double));
	double		*he=(double*)malloc(k01*sizeof(double));
	double		*ve=(double*)malloc(k02*sizeof(double));
	double		*de=(double*)malloc(k03*sizeof(double));
	double		*dre=(double*)malloc(k04*sizeof(double));
	double		mu2;
			
	//double		mu_vector2[6]= {0.0001,  0.001,  0.01,  0.5,  1.0, 3.0};
	//double		per_vector2[6]={0.100,   0.10,  0.10,  0.2,  0.1, 0.4};


	m=n;
	maxn=find_max_long(n_vector,LEN_mu);
	pixel_step=iter_pixel_step[0];

	/*Initial step*/
	M=M0+maxn-1;
	N=N0+maxn-1;
	med_grad=(double*)malloc(M0*N0*sizeof(double));
	zero_filling(med_grad,N0*M0);
	/*pic=image_extend(ori_pic,maxn-1,maxn-1);*/
	pic=malloc(N*M*sizeof(double));
	image_extend(pic,ori_pic,M0,N0,maxn-1,maxn-1);
	/*new_pic=zeros(M,N);*/
	new_pic=(double*) malloc(M*N*sizeof(double));
	zero_filling(new_pic,M*N);
	/*index_pic=zeros(M,N);*/
	index_pic=(long*)malloc(M*N*sizeof(long));
	zero_filling_long(index_pic,M*N);
	temp_array=(double*)malloc(M0*N0*sizeof(double));
	zero_filling(temp_array,M0*N0);
	


	/*Initial phase. Construct Kappa - matrix.*/
	x=(double *)malloc(n*sizeof(double));
	/*x=0:1/(n-1):1;*/
	for(i=0;i<n;i++)
		x[i]=(double)i/(n-1);
	y=(double*)malloc(m*sizeof(double));
	/*y=0:1/(m-1):1;*/
	for(i=0;i<n;i++)
		y[i]=(double)i/(m-1);
	/*Kappa=zeros(n,m,n,m);*/
	Kappa1=(double*)malloc(n*n*n*n*sizeof(double));
	zero_filling(Kappa1,n*n*n*n);

	region=(double*)malloc(maxn*maxn*sizeof(double));
	new_region=(double*)malloc(maxn*maxn*sizeof(double));
	f_opt=malloc(maxn*maxn*sizeof(double));

	sigma=sigma_min;
	lambda=lambda_min;
	/*construct Kappa matrix (kernel)*/
	for(j0=0;j0<n;j0++)
		for(i0=0;i0<m;i0++)
			for(j1=0;j1<n;j1++)
				for(i1=0;i1<m;i1++)
					Kappa1[j0*m*n*m + i0*n*m + j1*m + i1]=exp( -( (x[i0]-x[i1])*(x[i0]-x[i1]) +(y[j0]-y[j1])*(y[j0]-y[j1]) ) / (2*sigma*sigma));

	percentage=0;
	print=0;


	/*Testing*/
	region[0]=20;   region[1]=22;  region[2]=100;   region[3]=180;   region[4]=185;
	region[5]=20;   region[6]=22;  region[7]=100;   region[8]=180;   region[9]=185;
	region[10]=20;   region[11]=22;  region[12]=100;   region[13]=180;   region[14]=185;
	region[15]=20;   region[16]=22;  region[17]=100;   region[18]=180;   region[19]=185;
	region[20]=20;   region[21]=22;  region[22]=100;   region[23]=180;   region[24]=185;
	mu=0.001;
	mu2=5;
	how_many_steps=proj_optim_l1_norm_for_edges2(f_opt, new_region, h, he, ve, de, dre, n, n, k01, k02, k03, k04, region, (double*)Kappa1,  lambda, b0,  b1,  epsilon0, rho1, rho2, rho3, rho4, slope,  mu, mu2);
	grad=PSNR_calc(region,new_region,5*5,255);

	for (i=1+(m-1)/2;i<=M0+(m-1)/2;i+=pixel_step){
		for (j=1+(n-1)/2;j<=N0+(n-1)/2;j+=pixel_step){
			/*region=pic(i-(m-1)/2:i+(m-1)/2 , j-(n-1)/2:j+(n-1)/2);*/
			for (k=1;k<=n;k++)
				for(l=1;l<=n;l++)
					region[(k-1)*n+l-1]=pic[(i-(m-1)/2 -1 + k-1)*N + j-(n-1)/2 -1 + l-1];

			/*solving the minimization problem*/
			mu=initial_mu;
			mu2=initial_mu;

			/*[f_opt new_r h he ve de dre]=mex_proj_optim_l1_norm_for_edges(region,lambda,b0,b1,epsilon0,Kappa(1:m,1:n,1:m,1:n),rho1,rho2,rho3,rho4,slope,mu);*/
			how_many_steps=proj_optim_l1_norm_for_edges2(f_opt, new_region, h, he, ve, de, dre, n, n, k01, k02, k03, k04, region, (double*)Kappa1,  lambda, b0,  b1,  epsilon0, rho1, rho2, rho3, rho4, slope,  mu, mu2);
			//how_many_steps=proj_optim_l1_norm_for_edges(f_opt, new_region, &h2, he, ve, de, dre, n, n, k01, k02, k03, k04, region, (double*)Kappa1,  lambda, b0,  b1,  epsilon0, rho1, rho2, rho3, rho4, slope,  mu);
			//how_many_steps=proj_optim_l1_norm(f_opt, new_region, &h, n, n, region, (double*)Kappa1,  10*lambda, b0,  b1,  epsilon0);
			/*new_pic(i-(m-1)/2:i+(m-1)/2 , j-(n-1)/2:j+(n-1)/2)=new_pic(i-(m-1)/2:i+(m-1)/2 , j-(n-1)/2:j+(n-1)/2)+new_region;*/
			k0=0;
			for (k=i-(m-1)/2;k<=i+(m-1)/2;k++){
				k0++;
				l0=0;
				for (l=j-(n-1)/2;l<=j+(n-1)/2;l++){
					l0++;
					new_pic[(k-1)*N+l-1]=new_pic[(k-1)*N+l-1]+new_region[(k0-1)*n+l0-1];
				}
			}
			k0=0;
			for (k=i-(m-1)/2;k<=i+(m-1)/2;k++){
				k0++;
				l0=0;
				for (l=j-(n-1)/2;l<=j+(n-1)/2;l++){
					l0++;
					index_pic[(k-1)*N+l-1]=index_pic[(k-1)*N+l-1]+1;
				}
			}
			percentage=percentage+1.0/N0/M0*pixel_step * pixel_step;
			if ((long)floor(percentage*1000)%10==0){
				if ( ( (long)floor(percentage*100)>=1 ) ){
					if (print==0){
						if ( (long)floor(percentage*100)<10 )
							printf("\b\b\b");
						else
							printf("\b\b\b\b");
					}
				}
				else
					if (print==0){
						printf("Step 0. Completed: ",(long)floor(percentage*100));
					}
				if (print==0){
					printf("%d %%",(long)floor(percentage*100));
					print=1;
				}
			}
			else
				print=0;
		}
	}
	for(i=1;i<=M0;i++)
		for(j=1;j<=N0;j++)
			return_pic[(i-1)*N0+j-1]=new_pic[(i+maxn/2-1)*N+j+maxn/2-1]/index_pic[(i+maxn/2-1)*N+j+maxn/2-1];

	free(Kappa1);

	/*for(k=0;k<LEN_mu;k++)
		n_vector[k]-=2;*/

	Kappa=(unsigned long*)malloc(LEN_mu*sizeof(unsigned long));
	for (k=0;k<LEN_mu;k++){
		n=n_vector[k];
		m=n;
		free(x);
		free(y);
		x=(double *)malloc(n*sizeof(double));
		for(i=0;i<n;i++)
			x[i]=(double)i/(n-1);
		y=(double*)malloc(m*sizeof(double));
		for(i=0;i<m;i++)
			y[i]=(double)i/(m-1);
		Kappa1=(double*)malloc(n*n*n*n*sizeof(double));
		for (j0=0;j0<n;j0++)
			for (i0=0;i0<m;i0++)
				for (j1=0;j1<n;j1++)
					for (i1=0;i1<m;i1++)
						Kappa1[j0*n*n*n + i0*n*n + j1*n + i1]=exp( -( (x[i0]-x[i1])*(x[i0]-x[i1])+(y[j0]-y[j1])*(y[j0]-y[j1]) ) / (2*sigma*sigma));
		Kappa[k]=(unsigned long)Kappa1;
	}


	


	for (step=1;step<steps;step++){
		printf("\n");
		/*here new_pic*/
		zero_filling(new_pic,N*M);
		image_extend(new_pic,return_pic,M0,N0,maxn-1,maxn-1);
		window_median_grad_calc(med_grad,new_pic,M,N,maxn,maxn);
		max_grad=find_max(med_grad,N0*M0);
		min_grad=find_min(med_grad,N0*M0);
		zero_filling(new_pic,N*M);
		zero_filling_long(index_pic,N*M);

		compare=&doubles_compare;
		for(k=0;k<M0*N0;k++)
			temp_array[k]=med_grad[k];
		qsort(temp_array,N0*M0,sizeof(double),compare);

		percentage=0;
		print=0;
		pixel_step=iter_pixel_step[step];
		metritis=0;
		/*taking into acount the edges...*/
		sigma=sigma_min;
		lambda=lambda_min;
		/*construct Kappa matrix (kernel)*/
		
		/*from fine edges to smooth*/
		percent=0;
		for (k=1;k<=LEN_mu-1;k++){
			percent=per_vector[k-1]+percent;
			thres[k-1]=temp_array[(long)round((1-percent)*M0*N0)];
			metr[k-1]=0;
		}
		metr[LEN_mu-1]=0;
		i0=0-pixel_step;
		for (i=1+(maxn-1)/2;i<=M0+(maxn-1)/2;i+=pixel_step){
			i0+=pixel_step;
			j0=-pixel_step;
			for (j=1+(maxn-1)/2; j<=N0+(maxn-1)/2; j+=pixel_step){
				j0+=pixel_step;
				grad=med_grad[i0*N0+j0];
				k=1;
				while (k<LEN_mu){
					if (grad<thres[k-1])
						k=k+1;
					else
						break;
				}

				mu=mu_vector[k-1];
				mu2=mu;
				n=n_vector[k-1];/*here*/
				m=n;
				metr[k-1]=metr[k-1]+1;
				method=method_vector[k-1];

				/*region=pic(i-(m-1)/2:i+(m-1)/2 , j-(n-1)/2:j+(n-1)/2);*/
				for (k1=1;k1<=n;k1++)
					for(l=1;l<=n;l++)
						region[(k1-1)*n+l-1]=pic[(i-(m-1)/2-1 + k1-1)*N + j-(n-1)/2 -1 + l-1];

				
				if (method==0) /*simple gaussian kernel*/
					how_many_steps=proj_optim_l1_norm(f_opt, new_region, &h2, n, n, region, (double*)Kappa[k-1],  lambda, b0,  b1,  epsilon0);
				else
					how_many_steps=proj_optim_l1_norm_for_edges2(f_opt, new_region, h, he, ve, de, dre, n, n, k01, k02, k03, k04, region, (double*)Kappa[k-1],  lambda, b0,  b1,  epsilon0, rho1, rho2, rho3, rho4, slope,  mu, mu2);
					//how_many_steps=proj_optim_l1_norm_for_edges(f_opt, new_region, &h2, he, ve, de, dre, n, n, k01, k02, k03, k04, region, (double*)Kappa[k-1],  lambda, b0,  b1,  epsilon0, rho1, rho2, rho3, rho4, slope,  mu);
				
				k0=0;
				for (k1=i-(m-1)/2;k1<=i+(m-1)/2;k1++){
					k0++;
					l0=0;
					for (l=j-(n-1)/2;l<=j+(n-1)/2;l++){
						l0++;
						new_pic[(k1-1)*N+l-1]=new_pic[(k1-1)*N+l-1]+new_region[(k0-1)*n+l0-1];
					}
				}
				k0=0;
				for (k1=i-(m-1)/2;k1<=i+(m-1)/2;k1++){
					k0++;
					l0=0;
					for (l=j-(n-1)/2;l<=j+(n-1)/2;l++){
						l0++;
						index_pic[(k1-1)*N+l-1]=index_pic[(k1-1)*N+l-1]+1;
					}
				}
				percentage=percentage+1.0/N0/M0*pixel_step * pixel_step;
				if ((long)floor(percentage*1000)%10==0){
					if ( ( (long)floor(percentage*100)>=1 ) ){
						if (print==0){
							if ( (long)floor(percentage*100)<10 )
								printf("\b\b\b");
							else
								printf("\b\b\b\b");
						}
					}
					else
						if (print==0){
							printf("Step %d. Completed: ",step,(long)floor(percentage*100));
						}
					if (print==0){
						printf("%d %%",(long)floor(percentage*100));
						print=1;
					}
				}
				else
					print=0;
				}
		}
		for(i=1;i<=M0;i++)
			for(j=1;j<=N0;j++)
				return_pic[(i-1)*N0+j-1]=new_pic[(i+maxn/2-1)*N+j+maxn/2-1]/index_pic[(i+maxn/2-1)*N+j+maxn/2-1];
	
	}

	for(i=0;i<N0*M0;i++){
		if (return_pic[i]>b1)
			return_pic[i]=b1;
		else if (return_pic[i]<b0)
			return_pic[i]=b0;
	}
	
	/*clean up memory*/
	free(region);
	free(new_region);
	free(thres);
	free(metr);
	free(f_opt);
	free(med_grad);
	free(pic);
	free(new_pic);
	free(index_pic);
	free(temp_array);
	free(x);
	free(y);
	free(he);
	free(ve);
	free(de);
	free(dre);
	for(k=0;k<LEN_mu;k++)
		free((double*)Kappa[k]);
	free(Kappa);

}






void		replace_with_min_in_window(double* buffer, int M0, int N0, int n){
	long	i,j,k,l;
	double	min;
	double*	buffer2=(double *)malloc(M0*N0*sizeof(double));

	for(i=0+n/2;i<M0-n/2;i++){
		for(j=0+n/2;j<N0-n/2;j++){
			min=1000000000;
			for (k=-n/2;k<=n/2;k++)
				for (l=-n/2;l<=n/2;l++)
					if (buffer[(i+k)*N0+j+l] < min)
						min = buffer[(i+k)*N0+j+l];
			buffer2[i*N0+j]=min;
		}
	}
	for(i=0+n/2;i<M0-n/2;i++){
		for(j=0+n/2;j<N0-n/2;j++){
			buffer[i*N0+j]=buffer2[i*N0+j];
		}
	}
	free(buffer2);
			
}



void		fix_edge_values(double *pic, double* edge_value, int M0, int N0, int n, int k_max, double b0, double b1){

	int		stop,k,k_n,temp_step,temp_counter1,temp_counter2,temp_sum,temp_sum2;
	long	i_n,j_n,i,j,l,l2,i2,j2,i1,j1,fix,n2;
	double	mean[9],center_mean,E;



	for (i=0;i<M0;i++){
		for(j=0;j<N0;j++){
			k=edge_value[i*N0+j];
			stop=0;
			while ((k<k_max)&&(stop==0)){
				temp_step=n;
				/*check neighboors*/
				/*   6   7   8
					 4   *   5
					 1   2   3
				*/
				temp_counter1=0;
				temp_counter2=0;
				temp_sum=0;
				/*1*/
				i_n=i-temp_step;
				j_n=j-temp_step;
				if ((i_n>=0)&&(i_n<M0-1)&&(j_n>=0)&&(j_n<N0-1)){
					k_n=edge_value[i_n*N0+j_n];
					if (k_n<=k)
						temp_counter1++;
					if (k_n<=k+1)
						temp_counter2++;
					temp_sum+=k_n;
				}
				/*2*/
				i_n=i-temp_step;
				j_n=j;
				if ((i_n>=0)&&(i_n<M0-1)&&(j_n>=0)&&(j_n<N0-1)){
					k_n=edge_value[i_n*N0+j_n];
					if (k_n<=k)
						temp_counter1++;
					if (k_n<=k+1)
						temp_counter2++;
					temp_sum+=k_n;
				}
				/*3*/
				i_n=i-temp_step;
				j_n=j+temp_step;
				if ((i_n>=0)&&(i_n<M0-1)&&(j_n>=0)&&(j_n<N0-1)){
					k_n=edge_value[i_n*N0+j_n];
					if (k_n<=k)
						temp_counter1++;
					if (k_n<=k+1)
						temp_counter2++;
					temp_sum+=k_n;
				}
				/*4*/
				i_n=i;
				j_n=j-temp_step;
				if ((i_n>=0)&&(i_n<M0-1)&&(j_n>=0)&&(j_n<N0-1)){
					k_n=edge_value[i_n*N0+j_n];
					if (k_n<=k)
						temp_counter1++;
					if (k_n<=k+1)
						temp_counter2++;
					temp_sum+=k_n;
				}
				/*5*/
				i_n=i;
				j_n=j+temp_step;
				if ((i_n>=0)&&(i_n<M0-1)&&(j_n>=0)&&(j_n<N0-1)){
					k_n=edge_value[i_n*N0+j_n];
					if (k_n<=k)
						temp_counter1++;
					if (k_n<=k+1)
						temp_counter2++;
					temp_sum+=k_n;
				}
				/*6*/
				i_n=i+temp_step;
				j_n=j-temp_step;
				if ((i_n>=0)&&(i_n<M0-1)&&(j_n>=0)&&(j_n<N0-1)){
					k_n=edge_value[i_n*N0+j_n];
					if (k_n<=k)
						temp_counter1++;
					if (k_n<=k+1)
						temp_counter2++;
					temp_sum+=k_n;
				}
				/*7*/
				i_n=i+temp_step;
				j_n=j;
				if ((i_n>=0)&&(i_n<M0-1)&&(j_n>=0)&&(j_n<N0-1)){
					k_n=edge_value[i_n*N0+j_n];
					if (k_n<=k)
						temp_counter1++;
					if (k_n<=k+1)
						temp_counter2++;
					temp_sum+=k_n;
				}
				/*8*/
				i_n=i+temp_step;
				j_n=j+temp_step;
				if ((i_n>=0)&&(i_n<M0-1)&&(j_n>=0)&&(j_n<N0-1)){
					k_n=edge_value[i_n*N0+j_n];
					if (k_n<=k)
						temp_counter1++;
					if (k_n<=k+1)
						temp_counter2++;
					temp_sum+=k_n;
				}


				if (temp_counter1<=1){
					if (temp_counter2>=2)
						k+=1;
					else{
						if (temp_counter2>=1)
							k+=2;
						else
							k+=3;
					}
				}
				else
					stop=1;
				if (k>k_max)
					k=k_max;
				edge_value[i*N0+j]=k;
			}
		}
	}

	n2=(n+1)/2;
	for (i=0+n2+n2/2;i<M0-n2-n2/2;i++){
		for(j=0+n2+n2/2;j<N0-n2-n2/2;j++){
			/*find mean value of the block nxn centered at i,j*/
			center_mean=0;
			for(i1=-n2/2;i1<=n2/2;i1++){
				for(j1=-n2/2;j1<=n2/2;j1++){
					center_mean+=pic[(i+i1)*N0+(j+j1)];
				}
			}
			center_mean/=n2*n2;
			
			/*check neighboors*/
				/*   6   7   8
					 4   *   5
					 1   2   3
				*/

			/*mean 1*/
			mean[1]=0;
			for(i1=-n2/2;i1<=n2/2;i1++){
				for(j1=-n2/2;j1<=n2/2;j1++){
					mean[1]+=pic[(i-n2+i1)*N0+(j-n2+j1)];
				}
			}
			mean[1]/=n2*n2;

			/*mean 2*/
			mean[2]=0;
			for(i1=-n2/2;i1<=n2/2;i1++){
				for(j1=-n2/2;j1<=n2/2;j1++){
					mean[2]+=pic[(i-n2+i1)*N0+(j+j1)];
				}
			}
			mean[2]/=n2*n2;

			/*mean 3*/
			mean[3]=0;
			for(i1=-n2/2;i1<=n2/2;i1++){
				for(j1=-n2/2;j1<=n2/2;j1++){
					mean[3]+=pic[(i-n2+i1)*N0+(j+n2+j1)];
				}
			}
			mean[3]/=n2*n2;

			/*mean 4*/
			mean[4]=0;
			for(i1=-n2/2;i1<=n2/2;i1++){
				for(j1=-n2/2;j1<=n2/2;j1++){
					mean[4]+=pic[(i+i1)*N0+(j-n2+j1)];
				}
			}
			mean[4]/=n2*n2;

			/*mean 5*/
			mean[5]=0;
			for(i1=-n2/2;i1<=n2/2;i1++){
				for(j1=-n2/2;j1<=n2/2;j1++){
					mean[5]+=pic[(i+i1)*N0+(j+n2+j1)];
				}
			}
			mean[5]/=n2*n2;

			/*mean 6*/
			mean[6]=0;
			for(i1=-n2/2;i1<=n2/2;i1++){
				for(j1=-n2/2;j1<=n2/2;j1++){
					mean[6]+=pic[(i+n2+i1)*N0+(j-n2+j1)];
				}
			}
			mean[6]/=n2*n2;

			/*mean 7*/
			mean[7]=0;
			for(i1=-n2/2;i1<=n2/2;i1++){
				for(j1=-n2/2;j1<=n2/2;j1++){
					mean[7]+=pic[(i+n2+i1)*N0+(j+j1)];
				}
			}
			mean[7]/=n2*n2;

			/*mean 8*/
			mean[8]=0;
			for(i1=-n2/2;i1<=n2/2;i1++){
				for(j1=-n2/2;j1<=n2/2;j1++){
					mean[8]+=pic[(i+n2+i1)*N0+(j+n2+j1)];
				}
			}
			mean[8]/=n2*n2;


			E=(b1-b0)/20;
			fix=0;
			for (l=1;l<=8;l++){
				for (l2=l+1;l2<=8;l2++){
					if (fabs(mean[l]-mean[l2])>E){
						fix=1;
						break;
					}
				}
				if (fix==1)
					break;
			}
			
			E=(b1-b0)/15;
			for (l=1;l<=8;l++){
				if (fabs(mean[l]-center_mean)<E){
					fix=1;
					break;
				} 
			}
			if (fix==0)
				edge_value[i*N0+j]+=3;
			if (edge_value[i*N0+j]>k_max)
				edge_value[i*N0+j]=k_max;

		}
	}
}







void		Kernel_Denoising_semi_parametric(/*output*/double *return_pic, /*inputs*/double *ori_pic, long M0, long N0, long n, long steps, double lambda, double sigma,double b0, double b1, double epsilon0, double initial_mu, double *mu_vector, double *mu2_vector, double *calc_type, long LEN_mu, double *per_vector, long *n_vector, long *method_vector, long *iter_pixel_step, long *psi_vector, long *N_psi_vector, long *gamma_vector){
	
	double			E1,E2;
	long			maxn,step,metritis;
	long			pixel_step,m,M,N,i,j,i0,j0,i1,j1,k,l,min;
	double			*new_pic,mu;
	long			*index_pic,ii;
	double			*Kappa1;
	unsigned long	*Kappa;
	double			*x,*y;
	double			percentage,percent,grad,temp_sum;
	long			print=0,k0,l0;
	double			*pic;

	double		*region,*noisy_region;
	double		*new_region;
	double		*med_grad,*edge_value;
	double		*temp_array;
	double		*thres=(double*)malloc(LEN_mu*sizeof(double));
	double		*metr=(double*)malloc(LEN_mu*sizeof(double));

	double		*f_opt,max_grad,min_grad,temp_step;
	long			how_many_steps,method,k1, i_n, j_n, temp_counter1,temp_counter2,stop, k_n;
	long			(*compare)(const double*,const double*);

	double		h2;
	double		*h=(double*)malloc(4*sizeof(double));
	double		mu2;
	int			v;
	long*		psi;
	double*		gamma;
	long		N_psi;
	short*		psi_used;
	double*		b_psi;


	m=n;
	maxn=find_max_long(n_vector,LEN_mu);
	pixel_step=iter_pixel_step[0];

	/*Initial step*/
	M=M0+maxn-1;
	N=N0+maxn-1;
	med_grad=(double*)malloc(M0*N0*sizeof(double));
	zero_filling(med_grad,N0*M0);
	edge_value=(double*)malloc(M0*N0*sizeof(double));
	zero_filling(edge_value,N0*M0);
	/*pic=image_extend(ori_pic,maxn-1,maxn-1);*/
	pic=malloc(N*M*sizeof(double));
	image_extend(pic,ori_pic,M0,N0,maxn-1,maxn-1);
	/*new_pic=zeros(M,N);*/
	new_pic=(double*) malloc(M*N*sizeof(double));
	zero_filling(new_pic,M*N);
	/*index_pic=zeros(M,N);*/
	index_pic=(long*)malloc(M*N*sizeof(long));
	zero_filling_long(index_pic,M*N);
	temp_array=(double*)malloc(M0*N0*sizeof(double));
	zero_filling(temp_array,M0*N0);
	


	/*Initial phase. Construct Kappa - matrix.*/
	x=(double *)malloc(n*sizeof(double));
	/*x=0:1/(n-1):1;*/
	for(i=0;i<n;i++)
		x[i]=(double)i/(n-1);
	y=(double*)malloc(m*sizeof(double));
	/*y=0:1/(m-1):1;*/
	for(i=0;i<n;i++)
		y[i]=(double)i/(n-1);
	/*Kappa=zeros(n,m,n,m);*/
	Kappa1=(double*)malloc(n*n*n*n*sizeof(double));
	zero_filling(Kappa1,n*n*n*n);

	region=(double*)malloc(maxn*maxn*sizeof(double));
	new_region=(double*)malloc(maxn*maxn*sizeof(double));
	f_opt=malloc(maxn*maxn*sizeof(double));

	/*construct Kappa matrix (kernel)*/
	for(j0=0;j0<n;j0++)
		for(i0=0;i0<n;i0++)
			for(j1=0;j1<n;j1++)
				for(i1=0;i1<n;i1++)
					Kappa1[j0*n*n*n + i0*n*n + j1*n + i1]=exp( -( (x[i0]-x[i1])*(x[i0]-x[i1]) +(y[j0]-y[j1])*(y[j0]-y[j1]) ) / (2*sigma*sigma));

	percentage=0;
	print=0;

	for (i=1+(n-1)/2;i<=M0+(maxn-1)/2;i+=pixel_step){
		for (j=1+(n-1)/2;j<=N0+(maxn-1)/2;j+=pixel_step){
			/*region=pic(i-(m-1)/2:i+(m-1)/2 , j-(n-1)/2:j+(n-1)/2);*/
			for (k=1;k<=n;k++)
				for(l=1;l<=n;l++)
					region[(k-1)*n+l-1]=pic[(i-(n-1)/2 -1 + k-1)*N + j-(n-1)/2 -1 + l-1];
			/*solving the minimization problem*/
			v=n/2-1;
			psi=psi_vector[v];
			gamma=gamma_vector[v];
			N_psi=N_psi_vector[v];
			psi_used=(short*)malloc(N_psi*sizeof(short));
			b_psi=(double*)malloc(N_psi*sizeof(double));
			constant_filling_short(psi_used,1,N_psi);
			E1=0.5;
			E2=0.8;
			mu=initial_mu;
			mu2=initial_mu;

			//how_many_steps=progressive_proj_optim_l1_l2_norm_semi_parametric(f_opt,new_region, h, b_psi, psi, gamma, psi_used, region, (double*)Kappa1, n, N_psi, lambda, b0,  b1, epsilon0, mu, mu2,E1,E2,2);
			how_many_steps=proj_optim_l1_l2_norm_semi_parametric(f_opt,new_region, h, b_psi, psi, gamma, psi_used, region, (double*)Kappa1, n, N_psi, lambda, b0,  b1, epsilon0, mu, mu2,E1,E2);
			/*[f_opt new_r h he ve de dre]=mex_proj_optim_l1_norm_for_edges(region,lambda,b0,b1,epsilon0,Kappa(1:m,1:n,1:m,1:n),rho1,rho2,rho3,rho4,slope,mu);*/
			//how_many_steps=proj_optim_l1_norm_for_edges2(f_opt, new_region, h, he, ve, de, dre, n, n, k01, k02, k03, k04, region, (double*)Kappa1,  lambda, b0,  b1,  epsilon0, rho1, rho2, rho3, rho4, slope,  mu, mu2);
			//how_many_steps=proj_optim_l1_norm_for_edges(f_opt, new_region, &h2, he, ve, de, dre, n, n, k01, k02, k03, k04, region, (double*)Kappa1,  lambda, b0,  b1,  epsilon0, rho1, rho2, rho3, rho4, slope,  mu);
			//how_many_steps=proj_optim_l1_norm(f_opt, new_region, &h, n, n, region, (double*)Kappa1,  10*lambda, b0,  b1,  epsilon0);
			/*new_pic(i-(m-1)/2:i+(m-1)/2 , j-(n-1)/2:j+(n-1)/2)=new_pic(i-(m-1)/2:i+(m-1)/2 , j-(n-1)/2:j+(n-1)/2)+new_region;*/
			free(psi_used);
			free(b_psi);
			k0=0;
			for (k=i-(m-1)/2;k<=i+(m-1)/2;k++){
				k0++;
				l0=0;
				for (l=j-(n-1)/2;l<=j+(n-1)/2;l++){
					l0++;
					new_pic[(k-1)*N+l-1]=new_pic[(k-1)*N+l-1]+new_region[(k0-1)*n+l0-1];
					index_pic[(k-1)*N+l-1]=index_pic[(k-1)*N+l-1]+1;
				}
			}
			percentage=percentage+1.0/N0/M0*pixel_step * pixel_step;
			if ((long)floor(percentage*1000)%10==0){
				if ( ( (long)floor(percentage*100)>=1 ) ){
					if (print==0){
						if ( (long)floor(percentage*100)<10 )
							printf("\b\b\b");
						else
							printf("\b\b\b\b");
					}
				}
				else
					if (print==0){
						printf("Step 0. Completed: ",(long)floor(percentage*100));
					}
				if (print==0){
					printf("%d %%",(long)floor(percentage*100));
					print=1;
				}
			}
			else
				print=0;
		}
	}
	for(i=1;i<=M0;i++)
		for(j=1;j<=N0;j++){
			if (index_pic[(i+maxn/2-1)*N+j+maxn/2-1]!=0){
				return_pic[(i-1)*N0+j-1]=new_pic[(i+maxn/2-1)*N+j+maxn/2-1]/index_pic[(i+maxn/2-1)*N+j+maxn/2-1];
			}
			else
				return_pic[(i-1)*N0+j-1]=0;
		}

	free(Kappa1);

	/*for(k=0;k<LEN_mu;k++)
		n_vector[k]-=2;*/

	Kappa=(unsigned long*)malloc(LEN_mu*sizeof(unsigned long));
	for (k=0;k<LEN_mu;k++){
		n=n_vector[k];
		free(x);
		free(y);
		x=(double *)malloc(n*sizeof(double));
		for(i=0;i<n;i++)
			x[i]=(double)i/(n-1);
		y=(double*)malloc(n*sizeof(double));
		for(i=0;i<n;i++)
			y[i]=(double)i/(n-1);
		Kappa1=(double*)malloc(n*n*n*n*sizeof(double));
		for (j0=0;j0<n;j0++)
			for (i0=0;i0<n;i0++)
				for (j1=0;j1<n;j1++)
					for (i1=0;i1<n;i1++)
						Kappa1[j0*n*n*n + i0*n*n + j1*n + i1]=exp( -( (x[i0]-x[i1])*(x[i0]-x[i1])+(y[j0]-y[j1])*(y[j0]-y[j1]) ) / (2*sigma*sigma));
		Kappa[k]=(unsigned long)Kappa1;
	}


	


	for (step=1;step<steps;step++){
		printf("\n");
		/*here new_pic*/
		zero_filling(new_pic,N*M);
		image_extend(new_pic,return_pic,M0,N0,maxn-1,maxn-1);
		//window_max_grad_calc(med_grad,new_pic,M,N,maxn,maxn);
		if (find_max(calc_type,LEN_mu)==1)
			window_median_grad_calc(med_grad,new_pic,M,N,maxn,maxn);
		else
			window_norm_grad_calc(med_grad,new_pic,M,N,maxn,maxn);
		max_grad=find_max(med_grad,N0*M0);
		min_grad=find_min(med_grad,N0*M0);
		zero_filling(new_pic,N*M);
		zero_filling_long(index_pic,N*M);

		compare=&doubles_compare;
		for(k=0;k<M0*N0;k++)
			temp_array[k]=med_grad[k];
		qsort(temp_array,N0*M0,sizeof(double),compare);

		percentage=0;
		print=0;
		pixel_step=iter_pixel_step[step];
		metritis=0;
		sigma=sigma;
		lambda=lambda;
		
		/*from fine edges to smooth*/
		percent=0;
		for (k=1;k<=LEN_mu-1;k++){
			percent=per_vector[k-1]+percent;
			thres[k-1]=temp_array[(long)round((1-percent)*M0*N0)];
			metr[k-1]=0;
		}
		metr[LEN_mu-1]=0;
		i0=0-1;
		for (i=1+(maxn-1)/2;i<=M0+(maxn-1)/2;i+=1){
			i0+=1;
			j0=-1;
			for (j=1+(maxn-1)/2; j<=N0+(maxn-1)/2; j+=1){
				j0+=1;
				grad=med_grad[i0*N0+j0];
				k=1;
				while (k<LEN_mu){
					if (grad<thres[k-1])
						k=k+1;
					else
						break;
				}
				edge_value[i0*N0+j0]=k;
			}
		}
		fix_edge_values(return_pic,edge_value,M0,N0,maxn,LEN_mu-1,b0,b1);
		if (find_max(calc_type,LEN_mu)==2.0)
			replace_with_min_in_window(edge_value, M0, N0, maxn);
		

		i0=0-pixel_step;
		for (i=1+(maxn-1)/2;i<=M0+(maxn-1)/2;i+=pixel_step){
			i0+=pixel_step;
			j0=-pixel_step;
			for (j=1+(maxn-1)/2; j<=N0+(maxn-1)/2; j+=pixel_step){
				j0+=pixel_step;
				k=edge_value[i0*N0+j0];
				

				mu=mu_vector[k-1];
				mu2=mu2_vector[k-1];
				n=n_vector[k-1];/*here*/
				m=n;
				metr[k-1]=metr[k-1]+1;
				method=method_vector[k-1];

				/*region=pic(i-(m-1)/2:i+(m-1)/2 , j-(n-1)/2:j+(n-1)/2);*/
				for (k1=1;k1<=n;k1++)
					for(l=1;l<=n;l++)
						region[(k1-1)*n+l-1]=pic[(i-(m-1)/2-1 + k1-1)*N + j-(n-1)/2 -1 + l-1];
				
				if (method==0) /*simple gaussian kernel*/
					how_many_steps=proj_optim_l1_norm(f_opt, new_region, &h2, n, n, region, (double*)Kappa[k-1],  lambda, b0,  b1,  epsilon0);
				else{
					//how_many_steps=proj_optim_l1_norm_for_edges2(f_opt, new_region, h, he, ve, de, dre, n, n, k01, k02, k03, k04, region, (double*)Kappa[k-1],  lambda, b0,  b1,  epsilon0, rho1, rho2, rho3, rho4, slope,  mu, mu2);
					//how_many_steps=proj_optim_l1_norm_for_edges(f_opt, new_region, &h2, he, ve, de, dre, n, n, k01, k02, k03, k04, region, (double*)Kappa[k-1],  lambda, b0,  b1,  epsilon0, rho1, rho2, rho3, rho4, slope,  mu);
					v=n/2-1;
					psi=psi_vector[v];
					gamma=gamma_vector[v];
					N_psi=N_psi_vector[v];
					psi_used=(short*)malloc(N_psi*sizeof(short));
					b_psi=(double*)malloc(N_psi*sizeof(double));
					constant_filling_short(psi_used,1,N_psi);
					if ( (method>=2) ){
						E1=0.1;
						E2=0.5;
					}
					else{
						E1=0.5;
						E2=0.8;
					}

					how_many_steps=proj_optim_l1_l2_norm_semi_parametric(f_opt,new_region, h, b_psi, psi, gamma, psi_used, region, (double*)Kappa[k-1], n, N_psi, lambda, b0,  b1, epsilon0, mu, mu2,E1,E2);
					//how_many_steps=progressive_proj_optim_l1_l2_norm_semi_parametric(f_opt,new_region, h, b_psi, psi, gamma, psi_used, region, (double*)Kappa[k-1], n, N_psi, lambda, b0,  b1, epsilon0, mu, mu2,E1,E2,10);
					
					free(psi_used);
					free(b_psi);
				}

				if  (! ((pixel_step>1)||(calc_type[k-1]==1)) ){ 
					new_pic[(i-1)*N+j-1]=new_region[(n/2)*n +n/2];
					index_pic[(i-1)*N+(j-1)]=-1;
				}
				else{
				k0=0;
				for (k1=i-(m-1)/2;k1<=i+(m-1)/2;k1++){
					k0++;
					l0=0;
					for (l=j-(n-1)/2;l<=j+(n-1)/2;l++){
						l0++;
						if (index_pic[(k1-1)*N+l-1]!=-1){
							new_pic[(k1-1)*N+l-1]=new_pic[(k1-1)*N+l-1]+new_region[(k0-1)*n+l0-1];
							index_pic[(k1-1)*N+l-1]=index_pic[(k1-1)*N+l-1]+1;
						}
					}
				}
				}

				percentage=percentage+1.0/N0/M0*pixel_step * pixel_step;
				if ((long)floor(percentage*1000)%10==0){
					if ( ( (long)floor(percentage*100)>=1 ) ){
						if (print==0){
							if ( (long)floor(percentage*100)<10 )
								printf("\b\b\b");
							else
								printf("\b\b\b\b");
						}
					}
					else
						if (print==0){
							printf("Step %d. Completed: ",step,(long)floor(percentage*100));
						}
					if (print==0){
						printf("%d %%",(long)floor(percentage*100));
						print=1;
					}
				}
				else
					print=0;
				}
		}
		for(i=1;i<=M0;i++)
			for(j=1;j<=N0;j++){
				if (index_pic[(i+maxn/2-1)*N+j+maxn/2-1]!=0){
					if (index_pic[(i+maxn/2-1)*N+j+maxn/2-1]==-1)
						return_pic[(i-1)*N0+j-1]=new_pic[(i+maxn/2-1)*N+j+maxn/2-1];
					else
						return_pic[(i-1)*N0+j-1]=new_pic[(i+maxn/2-1)*N+j+maxn/2-1]/index_pic[(i+maxn/2-1)*N+j+maxn/2-1];
				}
				else
					return_pic[(i-1)*N0+j-1]=0;
				if ((return_pic[(i-1)*N0+j-1]>=0)&&(return_pic[(i-1)*N0+j-1]<=255))
					i=i;
				else
					j=j;
			}
	
	}

	for(i=0;i<N0*M0;i++){
		if (return_pic[i]>b1)
			return_pic[i]=b1;
		else if (return_pic[i]<b0)
			return_pic[i]=b0;
	}
	
	/*clean up memory*/
	free(region);
	free(new_region);
	free(thres);
	free(metr);
	free(f_opt);
	free(med_grad);
	free(edge_value);
	free(pic);
	free(new_pic);
	free(index_pic);
	free(temp_array);
	free(x);
	free(y);
	free(h);
	for(k=0;k<LEN_mu;k++)
		free((double*)Kappa[k]);
	free(Kappa);

}



















long main(void)
{
	long			N, M,i;
	time_t			up_time1, up_time2;

	double			*pic,lambda_min,lambda_max,sigma_min,sigma_max,epsilon0;
	double			*new_pic,*ori_pic;
    double			mu_max,mu_min;
	/*Input parameters.*/
	double			mu_vector[6]= {0.01, 0.1, 0.5 , 5, 50, 100};	// vector \mu
	double			mu2_vector[6]= {0,  0,  0,   0.1,   1,  3};		// vector \mu_1
	double			calc_type[6]={1,1,1,1,1,1};						//vector c
	double			per_vector[6]={0.10, 0.1, 0.15, 0.1, 0.20, 0.4};	//vector p
	long			n_vector[6]={5, 5, 5, 5, 5, 5};					//vector N
	long			method_vector[6]={2, 2, 1, 1, 1, 0};			//vector m		
	long			pixel_step[3]={3, 3, 1};						//vector s
	long			n=5;											//number N_0
	long			steps=3;										//number \nu
	double			initial_mu=mu_vector[1];						//number \mu_0
	long			LEN_mu=6;										//number L
	double			b0=0;
	double			b1=255;
	double			sigma=3;										//\sigma
	double			lambda=1;										//\lambda
	/*end*/
	unsigned long	seed,temp;
	FILE			*original_file, *noisy_file, *denoised_file;
	long			return_items,offset;
	int				return_int;
	short			return_short;
	unsigned char	*temp_buffer, *address,return_char,*temp_buffer2;
	char			name1[255],name2[255], name3[255];
	long*			psi_vector;
	long*			N_psi_vector;
	long*			gamma_vector;




	
	initialize_psi_vector(&psi_vector, &N_psi_vector, &gamma_vector);
	M=512;
	N=512;
	epsilon0=0.0;

	lambda_max=1;/*0.03*/
	sigma_max=3;
	lambda_min=1;
	sigma_min=3;
	

	/*User input*/
	printf ("Give the name of the original image:");
	gets(name1);
	printf("Give the name of the noisy image:");
	gets(name2);
	printf("Give the name of the file that I will store the denoised image:");
	gets(name3);


	/*Reading the original file*/
	original_file=fopen(name1,"rb");
	if (original_file==NULL){
		printf("There is no such original file");
		gets(name1);
		exit(-1);
	}
	/*first two bytes show that this is a bitmap*/
	return_items=fread(&return_short,sizeof(short),1,original_file);
	if ( (return_items!=1) || ( ((unsigned short)return_short!=('M'<<8)+'B') && ( (unsigned short)return_short!=('A'<<8)+'B') )){
		printf("Not a bitmap file");
		gets(name1);
		exit(-1);
	}
	/*Get Data offset*/
	return_items=fseek(original_file,0x0A,SEEK_SET);
	return_items=fread(&return_int,sizeof(long),1,original_file);
	offset=return_int;
	/*Get dimensions*/
	return_items=fseek(original_file,0x12,SEEK_SET);
	return_items=fread(&return_int,sizeof(long),1,original_file);
	if(return_items!=1){
		gets(name1);
		exit(-1);
	}
	N=(unsigned long) return_int;
	return_items=fread(&return_int,sizeof(long),1,original_file);
	if(return_items!=1){
		gets(name1);
		exit(-1);
	}
	M=(unsigned long) return_int;
	/*Bit DEPTH - IS ti grayscale?*/
	return_items=fseek(original_file,0x1C,SEEK_SET);
	return_items=fread(&return_short,sizeof(short),1,original_file);
	if((return_items!=1) || (return_short!=8)){
		gets(name1);
		exit(-1);
	}
	/*Compressed?*/
	return_items=fread(&return_int,sizeof(long),1,original_file);
	if((return_items!=1) || (return_int!=0)){
		gets(name1);
		exit(-1);
	}

	ori_pic=(double*)malloc(M*N*sizeof(double));
	pic=(double*)malloc(M*N*sizeof(double));
	new_pic=(double*)malloc(M*N*sizeof(double));
	temp_buffer=(unsigned char *)malloc(M*N*sizeof(char));

	/*read the pixel values*/
	return_items=fseek(original_file,offset,SEEK_SET);
	address=temp_buffer;
	for (i=0;i<M;i++){
		return_items=fread(address,sizeof(unsigned char), N, original_file);
		fseek(original_file,(4-N%4)%4,SEEK_CUR);
		address+=N;
	}

	for (i=0;i<M*N;i++)
		ori_pic[i]=temp_buffer[i];



	/*Reading the noisy image*/
	noisy_file=fopen(name2,"rb");
	if (noisy_file==NULL){
		printf("There is no such file");
		gets(name1);
		exit(-1);
	}
	/*first two bytes show that this is a bitmap*/
	return_items=fread(&return_short,sizeof(short),1,noisy_file);
	if ( (return_items!=1) || ( ((unsigned short)return_short!=('M'<<8)+'B') && ( (unsigned short)return_short!=('A'<<8)+'B') )){
		printf("Not a bitmap file");
		gets(name1);
		exit(-1);
	}
	/*Get Data offset*/
	return_items=fseek(noisy_file,0x0A,SEEK_SET);
	return_items=fread(&return_int,sizeof(long),1,noisy_file);
	offset=return_int;
	/*Get dimensions*/
	return_items=fseek(noisy_file,0x12,SEEK_SET);
	return_items=fread(&return_int,sizeof(long),1,noisy_file);
	if  (return_items!=1){
		gets(name1);
		exit(-1);
	}
	if (return_int!=N){
		printf("The images must have the same size\n");
		gets(name1);
		exit(-1);
	}
	return_items=fread(&return_int,sizeof(long),1,noisy_file);
	if(return_items!=1){
		gets(name1);
		exit(-1);
	}
	if (return_int!=M){
		printf("The images must have the same size\n");
		gets(name1);
		exit(-1);
	}
	/*Bit DEPTH - Is it grayscale?*/
	return_items=fseek(noisy_file,0x1C,SEEK_SET);
	return_items=fread(&return_short,sizeof(short),1,noisy_file);
	if((return_items!=1) || (return_short!=8)){
		gets(name1);
		exit(-1);
	}
	/*Compressed?*/
	return_items=fread(&return_int,sizeof(long),1,noisy_file);
	if((return_items!=1) || (return_int!=0)){
		gets(name1);
		exit(-1);
	}


	/*read the pixel values*/
	return_items=fseek(noisy_file,offset,SEEK_SET);
	address=temp_buffer;
	for (i=0;i<M;i++){
		return_items=fread(address,sizeof(unsigned char), N, noisy_file);
		if (return_items!=N){
			gets(name1);
			exit(-1);
		}
		fseek(noisy_file,(4-N%4)%4,SEEK_CUR);
		address+=N;
	}

	for (i=0;i<M*N;i++)
		pic[i]=temp_buffer[i];


	/*Remove this code if you want to input the parameters in the code and compile*/
	/*read the user-defined parameters*/
	printf("Give N0 (positive integer): ");
	scanf("%d",&n);

	printf("Give the 6 possitive integer numbers for the vector N:\n");
	for(i=0;i<6;i++){
		printf("N[%d]= ",i);
		scanf("%d",&n_vector[i]);
	}

	printf("Give the 6 possitive real numbers for the vector p:\n");
	for(i=0;i<6;i++){
		printf("p[%d]= ",i);
		scanf("%f",&per_vector[i]);
	}




	up_time1=clock();
	printf("Starting\n");
	//Kernel_Denoising(new_pic, pic, M, N, n, steps, lambda_min, lambda_max, sigma_min, sigma_max, b0,  b1,  epsilon0,  initial_mu,  mu_vector,  LEN_mu, per_vector, n_vector, method_vector,  pixel_step);
	Kernel_Denoising_semi_parametric(new_pic, pic, M, N, n, steps, lambda, sigma, b0, b1, epsilon0, initial_mu, mu_vector, mu2_vector, calc_type, LEN_mu,  per_vector, n_vector, method_vector, pixel_step, psi_vector, N_psi_vector, gamma_vector);

	up_time2=clock();



	denoised_file=fopen(name3,"wb");
	if (denoised_file==NULL){
		gets(name1);
		exit(-1);
	}
	temp_buffer2=(unsigned char*)malloc(offset*sizeof(char));
	return_items=fseek(noisy_file,0x00,SEEK_SET);
	return_items=fread(temp_buffer2,sizeof(char),offset,noisy_file);
	if(return_items!=offset){
		gets(name1);
		exit(-1);
	}
	return_items=fwrite(temp_buffer2,sizeof(char),offset,denoised_file);
	if(return_items!=offset){
		exit(-1);
		gets(name1);
	}
	for (i=0;i<M*N;i++)
		temp_buffer[i]=new_pic[i];
	for(i=0;i<4;i++)
		temp_buffer2[i]=0;
	address=temp_buffer;
	for (i=0;i<M;i++){
		return_items=fwrite(address,sizeof(unsigned char), N, denoised_file);
		if(return_items!=N){
			gets(name1);
			exit(-1);
		}
		return_items=fwrite(temp_buffer2,sizeof(unsigned char), (4-N%4)%4, denoised_file);
		if(return_items!=(4-N%4)%4){
			gets(name1);
			exit(-1);
		}
		address+=N;
	}


	fclose(denoised_file);
	fclose(noisy_file);
	fclose(original_file);

	printf("\n");
	printf("----------------------------------------------------------------------------\n");
	printf("PSNR between original and noisy image is: %.3f\n", PSNR_calc(ori_pic,pic,N*M,255));
	printf("PSNR between original and denoised image is: %.3f\n", PSNR_calc(ori_pic,new_pic,N*M,255));
	printf("Time pased: %d secs\n", (up_time2-up_time1)/CLOCKS_PER_SEC);
	printf("----------------------------------------------------------------------------\n");
	printf("PRESS ENTER TO QUIT");
	

	
	free(ori_pic);
	free(new_pic);
	free(pic);
	free(temp_buffer);
	free(temp_buffer2);
	printf ("\a") ;
	printf ("\a") ;
	printf ("\a\n") ;
	gets(name1);
}







int test_main (void)
{
	long			N, M,i;
	time_t			up_time1, up_time2;

	double			*pic,lambda_min,lambda_max,sigma_min,sigma_max,b0,b1,epsilon0,initial_mu;
	long			n,steps,LEN_mu;
	double			*new_pic,*ori_pic;
    double			mu_max,mu_min;
	double			mu_vector[6]= {0.09,  0.7,  2,   10,   10,  10};
	double			per_vector[6]={0.100, 0.10, 0.10, 0.1, 0.20, 0.4};
	long			n_vector[6]={5, 5, 5, 5, 5, 5};
	long			method_vector[6]={2, 2, 1, 1, 1, 0};
	long			pixel_step[3]={3, 3, 1};
	unsigned long	seed,temp;
	FILE			*original_file, *noisy_file, *denoised_file;
	long			return_items,offset;
	int				return_int;
	short			return_short;
	unsigned char	*temp_buffer, *address,return_char,*temp_buffer2;
	char			name1[255],name2[255], name3[255];
	long*			psi_vector;
	long*			N_psi_vector;
	long*			gamma_vector;

	double			E1,E2,E3;
	long			maxn,step,metritis;
	long			m,j,i0,j0,i1,j1,k,l;
	double			sigma,lambda,mu;
	long			*index_pic,ii;
	double			*Kappa1;
	unsigned long	*Kappa;
	double			*x,*y;
	double			percentage,percent,grad;
	long			print=0,k0,l0;

	double		*region,*noisy_region;
	double		*new_region;
	double		*med_grad;
	double		*temp_array;

	double		*f_opt,max_grad,min_grad;
	long			how_many_steps,method,k1;
	long			(*compare)(const double*,const double*);

	double		h2;
	double		*h=(double*)malloc(4*sizeof(double));
	double		mu2;
	int			v;
	long*		psi;
	double*		gamma;
	long		N_psi;
	short*		psi_used;
	double*		b_psi;
	
	
	
	initialize_psi_vector(&psi_vector, &N_psi_vector, &gamma_vector);
	steps=3;
	initial_mu=0.4;/*0.4*/
	M=512;
	N=512;
	n=5;
	epsilon0=0.0;
	b0=0;
	b1=255;
	lambda_max=0.05;/*0.03*/
	sigma_max=3;
	lambda_min=0.05;
	sigma_min=3;
	mu_max=3;
	mu_min=0.01;
	LEN_mu=6;



	/*User input*/
	printf ("Give the name of the original image:");
	gets(name1);
	printf("Give the name of the noisy image:");
	gets(name2);
	printf("Give the name of the file that I will store the denoised image:");
	gets(name3);



	/*Reading the original file*/
	original_file=fopen(name1,"rb");
	if (original_file==NULL){
		gets(name1);
		exit(-1);
	}
	/*first two bytes show that this is a bitmap*/
	return_items=fread(&return_short,sizeof(short),1,original_file);
	if ( (return_items!=1) || ( ((unsigned short)return_short!=('M'<<8)+'B') && ( (unsigned short)return_short!=('A'<<8)+'B') )){
		printf("Not a bitmap file");
		exit(-1);
	}
	/*Get Data offset*/
	return_items=fseek(original_file,0x0A,SEEK_SET);
	return_items=fread(&return_int,sizeof(long),1,original_file);
	offset=return_int;
	/*Get dimensions*/
	return_items=fseek(original_file,0x12,SEEK_SET);
	return_items=fread(&return_int,sizeof(long),1,original_file);
	if(return_items!=1)
		exit(-1);
	N=(unsigned long) return_int;
	return_items=fread(&return_int,sizeof(long),1,original_file);
	if(return_items!=1)
		exit(-1);
	M=(unsigned long) return_int;
	/*Bit DEPTH - IS ti grayscale?*/
	return_items=fseek(original_file,0x1C,SEEK_SET);
	return_items=fread(&return_short,sizeof(short),1,original_file);
	if((return_items!=1) || (return_short!=8))
		exit(-1);
	/*Compressed?*/
	return_items=fread(&return_int,sizeof(long),1,original_file);
	if((return_items!=1) || (return_int!=0))
		exit(-1);

	ori_pic=(double*)malloc(M*N*sizeof(double));
	pic=(double*)malloc(M*N*sizeof(double));
	new_pic=(double*)malloc(M*N*sizeof(double));
	temp_buffer=(unsigned char *)malloc(M*N*sizeof(char));

	/*read the pixel values*/
	return_items=fseek(original_file,offset,SEEK_SET);
	address=temp_buffer;
	for (i=0;i<M;i++){
		return_items=fread(address,sizeof(unsigned char), N, original_file);
		fseek(original_file,(4-N%4)%4,SEEK_CUR);
		address+=N;
	}

	for (i=0;i<M*N;i++)
		ori_pic[i]=temp_buffer[i];



	/*Reading the noisy image*/
	noisy_file=fopen(name2,"rb");
	if (noisy_file==NULL)
		exit(-1);
	/*first two bytes show that this is a bitmap*/
	return_items=fread(&return_short,sizeof(short),1,noisy_file);
	if ( (return_items!=1) || ( ((unsigned short)return_short!=('M'<<8)+'B') && ( (unsigned short)return_short!=('A'<<8)+'B') )){
		printf("Not a bitmap file");
		exit(-1);
	}
	/*Get Data offset*/
	return_items=fseek(noisy_file,0x0A,SEEK_SET);
	return_items=fread(&return_int,sizeof(long),1,noisy_file);
	offset=return_int;
	/*Get dimensions*/
	return_items=fseek(noisy_file,0x12,SEEK_SET);
	return_items=fread(&return_int,sizeof(long),1,noisy_file);
	if  (return_items!=1)
		exit(-1);
	if (return_int!=N){
		printf("The images must have the same size\n");
		exit(-1);
	}
	return_items=fread(&return_int,sizeof(long),1,noisy_file);
	if(return_items!=1)
		exit(-1);
	if (return_int!=M){
		printf("The images must have the same size\n");
		exit(-1);
	}
	/*Bit DEPTH - IS ti grayscale?*/
	return_items=fseek(noisy_file,0x1C,SEEK_SET);
	return_items=fread(&return_short,sizeof(short),1,noisy_file);
	if((return_items!=1) || (return_short!=8))
		exit(-1);
	/*Compressed?*/
	return_items=fread(&return_int,sizeof(long),1,noisy_file);
	if((return_items!=1) || (return_int!=0))
		exit(-1);


	/*read the pixel values*/
	return_items=fseek(noisy_file,offset,SEEK_SET);
	address=temp_buffer;
	for (i=0;i<M;i++){
		return_items=fread(address,sizeof(unsigned char), N, noisy_file);
		if (return_items!=N)
			exit(-1);
		fseek(noisy_file,(4-N%4)%4,SEEK_CUR);
		address+=N;
	}

	for (i=0;i<M*N;i++)
		pic[i]=temp_buffer[i];


	n=5;
	m=n;
	region=(double*)malloc(n*n*sizeof(double));
	new_region=(double*)malloc(n*n*sizeof(double));
	noisy_region=malloc(n*n*sizeof(double));
	f_opt=malloc(n*n*sizeof(double));

	x=(double *)malloc(n*sizeof(double));
	/*x=0:1/(n-1):1;*/
	for(i=0;i<n;i++)
		x[i]=(double)i/(n-1);
	y=(double*)malloc(m*sizeof(double));
	/*y=0:1/(m-1):1;*/
	for(i=0;i<n;i++)
		y[i]=(double)i/(n-1);
	/*Kappa=zeros(n,m,n,m);*/
	Kappa1=(double*)malloc(n*n*n*n*sizeof(double));
	zero_filling(Kappa1,n*n*n*n);

	sigma=sigma_min;
	lambda=lambda_min;
	/*construct Kappa matrix (kernel)*/
	for(j0=0;j0<n;j0++)
		for(i0=0;i0<n;i0++)
			for(j1=0;j1<n;j1++)
				for(i1=0;i1<n;i1++)
					Kappa1[j0*n*n*n + i0*n*n + j1*n + i1]=exp( -( (x[i0]-x[i1])*(x[i0]-x[i1]) +(y[j0]-y[j1])*(y[j0]-y[j1]) ) / (2*sigma*sigma));

	percentage=0;
	print=0;

	i=6;
	j=6;

	i=32;
	j=62;

	i=273;
	j=234;

	/*noisy region*/
	for (k=1;k<=n;k++)
		for(l=1;l<=n;l++)
			noisy_region[(k-1)*n+l-1]=pic[(i-(n-1)/2 -1 + k-1)*N + j-(n-1)/2 -1 + l-1];

	/*original region*/
	for (k=1;k<=n;k++)
		for(l=1;l<=n;l++)
			region[(k-1)*n+l-1]=ori_pic[(i-(n-1)/2 -1 + k-1)*N + j-(n-1)/2 -1 + l-1];

	for (k=1;k<=n;k++)
		for(l=1;l<=n;l++)
			noisy_region[(k-1)*n+l-1]=region[(k-1)*n+l-1];
	noisy_region[0]-=100;
	noisy_region[10]+=100;
	noisy_region[16]-=100;

	mu=0.1;
	mu2=0.1;
	v=1;
	psi=psi_vector[v];
	gamma=gamma_vector[v];
	N_psi=N_psi_vector[v];
	psi_used=(short*)malloc(N_psi*sizeof(short));
	b_psi=(double*)malloc(N_psi*sizeof(double));
	constant_filling_short(psi_used,1,N_psi);
	how_many_steps=proj_optim_l1_l2_norm_semi_parametric(f_opt,new_region, h, b_psi, psi, gamma, psi_used, noisy_region, (double*)Kappa1, n, N_psi, lambda, b0,  b1, epsilon0, mu, mu2,0.001,0.5);
	grad=PSNR_calc(region,new_region,5*5,255);
	printf("end");
}