########################################################################
### FILE:	stagecontrol.mod
### PURPOSE:	Methods for operating STAGe subsystem
### SCOPE:	STAGe: branch
### AUTHORS:	Tor Slettnes <tor@slett.net>
###
### Copyrights (C) 2011 Life Technologies.  All rights reserved.
########################################################################

### Method to determine motion ranges on specified axes (X/Y/Z by default)
    DEFine AutoRANGe= $conditional$=False $bounds$=min,max $axis$ <quote>

        IF $conditional$ <quote>
            $axis$:RANGE?
            IF $[ "$1$2" == "" ] <quote.if>
               INFO "Stage axis $axis$ range has not been set on this instrument; will do so now."
            </quote.if>
            ELSE \
               RETURN
        </quote>

        ### Clear any current range to allow movement
        $axis$:RANGE~  

        ### Set margins to 0.5mm
        #$axis$:SCALE?
        #SET margin $[ 500 * $2 ]
        SET margin $(:MCB:MOTion:$axis$:VerifyPositionTolerance?)

        ### Run asynchronously for the remainder of this method
        NEXT

        IF $[ "min" in "$bounds$" ] <quote>
            SET min $(FINDMIN $axis$)
        </quote>

        IF $[ "max" in "$bounds$" ] <quote>
            SET max $(FINDMAX $axis$)
        </quote>

        INFO "Setting axis $axis$ range: ${min:-(No Limit)} - ${max:-(No Limit)} steps"
        RUN "$axis$:RANGe= -steps ${min:+-min=${min}} ${max:+-max=${max}}"

        TRY $axis$:HOME $axis$:HOME
    </quote>


    DESCribe AutoRANGe= <quote>
        Move the stage to travel limits in order to determine and set the 
        motion ranges for the specified axes (X/Y/Z by default).
    </quote>


    DEFINT FINDMIN $axis$ <quote>
        ### Home axis, record encoder position
        TRY $axis$:HOME $axis$:HOME
        SET home $(:MCB:MOTion:$axis$:EncoderPOSition?)
        PUBLish $axis$ "Home at encoder ${home}"

        ### Reset reverse limit switch register
        :MCB:MOTion:$axis$:ReverseLimitSwitchPosition 0

        ### Travel to the negative limit, record position
        SET tln $($axis$:REGisters:TravelLimitNegative?)
        TRY <q.try>
            $axis$:POSition -steps -absolute ${tln}
            SET min   $(:MCB:MOTion:$axis$:POSition?)
        </q.try> <q.except>
            $axis$:StatusBits? RevLimitSwitch
            UNLess $1 ERRor
            SET min $[ $(:MCB:MOTion:$axis$:ReverseLimitSwitchPosition?) + ${margin} ]
        </q.except>

        PUBLish $axis$ "Found minimum position to be ${min}"

        ### Wait a second for what seems to be a MCB race condition
        WAIT 1

        RETURN ${min}
    </quote>


    DEFINT FINDMAX $axis$ <quote>
        ### Home axis
        TRY $axis$:HOME $axis$:HOME

        ### Reset reverse limit switch register
        :MCB:MOTion:$axis$:ReverseLimitSwitchPosition 0

        ### Travel to the positive limit, record position
        SET tlp $($axis$:REGisters:TravelLimitPositive?)
        TRY <q.try>
            $axis$:POSition -steps -absolute ${tlp}
    	SET max $(:MCB:MOTion:$axis$:POSition?)
        </q.try> <q.except>
            $axis$:StatusBits? FwdLimitSwitch
            UNLess $1 ERRor
            SET max $[ $(:MCB:MOTion:$axis$:ForwardLimitSwitchPosition?) - ${margin} ]
        </q.except>

        PUBLish $axis$ "Found maximum position to be ${max}"

        ### Wait a second for what seems to be a MCB race condition
        WAIT 1

        RETURN ${max}
    </quote>


