program sncGoniMotionX ( "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 errorX;
float X;

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


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

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

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

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 stateTextX;
assign stateTextX to "{C}:SNL:stateTextX";
monitor stateTextX;
string errorMesage;
assign errorMesage to "{C}:SNL:ERR_MESAGE";
monitor errorMesage;
short error;
assign error to "{C}:SNL:error";
monitor error;

/*  motor X  */
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";
monitor stopX;

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


ss MoveInX {
	/* Initialization */
	state INIT {
		entry {
			sprintf(stateTextX,"INIT");
			pvPut(stateTextX, SYNC); 
			error=0;
			pvPut(error, SYNC);
			moveXDone=0;
			pvPut(moveXDone, SYNC);
			/*printf("   Start  sncGoniMotionX \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(stateTextX,"STANDBY");
			pvPut(stateTextX, SYNC); 
  			/*printf("    ***STANDBY MotionX ****n_Rad= %d  n_RadC= %d \n",n_Rad,n_RadC); */

	}
		
/*------------ Move X   ----------*/

		when( delay(0.2)&&runMoveX == 1 ) {
		/*  start Moving  */
			moveXDone=0;
			pvPut(moveXDone, SYNC);
			runMoveX=0;
			pvPut(runMoveX, SYNC);
  			/*printf("    ***sncGoniMotionX **** Radiator= %d  \n",n_Rad ); */
	
 		} state MOVE_X

	}


/*-------- MOVE_X-------*/
	state MOVE_X {
		entry { 
			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(stateTextX,"MOVING X ");
			pvPut(stateTextX, SYNC); 
			error=0;
		}
		/* wait for end of movement */
		when(delay(0.5)&& dmovX == 1 ) {
 			if((moveInX-RbvX)>=0.05 || (moveInX-RbvX)<=-0.05){
			   error=1;
			   pvPut(error, SYNC);
			
			   errorX=1;
    			/*   printf(" ********* wrong position in X   moveInX %6.3f  RbvY= %6.3f   \n",moveInX,RbvX);  */
			
			}
			else{
 			   errorX=0;
			}
			
		}state MOVE_X_DONE
 
	
	}

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

		when( delay(0.2) ) {
		   if(errorX==0){
			moveXDone=1;
			pvPut(moveXDone, SYNC);
		    	/*printf("++ sncGoniMotionX ++++++ MOVE_X_DONE +++++++++\n"); */
		   }
		} state  STANDBY

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

	}

}