#%TITLE% WBATT.MAC
#%NAME%
#%B%wbatt.mac%B% - set the white beam ettenuators (ICEPAP)
#
#%CATEGORY% Positioning
#%DESCRIPTION%
#%END%

#%UU%
#%MDESC% Initialise the table of parameters for the existing attenuator
#types.
def _attinittable() '{
global ATTTABLE

  ATTTABLE[1]["nbfilt"] = 4
  ATTTABLE[1]["step_size"] = 4000*35
  ATTTABLE[1]["filter_distance_mm"] = 35

  ATTTABLE[2]["nbfilt"] = 5
  ATTTABLE[2]["step_size"] = 2500*24.5
  ATTTABLE[2]["filter_distance_mm"] = 24.5

  ATTTABLE[3]["nbfilt"] = 4
  ATTTABLE[3]["step_size"] = 10560  
  ATTTABLE[3]["filter_distance_mm"] = 33

  ATTTABLE[4]["nbfilt"] = 5
  ATTTABLE[4]["step_size"] = 7840
  ATTTABLE[4]["filter_distance_mm"] = 24.5

  ATTTABLE[5]["nbfilt"] = 5
  ATTTABLE[5]["step_size"] = 12250
  ATTTABLE[5]["filter_distance_mm"] = 24.5

  ATTTABLE[6]["nbfilt"] = 4
  ATTTABLE[6]["step_size"] = 15000
  ATTTABLE[6]["filter_distance_mm"] = 30

  ATTTABLE[7]["nbfilt"] = 4
  ATTTABLE[7]["step_size"] = 11200
  ATTTABLE[7]["filter_distance_mm"] = 35

  ATTTABLE[8]["nbfilt"] = 4
  ATTTABLE[8]["step_size"] = 9600
  ATTTABLE[8]["filter_distance_mm"] = 30

  ATTTABLE[9]["nbfilt"] = 4
  ATTTABLE[9]["step_size"] = 16500
  ATTTABLE[9]["filter_distance_mm"] = 30

  ATTTABLE[10]["nbfilt"] = 4
  ATTTABLE[10]["step_size"] = -1
  ATTTABLE[10]["filter_distance_mm"] = -1
}'

#%UU% [filename]
#%MDESC% Initialise the attenuator motor, answering questions or get the
#information from %B%filename%B%.
def attsetup '{
global _ATTMOT
global ATTTYPE[] ATTSEC[]
global ATT_CHECK
global ATT_LOCKED_MSG
local mnum nbfilt str

  _attinittable()

  if ($# == 0) {
    # Axis Menmonic
    _ATTMOT = getval("Attenuator motor mnemonic: ",_ATTMOT)
    mnum =_check_mne(_ATTMOT)

    # Axis Type
    ATTTYPE[_ATTMOT] = getval("Attenuator type", ATTTYPE[_ATTMOT])

    if (ATTTABLE[ATTTYPE[_ATTMOT]]["nbfilt"] == 0) {
      printf ("Total number of filters, including empty ones")
      ATTTABLE[ATTTYPE[_ATTMOT]]["nbfilt"] = getval("", \
		ATTTABLE[ATTTYPE[_ATTMOT]]["nbfilt"])
      printf ("Distance between the filters [mm]")
      ATTTABLE[ATTTYPE[_ATTMOT]]["filter_distance_mm"] = getval("", \
		ATTTABLE[ATTTYPE[_ATTMOT]]["filter_distance_mm"])
      printf ("Motor %s steps/mm", _ATTMOT)
      ATTTABLE[ATTTYPE[_ATTMOT]]["step_size"] = getval("", \
		fabs(motor_par(mnum,"step_size")))
    }

    if (ATT_CHECK) {
      if (ATTTABLE[ATTTYPE[_ATTMOT]]["step_size"] != -1) {
        if ((s1 = ATTTABLE[ATTTYPE[_ATTMOT]]["step_size"]) != \
	      (s2 = fabs(motor_par(mnum,"step_size"))))
        printf ("Warning: motor %s should have %d step/mm", _ATTMOT, s1, s2)
        printf ("in config, but has %d\n")			     
      }
    }
  } else {
  }
  if (whatis("user_att_locked") == 0)
    eval("def user_att_locked\'{ }\'")

  if (whatis("user_att_endmove")  == 0)
    eval("def user_att_endmove\'{ }\'")
}'

