CallFunction CenterBeamCLA1 200 100 2000 10


# ============================================================
# Adaptive self-calibrating beam centring with CLA1
# JEOL TemExt Plugin / SerialEM
# ============================================================

Function CenterBeamCLA1 4 0 initialDelta minResponse maxDelta tolerance

    # --------------------------------------------------------
    # Read original CLA1 state
    # --------------------------------------------------------

    Plugin JEOL GetCLA1
    initialCLA1X = $JEOLVal1
    initialCLA1Y = $JEOLVal2

    # --------------------------------------------------------
    # Measure initial beam position
    # --------------------------------------------------------

    T
    MeasureBeamPosition

    initialBeamX = $RepVal1
    initialBeamY = $RepVal2

    initialError = SQRT ( $initialBeamX * $initialBeamX + $initialBeamY * $initialBeamY )

    Echo Initial beam offset: X = $initialBeamX px, Y = $initialBeamY px

    If $initialError <= $tolerance
        Echo Beam already centred within requested tolerance.
        Return
    EndIf


    # ========================================================
    # Determine measurable X perturbation
    # ========================================================

    deltaX = $initialDelta
    validX = 0

    Loop 10

        testX = $initialCLA1X + $deltaX

        Plugin JEOL SetDecCLA1 $testX $initialCLA1Y

        T
        MeasureBeamPosition

        responseXX = $RepVal1 - $initialBeamX
        responseYX = $RepVal2 - $initialBeamY

        responseX = SQRT ( $responseXX * $responseXX + $responseYX * $responseYX )

        Plugin JEOL SetDecCLA1 $initialCLA1X $initialCLA1Y

        If $responseX >= $minResponse
            validX = 1
            Break
        EndIf

        deltaX = $deltaX * 2

        If $deltaX > $maxDelta
            Break
        EndIf

    EndLoop

    If $validX == 0
        Echo ERROR: Unable to obtain measurable CLA1-X response.
        Plugin JEOL SetDecCLA1 $initialCLA1X $initialCLA1Y
        Return
    EndIf


    # ========================================================
    # Determine measurable Y perturbation
    # ========================================================

    deltaY = $initialDelta
    validY = 0

    Loop 10

        testY = $initialCLA1Y + $deltaY

        Plugin JEOL SetDecCLA1 $initialCLA1X $testY

        T
        MeasureBeamPosition

        responseXY = $RepVal1 - $initialBeamX
        responseYY = $RepVal2 - $initialBeamY

        responseY = SQRT ( $responseXY * $responseXY + $responseYY * $responseYY )

        Plugin JEOL SetDecCLA1 $initialCLA1X $initialCLA1Y

        If $responseY >= $minResponse
            validY = 1
            Break
        EndIf

        deltaY = $deltaY * 2

        If $deltaY > $maxDelta
            Break
        EndIf

    EndLoop

    If $validY == 0
        Echo ERROR: Unable to obtain measurable CLA1-Y response.
        Plugin JEOL SetDecCLA1 $initialCLA1X $initialCLA1Y
        Return
    EndIf


    # ========================================================
    # Construct and validate local response matrix
    #
    # J = | responseXX/deltaX   responseXY/deltaY |
    #     | responseYX/deltaX   responseYY/deltaY |
    # ========================================================

    J11 = $responseXX / $deltaX
    J21 = $responseYX / $deltaX

    J12 = $responseXY / $deltaY
    J22 = $responseYY / $deltaY

    determinant = $J11 * $J22 - $J12 * $J21

    Echo Local response matrix:
    Echo $J11   $J12
    Echo $J21   $J22
    Echo Determinant: $determinant

    # A scale-normalised conditioning test should be used
    # in the final implementation rather than an arbitrary
    # absolute determinant threshold.


    # ========================================================
    # Solve J * correction = -initialErrorVector
    # ========================================================

    correctionX = (-$J22 * $initialBeamX + $J12 * $initialBeamY) / $determinant

    correctionY = ( $J21 * $initialBeamX - $J11 * $initialBeamY) / $determinant

    targetCLA1X = ROUND ( $initialCLA1X + $correctionX 0 )
    targetCLA1Y = ROUND ( $initialCLA1Y + $correctionY 0 )


    # ========================================================
    # Apply calculated correction
    # ========================================================

    Echo Applying CLA1 correction:
    Echo X: $correctionX DAC
    Echo Y: $correctionY DAC

    Plugin JEOL SetDecCLA1 $targetCLA1X $targetCLA1Y


    # ========================================================
    # Verify result
    # ========================================================

    T
    MeasureBeamPosition

    finalX = $RepVal1
    finalY = $RepVal2

    finalError = SQRT ( $finalX * $finalX + $finalY * $finalY )

    Echo Final beam offset: X = $finalX px, Y = $finalY px
    Echo Residual radial error: $finalError px

EndFunction