#!/usr/bin/tclsh8.3

# Put loco on track segment X5 ish, facing anticlockwise
#  ./measure-speeds /dev/railway AX speed 2 50 0 `seq 1 126` |tee santafe.speeds
#                                     loco^ ^. ^reverse
#                                            `-fastest "slow" speed

# For stop distance at speed (say) 126,
# Put loco on track segment X5 ish, facing ANTICLOCKWISE
#  ./measure-speeds /dev/railway AX stop 2 50 0 40
# OR put on track segment or C7 ish, facing CLOCKWISE
#  ./measure-speeds /dev/railway C stop 2 50 0 40

# When committing, it would be best to include the command line arguments
# in the commit message.

# debugging:
# ./measure-speeds [stuff] 2>&1 | perl -pe '$|=1; s/\r/\n/g' | 3<&0 tclsh8.3 <(echo 'set f [open /dev/fd/3]; proc cm {} { clock clicks -milliseconds }; set z [cm]; while {[gets $f l]>=0} { puts [format "%-10s %s" [expr {[cm]-$z}] $l] }') 

load chiark_tcl_hbytes-1.so

source lib.tcl

proc debug_r {m} { puts -nonewline stderr "$m\r" }
proc debug {m} { puts stderr $m }

proc send-now {str} {
    global rwy
    puts -nonewline $rwy [hbytes h2raw $str]
}

proc startup {} {
    global port rwy buf last_fast_speed noise funcs_msgs loco
    global detend settle_detend
    set detend {}
    set settle_detend {}
    set rwy [open $port r+]
    fconfigure $rwy -blocking no -buffering none -encoding binary \
	    -translation binary
    send-now 10
    after 100
    send-now 10
    after 1000
    read -nonewline $rwy
    fileevent $rwy readable readable
    set buf {}
    set last_fast_speed -1
    set noise {}
    for {set i 0} {$i < 256} {incr i} { append noise \\x [hbytes random 1] }
#    foreach f {funcs0to4 funcs5to8} {
#    }
    foreach f {funcs5to8} {
	set m [exec ./adhoc-test -s/dev/stdout $f $loco 0x1fff]
	set m [hbytes raw2h $m]
	lappend funcs_msgs ffff$m {}
    }
    lappend funcs_msgs {}
}

proc readable {} {
    global rwy buf
    append buf [hbytes raw2h [read $rwy]]
    while {[regexp {^((?:[89a-f].)*[0-7].)(.*)$} $buf dummy msg buf]} {
	proc-msg $msg
    }
}

proc settle-cancel-after {} {
    global settle_after
    catch {
	after cancel $settle_after
	unset settle_after
    }
}

proc proc-msg {msg} {
    global until detend settle_detend settle_time settle_after
    global pointing pointed
    switch -glob $msg {
	90?? {
	    if {[lsearch $settle_detend $msg]<0} { debug_r "o $msg"; return }
	    debug_r "= $msg"
	    settle-cancel-after
	    set settle_after [after $settle_time { set until 1}]
	    return
	}
	01 - 02 - 03 - 20 return
	28 {
	    if {[llength $pointing]} {
		set point [lindex $pointing 0]
		set pointing [lrange $pointing 1 end]
		puts stderr "> $point"
		send-now $point
	    } else {
		set pointed 1
	    }
	    return
	}
	98?? {
	    if {[lsearch $detend $msg]<0} { debug_r "I $msg"; return }
	    debug_r "* $msg"
	    if {$settle_time>0} {
		settle-cancel-after
	    } else {
		set until 1
	    }
	    return
	}
    }
    puts stderr "huh? $msg"
    exit 1
}

proc use-segment {name_AX seg_AX name_C_unused seg_C} {
    global track segs
    set segs($name_AX) 98[set seg_$track]
}

proc define-segments {} {
		# For speed or stopping 	For stopping distance
		# stopping distance		tests only
    use-segment		A5 14				C1 11
    use-segment		X7 02				C9 2e
    use-segment		X5 04				C8 34
    use-segment		X6 0a				C7 32
    use-segment		X8 09				C5 2f
}

proc normalise1 {seg_unused feat_unused pos_unused hex} {
    global pointing
    lappend pointing $hex
}

proc normalise_C {} {
    normalise1		C5 P 0	a045
    normalise1		C9 P 0	a021
    normalise1		C0 P 0	a067
}
proc normalise_AX {} {
    normalise1		X7 P 0	a001
    normalise1		X8 P 0	a041
    normalise1		A6 P 0	a043
    normalise1		A5 P 0	a003
    normalise1		A6 J 0	a060
    normalise1		A5 J 0	a005
}

proc speed-msg {speed} {
    global loco reverse
    list speed126 $loco [expr {abs($speed)}] [expr {$speed<0 != $reverse}]
}

proc run-until {speed new_detend} {
    global loco until detend settle_time settle_detend
    set detend $new_detend
    set settle_detend {}
    set settle_time -1
    catch { unset until }
    xmit [speed-msg $speed]
    vwait until
}

proc run-until-not {speed new_detend new_settle} {
    global loco until detend settle_time settle_detend
    set detend $new_detend
    set settle_detend $new_detend
    hbytes xor settle_detend 0800
    set settle_time $new_settle
    catch { unset until }
    xmit [speed-msg $speed]
    vwait until
}

proc instruct-speed-for {speed ms} {
    global loco until detend settle_detend
    catch { unset until }
    set detend {}
    set settle_detend {}
    xmit [speed-msg $speed]
    after $ms { set until 1 }
    vwait until
}

