program sncGoniMotionMain ( "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 <ctype.h>
%% #include <time.h>
%% #include <stdlib.h>
%% #include <unistd.h>

short moveto_radiator;
float X;
float Y;
float R;
int print_flag=0;

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

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

short moveto_retract;
assign moveto_retract to "{C}:SNL:moveToRetract";
monitor moveto_retract;



/* Coordinates of retract position */
float retractX;
assign retractX to "{C}:SNL:retractX";
monitor retractX;
float retractY;
assign retractY to "{C}:SNL:retractY";
monitor retractY;
float retractR;
assign retractR to "{C}:SNL:retractR";
monitor retractR;

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

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 runMove;
assign runMove to "{C}:SNL:runMove";
monitor runMove;
short runMoveX;
assign runMoveX to "{C}:SNL:runMoveX";
monitor runMoveX;
short runMoveY;
assign runMoveY to "{C}:SNL:runMoveY";
monitor runMoveY;
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 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;

/*  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;

/*  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;

/*  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 ssXYPhi {
	/* Initialization */
	state INIT {
		entry {
			sprintf(stateText,"INIT");
			pvPut(stateText, SYNC); 
			start=0;
			pvPut(start, SYNC);
			runMove=0;
			pvPut(runMove, SYNC);
			error=0;
			pvPut(error, SYNC);
			moveDone=0;
			pvPut(moveDone, SYNC);
			moveto_retract=0;	
			pvPut(moveto_retract, SYNC);
			sprintf(errorMesage," ");
			pvPut(errorMesage, SYNC);
			/*printf("   Start  sncGoniMotionMain \n"); */ 
		}
		when(delay(1.) ) {
			 /*Go to STANDBY state without doing anything */
			r1Macro = macValueGet( "R1" );
			r2Macro = macValueGet( "R2" );
			r3Macro = macValueGet( "R3" );
			pMacro = macValueGet( "P"  );


			retractX=0.;
    			retractY=0.;
    			retractR=0.;
 			pvPut(retractX, SYNC);
 			pvPut(retractY, SYNC);
 			pvPut(retractR, SYNC);
		} state STANDBY
	
	}

/*--------------STANDBY  */
  	/* Standby waiting for requests for Movements */
	state STANDBY {
		entry { 
			sprintf(stateText,"STANDBY");
			pvPut(stateText, SYNC); 
			moveto_radiator=0;
			moveto_retract=0;
			pvPut(moveto_retract, SYNC);
			n_Rad=-100;
			pvPut(n_Rad, SYNC);
			if(print_flag==1){
			   printf("    ***STANDBY*** n_RadC= %d  \n",n_RadC); 
			}
	}
		

/*------------ Move X and Y to the cell  ----------*/
		
		when (abort == 1  ) {
			abort=0;
			pvPut(abort, SYNC); 
			sprintf(errorMesage,"Nothing to ABORT ");
			pvPut(errorMesage, SYNC);
		} state STANDBY

		when( delay(0.5) && runMove == 1 ) {
		/*  start Moving  */
			abort = 0;
			pvPut(abort, SYNC); 
			moveDone=0;
			pvPut(moveDone, SYNC);
			n_RadC=-100;
			pvPut(n_RadC, SYNC);
			moveto_radiator=1;
			sprintf(errorMesage," ");
			pvPut(errorMesage, SYNC);
			moveto_retract=0;	
			pvPut(moveto_retract, SYNC);
	
 		} state GET_RAD_COORDINATES
		when( delay(0.2)&&retract == 1 ) {
			error=0;
			pvPut(error, SYNC);
			abort = 0;
			pvPut(abort, SYNC); 
			sprintf(errorMesage," ");
			pvPut(errorMesage, SYNC);
			sprintf(stateText,"GO RETRACT");
			pvPut(stateText, SYNC); 
			moveInX=retractX;
			moveInY=retractY;
			moveInR=retractR;
			moveto_retract=1;
			pvPut(moveto_retract, SYNC);
			n_RadC=-100;
			pvPut(n_RadC, SYNC);
			moveto_radiator=0;
			if(print_flag==1){
  			   printf("    ***Move to Retract position X= %6.3f Y= %6.3f ROLL= %6.3f\n",moveInX,moveInY,moveInR);
			}    
		} state MOVE_TO_RETRACT

	}
/*--------  GET_COORDINATES OF RADIATOR-------*/
 	state GET_RAD_COORDINATES {
		entry { 
			sprintf(stateText,"GET_COORDINATES");
			pvPut(stateText, SYNC); 
		}
		
		when( delay(1.) ) {
		/*  start Moving  */
   			/*printf("    *** Radiator= %d  \n",n_Rad ); */

			X=n_Rad*1.;
			Y=n_Rad*1.5;
			R=n_Rad*2.;
			
			moveInX=X;
    			moveInY=Y;
    			moveInR=R;
			pvPut(moveInX, SYNC); 
			pvPut(moveInY, SYNC); 
			pvPut(moveInR, SYNC); 
			
			moveXDone=0;
			pvPut(moveXDone, SYNC);
			moveYDone=0;
			pvPut(moveYDone, SYNC);
			moveRDone=0;
			pvPut(moveRDone, SYNC);
			n_RadC=-100;
			pvPut(n_RadC, SYNC);

   		} state MOVING
	}

/*-------- MOVING-------*/
 	state MOVING {
		entry { 
			sprintf(stateText,"START MOVING");
			pvPut(stateText, SYNC); 
			error=0;
			pvPut(error, SYNC);
			sprintf(errorMesage," ");
			pvPut(errorMesage, SYNC);
   		 	start=1;
			pvPut(start, SYNC);
		}
		when( delay(0.5)&& retracted==1) {
		/*  start Moving  */
			 			
 			if(print_flag==1){
 			   printf("     Move to Y= %6.2f then  X= %6.2f \n",moveInY,moveInX );
			}
			n_RadC=-100;
			pvPut(n_RadC, SYNC);

   		} state MOVE_From_Retract
		
		when( delay(0.5)&& retracted==0) {
		/*  start Moving inside X,Y and ROLL   */
			runMoveX=1;
			pvPut(runMoveX, SYNC);
			runMoveY=1;
			pvPut(runMoveY, SYNC);
			runMoveR=1;
			pvPut(runMoveR, SYNC);

			if(print_flag==1){			
  			   printf("     Move to X= %6.2f , Y= %6.2f , ROLL= %6.2f\n",moveInX,moveInY,moveInR );
 			}			
   		} state WAIT_MoveXYR
	}
	
	state WAIT_MoveXYR {
		entry { 
     			/*printf("    ***********entrY   WAIT_MoveXYR ********   \n"); */
		}
		
		when (delay(0.3)&& abort == 1  ) {
			if(print_flag==1){			
     			   printf("    ********* ABORT ********    \n"); 
			} 
		} state ABORTED
		when( delay(0.2)&&moveXDone == 1 && moveYDone == 1 && moveRDone == 1 && error != 1) {
			if(print_flag==1){			
			   printf(" **    Move in X and Y and ROLL is done:     \n");
   			   printf("    ************************************************   \n");
			}
			runMove=0;
			pvPut(runMove, SYNC);
			
		} state  MOVE_DONE
		when( delay(0.2) && error == 1) {
			printf(" ****   ERROR during movement in X or in Y or in ROLL  **** \n");
			sprintf(errorMesage,"Motors  problem:  Controller ERROR ? ");
			pvPut(errorMesage, SYNC);
			
		} state  STANDBY
	}
	
/*-------- MOVE_From Retracted position-------*/
	state MOVE_From_Retract {
		entry { 
			retracted=0;
			pvPut(retracted, SYNC);
		}
		/* start moving in Y and ROLL */

		when( delay(0.5) ) {
			retracted=0;
			pvPut(retracted, SYNC);
		
			runMoveR=1;
			pvPut(runMoveR, SYNC);
			runMoveY=1;
			pvPut(runMoveY, SYNC);
			if(print_flag==1){			
			   printf("    Move in Y: %6.3f  and in ROLL %6.3f \n",moveInY,moveInR);
   			}
		} state  WAIT_MoveYDone
	}

	state WAIT_MoveYDone {
		entry { 
		}

		when( delay(0.5)&& moveYDone == 1 && error != 1) {
			if(print_flag==1){			
			   printf(" **   Move in Y is done     \n");
   			}	
		} state  MoveX

		when (delay(0.3)&& abort == 1  ) {
			if(print_flag==1){			
     			   printf("    ********* ABORT ********     \n"); 
			} 
		} state ABORTED

		when( delay(0.2) && error == 1) {
			printf(" ****   ERROR during movement in Y  **** \n");		
			sprintf(errorMesage,"Motor Y  problem:  Controller ERROR ? ");
			pvPut(errorMesage, SYNC);
		} state  STANDBY
	}
	state MoveX {
		entry { 
		}
		/* start moving in X  */

		when( delay(0.5) ) {
			runMoveX=1;
			pvPut(runMoveX, SYNC);
		
			if(print_flag==1){			
			   printf("    Move in X: %6.3f    \n",moveInX);
			}
		} state  WAIT_MoveXDone
	}

	state WAIT_MoveXDone {
		entry { 
		}

		when( delay(0.5)&& moveXDone == 1 && moveRDone == 1 && error != 1) {
			if(print_flag==1){			
			   printf(" **   Move in X and in ROLL is done     \n");
   			}	
		} state  MOVE_DONE

		when (delay(0.3)&& abort == 1  ) {
			if(print_flag==1){			
     			   printf("    ********* ABORT ********    \n"); 
			} 
		} state ABORTED

		when( delay(0.2) && error == 1) {
			printf(" ****   ERROR during movement in X  **** \n");			
			sprintf(errorMesage,"Motor X  problem:  Controller ERROR ? ");
			pvPut(errorMesage, SYNC);
		} state  STANDBY
	}


 
/*-------- MOVE DONE------*/

	state MOVE_DONE {
		entry { 
			runMove=0;
			pvPut(runMove, SYNC);
			moveDone=1;
			pvPut(moveDone, SYNC);
			sprintf(stateText,"MOVE DONE");
			pvPut(stateText, SYNC); 
		}

		when( delay(0.2)&& moveto_radiator==1) {
			moveto_radiator=0;
			n_RadC=n_Rad;
			pvPut(n_RadC, SYNC);
		} state  STANDBY
	}



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

		when( delay(0.2) ) {
			abort = 0;
			pvPut(abort, SYNC); 
			stopX=1;
			pvPut(stopX, SYNC); 
			stopY=1;
			pvPut(stopY, SYNC); 
 			stopR=1;
			pvPut(stopR, SYNC); 
			if(print_flag==1){
    			   printf("*  ****** ABORTED******    \n"); 			
  			   printf("    ************************************************   \n");
			}			

		} state STANDBY

	}

