#
# mechonics.mac
# http://wikiserv.esrf.fr/bliss/index.php/Mechonics
# mechonics CN30 controller also known as PiCo 33 piezo
#


def mechonics_config(mne, type, unit, module, channel) '{
    global MECH_AXIS[] MECH_SPEED[] MECH_STEP[] MECH_CURR[] MECH_DEV[]
    global MECH_CURR_WAY[]
    global MECH_POS MECH_NEG

    if (mne == "..") {
        MECH_AXIS[1] = 0x00
        MECH_AXIS[2] = 0x40
        MECH_AXIS[3] = 0x80

        MECH_SPEED[1] = 0x30
        MECH_SPEED[2] = 0x20
        MECH_SPEED[3] = 0x10
        MECH_SPEED[4] = 0x00

        MECH_POS = 0x00
        MECH_NEG = 0x08

        MECH_STEP[1] = 1
        MECH_STEP[2] = 2
        MECH_STEP[3] = 5
        MECH_STEP[4] = 10
        MECH_STEP[5] = 20
        MECH_STEP[6] = 50
        MECH_STEP[7] = 100

        MECH_DEV[unit] = mechonics_ADDR
    }
}'

def mechonics_cmd(mne, cmd, p1, p2) '{
    local step stepsize

    if (mne != "..") {
        if (p2>0) {
            stepsize = motor_par(mne, "step_size_pos")
        } else {
            stepsize = motor_par(mne, "step_size_neg")
        }
        if (cmd == "start_one") {
            #MECH_CURR[mne] = MECH_CURR[mne] + p2
            step = int(p2 * stepsize + 0.5)
            dev  = motor_par(mne, "unit")
            axis = motor_par(mne, "channel")
            speed = 4
            MECH_CURR_WAY[mne] = stepsize
            _mechonics_mvr(mne, dev, axis, step, speed)

        } else if (cmd == "position") {
            return MECH_CURR[mne]
        } else if (cmd == "set_position") {
            MECH_CURR[mne] = p1
        }
    }
}'

def mechinics_par() '{
}'

def _mechonics_mvr(mne, dev, axis, step, speed) '{
    local raxis rspeed comm st dir stg pdir

    if (axis > 2) {
        raxis = MECH_AXIS[3]
    } else if (axis < 0) {
        raxis = MECH_AXIS[1]
    } else {
        raxis = MECH_AXIS[axis+1]
    }

    if (speed > 3) {
        rspeed = MECH_SPEED[4]
    } else if (speed < 0) {
        rspeed = MECH_SPEED[1]
    } else {
        rspeed = MECH_SPEED[speed+1]
    }

    if (step >= 0) {
        dir = MECH_POS
        pdir = 1
        stg = step
    } else {
        dir = MECH_NEG
        pdir = -1
        stg = fabs(step)
    }

    # Decompose movement in number of steps doable by the controller.
    for (; stg > 0 ;){
        if (stg >= 100)
            st = 7
        else if (stg >= 50)
            st = 6
        else if (stg >= 20)
            st = 5
        else if (stg >= 10)
            st = 4
        else if (stg >= 5)
            st = 3
        else if (stg >= 2)
            st = 2
        else if (stg >= 1)
            st = 1
        else
            st = 0

        if (st > 0) {
            comm = raxis + rspeed + dir + st
            # ex : 0x00   +  0x10   + 0x08 +   7
            # ie:  axis1    speed4  + neg  + steps
            _mechonics_write(dev, comm)

            _mechonics_read(dev)

            stg = stg - MECH_STEP[st]

            # 
            MECH_CURR[mne] += (pdir * MECH_STEP[st] / MECH_CURR_WAY[mne])
        }
    }
}'

def _mechonics_write(dev, comm) '{
    ser_put(MECH_DEV[dev], sprintf("%c",comm))
}'

def _mechonics_read(dev) '{
    local ret

    ret = ser_get(MECH_DEV[dev])

    return ret
}'