proc instruct-stop-for {ms} {
    instruct-speed-for 0 $ms
}

proc bgerror {m} {
    global errorInfo errorCode
    puts stderr "----------\nBGERROR\n$m\n$errorInfo\n$errorCode\n----------"
}

proc xmit-now {until_funcs} {
    global xmit_after xmit_msg funcs_msgs
    set m $xmit_msg
    if {![incr until_funcs -1]} {
	set funcs_msg [lindex $funcs_msgs 0]
	set funcs_msgs [lreplace $funcs_msgs 0 0]
	lappend funcs_msgs $funcs_msg
	append m $funcs_msg
	set until_funcs 5
    }
    send-now $m
    set xmit_after [after 10 xmit-now $until_funcs]
}
proc xmit {nmral} {
    global xmit_msg xmit_after detend settle_detend settle_time funcs_msgs
    set msg [eval [list exec ./adhoc-test -s/dev/stdout] $nmral]
    set xmit_msg [hbytes raw2h $msg]
    debug "> $xmit_msg $nmral (E $detend $settle_detend $settle_time) \[$funcs_msgs]"
    catch { after cancel $xmit_after }
    xmit-now 10
}

proc now-ms {} {
    clock clicks -milliseconds
}

proc now-ms-click {{returnthis {}}} {
    if {[string length $returnthis]} { return $returnthis }
    global noise
    set now [now-ms]
#    set f [open |[list sh -c {"$@" >/dev/dsp} x printf $noise]]
#    fileevent $f readable [list close $f]
    return $now
}

proc record-mm-per-s {how speed mm ms} {
    debug "                    $how $speed: $mm / $ms"
    set mmpers [expr {$mm*1.0/$ms}]
    puts [format "%s %3d %g" $how $speed $mmpers]
}
proc timing-start {{now {}}} {
    global start
    set start [now-ms-click $now]
}
proc timing-finish {{now {}}} {
    global start
    set finish [now-ms-click $now]
    return [expr {$finish-$start}]
}

proc goto-slow-start-position {speed} {
    global last_fast_speed segs
    if {$last_fast_speed > 0} {
	run-until -100 [list $segs(X5) $segs(X6)]
    } elseif {$last_fast_speed == 0} {
	run-until -100 [list $segs(X5) $segs(X6) $segs(X7)]
    } elseif {$last_fast_speed < 0} {
	run-until 100 [list $segs(X5) $segs(X6) $segs(X8)]
    }
    run-until -40 [list $segs(X7) $segs(X5)]
    run-until-not -40 $segs(X5) 600
    instruct-speed-for -40 [expr {$speed*$speed}]
    instruct-stop-for 1000
}

proc begin-slow-test {speed} {
    global segs
    debug "==================== $speed S ===================="
    goto-slow-start-position $speed
    run-until $speed $segs(X5)
}

proc slow-speed-test {speed} {
    global segs last_fast_speed
    begin-slow-test $speed
    timing-start
    run-until $speed $segs(X6)
    record-mm-per-s S $speed 231 [timing-finish]
    instruct-stop-for 100
    set last_fast_speed 0
}

proc slow-stop-test {speed} {
    begin-slow-test $speed
    finish-stop-test
}   

proc finish-stop-test {} {
    global forever
    after 10000 {
	send-now 10
	exit 0
    }
    run-until 0 {}
    vwait forever
}

proc prepare-fast-test {speed} {
    global last_fast_speed segs
    debug "==================== $speed F ===================="
    set start [now-ms]
    set acceltime [expr {abs($last_fast_speed-$speed)*100}]
    debug "A $acceltime"
    set finish [expr {$start + $acceltime}]
    while 1 {
	# we go round and round, keeping track of roughly where we
	#  are, until there has been enough time to get the speed right
	run-until $speed $segs(X8)
	run-until $speed $segs(A5)
	set deficit [expr {$finish - [now-ms]}]
	debug "W $deficit"
	if {$deficit < 0} break
    }
    set last_fast_speed $speed
}

proc fast-speed-test {speed} {
    global segs
    prepare-fast-test $speed
    set ms 0; set mm 0
    set mslaps 0; set mmlaps 0
    while {$ms < 2000} {
	run-until $speed $segs(X5)
	set now [now-ms-click]
	if {$ms} {
	    incr mslaps [timing-finish $now]
	    incr mmlaps 3624
	    debug "++ $mmlaps / $mslaps"
	}
	timing-start $now

	run-until $speed $segs(X6)

	incr ms [timing-finish]
	incr mm 231
	debug "+- $mm / $ms"
	run-until-not $speed $segs(X6) 600
    }
    record-mm-per-s F $speed $mm $ms
    if {$mslaps} { record-mm-per-s L $speed $mmlaps $mslaps }
}

proc fast-stop-test {speed} {
    global segs
    prepare-fast-test $speed
    run-until $speed $segs(X5)
    finish-stop-test
}

proc run-test {how speed} {
    global track fastestslow
    if {[string compare $track AX] && [string compare $how stop]} {
	error "$track $how ?"
    }
    if {$speed<=$fastestslow} {
	slow-$how-test $speed
    } else {
	fast-$how-test $speed
    }
}

manyset $argv port track how loco fastestslow reverse

define-segments
startup
normalise_$track
send-now 11
vwait pointed

foreach s [lrange $argv 6 end] {
    run-test $how $s
}
instruct-stop-for 1000
send-now 10
instruct-stop-for 500
debug FINISHED