/*  mex_getInput.c
  
  MEX interface to mpt_getInput
  
  Usage:
    [U, region] = mex_getInput(mpt_mexData(ctrl), X)
 
 'region' will be 0 if no feasible control action is found for state X
    
  WARNING: * only generates closed-loop control action!
           * value of absolute tolerance is hard-coded!
*/

/* Copyright (C) 2005 by Michal Kvasnica (kvasnica@control.ee.ethz.ch) */

/*  This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/ 

#define MPT_ABSTOL 1e-7

#include "mex.h"

double sub_getInput(double *, double *, const double *, const double *, const double *, const double *, const double *, const int , const int , const long);

void mexFunction(int nlhs, mxArray * plhs[], int nrhs, const mxArray * prhs[])
{
    int nr, nc;
    mxArray * field;
    int MPT_NX, MPT_NU;
    double * MPT_H, * MPT_K, * MPT_F, * MPT_G, * MPT_NC;
    long MPT_NR;
    double *X, *U, *region;
    char buf[100];

    if (nrhs < 1) {
        mexErrMsgTxt("Not enough input arguments!");
    }
    if (nlhs > 2) {
        mexErrMsgTxt("Too many output arguments!");
    }
    
    if (!mxIsStruct(prhs[0])) {
        /* first input argument must be a structure */
        mexErrMsgTxt("First input must be a structure!");
    }
    
    field = mxGetField(prhs[0], 0, "nx");
    if (field==NULL) {
        mexErrMsgTxt("First input must be a structure generated by 'mpt_mexData' !");
    }
    MPT_NX = (int)mxGetPr(field)[0];
    
    field = mxGetField(prhs[0], 0, "nu");
    if (field==NULL) {
        mexErrMsgTxt("First input must be a structure generated by 'mpt_mexData' !");
    }
    MPT_NU = (int)mxGetPr(field)[0];
    
    nr = mxGetM(prhs[1]);
    nc = mxGetN(prhs[1]);
    if ((nr != MPT_NX) || (nc != 1)) {
        sprintf(buf, "Input must be a %dx1 vector, you provided a %dx%d vector!", MPT_NX, nr, nc);
        mexErrMsgTxt(buf);
        return;
    }

    field = mxGetField(prhs[0], 0, "H");
    if (field==NULL) {
        mexErrMsgTxt("First input must be a structure generated by 'mpt_mexData' !");
    }
    MPT_H = (double *)mxGetPr(field);
    
    field = mxGetField(prhs[0], 0, "K");
    if (field==NULL) {
        mexErrMsgTxt("First input must be a structure generated by 'mpt_mexData' !");
    }
    MPT_K = (double *)mxGetPr(field);
    
    field = mxGetField(prhs[0], 0, "F");
    if (field==NULL) {
        mexErrMsgTxt("First input must be a structure generated by 'mpt_mexData' !");
    }
    MPT_F = (double *)mxGetPr(field);
    
    field = mxGetField(prhs[0], 0, "G");
    if (field==NULL) {
        mexErrMsgTxt("First input must be a structure generated by 'mpt_mexData' !");
    }
    MPT_G = (double *)mxGetPr(field);
    
    field = mxGetField(prhs[0], 0, "nc");
    if (field==NULL) {
        mexErrMsgTxt("First input must be a structure generated by 'mpt_mexData' !");
    }
    MPT_NC = (double *)mxGetPr(field);
    
    field = mxGetField(prhs[0], 0, "nr");
    if (field==NULL) {
        mexErrMsgTxt("First input must be a structure generated by 'mpt_mexData' !");
    }
    MPT_NR = (long)mxGetPr(field)[0];
    
    /* 1st output is the control input */
    plhs[0] = mxCreateDoubleMatrix(MPT_NU, 1, mxREAL);
    
    /* 2nd output is index of region which contains a given state
     * (will return 0 if no region was found)
     */
    plhs[1] = mxCreateDoubleMatrix(1, 1, mxREAL);

    X = mxGetPr(prhs[1]);
    U = mxGetPr(plhs[0]);
    region = mxGetPr(plhs[1]);

    /* compute control law for state X and return index of active region */
    *region = sub_getInput(X, U, MPT_H, MPT_K, MPT_F, MPT_G, MPT_NC, MPT_NX, MPT_NU, MPT_NR);

    return;
}

double sub_getInput(double *X, double *U, const double *MPT_H, const double *MPT_K, const double *MPT_F, const double *MPT_G, const double *MPT_NC, const int MPT_NX, const int MPT_NU, const long MPT_NR)
{
    int ix, iu, ic, nc, isinside;
    unsigned long ireg, abspos;
    double hx, region;
    #ifdef MPT_DEBUG_2
    double diff;
    #endif
    
    abspos = 0;
    region = 0;
    
    /* initialize U to zero*/
    for (iu=0; iu<MPT_NU; iu++) {
        U[iu] = 0;
    }
    
    for (ireg=0; ireg<MPT_NR; ireg++) {
        #ifdef MPT_DEBUG_2
        printf("\tChecking region: %d\n", ireg+1);
        #endif
        
        isinside = 1;
        nc = MPT_NC[ireg];
        for (ic=0; ic<nc; ic++) {
            hx = 0;
            for (ix=0; ix<MPT_NX; ix++) {
                hx = hx + MPT_H[abspos*MPT_NX+ic*MPT_NX+ix]*X[ix];
            }
            if ((hx - MPT_K[abspos+ic]) > MPT_ABSTOL) {
                /* constraint is violated, continue with next region */
                #ifdef MPT_DEBUG_2
                diff = hx - MPT_K[abspos+ic];
                printf("\t\tConstraint %d not satisfied with difference %e, abs_tol %e\n", ic + 1, diff, MPT_ABSTOL);
                #endif
                isinside = 0;
                break;
            } 
        }
        if (isinside==1) {
            /* state belongs to this region, extract control law and exit */
            region = ireg + 1;
            for (iu=0; iu<MPT_NU; iu++) {
                for (ix=0; ix<MPT_NX; ix++) {
                    U[iu] = U[iu] + MPT_F[ireg*MPT_NX*MPT_NU + iu*MPT_NX + ix]*X[ix];
                }
                U[iu] = U[iu] + MPT_G[ireg*MPT_NU + iu];
            }
            return region;
        }
        abspos = abspos + MPT_NC[ireg];
    }
    #ifdef MPT_DEBUG_2
    printf("\tNo region found!!!\n");
    #endif
    return region;
}
