program sncGoniMotion ( "C=GONI,P=HD:GONI,R1=X,R2=Y ")

option +r;		/* Reentrant code			*/
option -a;		/* Asynchronous pvGet 			*/
option +d;		/* Turn on debug message 		*/

%% #include <time.h>
%% #include <ctype.h>

short errorX;
short errorY;
short nnx;
short nny;

float absOffset;
float dxx;
float dyy;

char* pMacro;
char* r1Macro;
char* r2Macro;

float moveInX;
assign moveInX to "{C}:SNL:moveInX";
monitor moveInX;
float moveInY;
assign moveInY to "{C}:SNL:moveInY";
monitor moveInY;

/* delta(X) and delta(Y) step size of tuning*/
float dX;
assign dX to "{C}:SNL:dX";
monitor dX;
float dY;
assign dY to "{C}:SNL:dY";
monitor dY;
/* max movement in(X) or (Y) for the tuning (< 1cm)  */
float maxdX;
assign maxdX to "{C}:SNL:maxdX";
monitor maxdX;
float maxdY;
assign maxdY to "{C}:SNL:maxdY";
monitor maxdY;
/* Coordinates of central window (marked as x) */
float center;
assign center to "{C}:SNL:center";
monitor center;
float centered;
assign centered to "{C}:SNL:centered";
monitor centered;
/* Coordinates of retract position */
float retractX;
assign retractX to "{C}:SNL:retractX";
monitor retractX;
float retractY;
assign retractY to "{C}:SNL:retractY";
monitor retractY;
/* Coordinates of reference position (coordinates of center of 1st pad)  */
float pivotX;
assign pivotX to "{C}:SNL:pivotX";
monitor pivotX;
float pivotY;
assign pivotY to "{C}:SNL:pivotY";
monitor pivotY;
/* distance betwen two  pads)  */
float stepX;
assign stepX to "{C}:SNL:stepX";
monitor stepX;
float stepY;
assign stepY to "{C}:SNL:stepY";
monitor stepY;

float offsetX;
assign offsetX to "{C}:SNL:offsetX";
monitor offsetX;
float saveOffsetX;
assign saveOffsetX to "{C}:SNL:saveOffsetX";
monitor saveOffsetX;

float offsetY;
assign offsetY to "{C}:SNL:offsetY";
monitor offsetY;
float saveOffsetY;
assign saveOffsetY to "{C}:SNL:saveOffsetY";
monitor saveOffsetY;
short n_y;
assign n_y to "{C}:SNL:N_Y";
monitor n_y;

short n_x;
assign n_x to "{C}:SNL:N_X";
monitor n_x;
short n_xtarget;
assign n_xtarget to "{C}:SNL:N_Xtarget";
monitor n_xtarget;
short n_ytarget;
assign n_ytarget to "{C}:SNL:N_Ytarget";
monitor n_ytarget;

short saveOffsets;
assign saveOffsets to "{C}:SNL:saveOffsets";
monitor saveOffsets;

short moveDone;
assign moveDone to "{C}:SNL:moveDone";
monitor moveDone;

short start;
assign start to "{C}:SNL:start";
monitor start;
short abort;
assign abort to "{C}:SNL:abort";
monitor abort;

short retract;
assign retract to "{C}:SNL:retract";
monitor retract;

short retracted;
assign retracted to "{C}:SNL:retracted";
monitor retracted;

short moveXR;
assign moveXR to "{C}:SNL:moveXRight";
monitor moveXR;
short moveXL;
assign moveXL to "{C}:SNL:moveXLeft";
monitor moveXL;
short moveYU;
assign moveYU to "{C}:SNL:moveYUp";
monitor moveYU;
short moveYD;
assign moveYD to "{C}:SNL:moveYDown";
monitor moveYD;

short runMove;
assign runMove to "{C}:SNL:runMove";
monitor runMove;


string stateText;
assign stateText to "{C}:SNL:stateText";
monitor stateText;
string errorMesage;
assign errorMesage to "{C}:SNL:ERR_MESAGE";
monitor errorMesage;
short error;
assign error to "{C}:SNL:error";
monitor error;

float valX;
assign valX to "{P}:{R1}.VAL";
monitor valX;

short dmovX;
assign dmovX to "{P}:{R1}.DMOV";
monitor dmovX;

short stopX;
assign stopX to "{P}:{R1}.STOP";


float RbvX;
assign RbvX to "{P}:{R1}.RBV";
monitor RbvX;
float RlvX;
assign RlvX to "{P}:{R1}.RLV";
monitor RlvX;

