#!/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" || { echo "E: Cannot change directory to '$T'"; exit 1; }
node=$(uname -n)
machine=$(uname  -m)
date=$(date +%d%b%Y)
krelease=$(uname -r)
kversion=$(uname -v)

calc() { awk "BEGIN { print ($1); }" < /dev/null; }
icalc() { awk "BEGIN { printf \"%.0f\n\", ($1); }" < /dev/null; }

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
}

usage () {
    echo "Usage:"
    echo "       latency-test [base-period [servo-period]]"
    echo "   or:"
    echo "       latency-test [servo-period]  # for single thread"
    echo "   or:"
    echo "       latency-test -h | --help     # (this text)"
    echo ""
    echo "Example:"
    echo "       latency-test 50000 1000000"
    echo ""
    echo "Defaults:     base-period=${BASE}nS servo-period=${SERVO}nS"
    echo "Equivalently: base-period=$(human_time "$BASE") servo-period=$(human_time "$SERVO")"
    echo ""
    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 "If two periods are specified then the shortest time period will"
    echo "always be the base-period"
    echo ""
    echo "The worst-case latency seen in any run of latency-test"
    echo "is written to the file ~/.latency"
    exit 1
}

BASE=$(parse_time 25us); SERVO=$(parse_time 1ms)

case $1 in
  -h|--help) usage;;
esac

case $# in
0) ;;
1) BASE=0; SERVO=$(parse_time "$1") ;;
2) BASE=$(parse_time "$1"); SERVO=$(parse_time "$2") ;;
*) usage;;
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")

# --- status-bar fields (colour red only when they need attention) ---
# realtime: type from 'realtime verify'; non-zero exit means none.
RED="#cc4444"
if RT_TEXT=$(realtime verify 2>/dev/null); then
    RT_FG=""
else
    RT_FG="foreground=\"$RED\""
fi
GOV=$(cat /sys/devices/system/cpu/cpu0/cpufreq/scaling_governor 2>/dev/null)
[ -z "$GOV" ] && GOV="gov?"
if [ "$GOV" = performance ]; then GOV_FG=""; else GOV_FG="foreground=\"$RED\""; fi
if grep -q 'isolcpus=' /proc/cmdline 2>/dev/null; then
    ISOL_TEXT=$(tr ' ' '\n' < /proc/cmdline | grep '^isolcpus=' | head -1)
else
    ISOL_TEXT="no_isolcpus"
fi

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 pyvcp lat.xml
net sl timedelta.0.max => lat.sl
net sj timedelta.0.jitter => lat.sj
net st timedelta.0.out => lat.st
net reset lat.reset => timedelta.0.reset
waitusr lat
loadusr -w bash latexit.sh
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 pyvcp lat.xml
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
net reset lat.reset => timedelta.0.reset timedelta.1.reset
waitusr lat
loadusr -w bash latexit.sh
EOF
fi

cat > lat.xml <<EOF
<pyvcp>
<title title="LinuxCNC / HAL Latency Test"/>
<axisoptions/>
<table>

<tablerow/><tablespan columns="5"/><label wraplength="5i" justify="left">
<text>
"""
$machine on host $node $date
Kernel-release=$krelease
Kernel-version=$kversion

Let this test run for a few minutes, then note the maximum Jitter. You will use it while configuring LinuxCNC.

While the test is running, you should "abuse" the computer. Move windows around on the screen. Surf the web. Copy some large files around on the disk. Play some music. Run an OpenGL program such as glxgears. The idea is to put the PC through its paces while the latency test checks to see what the worst case numbers are."""
</text>
</label>

<tablerow/>
<label/>
  <label text="Max Interval (ns)" font="Helvetica 12"/>
  <label text="Max Jitter (ns)" font="Helvetica 12 bold"/>
  <label text="Last interval (ns)" font="Helvetica 12"/>

<tablerow/><label text="Servo thread ($SERVO_HUMAN):"/><s32 halpin="sl"/><s32 halpin="sj" font="Helvetica 12 bold"/><s32 halpin="st"/>
EOF

if [ "$BASE" -ne 0 ]; then
cat >> lat.xml <<EOF
<tablerow/><label text="Base thread ($BASE_HUMAN):"/><s32 halpin="bl"/><s32 halpin="bj" font="Helvetica 12 bold"/><s32 halpin="bt"/>
EOF
fi

cat >> lat.xml <<EOF
<tablerow/>
  <button text="Reset Statistics" halpin="reset" />

<tablerow/><tablesticky sticky="we"/><tablespan columns="5"/>
<table flexible_columns="[1,2,3]" uniform_columns="aaa">
<tablesticky sticky="we"/>
<tablerow/>
  <label text="$RT_TEXT" $RT_FG relief="sunken" bd="1" anchor="w" padx="6"/>
  <label text="$GOV" $GOV_FG relief="sunken" bd="1" anchor="center" padx="6"/>
  <label text="$ISOL_TEXT" relief="sunken" bd="1" anchor="e" padx="6"/>
</table>

</table>
</pyvcp>
EOF

if [ "$BASE" -eq 0 ]; then
    SIGNALS="halcmd gets sj"
else
    SIGNALS="halcmd gets sj; halcmd gets bj"
fi

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

halrun lat.hal