#%UU% <motor_mnemonic [calibration_step [sign]]>
#%MDESC% Find the "second filter". Set it as reference position (2) and step
#on it. The motor will move to a  hardware limit switch and then try to find
#the second filter position with the help of the home switches. The direction
#of the movement %B%sign%B% is defined as 1 (from negative limit in positive
#direction) or -1 (positive limit in negative direction). The precision for
#the home switch search %B%calibration_step%B% [mm] could be defined - default
#value is 0.004 mm.
def attref '{
local mnum nbfilt
global _CALIB_STEP _SIGN _ATTDEBUG
global _ATTMOT

  if (_att_locked()) {
    printf("%s\n", ATT_LOCKED_MSG)
    exit
  }

  if ($# < 1) {
    printf ("Usage: attref motor_mnemonic [ calib_step [direction]]\n")
    _ATTMOT = getval("Attenuator motor mnemonic: ",_ATTMOT)
    #_CALIB_STEP = fabs(getval("Attenuator calibration step [mm or step]: ", _CALIB_STEP))
    #_SIGN = ("Direction to scan - 1(positive)/-1(negative)",  _SIGN)
    eval(sprintf("_check0 %s", _ATTMOT))
    mnum = motor_num(_ATTMOT)
  } else {
    _check0 "$1"
    _ATTMOT ="$1"
    mnum = $1
    _CALIB_STEP = fabs($2)
    _SIGN = $3
  }
 
  if (!_CALIB_STEP)
    _CALIB_STEP = 0.004

  if (!_SIGN)
    _SIGN = 1

  if (ATTTABLE[ATTTYPE[_ATTMOT]]["filter_distance_mm"] == -1) {
    printf("Axis without switches! Cannot get reference, exit.\n")
    exit
  }

  if (ATTTYPE[_ATTMOT] == 0 || \
	(nbfilt = ATTTABLE[ATTTYPE[_ATTMOT]]["nbfilt"]) == 0) {
    printf("Please, run attsetup first, exit\n")
    exit
 } 

  printf("**** Searching reference position of %s ****\n", _ATTMOT)
  _attref (mnum)
}'

#%IU% (mnum)
#%MDESC% Internal function to find the "zero reference filter". The algorithm
#is based on the fact that the first home switch is passed when we move 
#0.5 filter distances (i.e.33mm/2 = 16mm) from the limit. The %B%mnum%B% is
#the axis ordinal number in the motors list.
def _attref (mnum)'{
local a0 a1 dist step mid[] dist_mm
global ATTFILTPARS[]

  a0 = _att_gotolim(mnum,-1* _SIGN)
  printf ("Limit switch reacheat at position %.2f\n", a0)

  step = _CALIB_STEP
  sleep(.1)

  dist_mm = ATTTABLE[ATTTYPE[motor_mne(mnum)]]["filter_distance_mm"]

  #check if home switch active
  if (_attswitchf (mnum, 0, 99) == 0) {
    printf ("\t - both limit and home switches active\n")
    a0 = A[mnum]
  } else {
    printf("\t - move until the home switch of the first filter reached...\n")
    a0 = _attswitchf (mnum,step,4)
  }
  printf ("Home switch reached at position %.2f\n", a0)
  printf("\t - move until the home switch of the first filter left...\n")
  a1 = _attswitchf (mnum,step,0)
  printf ("Home switch left at position %.2f\n", a1)
  mid[0] = a1 - _SIGN * fabs(a1-a0)/2
  printf ("Home switch size %.2f mm\n", fabs((a1-a0) * dist_mm))

  printf ("\t - go to a position between first and second filter...\n")
  waitmove ; get_angles
  A[mnum] += 0.5 * _SIGN
  move_em
  _show_waitf()

  printf("\t - move until the home switch of the second filter reached...\n")
  a0 = _attswitchf (mnum,step,4)
  printf ("Home switch reached at position %.2f\n", a0)
  printf("\t - move until the home switch of the second filter left...\n")
  a1 = _attswitchf (mnum,step,0)
  printf ("Home switch left at position %.2f\n", a0)
  mid[1] = a1 - _SIGN * fabs(a1-a0)/2
  printf ("Home switch size %.2f mm\n", fabs(a1-a0))

  dist = fabs(mid[1] - mid[0])
  printf ("\t - Filter distance: %.2f mm. Filter (switch) size: %.2f mm\n", \
	dist*dist_mm, fabs((a1-a0)*dist_mm))
  printf ("Moving to the centre of the second filter (%.2f)\n", mid[1])
  A[mnum] = mid[1]
  move_all; waitmove

  chg_dial(mnum,2)
  ATTFILTPOS[motor_mne(mnum)][2] = 2
  ATTFILTPOS[motor_mne(mnum)][1] = 2 - (dist*_SIGN)
  ATTFILTPARS[motor_mne(mnum)]["filtpos2"] = 2
  ATTFILTPARS[motor_mne(mnum)]["filtpos1"] = 2 - (dist*_SIGN)
}'