float valY;
assign valY to "{P}:{R2}.VAL";
monitor valY;
short dmovY;
assign dmovY to "{P}:{R2}.DMOV";
monitor dmovY;

short stopY;
assign stopY to "{P}:{R2}.STOP";
monitor stopY;

float RbvY;
assign RbvY to "{P}:{R2}.RBV";
monitor RbvY;
float RlvY;
assign RlvY to "{P}:{R2}.RLV";
monitor RlvY;


ss ssXYPhi {
	/* Initialization */
	state INIT {
		entry {
			sprintf(stateText,"INIT");
			pvPut(stateText, SYNC); 
			dX=0.01;
			pvPut( dX, SYNC);
			dY=0.02;
			pvPut( dY, SYNC);
			maxdX=0.5;
			pvPut( maxdX, SYNC);
			maxdY=0.5;
			pvPut( maxdY, SYNC);
			stepX=5.;
 			pvPut(stepX, SYNC);
			stepY=5.;
 			pvPut(stepY, SYNC);
			
			pivotX=57.5;
 			pvPut(pivotX, SYNC);
			pivotY=57.5;
 			pvPut(pivotY, SYNC);
			offsetX=0;
			pvPut(offsetX, SYNC);
			offsetY=0;
			pvPut(offsetY, SYNC);
			start=0;
			pvPut(start, SYNC);
			runMove=0;
			pvPut(runMove, SYNC);
			error=0;
			pvPut(error, SYNC);
			saveOffsetX=0.;
			pvPut(saveOffsetX, SYNC);
			saveOffsetY=0.;
			pvPut(saveOffsetY, SYNC);
			moveDone=0;
			pvPut(moveDone, SYNC);
			
/*			printf("   Start   \n");  */
		}
		when(delay(1.) ) {
			/* Go to STANDBY state without doing anything */
			r1Macro = macValueGet( "R1" );
			r2Macro = macValueGet( "R2" );
			pMacro = macValueGet( "P"  );


			retractX=pivotX-6.*stepX;
    			retractY=pivotY;
 			pvPut(retractX, SYNC);
 			pvPut(retractY, SYNC);
		} state STANDBY
	
	}

/*--------------STANDBY  */
  	/* Standby waiting for requests for Movements */
	state STANDBY {
		entry { 
			sprintf(stateText,"STANDBY");
			pvPut(stateText, SYNC); 
			n_xtarget=-1;
			pvPut(n_xtarget, SYNC);
			n_ytarget=-1;
			pvPut(n_ytarget, SYNC);
		}
		
		when (abort == 1  ) {
			abort=0;
			pvPut(abort, SYNC); 
			sprintf(errorMesage,"Nothing to ABORT ");
			pvPut(errorMesage, SYNC);
		} state STANDBY
/*------------ Move X right/left in dX ----------*/
		when( moveXR == 1 || moveXL == 1) {
			
 		} state MOVE_RIGHT_LEFT

/*------------ Move Y Up/Down in dY ----------*/
		when( moveYU == 1 || moveYD == 1) {
			
 		} state MOVE_Up_Down

/*------------ Save OffsetX and OffsetY ----------*/

		when( saveOffsets == 1 ) {
	
 		} state SAVE_OFFSETS

/*------------ Retract  ----------*/
		when( retract == 1 ) {
		 	centered=0;
			pvPut(centered, SYNC);
	
 			moveDone=0;
			pvPut(moveDone, SYNC);
		} state RETRACT
/*------------ Center  ----------*/
		when( center == 1 ) {
	
 		 	retracted=0;
			pvPut(retracted, SYNC);
			moveDone=0;
			pvPut(moveDone, SYNC);
		} state CENTER

/*------------ Move X and Y to the cell  ----------*/

		when( runMove == 1 ) {
		/*  start Moving  */
		 	centered=0;
			pvPut(centered, SYNC);
		 	retracted=0;
			pvPut(retracted, SYNC);
			moveDone=0;
			pvPut(moveDone, SYNC);
	
 		} state MOVING
	}
/*-----------*/
/*-------- Save Offsets-------*/
	state SAVE_OFFSETS {
		entry { 
			sprintf(stateText,"SAVE OFFSETS");
			pvPut(stateText, SYNC); 
			saveOffsets=0.,
			pvPut(saveOffsets, SYNC); 
			
		}

		when( delay(0.5) ) {
		/*	printf("   Save OffsetX: %6.3f  OffsetY: %6.3f   \n",offsetX,offsetY);  */
 			saveOffsetX=offsetX;
			pvPut(saveOffsetX, SYNC);
 			saveOffsetY=offsetY;
			pvPut(saveOffsetY, SYNC);
		} state STANDBY
	}
/*-------- CENTER-------*/
 	state CENTER {
		entry { 
			sprintf(stateText,"IN BEAM");
			pvPut(stateText, SYNC); 
			error=0;
			pvPut(error, SYNC);
			sprintf(errorMesage," ");
			pvPut(errorMesage, SYNC);
		}
		
		when( delay(0.5) ) {
			    moveInX=pivotX;
 			    moveInY=pivotY;
   		/*	printf("    *************Move to Center positions X= %6.3f Y= %6.3f \n",moveInX,moveInY); */
    		 	    start=0;
			    pvPut(start, SYNC);
 			
		} state MOVE_X


	}
/*-------- RETRACT-------*/
 	state RETRACT {
		entry { 
			sprintf(stateText,"RETRACT");
			pvPut(stateText, SYNC); 
			error=0;
			pvPut(error, SYNC);
			sprintf(errorMesage," ");
			pvPut(errorMesage, SYNC);
		}
		
		when( delay(0.5) ) {
			    moveInX=retractX;
			    moveInY=retractY;
   		/*	printf("    *************Move to Retract positions X= %6.3f Y= %6.3f \n",moveInX,moveInY); */
    		 	    start=0;
			    pvPut(start, SYNC);
 			
		} state MOVE_X


	}
/*-------- MOVING-------*/
 	state MOVING {
		entry { 
			sprintf(stateText,"START MOVING");
			pvPut(stateText, SYNC); 
			runMove=0;
			pvPut(runMove, SYNC);
			error=0;
			pvPut(error, SYNC);
			sprintf(errorMesage," ");
			pvPut(errorMesage, SYNC);
   		 	start=1;
			pvPut(start, SYNC);
		}
		
		when( delay(0.5) ) {
		/*  start Moving  */
   		/*	printf("    *** n_X= %d n_Y= %d \n",n_x,n_y ); */
			n_xtarget=n_x;
			pvPut(n_xtarget, SYNC);
			n_ytarget=n_y;
			pvPut(n_ytarget, SYNC);
			nnx=n_x-1; 
			if(n_x<0){
			  nnx=n_x;
			}
			nny=n_y-1; 
			if(n_y<0){
			  nny=n_y;
			}
			moveInX=pivotX+nnx*stepX+stepX/2.+saveOffsetX;
    			moveInY=pivotY+nny*stepY+stepY/2.+saveOffsetY;
			
   		/*	printf("    ************* Move to  positions X= %6.3f Y= %6.3f \n",moveInX,moveInY ); 
		*/
			
   		} state MOVE_X

	}

/*-------- MOVE_X-------*/
	state MOVE_X {
		entry { 
			sprintf(stateText,"MOVING X");
			pvPut(stateText, SYNC); 
			error=0;
			pvPut(moveInX, SYNC); 
			pvPut(moveInY, SYNC); 
			valX=moveInX;
			pvPut(valX);
		}

		when( delay(0.5) ) {
/*			printf("    Move in X: %6.3f    \n",moveInX);
    			printf("    ************************************************   \n");*/

		} state  MOVINGX_STATUS
	}
	state  MOVINGX_STATUS{
	
		entry { 
			sprintf(stateText,"MOVING X ");
			pvPut(stateText, SYNC); 
			error=0;
		}
		when (abort == 1  ) {
			stopX=1;
			pvPut(stopX, SYNC); 
     		/*	printf("    ***********ABORT********  abort=%d   stopX=%d  \n",abort,stopX); */
			/*  ABORT go to ABORTED state  */ 
		} state ABORTED
		/* wait for end of movement */
		when( dmovX == 1 ) {
 			if((moveInX-RbvX)>=0.05 || (moveInX-RbvX)<=-0.05){
			   error=1;
			   pvPut(error, SYNC);
			
			   sprintf(errorMesage,"wrong position ");
			   pvPut(errorMesage, SYNC);
			   errorX=1;
    			/*   printf(" ********* wrong position in X   moveInX %6.3f  RbvX= %6.3f   \n",moveInX,RbvX);  */
			
			}
			else{
 			   errorX=0;
			}
			
		}state MOVE_X_DONE
 
	
	}

/*-------- MOVE_X DONE-------*/
	state MOVE_X_DONE {
		entry { 
			sprintf(stateText,"MOVE X DONE");
			pvPut(stateText, SYNC); 
		}

		when( delay(0.2) ) {
		   if(errorX==0){
		    /*	printf("++++++++ MOVE_X_DONE +++++++++\n"); */
		   }
		} state  MOVE_Y

		when( delay(0.5) ) {
		   if(errorX==1){
		    /*	printf("++++++++ MOVE_X ERROR +++++++++\n"); */
		   }
		} state  STANDBY

	}




/*-------- MOVE_Y-------*/
	state MOVE_Y {
		entry { 
			sprintf(stateText,"MOVING Y");
			pvPut(stateText, SYNC); 
			valY=moveInY;
			pvPut(valY);
		}

		when( delay(0.5) ) {
/*			printf("    Move in Y: %6.3f    \n",moveInY);
   			printf("    ************************************************   \n");*/

		} state  MOVINGY_STATUS
	}
	state  MOVINGY_STATUS{
	
		entry { 
			sprintf(stateText,"MOVING Y ");
			pvPut(stateText, SYNC); 
		}
		when (abort == 1  ) {
			stopY=1;
			pvPut(stopY, SYNC); 
     	/*		printf("    ***********ABORT********  abort=%d   stopY=%d  \n",abort,stopY); */
			/*  ABORT go to ABORTED state  */ 
		} state ABORTED
		/* wait for end of movement */
		when(delay(0.2)&& dmovY == 1 ) {
			if(retract==1){
 			    retract=0;
			    pvPut(retract, SYNC);
			    retracted=1;
			    pvPut(retracted, SYNC);
			}
			if(center==1){
 			    center=0;
			    pvPut(center, SYNC);
			    centered=1;
			    pvPut(centered, SYNC);
			}

 			if((moveInY-RbvY)>=0.05 || (moveInY-RbvY)<=-0.05){
			   error=1;
			   pvPut(error, SYNC);
			
			   sprintf(errorMesage,"wrong position ");
			   pvPut(errorMesage, SYNC);
			   errorY=1;
    			/*   printf(" ********* wrong position in Y   moveInY %6.3f  RbvY= %6.3f   \n",moveInY,RbvY);  */
			
			}
			else{
 			   errorY=0;
			}
			
		}state MOVE_Y_DONE
 
	
	}

/*-------- MOVE_Y DONE-------*/
	state MOVE_Y_DONE {
		entry { 
			sprintf(stateText,"MOVE Y DONE");
			pvPut(stateText, SYNC); 
		    /*	printf("++++++++ MOVE_Y_DONE +++++++++\n"); */
		}

		when( delay(0.5) ) {
			moveDone=1;
			pvPut(moveDone, SYNC);
		} state  STANDBY
	}




/*---------ABORT------*/
	state ABORTED {
		entry { 
			sprintf(stateText,"ABORTED");
			pvPut(stateText, SYNC); 
   		 	start=0;
			pvPut(start, SYNC);
 			retract=0;
			pvPut(retract, SYNC);
			runMove=0;
			pvPut(runMove, SYNC);
			
			abort = 0;
			pvPut(abort, SYNC); 
			sprintf(errorMesage,"ABORTED ");
			pvPut(errorMesage, SYNC);
		}

		when( delay(0.2) ) {
   /* 			printf("*  ****** ABORT******   %d    \n",stopX); */

		} state STANDBY


	}
/*------------ STATE Move X right/left in dX ----------*/
 	state MOVE_RIGHT_LEFT {
		entry { 
			if (moveXR==1){
			   sprintf(stateText,"MOVE Right");
			   pvPut(stateText, SYNC);
			   moveXR=0;
			   pvPut(moveXR, SYNC);
			   dxx=dX;
			}

			if (moveXL==1){
			   sprintf(stateText,"MOVE Left");
			   pvPut(stateText, SYNC);
			   dxx=-dX;
			   moveXL=0;
			   pvPut(moveXL, SYNC);
			}
		}
		
		when( delay(0.5) ) {
		/*  start Moving X */
 			error=0;
			pvPut(error, SYNC);
			sprintf(errorMesage,"  ");
			pvPut(errorMesage, SYNC);
			if((offsetX)<0){
			    absOffset=-offsetX-dxx;
			}
			else{
			    absOffset=offsetX+dxx;
			}
			if(absOffset>maxdX) {
			   error=1;
			   pvPut(error, SYNC);
			   sprintf(errorMesage,"BIG Offset in X ");
			   pvPut(errorMesage, SYNC);
			   dxx=0;
    		/*	printf(" ********* BIG OFFSET in X   %6.3f   absOffsetX= %6.3f \n",offsetX,absOffset);  */
			}
   			offsetX=offsetX+dxx;
 			pvPut(offsetX, SYNC);
   		/*	printf("   \n");
    			printf("    *  XPS: %s Motor: %s     Moving dX    %6.3f  \n",pMacro,r1Macro,dxx);  */
		} state MOVE_X_REL
	}
/*-------- MOVE_X-------*/
	state MOVE_X_REL {
		entry { 
			sprintf(stateText,"MOVE RELATIVE X");
			pvPut(stateText, SYNC); 
		}

		when( delay(0.5) ) {
		/*	printf("    Move Relative in X: %6.3f    \n",dxx);
    			printf("    ************************************************   \n");  */
			RlvX=dxx;
			pvPut(RlvX, SYNC);

		} state  STATUS_X
	}
	state  STATUS_X{
	
		entry { 
			sprintf(stateText,"MOVE RELATIVE X");
			pvPut(stateText, SYNC); 
			error=0;
			moveInX=moveInX+dxx;
		}
		when( dmovX == 1 ) {
 			if((moveInX-RbvX)>=0.05 || (moveInX-RbvX)<=-0.05){
			   error=1;
			   pvPut(error, SYNC);
			
			   sprintf(errorMesage,"wrong position ");
			   pvPut(errorMesage, SYNC);
			   errorX=1;
    			/*   printf(" ********* wrong position in X   moveInX %6.3f  RbvX= %6.3f   \n",moveInX,RbvX);  */
			
			}
			else{
 			   errorX=0;
			}
			
		}state STANDBY
 
	
	}
/*------------ STATE Move Y Up/Down in dY ----------*/
 	state MOVE_Up_Down {

 		entry { 
			if (moveYU==1){
			   sprintf(stateText,"MOVE Up");
			   pvPut(stateText, SYNC);
			   moveYU=0;
			   pvPut(moveYU, SYNC);
			   dyy=dY;
			}

			if (moveYD==1){
			sprintf(stateText,"MOVE Down");
			pvPut(stateText, SYNC);
			   moveYD=0;
			   pvPut(moveYD, SYNC);
			   dyy=-dY;
			}
		}
		
		when( delay(0.5) ) {
		/*  start Moving Y */
 			error=0;
			pvPut(error, SYNC);
			sprintf(errorMesage,"  ");
			pvPut(errorMesage, SYNC);
			if((offsetY)<0){
			    absOffset=-offsetY-dyy;
			}
			else{
			    absOffset=offsetY+dyy;
			}
			if(absOffset>maxdY) {
			   error=1;
			   pvPut(error, SYNC);
			   sprintf(errorMesage,"BIG Offset in Y ");
			   pvPut(errorMesage, SYNC);
			   dyy=0;
    		/*	printf(" ********* BIG OFFSET in Y   %6.3f   absOffsetY= %6.3f \n",offsetY,absOffset);  */
			}
   			offsetY=offsetY+dyy;
 			pvPut(offsetY, SYNC);
   		/*	printf("   \n");
    			printf("    *  XPS: %s Motor: %s     Moving dY    %6.3f  \n",pMacro,r2Macro,dyy); */
		} state MOVE_Y_REL
	}
/*-------- MOVE_Y Relative------*/
	state MOVE_Y_REL {
		entry { 
			sprintf(stateText,"MOVE RELATIVE Y");
			pvPut(stateText, SYNC); 
		}

		when( delay(0.5) ) {
	/*		printf("    Move Relative in Y: %6.3f    \n",dyy);
    			printf("    ************************************************   \n"); */
			RlvY=dyy;
			pvPut(RlvY, SYNC);

		} state  STATUS_Y
	}
	state  STATUS_Y{
	
		entry { 
			sprintf(stateText,"MOVE RELATIVE Y");
			pvPut(stateText, SYNC); 
			error=0;
			moveInY=moveInY+dyy;
		}
		when( dmovY == 1 ) {
 			if((moveInY-RbvY)>=0.05 || (moveInY-RbvY)<=-0.05){
			   error=1;
			   pvPut(error, SYNC);
			
			   sprintf(errorMesage,"wrong position ");
			   pvPut(errorMesage, SYNC);
			   errorY=1;
    			/*   printf(" ********* wrong position in Y   moveInY %6.3f  RbvY= %6.3f   \n",moveInY,RbvY); */
			
			}
			else{
 			   errorY=0;
			}
			
		}state STANDBY
 
	
	}



}