#include "mex.h"
#include "loqo.h"
#include "myalloc.h"
#include <math.h>
#include <stdio.h>

void cleanup( void );

/* double _HUGE=1.0e+300; */

int mloqo(
    int m, int n, 
    double c[], 
    double A[], int iA[], int kA[], 
    double b[], 
    double Q[], int iQ[], int kQ[],
    double l[], double u[],
    double x0[],
    double x[], double y[], 
    int neq, int verbose,
	int loqo_interlim, int loqo_sigfig, double loqo_timlim, int loqo_order,
	int loqo_perm, double loqo_epsnum, double loqo_epssol, double loqo_inftol)
{
    int i, j, k, nz, qnz;
    int status;
    static LOQO *lp=NULL;
    double *r=NULL;

    if (kA != NULL) {nz  = kA[n];} else { nz = 0;}
    if (kQ != NULL) {qnz = kQ[n];} else {qnz = 0;}

    if (kA == NULL) { CALLOC(kA, n+1, int); }

    if (lp == NULL) {lp = openlp();}
    lp->m   = m;
    lp->n   = n;
    lp->nz  = nz;
    lp->qnz = qnz;
    lp->c  = c;
    lp->A  = A;
    lp->iA = iA;
    lp->kA = kA;
    lp->b  = b;
    lp->Q  = Q;
    lp->iQ = iQ;
    lp->kQ = kQ;
    lp->l  = l;
    lp->u  = u;
    lp->verbose = verbose;
	lp->itnlim = loqo_interlim;
	lp->sf_req = loqo_sigfig;
	lp->timlim = loqo_timlim;
	lp->pdf = loqo_order;
	lp->epsnum = loqo_epsnum;
	lp->inftol = loqo_inftol;
	lp->method = loqo_perm;
	lp->epssol = loqo_epssol;
    lp->mufactor = 0.0;

    if (x0 != NULL) {
	MALLOC( lp->x, n, double );
	for (j=0; j<n; j++) lp->x[j] = x0[j];
    } else {
	FREE( lp->x );
	FREE( lp->y );
    }

    MALLOC(lp->r, m, double); r = lp->r;
    for (i=0; i<neq; i++) r[i] = 0;
    for (   ; i<m;   i++) r[i] = HUGE_VAL;

    if (l == NULL) {
	    MALLOC(lp->l,n,double); l = lp->l;
	    for (j=0; j<n; j++) l[j] = -HUGE_VAL;
    }

    for (k=0; k< nz; k++) A[k] *= -1;
    for (i=0; i<m;   i++) b[i] *= -1;

    lp->honor_bnds = 0;
    lp->pred_corr = 1; 
    lp->mufactor = 0;  

    status = solvelp(lp);

    for (k=0; k< nz; k++) A[k] *= -1;
    for (i=0; i<m;   i++) b[i] *= -1;

    if (x != NULL) for (j=0; j<n; j++) x[j] = lp->x[j];
    if (y != NULL) for (i=0; i<m; i++) y[i] = lp->y[i];

    inv_clo();

    return status;
}

