/*
 * Copyright (c) 1994 by Lieven Vandenberghe and Stephen Boyd. Permission to
 * use, copy, modify, and distribute this software for any purpose without
 * fee is hereby granted, provided that this entire notice is included in all
 * copies of any software which is or includes a copy or modification of this
 * software and in all copies of the supporting documentation for such
 * software. This software is being provided "as is", without any express or
 * implied warranty.  In particular, the authors do not make any
 * representation or warranty of any kind concerning the merchantability of
 * this software or its fitness for any particular purpose.
 */

#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#ifdef userusage
#include <sys/resource.h>
#else
#include <sys/times.h>
#endif

#include "sp.h"
#include "mex.h"

#ifndef CLK_TCK
#define CLK_TCK 60
#endif

#ifdef __STDC__
void 
mexFunction(
	    int nlhs, Matrix * plhs[],
	    int nrhs, Matrix * prhs[]
)
#else
mexFunction(nlhs, plhs, nrhs, prhs)
	int             nlhs;
	Matrix         *plhs[];
	int             nrhs;
	Matrix         *prhs[];
#endif


/*
 * [x,Z,ul,infostr,time] = ...
 * sp(F,blck_szs,c,x0,Z0,nu,abstol,reltol,tv,maxiters);
 * 
 * Solves semidefinite program
 * 
 * minimize    c'*x subject to  F_0 + x_1*F_1 + ... + x_m*F_m  >= 0
 * 
 * and its dual
 * 
 * maximize    -Tr F_0 Z subject to  Tr F_i Z = c_i, i=1,...,m Z >= 0
 * 
 * Convergence criterion: (1) maxiters is exceeded (2) duality gap is less than
 * abstol (3) primal and dual objective are both positive and duality gap is
 * less than (reltol * dual objective) or primal and dual objective are both
 * negative and duality gap is less than (reltol * minus the primal
 * objective) (4) reltol is negative and primal objective is less than tv (5)
 * reltol is negative and dual objective is greater than tv
 * 
 * 
 * Input arguments: - F:        matrix with m+1 columns, F = [ F_0^1(:)
 * F_1^1(:) ... F_m^1(:) ] [ F_0^2(:)  F_1^2(:) ... F_m^2(:) ] [ ...
 * ...     ...    ...   ] [ F_0^L(:)  F_1^L(:) ... F_m^L(:) ] F_i^j is the
 * jth diagonal block of F_i - blck_szs: L-vector with dimensions of the
 * diagonal blocks the jth block of F (F_i^j has dimension blck_szs(j) ) - c:
 * m-vector, primal objective - x0:       m-vector, must be strictly primal
 * feasible - Z0:       [ Z0^1(:); ... ; Z0^L(:) ], must be strictly dual
 * feasible - nu:       >= 1.0 - abstol:   absolute tolerance - reltol:
 * negative tolerance - tv:       target value - maxiters: max. no of
 * iterations (maxiters >= 0)
 * 
 * Output arguments: - x, Z:     last primal and dual iterates - ul[2]:    ul[0]
 * is c'*x;  ul[1] is -Tr F_0*Z - infostr:  'maxiters exceeded', 'target
 * reached', 'unachievable target', 'relative accuracy reached', 'absolute
 * accuracy reached', or 'error occurred in sp' - time:     [ user time
 * (sec.), system time (sec.), no of iters ]
 */