#%IU% (mnum, sign)
#%MDESC% Go to positive or negative limit (positive means same sign as the
#step_size (positive motor movement). Show some animation during the move.
#Return the current motor position. 
def _att_gotolim(mnum, sign) '{
local stepsign way
  stepsign = fabs(motor_par(mnum, "step_size")) / motor_par(mnum, "step_size")
  if (stepsign * sign < 0) {
    way = "negative"
    chg_dial(mnum, "lim-")
  } else {
    way = "positive"
    chg_dial(mnum, "lim+")
  }
  printf ("\t- go to %s limit, please wait...\n", way)
  _show_waitf()
  return(A[mnum])
}'

#%IU% (motor_mnemonic, motor_steps, parameter)
#%MDESC% If par=99, return the home switch status (0-active, 4-nonactive).
#Otherwise check if home switch signal is active (par=0) or not (par=4) and
#move the motor untill the desired status reached. Retunt the current motor
#position.
def _attswitchf (mnum, mstep, par) '{
local chnum device pos

  device = motor_par(mnum,"device_id")
  if (device == "icepap") {
    pos = _attswitchicepap(mnum, mstep, par)
  }
  return(pos)
}'
#%IU% (motor_number, motor_steps, parameter)
#%MDESC% If par=99, return the home switch status (0-active, 4-nonactive).
#Otherwise check if home switch signal is active (par=0) or not (par=4) and
#move the motor %B%motor_steps%B% untill the desired status reached.
#Retunt the current motor position.
def _attswitchicepap(mnum, mstep, par) '{
local stat ret cmd sign

  if (par == 99) {
    stat = motor_par(mnum,"home_active")
    stat==0?(ret = 4 ):(ret = 0)
    return(ret)
  }

  sign = 1
  if (motor_par(mnum, "step_size") < 0)
    sign = -1

  (_SIGN*sign > 0) ? (cmd = sprintf ("motor_par(%d,\"high_lim_set\")",mnum)):\
	(cmd = sprintf ("motor_par(%d,\"low_lim_set\")",mnum))
  if (par == 0) {
    while(motor_par(mnum,"home_active") != 0) {
      stat = eval(cmd)
      if (stat == 0) {
        A[mnum] += mstep * _SIGN
        move_all
        _show_waitf()
      } else {
        return A[mnum]
      }
    }
  } else {
    while(motor_par(mnum,"home_active") == 0) {
      stat = eval(cmd)
      if (stat == 0) {
        A[mnum] += mstep * _SIGN
        move_all
        _show_waitf()
      } else {
        return A[mnum]
      }
    }
  }
  return A[mnum]
}'


#%IU% (motor_number, sign)
#%MDESC% Return the limit switch status (0-nonactive, 1=-active). Check low or
#high  limit switch according to the sign.
#position.
def _attlimitswitchf (mnum, sign) '{
local device ret

  ret = 0
  device = motor_par(mnum,"device_id")
  if (device == "icepap") {
    if (sign > 0) {
      ret = motor_par(mnum,"high_lim_set")
    } else {
      ret = motor_par(mnum,"low_lim_set")
    }
  }
  if (ret != 0)
    ret = 1
  return(ret)
}'

#%IU% ()
#%MDESC% Draw a bar while action goin on
def _show_waitf() '{
local m
  printf(" ")
  while(wait(0x21)){
    printf("\b%s", substr("-\\|/", (m=++m%4)+1, 1))
    sleep(.05)
  }
  printf("\b")
  getangles
}'