void mexFunction(
    int nlhs, mxArray *plhs[],
    int nrhs, const mxArray *prhs[]
)
{
    double *c=NULL, *b=NULL, *A=NULL, *Q=NULL, 
	   *l=NULL, *u=NULL, *x=NULL, *y=NULL, *x0=NULL;
    int *iA=NULL, *kA=NULL, *iQ=NULL, *kQ=NULL;
    int m=0, n=0, numParams;
	double* prParams = NULL;
	unsigned int neq=0, verbose=0, status;
	static char *str[] = {
	"OPTIMAL SOLUTION FOUND\n",
	"SUBOPTIMAL SOLUTION FOUND\n",
	"ITERATION LIMIT\n",
	"PRIMAL INFEASIBLE\n",
	"DUAL INFEASIBLE\n",
	"PRIMAL and/or DUAL INFEASIBLE\n",
	"PRIMAL and/or DUAL INFEASIBLE (INCONSISTENT EQUATIONS)\n",
	"PRIMAL UNBOUNDED\n",
	"DUAL UNBOUNDED\n",
	"TIME LIMIT\n"
    };
	/* loqo default parameters */
	unsigned int loqo_interlim=200, loqo_sigfig=6, loqo_order=UNSET, loqo_perm=1;
	double loqo_timlim=HUGE_VAL, loqo_epsnum=0.0f, loqo_epssol=1.0e-6, loqo_inftol=1.0e-5;


    if (nrhs > 10 || nrhs < 1) {
	    mexErrMsgTxt("Usage: [x,lambda,how] "
		         "= loqolpqp(H,c,A,b,l,u,x0,neqcstr,display,paramlist)");
	    return;
    }
    switch (nrhs) {
	case 10:
		if (mxGetM(prhs[9]) != 0 || mxGetN(prhs[9]) != 0) {
			/* process loqo parameters */

			numParams = MAX(mxGetM(prhs[9]), mxGetN(prhs[9]));
			prParams = mxGetPr(prhs[9]);
		
			if (numParams > 8){
				mexErrMsgTxt("Parameter list usage: [interlim,sigfig,timlim,order,perm,epsnum,epssol,inftol]");
				return;	
			}
			switch (numParams) {
			case 8:
				if (!mxIsNaN(prParams[7])){
					if (prParams[7] < 0){
						mexErrMsgTxt("Eighth parameter (inftol) must be "
							"a positive real scalar.");
						return;
					}
					loqo_inftol = (double) prParams[7];
				}
			case 7:
				if (!mxIsNaN(prParams[6])){
					if (prParams[6] < 0){
						mexErrMsgTxt("Seventh parameter (epssol) must be "
							"a positive real scalar.");
						return;
					}
					loqo_epssol = (double) prParams[6];
				}
			case 6:
				if (!mxIsNaN(prParams[5])){
					if (prParams[5] < 0){
						mexErrMsgTxt("Sixth parameter (epsnum) must be "
							"a positive real scalar.");
						return;
					}
					loqo_epsnum = (double) prParams[5];
				}			
			case 5:
				if (!mxIsNaN(prParams[4])){
					if ( (prParams[4] < 0)
					|| (prParams[4] > 2) ){
						mexErrMsgTxt("Fifth parameter (perm) must be "
							"0 (MINLOCFILL), 1 (MINDEG), 2 (NOREORD).");
						return;
					}
					loqo_perm = (unsigned int) prParams[4];
				}
			case 4:
				if (!mxIsNaN(prParams[3])){
					if ( (prParams[3] < 0)
					|| (prParams[3] > 2) ){
						mexErrMsgTxt("Fourth parameter (order) must be "
							"0 (UNSET), 1 (PRIMAL), 2 (DUAL).");
						return;
					}
					loqo_order = (unsigned int) prParams[3];
				}
			case 3:
				if (!mxIsNaN(prParams[2])){
					if (prParams[2] < 0){
						mexErrMsgTxt("Third parameter (timlim) must be "
							"a positive real scalar.");
						return;
					}
					loqo_timlim = (double) prParams[2];
				}			
			case 2:
				if (!mxIsNaN(prParams[1])){
					if (prParams[1] < 0){
						mexErrMsgTxt("Second parameter (sigfig) must be "
							"a positive integer scalar.");
						return;
					}
					loqo_sigfig = (unsigned int) prParams[1];
				}			
			case 1:
				if (!mxIsNaN(prParams[0])){
					if (prParams[0] < 0){
						mexErrMsgTxt("First parameter (interlim) must be "
							"a positive integer scalar.");
						return;
					}
					loqo_interlim = (unsigned int) prParams[0];
				}
			break;
			}
		}
    case 9:
	    if (mxGetM(prhs[8]) != 0 || mxGetN(prhs[8]) != 0) {
		    if (!mxIsNumeric(prhs[8]) || mxIsComplex(prhs[8]) 
		     ||  mxIsSparse(prhs[8])
		     || !(mxGetM(prhs[8])==1 && mxGetN(prhs[8])==1)) {
			 mexErrMsgTxt("Ninth argument (display) must be "
				      "an integer scalar.");
			 return;
		    }
		    verbose = (unsigned int) *mxGetPr(prhs[8]);
	    }
    case 8:
	    if (mxGetM(prhs[7]) != 0 || mxGetN(prhs[7]) != 0) {
		    if (!mxIsNumeric(prhs[7]) || mxIsComplex(prhs[7]) 
		     ||  mxIsSparse(prhs[7])
		     || !(mxGetM(prhs[7])==1 && mxGetN(prhs[7])==1)) {
			 mexErrMsgTxt("Eighth argument (neqcstr) must be "
				      "an integer scalar.");
			 return;
		    }
		    neq = (unsigned int) *mxGetPr(prhs[7]);
	    }
    case 7:
	    if (mxGetM(prhs[6]) != 0 || mxGetN(prhs[6]) != 0) {
		    if (!mxIsNumeric(prhs[6]) || mxIsComplex(prhs[6]) 
		     ||  mxIsSparse(prhs[6])
		     || !mxIsDouble(prhs[6]) 
		     ||  mxGetN(prhs[6])!=1 ) {
			 mexErrMsgTxt("Seventh argument (x0) must be "
				      "a column vector.");
			 return;
		    }
		    x0 = mxGetPr(prhs[6]);
		    n  = mxGetM(prhs[6]);
	    }
    case 6:
	    if (mxGetM(prhs[5]) != 0 || mxGetN(prhs[5]) != 0) {
		    if (!mxIsNumeric(prhs[5]) || mxIsComplex(prhs[5]) 
		     ||  mxIsSparse(prhs[5])
		     || !mxIsDouble(prhs[5]) 
		     ||  mxGetN(prhs[5])!=1 ) {
			 mexErrMsgTxt("Sixth argument (u) must be "
				      "a column vector.");
			 return;
		    }
		    if (n != 0 && n != mxGetM(prhs[5])) {
			 mexErrMsgTxt("Dimension error (arg 6 and later).");
			 return;
		    }
		    u = mxGetPr(prhs[5]);
		    n = mxGetM(prhs[5]);
	    }
    case 5:
	    if (mxGetM(prhs[4]) != 0 || mxGetN(prhs[4]) != 0) {
		    if (!mxIsNumeric(prhs[4]) || mxIsComplex(prhs[4]) 
		     ||  mxIsSparse(prhs[4])
		     || !mxIsDouble(prhs[4]) 
		     ||  mxGetN(prhs[4])!=1 ) {
			 mexErrMsgTxt("Fifth argument (l) must be "
				      "a column vector.");
			 return;
		    }
		    if (n != 0 && n != mxGetM(prhs[4])) {
			 mexErrMsgTxt("Dimension error (arg 5 and later).");
			 return;
		    }
		    l = mxGetPr(prhs[4]);
		    n = mxGetM(prhs[4]);
	    }
    case 4:
	    if (mxGetM(prhs[3]) != 0 || mxGetN(prhs[3]) != 0) {
		    if (!mxIsNumeric(prhs[3]) || mxIsComplex(prhs[3]) 
		     ||  mxIsSparse(prhs[3])
		     || !mxIsDouble(prhs[3]) 
		     ||  mxGetN(prhs[3])!=1 ) {
			 mexErrMsgTxt("Fourth argument (b) must be "
				      "a column vector.");
			 return;
		    }
		    if (m != 0 && m != mxGetM(prhs[3])) {
			 mexErrMsgTxt("Dimension error (arg 4 and later).");
			 return;
		    }
		    b = mxGetPr(prhs[3]);
		    m = mxGetM(prhs[3]);
	    }
    case 3:
	    if (mxGetM(prhs[2]) != 0 || mxGetN(prhs[2]) != 0) {
		    if (!mxIsNumeric(prhs[2]) || mxIsComplex(prhs[2]) 
		     || !mxIsSparse(prhs[2]) ) {
			 mexErrMsgTxt("Third argument (A) must be "
				      "a sparse matrix.");
			 return;
		    }
		    if (m != 0 && m != mxGetM(prhs[2])) {
			 mexErrMsgTxt("Dimension error (arg 3 and later).");
			 return;
		    }
		    if (n != 0 && n != mxGetN(prhs[2])) {
			 mexErrMsgTxt("Dimension error (arg 3 and later).");
			 return;
		    }
		    m = mxGetM(prhs[2]);
		    n = mxGetN(prhs[2]);

		    A = mxGetPr(prhs[2]);
		    iA = mxGetIr(prhs[2]);
		    kA = mxGetJc(prhs[2]);
	    }
    case 2:
	    if (mxGetM(prhs[1]) != 0 || mxGetN(prhs[1]) != 0) {
		    if (!mxIsNumeric(prhs[1]) || mxIsComplex(prhs[1]) 
		     ||  mxIsSparse(prhs[1])
		     || !mxIsDouble(prhs[1]) 
		     ||  mxGetN(prhs[1])!=1 ) {
			 mexErrMsgTxt("Second argument (c) must be "
				      "a column vector.");
			 return;
		    }
		    if (n != 0 && n != mxGetM(prhs[1])) {
			 mexErrMsgTxt("Dimension error (arg 2 and later).");
			 return;
		    }
		    c = mxGetPr(prhs[1]);
		    n = mxGetM(prhs[1]);
	    }
    case 1:
	    if (mxGetM(prhs[0]) != 0 || mxGetN(prhs[0]) != 0) {
		    if (!mxIsNumeric(prhs[0]) || mxIsComplex(prhs[0]) 
		     || !mxIsSparse(prhs[0]) ) {
			 mexErrMsgTxt("First argument (H) must be "
				      "a sparse matrix.");
			 return;
		    }
		    if (n != 0 && n != mxGetM(prhs[0])) {
			 mexErrMsgTxt("Dimension error (arg 1 and later).");
			 return;
		    }
		    if (n != 0 && n != mxGetN(prhs[0])) {
			 mexErrMsgTxt("Dimension error (arg 1 and later).");
			 return;
		    }
		    n  = mxGetN(prhs[0]);
		    Q = mxGetPr(prhs[0]);
		    iQ = mxGetIr(prhs[0]);
		    kQ = mxGetJc(prhs[0]);
	    }
	    break;
    }
    if (nlhs > 3 || nlhs < 1) {
	    mexErrMsgTxt("Usage: [x,lambda,how] "
		         "= loqolpqp(H,c,A,b,l,u,x0,neqcstr,display)");
	    return;
    }

    switch (nlhs) {
    case 3:
    case 2:
	    plhs[1] = mxCreateDoubleMatrix(m, 1, mxREAL);
	    y = mxGetPr(plhs[1]);
    case 1:
	    plhs[0] = mxCreateDoubleMatrix(n, 1, mxREAL);
	    x = mxGetPr(plhs[0]);
	    break;
    }

    status = mloqo(m, n, c, A, iA, kA, b, Q, iQ, kQ, l, u, x0, x, y, neq, verbose,
		loqo_interlim, loqo_sigfig, loqo_timlim, loqo_order, loqo_perm, loqo_epsnum, loqo_epssol, loqo_inftol);

    if (nlhs == 3) {
	    plhs[2] = mxCreateString(str[status]);
    }

        mexAtExit(cleanup);
}

void cleanup(void) {
    mexPrintf("MEX-file is terminating, destroying array\n");
}