/*-----------*/
	state MOVE_TO_RETRACT {
		entry { 
		 	retracted=0;
			pvPut(retracted, SYNC);
			moveDone=0;
			pvPut(moveDone, SYNC);
			n_RadC=-100;
			pvPut(n_RadC, SYNC);
		}
			
		when( delay(0.5) ) {
 			pvPut(moveInX, SYNC);
			pvPut(moveInY, SYNC);
 			pvPut(moveInR, SYNC);
   		 	start=0;
			pvPut(start, SYNC);
			moveXDone=0;
			pvPut(moveXDone, SYNC);
			moveYDone=0;
			pvPut(moveYDone, SYNC);
			moveRDone=0;
			pvPut(moveRDone, SYNC);
		} state Move1X


	}
	state Move1X {
		entry { 
			sprintf(stateText,"MOVE X and ROLL");
			pvPut(stateText, SYNC); 
		}
		/* start moving in X an ROLL */

		when( delay(1.5) ) {
			runMoveX=1;
			pvPut(runMoveX, SYNC);
			if(print_flag==1){		
			   printf("    Move in X: %6.3f,  Move im ROLL  %6.3f \n",moveInX,moveInR);
   			}
		} state  WAIT_Move1XDone
	}

	state WAIT_Move1XDone {
		entry { 
		}

		when( delay(0.5)&& moveXDone == 1 && error != 1) {
			if(print_flag==1){		
			   printf(" **    Move in X is done:     \n");
   			}			
		} state  Move1Y

		when (delay(0.3)&& abort == 1  ) {
			if(print_flag==1){			
     			   printf("    ********* ABORT ********    \n"); 
			} 
		} state ABORTED

		when( delay(0.2) && error == 1) {
			printf(" ****   ERROR during movement in X  **** \n");			
			sprintf(errorMesage,"Motor X  problem:  Controller ERROR ? ");
			pvPut(errorMesage, SYNC);
		} state  STANDBY
	}

	state Move1Y {
		entry { 
			sprintf(stateText,"MOVE Y and ROLL");
			pvPut(stateText, SYNC); 
		}
		/* start moving in Y  */

		when( delay(0.5) ) {
			runMoveR=1;
			pvPut(runMoveR, SYNC);
			runMoveY=1;
			pvPut(runMoveY, SYNC);
		
			if(print_flag==1){		
			   printf("    Move in Y: %6.3f    \n",moveInY);
   			}
		} state  WAIT_Move1YRDone
	}

	state WAIT_Move1YRDone {
		entry { 
		}

		when( delay(0.5)&& moveYDone == 1 && moveRDone == 1 && error != 1) {
			if(print_flag==1){		
			   printf(" **    Move in Y and ROLL is done:     \n");
   			   printf("    ************************************************   \n");
   			}			
		} state MOVE_TO_RETR_DONE
		when (delay(0.3)&& abort == 1  ) {
			if(print_flag==1){			
     			   printf("    ********* ABORT ********    \n"); 
			} 
		} state ABORTED

		when( delay(0.2) && error == 1) {
			printf(" ****   ERROR during movement in Y or ROLL  **** \n");
			sprintf(errorMesage,"Motor Y or ROLL  problem:  Controller ERROR ? ");
			pvPut(errorMesage, SYNC);
			
		} state  STANDBY
	}

