#!/bin/bash
SCRIPT_LOCATION=$(dirname $(readlink -f $0));
if [ -f $SCRIPT_LOCATION/rip-environment ] && [ -z "$EMC2_HOME" ]; then
    . $SCRIPT_LOCATION/rip-environment
fi

T=`mktemp -d`
trap 'cd /; [ -d $T ] && rm -rf $T' SIGINT SIGTERM EXIT
cd $T

calc() { echo "scale=1; $1" | bc; }
icalc() { echo "scale=0; ($1)*1" | bc | sed 's/\..*//'; }

parse_time () {
    case $1 in
    -)   echo "0" ;;
    *ns) icalc "${1%ns}" ;;
    *us|*µs) icalc "1000*${1%us}" ;;
    *ms) icalc "1000*1000*${1%ms}" ;;
    *s)  icalc "1000*1000*1000*${1%s}" ;;
    *)   if [ $1 -lt 1000 ]; then icalc "1000$*1"; else icalc "$1"; fi ;;
    esac
}

human_time () {
    if [ "$1" -eq 0 ]; then echo "-"
    elif [ "$1" -ge 1000000000 ]; then echo "$(calc $1/1000/1000/1000)s"
    elif [ "$1" -ge 1000000 ]; then echo "$(calc $1/1000/1000)ms"
    elif [ "$1" -ge 1000 ]; then echo "$(calc $1/1000)µs"
    else echo "$1ns"
    fi
}

BASE=$(parse_time 25us); SERVO=$(parse_time 1ms)
case $# in
0) ;;
1) BASE=$(parse_time $1) ;;
2) BASE=$(parse_time $1); SERVO=$(parse_time $2) ;;
*)
    echo "Usage: latency-test [base-period [servo-period]]"
    echo "Default: latency-test 25us 1ms"
    echo "Times may be specified with suffix \"s\", \"ms\", \"us\" \"µs\", or \"ns\""
    echo "Times without a suffix and less than 1000 are taken to be in us;"
    echo "other times without a suffix are taken to be in ns"
    echo ""
    echo "The worst-case latency seen in any run of latency-test"
    echo "is written to the file ~/.latency"
    exit 1
esac

if [ "$BASE" -gt "$SERVO" ]; then TEMP=$BASE; BASE=$SERVO; SERVO=$TEMP; fi
if [ "$BASE" -eq "$SERVO" ]; then BASE=0; fi

BASE_HUMAN=$(human_time $BASE)
SERVO_HUMAN=$(human_time $SERVO)
if [ $BASE -eq 0 ]; then
cat > lat.hal <<EOF
loadrt threads name1=slow period1=$SERVO
loadrt timedelta count=1
addf timedelta.0 slow
start
loadusr -Wn lat python lat.py
net sl timedelta.0.max => lat.sl
net sj timedelta.0.jitter => lat.sj
net st timedelta.0.out => lat.st
waitusr lat
loadusr -w bash latexit.sh
EOF

cat > lat.py <<EOF
#!/usr/bin/python
import hal, time
h = hal.component("lat")
h.newpin("sl", hal.HAL_S32, hal.HAL_IN)
h.newpin("sj", hal.HAL_S32, hal.HAL_IN)
h.newpin("st", hal.HAL_S32, hal.HAL_IN)

h.newpin("bl", hal.HAL_S32, hal.HAL_IN)
h.newpin("bj", hal.HAL_S32, hal.HAL_IN)
h.newpin("bt", hal.HAL_S32, hal.HAL_IN)

h.ready()
print "All times are in ns"
print "%36s | %36s"%("Servo thread      ", "Base thread     ")
print "%10.10s | %10.10s | %10.10s | %10.10s | %10.10s | %10.10s"%("Max Interval","Max Jitter","Last interval","Max Interval","Max Jitter","Last interval",)

try:
	while 1:
		time.sleep(1)
		print "%10.10s | %10.10s | %10.10s | %10.10s | %10.10s | %10.10s"%(h["sl"], h["sj"], h["st"], h["bl"], h["bj"], h["bt"])
except KeyboardInterrupt:
	raise SystemExit
EOF

cat > latexit.sh <<EOF
L=\$((halcmd gets sj
    if [ -f $HOME/.latency ]; then cat $HOME/.latency; fi
    ) | sort -n | tail -1)
echo \$L > $HOME/.latency
EOF

else
cat > lat.hal <<EOF
loadrt threads name1=fast period1=$BASE name2=slow period2=$SERVO
loadrt timedelta count=2
addf timedelta.0 fast
addf timedelta.1 slow
start
loadusr -Wn lat python lat.py
net sl timedelta.1.max => lat.sl
net sj timedelta.1.jitter => lat.sj
net st timedelta.1.out => lat.st
net bl timedelta.0.max => lat.bl
net bj timedelta.0.jitter => lat.bj
net bt timedelta.0.out => lat.bt
waitusr lat
loadusr -w bash latexit.sh
EOF

cat > lat.py <<EOF
#!/usr/bin/python
import hal, time
h = hal.component("lat")
h.newpin("sl", hal.HAL_S32, hal.HAL_IN)
h.newpin("sj", hal.HAL_S32, hal.HAL_IN)
h.newpin("st", hal.HAL_S32, hal.HAL_IN)

h.newpin("bl", hal.HAL_S32, hal.HAL_IN)
h.newpin("bj", hal.HAL_S32, hal.HAL_IN)
h.newpin("bt", hal.HAL_S32, hal.HAL_IN)

h.ready()
print "All times are in ns"
print "%36s | %36s"%("Servo thread      ", "Base thread       ")
print "%10.10s | %10.10s | %10.10s | %10.10s | %10.10s | %10.10s"%("Max Interval","Max Jitter","Last interval","Max Interval","Max Jitter","Last interval",)

try:
	while 1:
		time.sleep(1)
		print "%10.10s | %10.10s | %10.10s | %10.10s | %10.10s | %10.10s"%(h["sl"], h["sj"], h["st"], h["bl"], h["bj"], h["bt"])
except KeyboardInterrupt:
	raise SystemExit
EOF

cat > latexit.sh <<EOF
L=\$((halcmd gets sj; halcmd gets bj;
    if [ -f $HOME/.latency ]; then cat $HOME/.latency; fi
    ) | sort -n | tail -1)
echo \$L > $HOME/.latency
EOF
fi


halrun lat.hal
