#!/bin/sh
# SmartCar Demo 控制脚本
# 用法: sh ctl.sh start | stop | init

DIR="$(dirname "$0")"
PWMCHIP=8
PWMPERIOD=50000

init_pins() {
    echo "[demo] 初始化引脚..."
    for pin in 73 12 13 67 72; do
        echo $pin > /sys/class/gpio/export 2>/dev/null
    done
    echo out > /sys/class/gpio/gpio73/direction 2>/dev/null
    echo out > /sys/class/gpio/gpio12/direction 2>/dev/null
    echo out > /sys/class/gpio/gpio13/direction 2>/dev/null
    echo in  > /sys/class/gpio/gpio67/direction 2>/dev/null
    echo in  > /sys/class/gpio/gpio72/direction 2>/dev/null
    echo 1 > /sys/class/gpio/gpio73/value 2>/dev/null
    echo 0 > /sys/class/gpio/gpio12/value 2>/dev/null
    echo 0 > /sys/class/gpio/gpio13/value 2>/dev/null

    for ch in 1 2; do
        echo $ch > /sys/class/pwm/pwmchip${PWMCHIP}/export 2>/dev/null
        echo ${PWMPERIOD} > /sys/class/pwm/pwmchip${PWMCHIP}/pwm${ch}/period 2>/dev/null
        echo 1 > /sys/class/pwm/pwmchip${PWMCHIP}/pwm${ch}/enable 2>/dev/null
        echo 0 > /sys/class/pwm/pwmchip${PWMCHIP}/pwm${ch}/duty_cycle 2>/dev/null
    done

    echo 0 > /sys/class/pwm/pwmchip1/export 2>/dev/null
    echo 3000000 > /sys/class/pwm/pwmchip1/pwm0/period 2>/dev/null
    echo 1 > /sys/class/pwm/pwmchip1/pwm0/enable 2>/dev/null
    echo 1520000 > /sys/class/pwm/pwmchip1/pwm0/duty_cycle 2>/dev/null

    echo 11  > "$DIR/speed" 2>/dev/null
    echo 8   > "$DIR/deadband" 2>/dev/null
    echo 1.5 > "$DIR/steer_gain" 2>/dev/null
    echo 40  > "$DIR/foresee" 2>/dev/null
    echo 0.7 > "$DIR/foresee_lost_scale" 2>/dev/null
    echo 0.5 > "$DIR/sharp_turn_scale" 2>/dev/null
    echo 0   > "$DIR/start" 2>/dev/null

    echo 0.3  > "$DIR/cone_avoid_gain"   2>/dev/null
    echo 30   > "$DIR/cone_avoid_range"  2>/dev/null
    echo 0.5  > "$DIR/cone_speed"        2>/dev/null
    echo 1    > "$DIR/cone_min_frames"   2>/dev/null
    echo 0    > "$DIR/cone_margin"       2>/dev/null
    echo 0.80 > "$DIR/cone_thresh"       2>/dev/null
    echo 30   > "$DIR/cone_hold_frames"   2>/dev/null

    echo 10   > "$DIR/brake_scale"        2>/dev/null
    echo 10000 > "$DIR/brake_max"         2>/dev/null

    echo 300  > "$DIR/lidar_thresh"       2>/dev/null
    echo 50   > "$DIR/lidar_near"         2>/dev/null
    echo 1200 > "$DIR/lidar_far"          2>/dev/null
    echo 1    > "$DIR/lidar_near_start"   2>/dev/null
    echo 4    > "$DIR/lidar_near_end"     2>/dev/null
    echo 6    > "$DIR/lidar_far_span"     2>/dev/null
    echo 3    > "$DIR/lidar_min_frames"   2>/dev/null
    echo 0.4  > "$DIR/lidar_avoid_gain"   2>/dev/null
    echo 30   > "$DIR/lidar_avoid_range"  2>/dev/null
    echo 0.4  > "$DIR/lidar_speed"        2>/dev/null
    echo 30   > "$DIR/lidar_hold_frames"  2>/dev/null
    echo 1    > "$DIR/lidar_enable"       2>/dev/null

    echo "[demo] 引脚初始化完成"
}

do_start() {
    echo "[demo] 启动..."
    echo 1 > "$DIR/start"
    cd "$DIR"
    LD_LIBRARY_PATH=/home/root/opencv/lib nohup ./smartcar_demo1 > "$DIR/out.log" 2>&1 &
    echo "[demo] PID=$!"
}

do_stop() {
    echo "[demo] 停止..."
    echo 0 > "$DIR/start" 2>/dev/null
    killall -9 smartcar_demo1 2>/dev/null
    sleep 0.3
    echo 0 > /sys/class/pwm/pwmchip${PWMCHIP}/pwm1/duty_cycle 2>/dev/null
    echo 0 > /sys/class/pwm/pwmchip${PWMCHIP}/pwm2/duty_cycle 2>/dev/null
    echo 0 > /sys/class/gpio/gpio73/value 2>/dev/null
    echo "[demo] 已停止, PWM已归零"
}

case "${1}" in
    start)
        init_pins
        do_start
        ;;
    stop)
        do_stop
        ;;
    init)
        init_pins
        ;;
    *)
        echo "用法: sh ctl.sh start | stop | init"
        exit 1
        ;;
esac