(*********************************************************************************
* Copyright: Bernecker+Rainer
* Author:    Mikael Regard
* Created:   Sep 12, 2019/8:21 AM 
 *********************************************************************************
 * Description:

 * 
 *Input:

 *
 *
 * Output:

 *********************************************************************************)


FUNCTION_BLOCK PM_Shuffle	
	
	CASE ShuffleState OF
		SHUFFLE_WAIT:
			IF(Execute)THEN
				Busy := TRUE;
				ShuffleState := SHUFFLE_GET_RANDOM_PARAM;
				IF(DistanceLimit = 0)THEN
					DistanceLimit := 1;
				END_IF;
				MoveCounter := 0;
			END_IF;
			
			
		SHUFFLE_GET_RANDOM_PARAM:
			FOR i := 0 TO UINT_TO_INT(NumOfXbot)*2-1 DO
				Rand_f_0[i].MaxLim := DistanceLimit;
				Rand_f_0[i].MinLim := DistanceLimit*(-1);
				Rand_f_0[i]();
			END_FOR;
			ShuffleState := SHUFFLE_ASSIGN_RANDOM_PARAM;

		SHUFFLE_ASSIGN_RANDOM_PARAM:
			FOR i := 0 TO UINT_TO_INT(NumOfXbot)-1 DO
				PM_LinearMotion_0[i].MsgFrame 	:= MsgFrame;
				PM_LinearMotion_0[i].ReadFrame	:= ReadFrame;
				PM_LinearMotion_0[i].CmdLabel 	:= i;
				PM_LinearMotion_0[i].XbotID := UINT_TO_USINT(XbotID[i]);
				PM_LinearMotion_0[i].TargetX := Rand_f_0[i].Out;
				PM_LinearMotion_0[i].TargetY := Rand_f_0[(NumOfXbot*2-1)-i].Out;
				PM_LinearMotion_0[i].PathType := 0;
				PM_LinearMotion_0[i].PositionMode := 1;
				PM_LinearMotion_0[i].FinalSpeed := 0;
				PM_LinearMotion_0[i].MaxAccel 	:= 10;
				PM_LinearMotion_0[i].MaxSpeed	:= 1;
				PM_LinearMotion_0[i].Execute    := TRUE;
				PM_LinearMotion_0[i]();
				
			END_FOR;	
			ShuffleState := SHUFFLE_MOVE;
			
		SHUFFLE_MOVE:
			AllMoved := TRUE;
			FOR i := 0 TO UINT_TO_INT(NumOfXbot)-1 DO
				IF(PM_LinearMotion_0[i].Error OR PM_LinearMotion_0[i].Done)THEN
					PM_LinearMotion_0[i].Execute := FALSE;
					TON_0.PT := REAL_TO_TIME(MAX(TIME_TO_REAL(TON_0.PT),PM_LinearMotion_0[i].TravelTime*1000));
				ELSIF(PM_LinearMotion_0[i].Execute)THEN
					AllMoved := FALSE;
				END_IF;
				PM_LinearMotion_0[i]();
			END_FOR
			IF(AllMoved)THEN
				TON_0.IN := TRUE;
				IF(TON_0.Q)THEN
					TON_0.IN := FALSE;
					ShuffleState := SHUFFLE_STOP;
				END_IF;
			END_IF;
			TON_0();
		SHUFFLE_STOP:
			PM_Stop_0.XbotID := 0;
			PM_Stop_0.MsgFrame := MsgFrame;
			PM_Stop_0.ReadFrame := ReadFrame;
			PM_Stop_0.Execute := TRUE;
			IF(PM_Stop_0.Done)THEN
				PM_Stop_0.Execute := FALSE;
				MoveCounter := MoveCounter+1;
				IF(MoveCounter >= NumOfMoves)THEN
					ShuffleState := SHUFFLE_DONE;
				ELSE
					ShuffleState := SHUFFLE_GET_RANDOM_PARAM;
				END_IF;
			ELSIF(PM_Stop_0.Error)THEN
				ErrorID := PM_Stop_0.ErrorID;
				PM_Stop_0.Execute := FALSE;
				ShuffleState := SHUFFLE_ERROR;
			END_IF;
			PM_Stop_0();
		
		SHUFFLE_DONE:

			IF(NOT Execute)THEN
				ShuffleState := SHUFFLE_WAIT;
			END_IF;
				
		SHUFFLE_ERROR:
			
			IF(NOT Execute)THEN
				ShuffleState := SHUFFLE_WAIT;
				ErrorID := 0;
			END_IF;
				
	END_CASE;
	
		
	Done 	:= ShuffleState = SHUFFLE_DONE;
	Error 	:= ShuffleState = SHUFFLE_ERROR;
	Busy    := (ShuffleState < SHUFFLE_DONE) AND (ShuffleState >SHUFFLE_WAIT);
		
			
		 
	

	
	
END_FUNCTION_BLOCK



