#!/bin/bash -e
#
# Control step motor rotate of sled

function set_gpio()
{
    NET_NAME=$1
    OUT_VAL=$2
    mapfile -t -d " " GPIO_INFO < <(gpiofind "$NET_NAME")
    if [ "${#GPIO_INFO[@]}" -ne 2 ]; then
        echo "set_gpio: can not find gpio, $NET_NAME"
        return 1
    fi
    echo -n "set_gpio: set $NET_NAME = $OUT_VAL"
    if ! gpioset "${GPIO_INFO[0]}" "${GPIO_INFO[1]%$'\n'}"="$OUT_VAL"; then
        echo " failed"
        return 1
    fi
    echo " success"
    return 0
}

#Get i2c bus number for sledN
function get_bus_num() {
    SLED_NUM=$1
    local bus=0

    if [[ "$SLED_NUM" = [0-5] ]]; then
        bus="$SLED_NUM"
    fi
    echo "$bus"
}

#Enable sledN Motor VRef
function open_vref() {
    i2cset -f -y "${1}" 0x67 0x06 0x95
}

#Disable sledN Motor VRef
function close_vref() {
    i2cset -f -y "${1}" 0x67 0x06 0x55
}

#######################################
# Setting step motor control pins to start/stop motor
# Arguments:
#   1. SLED NUMBER
#   2. Value of STBY RESET PIN
#   3. Value of ENABLE PIN
#   4. Value of DIRECTION PIN
#######################################
function set_motor() {
    STBY_PIN="SLED${1}_MD_STBY_RESET"
    EN_PIN="SLED${1}_MD_IOEXP_EN_FAULT"
    DIR_PIN="SLED${1}_MD_DIR"
    set_gpio "$STBY_PIN" "$2"
    set_gpio "$EN_PIN"   "$3"
    set_gpio "$DIR_PIN"  "$4"
}

function show_usage(){
    echo "Usage: motor-ctrl [sled0 | sled1 | sled2 | sled3 | sled4 | sled5]  [f r s]"
    echo "      f : Step Motor go forward"
    echo "      r : Step Motor go reverse"
    echo "      s : Step Motor stop "
}

if [ $# -ne 2 ]; then
    show_usage
    exit 1;
fi

if [[ "$1" =~ ^(slot[0-5]{1})$ ]] || [[ "$1" =~ ^(sled[0-5]{1})$ ]]; then
    SLED=$1
    SLED_NUM=${SLED:4}
    I2C_NUM=$(get_bus_num "$SLED_NUM")
    ACTION=$2
else
     echo "invalid sled name: $1"
    exit 1;
fi

if [[ "$ACTION" == "s" ]]; then
    echo "stop motor"
    set_motor "$SLED_NUM" 1 0 0
    close_vref "$I2C_NUM"
elif [[ "$ACTION" == "f" ]];then
    echo "start motor, direction:forward"
    set_motor  "$SLED_NUM" 1 1 0
    open_vref "$I2C_NUM"
elif [[ "$ACTION" == "r" ]];then
    echo "start motor, direction:reverse"
    set_motor "$SLED_NUM" 1 1 1
    open_vref "$I2C_NUM"
else
    echo "Error: Unknown action!"
    exit 1
fi

exit 0