{
	int             n, L, m, i, j, k, pd_sz, up_sz, up_pos, p_pos,
	                pos2, max_n, *blck_szs, lngth, lwork, info, info2,
	                info3, iters, *iwork;
	double          abstol, reltol, tv, nu, *work, utime, stime;
	int             int1 = 1;
	static int      firstcall = 1;
#ifdef userusage
	struct rusage   stats;
#else
	struct tms      stats;
#endif

	if (firstcall) {
		fprintf(stdout, "\n This is the beta version of SP.\
  Copyright Vandenberghe and Boyd.\n");
		firstcall = 0;
	}
	if (nrhs != 10)
		mexErrMsgTxt("Ten input arguments required.\n");
	else if (nlhs != 5)
		mexErrMsgTxt("Five output arguments required.\n");

	if (MIN(mxGetM(prhs[1]), mxGetN(prhs[1])) != 1)
		mexErrMsgTxt("2nd input argument must be a vector.\n");
	L = MAX(mxGetM(prhs[1]), mxGetN(prhs[1]));


	blck_szs = (int *) mxCalloc(L, sizeof(int));
	for (i = 0; i < L; i++) {
		blck_szs[i] = (int) mxGetPr(prhs[1])[i];
		if (blck_szs[i] < 0)
			mexErrMsgTxt("Elements of blck_szs must be positive.\n");
	}

	/* various dimensions */
	for (i = 0, n = 0, pd_sz = 0, up_sz = 0, max_n = 0; i < L; i++) {

		/* n: dimension */
		n += blck_szs[i];

		/* pd_sz: length of F_i in packed storage  */
		pd_sz += (blck_szs[i] * (blck_szs[i] + 1)) / 2;

		/* up_sz: length of F_i in unpacked storage */
		up_sz += SQR(blck_szs[i]);

		/* max_n: maximum block dimension */
		max_n = MAX(max_n, blck_szs[i]);

	}


	/* read F (unpacked storage); m = number of columns minus one */
	m = mxGetN(prhs[0]) - 1;
	if (mxGetM(prhs[0]) != up_sz)
		mexErrMsgTxt("Dimension of F does not match blck_szs.\n");


	/* pack F; loop over matrices F_j */
	for (j = 0, up_pos = 0, p_pos = 0; j < m + 1; j++)
		/*
		 * loop over blocks i=0,...,L-1 up_pos: position of (0,0)-elt
		 * of block i in unpacked storage
		 */
		for (i = 0; i < L; up_pos += SQR(blck_szs[i]), i++)
			/*
			 * loop over columns, k=0,...,blck_szs[i]-1 p_pos:
			 * position of (k,k)-element in packed storage
			 */
			for (k = 0, lngth = blck_szs[i]; k < blck_szs[i];
			     p_pos += lngth, lngth -= 1, k++)
				memmove(mxGetPr(prhs[0]) + p_pos,
					mxGetPr(prhs[0]) + up_pos + k * (blck_szs[i] + 1),
					lngth * sizeof(double));


	/* check dimension of c (3rd input argument) */
	if (MIN(mxGetM(prhs[2]), mxGetN(prhs[2])) != 1)
		mexErrMsgTxt("c must be a vector.\n");
	if (MAX(mxGetM(prhs[2]), mxGetN(prhs[2])) != m)
		mexErrMsgTxt("Dimensions of c and F do not match.\n");


	/* read x0 (4th input arg), copy in x (1st output arg) */
	if (MIN(mxGetM(prhs[3]), mxGetN(prhs[3])) != 1)
		mexErrMsgTxt("x0 must be a vector.\n");
	if (MAX(mxGetM(prhs[3]), mxGetN(prhs[3])) != m)
		mexErrMsgTxt("Dimensions of x0 and F do not match.\n");
	plhs[0] = mxCreateFull(m, 1, REAL);
	memcpy(mxGetPr(plhs[0]), mxGetPr(prhs[3]), m * sizeof(double));


	/* read Z0 (5th input arg), copy in Z (2nd output arg), packed stor. */
	if (mxGetN(prhs[4]) != 1)
		mexErrMsgTxt("Z0 must be a vector.\n");
	if (mxGetM(prhs[4]) != up_sz)
		mexErrMsgTxt("Dimension of Z0 does not match blck_szs.\n");
	plhs[1] = mxCreateFull(up_sz, 1, REAL);

	/*
	 * loop over blocks, i=0,...,L-1 up_pos: position of (0,0) element of
	 * block i in unpacked storage
	 */
	for (i = 0, up_pos = 0, p_pos = 0; i < L; up_pos += SQR(blck_szs[i]), i++)
		/*
		 * loop over columns, k=0,...,blck_szs[i]-1 p_pos: position
		 * of (k,k) element of block i in packed storage
		 */
		for (k = 0, lngth = blck_szs[i]; k < blck_szs[i];
		     p_pos += lngth, lngth -= 1, k++)
			memcpy(mxGetPr(plhs[1]) + p_pos,
			  mxGetPr(prhs[4]) + up_pos + k * (blck_szs[i] + 1),
			       lngth * sizeof(double));


	/* ul (third output arg) */
	plhs[2] = mxCreateFull(2, 1, REAL);


	/* read nu, abstol, reltol, tv, maxiters */
	nu = mxGetScalar(prhs[5]);
	abstol = mxGetScalar(prhs[6]);
	reltol = mxGetScalar(prhs[7]);
	tv = mxGetScalar(prhs[8]);
	iters = (int) mxGetScalar(prhs[9]);


	/*
	 * allocate work space
	 */

	lwork = (m + 2) * pd_sz + up_sz + 2 * n +
		MAX(3 * m, MAX(m + pd_sz * NB, 3 * max_n + max_n * (max_n + 1)));
	work = (double *) mxCalloc(lwork, sizeof(double));
	iwork = (int *) mxCalloc(m, sizeof(int));


	/*
	 * call sp
	 */

	/* initialize utime, stime */

#ifdef userusage
	info2 = getrusage(RUSAGE_SELF, &stats);
	utime = -(double) stats.ru_utime.tv_sec
		- ((double) stats.ru_utime.tv_usec) / 1e6;
	stime = -(double) stats.ru_stime.tv_sec
		- ((double) stats.ru_stime.tv_usec) / 1e6;
#else
	info2 = times(&stats);
	utime = -(double) stats.tms_utime / CLK_TCK;
	stime = -(double) stats.tms_stime / CLK_TCK;
#endif

	/* sp */
	info3 = sp(m, L, mxGetPr(prhs[0]), blck_szs, mxGetPr(prhs[2]),
		   mxGetPr(plhs[0]), mxGetPr(plhs[1]), mxGetPr(plhs[2]),
		 nu, abstol, reltol, tv, &iters, work, lwork, iwork, &info);


	/* update utime, stime */
#ifdef userusage
	info2 = getrusage(RUSAGE_SELF, &stats);
	utime += (double) stats.ru_utime.tv_sec
		+ ((double) stats.ru_utime.tv_usec) / 1e6;
	stime += (double) stats.ru_stime.tv_sec
		+ ((double) stats.ru_stime.tv_usec) / 1e6;
#else
	info2 = times(&stats);
	utime += (double) stats.tms_utime / CLK_TCK;
	stime += (double) stats.tms_stime / CLK_TCK;
#endif

	/*
	 * unpack Z
	 */

	/*
	 * loop over blocks i=L-1,...,0 up_pos: position of last element of
	 * block i in unpacked storage p_pos: position of last element of
	 * block i in packed storage
	 */
	for (i = L - 1, up_pos = up_sz - 1, p_pos = pd_sz - 1; i >= 0;
	     up_pos -= SQR(blck_szs[i]),
	     p_pos -= blck_szs[i] * (blck_szs[i] + 1) / 2, i--)
		/*
		 * loop over columns of blok i;  pos2 is position of
		 * (blck_szs[i]-k) x (blck_szs[i]-k) element of block i in
		 * packed storage
		 */
		for (k = 0, lngth = 1, pos2 = p_pos; k < blck_szs[i];
		     lngth += 1, pos2 -= lngth, k++)
			/* move subdiagonal part of column blck_szs[i]-k */
			memmove(mxGetPr(plhs[1]) + up_pos - k * (blck_szs[i] + 1),
			   mxGetPr(plhs[1]) + pos2, lngth * sizeof(double));


	/*
	 * loop over blocks i=0,...,L-1 up_pos: position of (0,0) element of
	 * block i
	 */
	for (i = 0, up_pos = 0; i < L; up_pos += SQR(blck_szs[i]), i++)
		/* loop over columns k=0,...,blck_szs[i]-1 */
		for (k = 0, lngth = blck_szs[i] - 1; k < blck_szs[i] - 1; lngth -= 1, k++)
			/*
			 * copy part of column k under diagonal to part of
			 * row k above the diagonal
			 */
			dcopy_(&lngth, mxGetPr(plhs[1]) + up_pos + k * (blck_szs[i] + 1) + 1,
			       &int1, mxGetPr(plhs[1]) + up_pos + (k + 1) * blck_szs[i] + k,
			       blck_szs + i);


	/*
	 * unpack F again
	 */

	/* loop over columns j=m,...,0  */
	for (j = m; j >= 0; j--) {

		/*
		 * loop over blocks i=L-1, ..., 0 up_pos = position of last
		 * element of block i in unpacked storage p_pos = position of
		 * last element of block i in packed storage
		 */
		for (i = L - 1, up_pos = (j + 1) * up_sz - 1, p_pos = (j + 1) * pd_sz - 1; i >= 0;
		     up_pos -= SQR(blck_szs[i]),
		     p_pos -= blck_szs[i] * (blck_szs[i] + 1) / 2, i--)
			/*
			 * loop over columns k=blck_szs[i]-1, ..., 0;  pos2
			 * is position of elt (blck_szs[i]-k) x
			 * (blck_szs[i]-k) of block i in packed storage
			 */
			for (k = 0, lngth = 1, pos2 = p_pos; k < blck_szs[i];
			     lngth += 1, pos2 -= lngth, k++)
				/*
				 * move subdiagonal part of column
				 * blck_szs[i]-k
				 */
				memmove(mxGetPr(prhs[0]) + up_pos - k * (blck_szs[i] + 1),
					mxGetPr(prhs[0]) + pos2, lngth * sizeof(double));

		/*
		 * loop over blocks i=0,..,L-1 up_pos: position of (0,0)
		 * element of block i
		 */
		for (i = 0, up_pos = j * up_sz; i < L; up_pos += SQR(blck_szs[i]), i++)
			/* loop over columns k=0,...,blck_szs[i]-1 */
			for (k = 0, lngth = blck_szs[i] - 1; k < blck_szs[i] - 1; lngth -= 1, k++)
				/*
				 * copy part of column k under diagonal to
				 * part of row k above the diagonal
				 */
				dcopy_(&lngth,
				       mxGetPr(prhs[0]) + up_pos + k * (blck_szs[i] + 1) + 1,
				       &int1,
				       mxGetPr(prhs[0]) + up_pos + (k + 1) * blck_szs[i] + k,
				       blck_szs + i);

	}


	/*
	 * infostr
	 */

	switch (info) {
	case 1:
		plhs[3] = mxCreateString("maxiters exceeded");
		break;
	case 2:
		plhs[3] = mxCreateString("absolute tolerance reached");
		break;
	case 3:
		plhs[3] = mxCreateString("relative tolerance reached");
		break;
	case 4:
		plhs[3] = mxCreateString("target value reached");
		break;
	case 5:
		plhs[3] = mxCreateString("target value unachievable");
		break;
	default:
		plhs[3] = mxCreateString("error occurred in sp");
	}


	/*
	 * time
	 */

	plhs[4] = mxCreateFull(1, 3, REAL);
	*mxGetPr(plhs[4]) = utime;
	*(mxGetPr(plhs[4]) + 1) = stime;
	*(mxGetPr(plhs[4]) + 2) = (double) iters;

	mxFree(blck_szs);
	mxFree(work);
	mxFree(iwork);

	if (info3)
		mexErrMsgTxt("Error in sp.\n");

}