### Aliases for backwards compatibility
    ALIas OFFSet   REFerence=
    ALIas OFFSet?  REFerence?


### Backwards compatible commands to set/get shutter/filter wheel positions
    DEFINT _SETORIGIN $axis$ $value$ <define>
        SET Fcount 5
        SET Scount 3

        $axis$:SCALE?
        SET scale $2
        SET count ${$axis$count:-0}

        IF ${count} <if>
            SET map ""
            REPEAT -counter=step ${count} -start=$value$ -step=${scale} <repeat>
                APPend map " $[ ${step} % $($axis$:StepsPerRevolution?) ]"
            </repeat>
            INFO "Converting $axis$ axis origin=$value$, scale=${scale} to position map:${map}"
            RUN "$axis$:PositionMAP= ${map}"
            CONFiguration= motionconfig.ini Origins $axis$ "$value$  # Retained for compatibility with IS v1.1 and earlier"
        </if>
        ELSE <else>
            ERROR -id=Depricated -axis=$axis$ "Command is depricated, and no longer supported for $axis$ axis."
        </else>
    </define>
    

    DEFINT ORIGin $axis*='' <define>
        WARNing "'STAGE:ORIGIN' has been depricated in favor of 'STAGE:<axis>:PositionMap='.  Please update your client software."

        ITERATE -key=axisvalue "$axis*" <iterate>
            SET axis  $[ "${axisvalue}".split('=')[0].lstrip('-').upper() ]
            SET origin $[ "${axisvalue}".split('=')[1] ]
            _SETORIGIN ${axis} ${origin}
        </iterate>
    </define>

    DESCRIBE ORIGin <describe>
        Set persistent reference positions for the filter wheel and/or shutter (F and S axes).

        NOTE: This command is depricated in favor of "STAGE:<axis>:PositionMap=".
              Unless backwards compatibility is specifically required, please use that command instead.

    Example:
        STAGE:ORIGin -f=43210 -s=1234
    </describe>


    DEFINE ORIGin? <define>
        SET origins ""
        ITERate -key=axis "X Y Z F S" <iterate>
            ${axis}:PositionMap?
	    SET origin $1
            APPEND origins " -${axis}=${origin:-$(${axis}:REFerence?)}"
        </iterate>
        RETURN ${origins:1}
    </define>


    DEFine PositionMapConvert $axis$ <define>
        SET origin $(CONFiguration? -ignoreMissing motionconfig.ini Origins $axis$)

        IF $[ ${#origin} and not len("$($axis$:PositionMAP?)") ] <if>
            _SETORIGIN $axis$ ${origin}
        </if>
    </define>


### Command to define default bounding boxes, if none exist
    DEFINT TetherCheCK <quote>
       IF $[ not "$(BoundingBox*)" ] <quote>
           BoundingBox+ Homing   -ymax=1000 -yhome
           BoundingBox+ Return   -xmin=62000 -xmax=62000 -zmax=100
           BoundingBox+ Scanning -ymin=120000 -ymax=300000 -zmax=100
           BoundingBox+ FlowChip1 -ymin=245000 -ymax=300000 -xmin=11000 -xmax=110000
           BoundingBox+ FlowChip2 -ymin=150000 -ymax=200000 -xmin=11000 -xmax=110000
       </quote>
    </quote>


### Command to disable axes, needed after power loss due to open door
    DEFine OFF $axes$="X,Y,Z,F,S" <quote>
        DEBUG "Disabling axes $axes$..."
        ITERate -key=axis -delimiter="," "$axes$" <quote.iterate>
            ${axis}:REGisters:COMMAND 0 -sync=0
        </quote.iterate>
    </quote>


    DEFine ON $axes$="X,Y,Z,F,S" <quote>
        DEBUG "Enabling axes $axes$..."
        ITERate -key=axis -delimiter="," "$axes$" <quote.iterate>
            ${axis}:REGisters:COMMAND 1 -sync=0
        </quote.iterate>
    </quote>