#%TITLE% autofocus.mac
#%NAME%
#  Macros for autofocus of minidiff camera from spec
#%CATEGORY% MX
#
#%END%

global AUTOFOCUS[]

def autofocus_setup '{
  local minidiffHo sampx sampy phi framegrabber

  if ($#) {
    minidiffHo = "$1"
  } else {
    minidiffHo = input("Minidiff hardware object (e.g. /minidiff): ")
  }

  if (xml_readRoles(minidiffHo) == 0) {
    sampx = XML_tmp["sampx"]["hwrid"]
    sampy = XML_tmp["sampy"]["hwrid"]
    phi = XML_tmp["phi"]["hwrid"]
    framegrabber = XML_tmp["camera"]["hwrid"]

    xml_read(sampx, "/device/specname")
    AUTOFOCUS["sampx"] = motor_num(XML_tmp[0]["__value__"])
    xml_read(sampy, "/device/specname")
    AUTOFOCUS["sampy"] = motor_num(XML_tmp[0]["__value__"])
    xml_read(phi, "/device/specname")
    AUTOFOCUS["phi"] = motor_num(XML_tmp[0]["__value__"])
    xml_read(framegrabber, "/device/taconame")
    AUTOFOCUS["framegrabber"] = XML_tmp[0]["__value__"]
  } else { 
    eprintf("Autofocus: error: cannot read motors by role in Hardware Object\n")  
    unglobal AUTOFOCUS
    global AUTOFOCUS[]
  }
}'

def autofocus '{
  local negativeStep, n_contrast
  local positiveStep, p_contrast
  local old_contrast, horizontal_shift 

  negativeStep = -0.1
  positiveStep = 0.1

  while ((negativeStep <= -0.015) || (positiveStep >= 0.005)) {
    _autofocus_change_contrast(positiveStep)

    p_contrast = esrf_io(AUTOFOCUS["framegrabber"], "DevCcdGetTGradient", 15000)

    horizontal_shift = negativeStep - positiveStep
    _autofocus_change_contrast(horizontal_shift)

    n_contrast = esrf_io(AUTOFOCUS["framegrabber"], "DevCcdGetTGradient", 15000)

    if ((p_contrast > old_contrast) && (p_contrast > n_contrast)) {
       _autofocus_change_contrast(positiveStep - negativeStep)
       positiveStep = positiveStep * 0.75
       negativeStep = negativeStep * 0.33
       old_contrast = p_contrast
    } else if ((n_contrast > old_contrast) && (n_contrast > p_contrast)) {
       positiveStep = positiveStep * 0.33
       negativeStep = negativeStep * 0.75
       old_contrast = n_contrast
    } else {
       _autofocus_change_contrast(-negativeStep)
       positiveStep = positiveStep * 0.5
       negativeStep = negativeStep * 0.5
    }
  }
}'

def _autofocus_change_contrast(horizontal_shift) '{
  local sampx sampy phi

  sampx = AUTOFOCUS["sampx"]
  sampy = AUTOFOCUS["sampy"]
  phi = AUTOFOCUS["phi"]

  A[sampx] += horizontal_shift*cos(A[phi]*3.14159/180)
  A[sampy] += horizontal_shift*sin(A[phi]*3.14159/180)

  move_em; waitmove
}'