#!/bin/bash -e

# Initialize for step motor of sled:
#   Enable pwm and setup pwm duty
#   Setup gpio pins for step motor control
#   Moving step motor back to initial position

export PATH=$PATH:/usr/libexec

# shellcheck source=meta-facebook/meta-bletchley/recipes-bletchley/plat-tools/files/bletchley-common-functions
source /usr/libexec/bletchley-common-functions

PWM_CLASS_PATH="/sys/class/pwm/pwmchip0"
#Sleld 1~6 using bmc pwm8~13 as motor driver stick
PWM_NUM_OFFSET=7
PWM_PERIOD=2500000 #400HZ
PWM_DUTY=250000    #PWM_PERIOD X 10%
CALIBRATE_TIMEOUT=120

#Enable pwm for sledN
function open_pwm() {
    local SLED_NUM="$1"
    echo "Open pwm of sled$SLED_NUM"
    PWM_NUM=$(( SLED_NUM + PWM_NUM_OFFSET ))
    PWM_PATH="${PWM_CLASS_PATH}/pwm${PWM_NUM}"
    if [ ! -d "$PWM_PATH" ];then
        echo "$PWM_NUM" > "${PWM_CLASS_PATH}/export"
    fi
    if [ -d "$PWM_PATH" ];then
        echo "set pwm period to $PWM_PERIOD ns"
        if ! echo "$PWM_PERIOD" > "${PWM_PATH}/period"; then
            echo "Error: set pwm period fail"
            return 1
        fi

        if ! echo 1 > "${PWM_PATH}/enable"; then
            echo "Error: set pwm enable fail"
            return 1
        fi

        if ! echo "$PWM_DUTY" > "${PWM_PATH}/duty_cycle"; then
            echo "Error: set pwm duty_cycle fail"
            return 1
        fi
    else
        echo "Error: ${PWM_PATH} not exist, export pwm${PWM_NUM} fail"
        return 1
    fi
}

#Init gpio pins for step motor control
function init_gpios() {
    echo "Init GPIOs:"
    motor_ctrl_gpio_pins_names=(    "SLED${1}_MD_STBY_RESET"
                                    "SLED${1}_MD_IOEXP_EN_FAULT"
                                    "SLED${1}_MD_DIR"
                                    "SLED${1}_MD_DECAY"
                                    "SLED${1}_MD_MODE1"
                                    "SLED${1}_MD_MODE2"
                                    "SLED${1}_MD_MODE3" )

    for  gpio_name in "${motor_ctrl_gpio_pins_names[@]}"; do
        set_gpio "$gpio_name"   0
    done
}

if [[ "$1" =~ ^(sled[1-6]{1})$ ]]; then
  SLED=$1
  SLED_NUM=${SLED:4}
else
  #show_usage
  echo "invalid sled name: ${1}"
  exit 1;
fi

#Check if sled is present
if ! is_sled_present "${SLED_NUM}"; then
    echo "${SLED} is not present, skip motor initialize"
    exit 1
fi

#Init gpios
init_gpios "$SLED_NUM"

#enable pwm
open_pwm "$SLED_NUM"

#SLED{N}_MS_DETECT1  (initial position)
DETECT_PIN1="SLED${SLED_NUM}_MS_DETECT1"
INIT_POS=$(get_gpio "$DETECT_PIN1")

if [ "$INIT_POS" -eq 1 ];then
    echo "Making motor back to initial position..."
    motor-ctrl "$SLED" r >/dev/null
    wait_gpio_falling "$DETECT_PIN1" "$CALIBRATE_TIMEOUT"
    motor-ctrl "$SLED" s >/dev/null
fi

INIT_POS=$(get_gpio "$DETECT_PIN1")
if [ "$INIT_POS" -eq 0 ];then
    echo "Motor calibrated to initial position."
    exit 0
else
    echo "Error: Step motor run over 1 cycle but switch never triggered"
    echo "Find motor initial position failed"
    exit 1
fi