/* $Revision: 1.2 $ */
/*
 *
 * YPRIME.C	Sample .MEX file corresponding to YPRIME.M
 *	        Solves simple 3 body orbit problem 
 *
 * The calling syntax is:
 *
 *		[yp] = yprime(t, y)
 *
 * Copyright (c) 1984-1998 by The MathWorks, Inc.
 * All Rights Reserved.
 */

#include <math.h>
#include "mex.h"
#include "ts_tools.h"

/* Input Arguments */

#define	X_IN	prhs[0]
#define	C_IN	prhs[1]
#define	V_IN	prhs[2]
#define	L_IN	prhs[3]
#define NORM 	prhs[4]
#define MB_TYP_IN prhs[5]
#define M_PAR_IN prhs[6]



/* Output Arguments */

#define	J_OUT	plhs[0]
#define	Y_OUT	plhs[1]


static unsigned char	cJacs = 1;
static unsigned char	vJacs = 2;
static unsigned char	lJacs = 4;
static unsigned char	iJacs = 8;
static unsigned char	allJacs = 15;


static void calcJacob(
		   double	*jac,
		   double	*f,
		   double	*x,
		   double	*cp,
		   double	*vp,
		   double	*lp,
		   int 		nIn,
		   int		nOut,
		   int		nRules,
		   int		norm,
		   int		memb_type,
		   double		m)
		   
{
	double sumMemb = 2.2251e-308;
	int i,j,r,theOut;
	int iOffIn,iOffOut,iOff2,iOff3,rOff,jOff;
	int nInOut,nInSq;
	double *xMinC;
	double *memb;
	double *y;
	double *fact1;
	
	nInOut=nOut*(nIn+1);
	nInSq=nIn*nIn;
	
	/* Allocate the space for the temporary variables */
	xMinC=(double*)mxCalloc(nRules*nIn,sizeof(double));
	y=(double*)mxCalloc(nRules*nOut,sizeof(double));
	fact1=(double*)mxCalloc(nRules*nOut,sizeof(double));
	memb=(double*)mxCalloc(nRules,sizeof(double));
	
	/* Compute a lot of things */
	
	process(f, xMinC, memb, y, &sumMemb, x, cp, vp, lp, nIn, nOut, nRules, norm, memb_type, m);
 	
 	
 	for (theOut=0; theOut<nOut; theOut++)
 	{
	 	/* Compute the jacobian with respect to the inputs */
		for(j=0,jOff=0;j<nIn;j++,jOff+=nOut )
	 	{
	 		*jac = 0;
			
			for (i=0,iOffIn=0,iOffOut=0,iOff2=0,iOff3=0; i<nRules; i++,iOffIn+=nIn,iOffOut+=nOut,iOff2+=nInSq,iOff3+=nInOut)
	 		{	
	 			double temp = 0;
	 			for (r=0,rOff=0;r<nIn;r++,rOff+=nIn)
	 			{
	 				temp += vp[iOff2+rOff+j] * xMinC[iOffIn+r];
	 			}
	 			
	 			switch (memb_type)
 				{	
 				case 1:
 		
 					*jac += memb[i] * (lp[iOff3+jOff+theOut] - 2 * temp * (y[iOffOut+theOut] - f[theOut]));
 					break;
 					
 				case 2:
 					*jac += memb[i] * lp[iOff3+jOff+theOut] - 2 * pow(memb[i],m)/(m-1) * temp * (y[iOffOut+theOut] - f[theOut]);
 					break;
 					
 				}
	 		}
	 		
	 		if (norm)
	 		{
	 			*jac++ /= sumMemb;
	 		}
	 		else
	 		{
	 			jac++;
	 		}
	 		
		}
 	}
 	
  	mxFree(xMinC);
 	mxFree(memb);
 	mxFree(y);
}

void mexFunction(
                 int nlhs,       mxArray *plhs[],
                 int nrhs, const mxArray *prhs[]
		 )
{
  double		*jac, *y;
  double		*x,*c,*v,*l;
  double		m;
  const int  	*dim_array_C;
  int			nRules,nIn,nOut,memb_type,norm;
  
           
  /* Check for proper number of arguments */
  if (nrhs >= 5)
	norm = *mxGetPr(NORM);
  else
	norm = 1;

  if (nrhs >= 6)
	memb_type = *mxGetPr(MB_TYP_IN);
  else
	memb_type = 1;

  if (nrhs >= 7)
	m = *mxGetPr(M_PAR_IN);
  else
	m = 2;

  if (nrhs < 4)
  {
  	mexErrMsgTxt("JACOB requires at least four input arguments.");
  } 
  
  if (nlhs > 2) {
    mexErrMsgTxt("JACOB requires at most two output argument.");
  }
  
  
  dim_array_C = mxGetDimensions(C_IN);
  nIn = dim_array_C[0];
  nRules = dim_array_C[1];
  
  nOut = mxGetM(L_IN);
 
  
  /* Create a matrix for the return arguments */
  
  J_OUT = mxCreateDoubleMatrix(nIn, nOut, mxREAL);
  Y_OUT = mxCreateDoubleMatrix(nOut, 1, mxREAL);
  
  
  /* Assign pointers to the various parameters */
  
  jac = mxGetPr(J_OUT);
  y =  mxGetPr(Y_OUT);
 
  x = mxGetPr(X_IN);
  c = mxGetPr(C_IN);
  v = mxGetPr(V_IN);
  l = mxGetPr(L_IN);
  
  
  /* Do the actual computations in a subroutine */
  
  calcJacob(jac,y,x,c,v,l,nIn,nOut,nRules,norm,memb_type,m);

}


