# Drive the 3 Monochromator blades as a "bunch"
#
# Author: Jing Chen, jgn@ansto.gov.au
#
# Date: 11/11/2011

# get a specified parameter value of a motor
proc getSetting {mot field} {

   switch $field {
     "position" {
	     set tmpStr1 [$mot]
	     set tmpStr2 [split $tmpStr1 "="]
	     array set tmpstr3 $tmpStr2
	     set ind "$mot "
	     return $tmpstr3($ind)
     }
     "softlowerlim" {
	     set tmpStr1 [$mot softlowerlim]
	     set tmpStr2 [split $tmpStr1 "="]
	     array set tmpstr3 $tmpStr2
	     set ind "$mot.softlowerlim "
	     return $tmpstr3($ind)
     }
     "softupperlim" {
             set tmpStr1 [$mot softupperlim]
             set tmpStr2 [split $tmpStr1 "="]
             array set tmpstr3 $tmpStr2
             set ind "$mot.softupperlim "
             return $tmpstr3($ind)
     }
   }
}


# DriveMono: drive the 3 Monochromators to translate as a "bunch"
proc DriveMono {mot focus} {
	variable currAngleMoma
        variable currAngleMomb
        variable currAngleMomc

	variable mraPosition
        variable mraSoftlowerlim
        variable mraSoftupperlim
	variable mrbPosition
        variable mrbSoftlowerlim
        variable mrbSoftupperlim
	variable mrcPosition
        variable mrcSoftlowerlim
        variable mrcSoftupperlim

	variable translateDis

	variable moveFlag

	# set moveFlag to 1 to move the three motors
	set moveFlag 1

        # Record current angle positions of the three motors
        set currAngleMoma [getSetting moma position]
        set currAngleMomb [getSetting momb position]
        set currAngleMomc [getSetting momc position]

	# Get current focus positions of the three motors mra, mrb and mrc
	set mraPosition [getSetting mra position]
	set mrbPosition [getSetting mrb position]
	set mrcPosition [getSetting mrc position]

	# Get soft lower/upper limits of mra, mrb and mrc
        set mraSoftlowerlim [getSetting mra softlowerlim]
	set mraSoftupperlim [getSetting mra softupperlim]
	set mrbSoftlowerlim [getSetting mrb softlowerlim]
	set mrbSoftupperlim [getSetting mrb softupperlim]
	set mrcSoftlowerlim [getSetting mrc softlowerlim]
	set mrcSoftupperlim [getSetting mrc softupperlim]

	# Calculate moving distance
	set translateDis [expr $focus - [getSetting $mot position]]

	# Determine if the movement is within the ranges of the three motors
	if {[expr $mraPosition + $translateDis] > $mraSoftupperlim ||
	    [expr $mraPosition + $translateDis] < $mraSoftlowerlim} {
	   broadcast "Error: the movement is out of the range limit of motor mra"
	   set moveFlag 0
	}

        if {[expr $mrbPosition + $translateDis] > $mrbSoftupperlim ||
	    [expr $mrbPosition + $translateDis] < $mrbSoftlowerlim} {
           broadcast "Error: the movement is out of the range limit of motor mrb"
           set moveFlag 0
        }

        if {[expr $mrcPosition + $translateDis] > $mrcSoftupperlim ||
	    [expr $mrcPosition + $translateDis] < $mrcSoftlowerlim} {
           broadcast "Error: the movement is out of the range limit of motor mrc"
           set moveFlag 0
        }

	# move the three motors
	if {$moveFlag == 1} {
		# Unlock the three motors
	        mra fixed -1
	        mrb fixed -1
	        mrc fixed -1

		drive moma $currAngleMomb momc $currAngleMomb
	        drive mra [expr $mraPosition + $translateDis] mrb [expr $mrbPosition + $translateDis] mrc [expr $mrcPosition + $translateDis]
        	drive moma $currAngleMoma momc $currAngleMomc

		# Lock the three motors again
	        mra fixed 1
        	mrb fixed 1
	        mrc fixed 1
	}
}



publish DriveMono user