#include "mex.h"
#include "math.h"

// compute the absolute maximum
double absmax(double *gradvec, int n, int *gradindex)
{
	double maxvalue;
	int i;
	maxvalue  = fabs(gradvec[0]);
	*gradindex = 0;
	for (i=1;i<n;i++)
	{
		if (fabs(gradvec[i]) > maxvalue)
		{
			maxvalue  = fabs(gradvec[i]);
			*gradindex = i;
		}
	}
	return maxvalue;
};

// dot multiplication
double dot(int p, int q, int dim,double *psamples)
{
	double sum = 0;
	int i, count1, count2;
	count1 = p*dim;
	count2 = q*dim;

	for (i=0; i< dim; i++)
	{
		sum += *(psamples + count1 + i) * (*(psamples + count2 + i));
	}
	return sum;
};

// Gaussian Kernel evaluation
double kernel(double kernelparam, int p,int q, int dim, double *psamples, double *square)
{
	double sum;
	sum = dot(p, q, dim, psamples);
	return exp(-kernelparam*(square[p] + square[q] - 2*sum));
};

void Rank1Update(double *R,double *beta, double gamma, int cnum, int n)
{
    int i,j,count=0;
    for(i=0;i<n;i++)
    {
        for(j=0;j<n;j++)
        {
            *(R+i*cnum+j) = *(R+i*cnum+j) + *(beta+i) * *(beta+j)*gamma; 
         }
      }
	for(i=0;i<n;i++)
	{
		*(R+i*cnum + n) = -gamma* *(beta+i);
	}
	for(i=0;i<n;i++)
	{
		*(R + n*cnum+i) = -gamma* *(beta+i);
	}
	*(R + n*cnum + n) = gamma;
};

void mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	double *psamples,*plabels, *square, *talpha, *alpha, *sv, *gradvec, *Kmatrix, *invK, *beta,*nsv,*bias;
	double param,lamda,tol,gamma,gradmax, tmp, tmp1;
	int *svi, *gradindex, n, d, cnum, i, j, p, q;
	
	psamples	= mxGetPr(prhs[0]);
	plabels		= mxGetPr(prhs[1]);
	param		= mxGetScalar(prhs[3]);
	lamda		= mxGetScalar(prhs[4]);
	tol			= mxGetScalar(prhs[5]);
	d			= mxGetM(prhs[0]);
	n			= mxGetN(prhs[0]);
	lamda		= (1.0/2.0)/lamda;
    
	cnum		= 6000;
    square		= mxCalloc(n, sizeof(double));
	gradvec		= mxCalloc(n,sizeof(double));
	talpha		= mxCalloc(n+1,sizeof(double));
	Kmatrix		= mxCalloc(n*cnum,sizeof(double));
	invK		= mxCalloc(cnum*cnum,sizeof(double));
	beta		= mxCalloc(n+1,sizeof(double));
    gradindex   = mxCalloc(1, sizeof(int));
	svi			= mxCalloc(n,sizeof(int));

    for (i=0; i<n; i++)
		square[i]	= dot(i, i, d, psamples);
    for (i=0;i<n;i++)
	{
		gradvec[i]	= -plabels[i];
	}
	gradmax = absmax(gradvec, n, gradindex);
	for (i=0;i<n;i++)
	{
		if (gradmax < tol)
		{
			break;
		}
		if (i == 0)
		{
			for(j=0;j<n;j++)
			{
				Kmatrix[j*cnum+i] = kernel(param,j,*gradindex,d,psamples,square);
			}
			Kmatrix[*gradindex*cnum + i] = Kmatrix[*gradindex*cnum + i] + lamda;
			invK[0] = -1*Kmatrix[*gradindex*cnum + i];
			invK[1] = 1;
			invK[cnum] = 1;
			invK[cnum+1] = 0;
			talpha[0] = invK[1]*plabels[*gradindex];
			talpha[1] = invK[cnum+1]*plabels[*gradindex];
			svi[i]    = *gradindex;
		}
		else
		{
			for(j=0;j<n;j++)
			{
				Kmatrix[j*cnum + i] = kernel(param,j,*gradindex,d,psamples,square);
			}
			Kmatrix[*gradindex*cnum + i] = Kmatrix[*gradindex*cnum + i] + lamda;

			for (p=0;p<=i;p++)
			{
				beta[p] = invK[p*cnum];
				for(q=1;q<=i;q++)
				{
					beta[p] = beta[p]+invK[p*cnum+q]*Kmatrix[svi[q-1]*cnum + i];
				}
			}
			tmp = beta[0];
			for (q=1;q<=i;q++)
			{
				tmp = tmp + beta[q]*Kmatrix[svi[q-1]*cnum + i];
			}
			gamma = 1/(Kmatrix[*gradindex*cnum + i] - tmp);
			Rank1Update(invK,beta,gamma,cnum,i+1);
			tmp1 = 0;
			for (q=1;q<=i;q++)
			{
				tmp1 = tmp1 + beta[q]*plabels[svi[q-1]];
			}
			tmp1 = gamma*(tmp1 - plabels[*gradindex]);
			svi[i] = *gradindex;
			for (q=0;q<=i;q++)
			{
				talpha[q] = talpha[q] + tmp1*beta[q];
			}
			talpha[i+1] = talpha[i+1] - tmp1;
		}

		for (p=0;p<n;p++)
		{
			gradvec[p] = talpha[0] - plabels[p];
			for (q=1;q<=i+1;q++)
			{
				gradvec[p] = gradvec[p] + talpha[q]*Kmatrix[p*cnum + q-1];
			}
		}
		for (p=0;p<i;p++)
			gradvec[svi[p]] = 0;

		gradmax = absmax(gradvec, n, gradindex);

	}

	plhs[0]		= mxCreateDoubleMatrix(i,1,mxREAL);
	plhs[1]		= mxCreateDoubleMatrix(i,1,mxREAL);
	plhs[2]		= mxCreateDoubleMatrix(1,1,mxREAL);
	plhs[3]		= mxCreateDoubleMatrix(1,1,mxREAL);
	alpha		= mxGetPr(plhs[0]);
	sv			= mxGetPr(plhs[1]);
	nsv			= mxGetPr(plhs[2]);
	bias		= mxGetPr(plhs[3]);
	for (q=1;q<=i;q++)
	{
		alpha[q-1] = talpha[q];
		sv[q-1]	 = svi[q-1]+1;
	}	
	nsv[0]		= i;
	bias[0]		= talpha[0];
	return;
}