program sncGoniMotionROLL ( "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 errorR;
float R;

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

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

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

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

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

/*  motor ROLL  */
float valR;
assign valR to "{P}:{R3}.VAL";
monitor valR;

short dmovR;
assign dmovR to "{P}:{R3}.DMOV";
monitor dmovR;

short stopR;
assign stopR to "{P}:{R3}.STOP";
monitor stopR;

float RbvR;
assign RbvR to "{P}:{R3}.RBV";
monitor RbvR;



ss MoveInR {
	/* Initialization */
	state INIT {
		entry {
			sprintf(stateTextR,"INIT");
			pvPut(stateTextR, SYNC); 
			error=0;
			pvPut(error, SYNC);
			moveRDone=0;
			pvPut(moveRDone, SYNC);
			/*printf("   Start  sncGoniMotionR \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(stateTextR,"STANDBY");
			pvPut(stateTextR, SYNC); 
  			/*printf("    ***STANDBY MotionR ****n_Rad= %d  n_RadC= %d \n",n_Rad,n_RadC);*/ 
 
	}
		

/*------------ Move R   ----------*/

		when( delay(0.2)&&runMoveR == 1 ) {
		/*  start Moving  */
			moveRDone=0;
			pvPut(moveRDone, SYNC);
			runMoveR=0;
			pvPut(runMoveR, SYNC);
  			/*printf("    ***sncGoniMotionR **** Radiator= %d  \n",n_Rad ); */
	
 		} state MOVE_R

	}


/*-------- MOVE_R-------*/
	state MOVE_R {
		entry { 
			valR=moveInR;
			pvPut(valR);
		}

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

		} state  MOVINGR_STATUS
	}
	state  MOVINGR_STATUS{
	
		entry { 
			sprintf(stateTextR,"MOVING R ");
			pvPut(stateTextR, SYNC); 
			error=0;
		}
		/* wait for end of movement */
		when(delay(0.5)&& dmovR == 1 ) {
 			if((moveInR-RbvR)>=0.05 || (moveInR-RbvR)<=-0.05){
			   error=1;
			   pvPut(error, SYNC);
			
			   errorR=1;
    			/*   printf(" ********* wrong position in R   moveInR %6.3f  RbvR= %6.3f   \n",moveInR,RbvR);  */
			
			}
			else{
 			   errorR=0;
			}
			
		}state MOVE_R_DONE
 
	
	}

/*-------- MOVE_R DONE-------*/
	state MOVE_R_DONE {
		entry { 
			sprintf(stateTextR,"MOVE R DONE");
			pvPut(stateTextR, SYNC); 
		}

		when( delay(0.2) ) {
		   if(errorR==0){
			moveRDone=1;
			pvPut(moveRDone, SYNC);
		    	/*printf("+++ sncGoniMotionR +++++ MOVE_R_DONE +++++++++\n");*/ 
		   }
		} state  STANDBY

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

	}

}