/*-------- MOVE DONE------*/

	state MOVE_TO_RETR_DONE {
		entry { 
			retract=0;
			pvPut(retract, SYNC);
			moveDone=1;
			pvPut(moveDone, SYNC);
			sprintf(stateText,"MOVE DONE");
			pvPut(stateText, SYNC); 
		}

		when(  moveto_retract==1) {
			moveto_retract=0;
			pvPut(moveto_retract, SYNC);
			
		} state  STANDBY

	}

}

ss retractPosition {
	/* Initialization */
	state INIT {
		entry {
			
		}
		when(delay(1.) ) {
			retracted=0;
			pvPut(retracted, SYNC);

		} state POSITION

	} 
	
	

/*-----------POSITION  */
	state POSITION {
		entry { 
 			retracted=0;
			pvPut(retracted, SYNC);
		}
		when(delay(2.5)&& dmovX == 1 && dmovY == 1 && dmovR == 1) {

 			if(abs(retractX-RbvX)<=0.03&& abs(retractY-RbvY)<=0.03&& abs(retractR-RbvR)<=0.03){
				retracted=1;
				pvPut(retracted, SYNC);
				n_RadC=1;
				pvPut(n_RadC, SYNC);
			}
			else{
 				retracted=0;
				pvPut(retracted, SYNC);
			}

		} state  POSITION

	}
}