program sncGoniMotionY ( "C=TSTGONI,P=TST:HD:GONI,R1=X,R2=Y,R3=ROLL ")

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

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

short errorY;
float Y;

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


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

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

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

short n_Rad;
assign n_Rad to "{C}:SNL:N_Radiator";
monitor n_Rad;
short n_RadC;
assign n_RadC to "{C}:SNL:N_RadiatorC";
monitor n_RadC;

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

/*  motor Y  */
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 MoveInY {
	/* Initialization */
	state INIT {
		entry {
			sprintf(stateTextY,"INIT");
			pvPut(stateTextY, SYNC); 
			error=0;
			pvPut(error, SYNC);
			moveYDone=0;
			pvPut(moveYDone, SYNC);
			/*printf("   Start  sncGoniMotionY \n"); */ 
		}
		when(delay(1.) ) {
			 /*Go to STANDBY state without doing anything */
			r1Macro = macValueGet( "R1" );
			r2Macro = macValueGet( "R2" );
			r3Macro = macValueGet( "R3" );
			pMacro = macValueGet( "P"  );


		} state STANDBY
	
	}

/*--------------STANDBY  */
  	/* Standby waiting for requests for Movements */
	state STANDBY {
		entry { 
			sprintf(stateTextY,"STANDBY");
			pvPut(stateTextY, SYNC); 
  			/*printf("    ***STANDBY MotionY ****n_Rad= %d  n_RadC= %d \n",n_Rad,n_RadC); */

	}
		
/*------------ Move Y   ----------*/

		when( delay(0.2)&&runMoveY == 1 ) {
		/*  start Moving  */
			moveYDone=0;
			pvPut(moveYDone, SYNC);
			runMoveY=0;
			pvPut(runMoveY, SYNC);
  			/*printf("    ***sncGoniMotionY **** Radiator= %d  \n",n_Rad ); */
	
 		} state MOVE_Y

	}


/*-------- MOVE_Y-------*/
	state MOVE_Y {
		entry { 
			valY=moveInY;
			pvPut(valY);
		}

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

		} state  MOVINGY_STATUS
	}

	state  MOVINGY_STATUS{
	
		entry { 
			sprintf(stateTextY,"MOVING Y ");
			pvPut(stateTextY, SYNC); 
			error=0;
		}
		/* wait for end of movement */
		when(delay(0.5)&& dmovY == 1 ) {
 			if((moveInY-RbvY)>=0.05 || (moveInY-RbvY)<=-0.05){
			   error=1;
			   pvPut(error, 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(stateTextY,"MOVE Y DONE");
			pvPut(stateTextY, SYNC); 
		}

		when( delay(0.2) ) {
		   if(errorY==0){
			moveYDone=1;
			pvPut(moveYDone, SYNC);
		    	/*printf("++ sncGoniMotionY ++++++ MOVE_Y_DONE +++++++++\n"); */
		   }
		} state  STANDBY

		when( delay(0.5) ) {
		   if(errorY==1){
			   sprintf(errorMesage,"Motor Y problem:  Controller ERROR ? ");
			   pvPut(errorMesage, SYNC);
		    /*	printf("++++++++ MOVE_ ERROR +++++++++\n"); */
		   }
		} state  STANDBY

	}

}