#include <math.h>
#include "mex.h"
#include "commtest.h"
#include "pvm3.h"

sendcmd(cmd,a1,a2,aout)
int cmd;
double *a1, *a2,*aout;
{
  static initialized = 0;
  static int bufid;
	static char host[64];
	static int inum;
	static int e;
	static char proc[32];
	static char ohost[256];
	int cc, tid, rstatus, dd;
	char buf[100];

        if(initialized == 0){
	  if ((e = pvm_mytid()) < 0) {
		pvm_perror("enroll");
		mexErrMsgTxt("Could not enroll with pvm");
	      }
	}

	if(initialized != 1){
	  e = pvm_spawn(CLI,(char**)0,1,MACHINE,1,&inum);  
/*	  e = pvm_spawn(CLI,(char**)0,1,"sashimi",1,&inum); */
					/*Selects a random machine */
	  if (e < 1) {
			mexErrMsgTxt("Initiate failed on remote host");
		     }
	  initialized = 1;
/*PvmTaskHost  +PvmTaskDebug */		
/* Receive confirmation of start */
		cc = pvm_recv(-1, -1);
		pvm_bufinfo(cc, (int*)0, (int*)0, &tid);
		pvm_upkstr(buf);
		printf("from t%x: %s\n", tid, buf);
	}
        bufid = pvm_initsend(PvmDataDefault);
        e = pvm_pkint(&cmd, 1, 1);
        if(cmd&SETVA){
          pvm_pkdouble(a1,1,1);
	  pvm_pkdouble(a2,1,1);
	}
        if(cmd&RELMOVE)
          pvm_pkdouble(a1,MSGLEN,1);
        if(cmd&WORLDMOVE)
          pvm_pkdouble(a1,MSGLEN,1);
	if (e = pvm_send(inum, MSGID)) {
			pvm_perror("snd");
			pvm_kill(inum);
		}
        if(cmd&POSQUERY){
          rstatus = pvm_pstat(inum);
          while ((rstatus == PvmOk) && pvm_probe(inum,MSGID)==0){
/*            sleep(POLLRATE/1000000); Doesn't work */
            for(dd=0;dd<10000;dd++); 
            rstatus = pvm_pstat(inum);
	  }
          if(rstatus != PvmOk){
            initialized = -1;
            mexErrMsgTxt("Remote process does not respond");
	  }
	  bufid = pvm_recv(-1,MSGID);
	  pvm_upkdouble(aout,MSGLEN,1);
	} 
}


/* Input Arguments */

#define	Y1_IN prhs[0]
#define Y2_IN prhs[1]

/* Output Arguments */

#define	YP_OUT	plhs[0]


#define	max(A, B)	((A) > (B) ? (A) : (B))
#define	min(A, B)	((A) < (B) ? (A) : (B))



void mexFunction(nlhs, plhs, nrhs, prhs)
int nlhs, nrhs;
Matrix *plhs[], *prhs[];
{
	double	*yp;
	double	*a,*y;
	unsigned int	m,n;
        int cmd;

	/* Check for proper number of arguments */

	if (nrhs > 3) {
		mexErrMsgTxt("ROBOTMOVEHILL input args have to be <= 3");
	} else if (nlhs > 1) {
		mexErrMsgTxt("ROBOTMOVEHILl requires one output or zero argument.");
	}
        /* Figure out command */
        cmd = 0;
        if (nrhs ==3){
	  cmd=cmd|WORLDMOVE;
        }
        if (nrhs ==1)
	  cmd=cmd|RELMOVE;
	if (nrhs ==2)
          cmd=cmd|SETVA;
	if (nlhs ==1)
	  cmd=cmd|POSQUERY;
	m = MSGLEN;
	n = 1;
	/* Assign pointers to the various parameters */
        if(cmd&RELMOVE){
	  y = mxGetPr(Y1_IN);
	}
        if(cmd&WORLDMOVE){
	  y = mxGetPr(Y1_IN);
	}
	if(cmd&SETVA){
          y = mxGetPr(Y1_IN);
	  a = mxGetPr(Y2_IN); 
	}
	/* Create a matrix for the return argument */
	if(cmd&POSQUERY){
	  YP_OUT = mxCreateFull(m, n, REAL);
	  yp = mxGetPr(YP_OUT);
	}

	/* Do the actual computations in a subroutine */
        sendcmd(cmd,y,a,yp);  
}