/* $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]
#define	SEL_IN	prhs[7]



/* 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,
		   	unsigned char	select)
		   
{
	double sumMemb = 2.2251e-308;
	int i,j,k,r,theOut;
	int iOffIn,iOffOut,iOff2,jOff;
	int nInSq;
	double *xMinC;
	double *memb;
	double *y;
	double *fact1;
	
	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);
 	
 	switch (memb_type)
 	{	
 		case 1:
 		
 		/* 	Compute the fact1 = (yi-y)*memb_i/sumMemb
 			In non normalised case: (yi-y)*memb_i		*/
 			
	 	if (norm)
	 	{
		 	for (j=0; j<nOut; j++)
			{
				for (i=0,iOffOut=0;i<nRules;i++,iOffOut+=nOut)
			 	{	
			 		fact1[iOffOut+j] = (y[iOffOut+j] - f[j]) * memb[i]/sumMemb;
			 	}
		 	}
		}
		else
		{
		 	for (j=0; j<nOut; j++)
			{
				for (i=0,iOffOut=0;i<nRules;i++,iOffOut+=nOut)
			 	{	
			 		fact1[iOffOut+j] = y[iOffOut+j] * memb[i];
			 	}
		 	}
		}
		
	 	break;
	 	
	 	case 2:
	 	if (norm)
	 	{
			for (j=0; j<nOut; j++)
			{
				for (i=0,iOffOut=0;i<nRules;i++,iOffOut+=nOut)
			 	{	
			 		fact1[iOffOut+j] = (y[iOffOut+j] - f[j]) * pow(memb[i],m)/(sumMemb*(m-1));
			 	}
		 	}
		}
		else
		{
			for (j=0; j<nOut; j++)
			{
				for (i=0,iOffOut=0;i<nRules;i++,iOffOut+=nOut)
			 	{	
			 		fact1[iOffOut+j] = y[iOffOut+j] * pow(memb[i],m)/(m-1);
			 	}
		 	}		
		}
	 	break;
	 	
	 }
 	
 	for (theOut=0; theOut<nOut; theOut++)
 	{
		/* Compute the jacobian with respect to the center */
		if ((select & cJacs) != 0)
		{
			for (i=0,iOffIn=0, iOffOut=0, iOff2=0; i<nRules; i++,iOffIn+=nIn, iOffOut+=nOut, iOff2+=nInSq)
		 	{
		 		for (j=0,jOff=0;j<nIn;j++,jOff+=nIn)
		 		{
		 			*jac = 0;
					for(r=0;r<nIn;r++)
		 			{
		 				*jac += vp[iOff2+jOff+r] * xMinC[iOffIn+r];
		 			}
		 			*jac++ *= 2 * fact1[iOffOut+theOut];
		 		}
	 		}
	 	}
	 	
	 	/* Compute the jacobian with respect to the variance */
		if ((select & vJacs) != 0)
		{
			for (i=0,iOffIn=0, iOffOut=0, iOff2=0; i<nRules; i++,iOffIn+=nIn, iOffOut+=nOut, iOff2+=nInSq)
		 	{
		 		for (j=0,jOff=0;j<nIn;j++,jOff+=nIn)
		 		{
		 			for(k=0;k<nIn;k++)
		 			{	
		 				*jac = - xMinC[iOffIn+j] * xMinC[iOffIn+k] * fact1[iOffOut+theOut];
		 				jac++;
		 			}
		 		}
		 	}
	 	}
	 	
	 	/* Compute the jacobian with respect to the consequent parameters */
		if ((select & lJacs) != 0)
		{
			for (i=0; i<nRules; i++)
		 	{
		 		/* 	Among all the consequent parameters, the only ones that are not zero
		 			are the ones corresponding to the output theOut							*/
		 			
		 		for (j=0; j<nIn; j++)
		 		{
		 			for (k=0;k<nOut;k++)
		 			{
		 				if (k==theOut)
		 					if (norm)
	 						{
		 						*jac++ = memb[i] * x[j] / sumMemb;
		 					}
		 					else
		 					{
		 						*jac++ = memb[i] * x[j];
		 					}
		 				else
		 					*jac++ = 0;								// For the others it is zero
		 			}
		 		}
		 		
		 		if ((select & iJacs) != 0) 
		 			for (k=0;k<nOut;k++)
		 			{
		 				if (k==theOut)
		 					if (norm)
	 						{
		 						*jac++ = memb[i] / sumMemb;
		 					}
		 					else
		 					{
		 						*jac++ = memb[i];
		 					}
		 				else
		 					*jac++ = 0;
		 			}
		 		else
		 			for (k=0;k<nOut;k++)
		 			{
		 				*jac++ = 0;
		 			}
		 	}
	 	}
	}
 	mxFree(xMinC);
 	mxFree(memb);
 	mxFree(fact1);
 	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,totP,memb_type,norm;
  unsigned char select;

           
  /* 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 < 8)
  {
 	select = allJacs;
  } else if (nrhs == 7) 
  {
  	select=*(unsigned char*)mxGetPr(SEL_IN);
  }	else {
    mexErrMsgTxt("JACOB requires four or five 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);
 
  totP = 0;
  if ((select & cJacs) != 0)
  	totP += nIn;
  if ((select & vJacs) != 0)
  	totP += nIn * nIn;
  if ((select & lJacs) != 0)
  	totP += nOut * (nIn+1);
  	
  totP *= nRules;
  
  /* Create a matrix for the return arguments */
  
  J_OUT = mxCreateDoubleMatrix(totP, 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 */
  
  if (totP > 0)
  	calcJacob(jac,y,x,c,v,l,nIn,nOut,nRules,norm,memb_type,m,select);
  return;
}


