#!/usr/bin/tclsh8.3

# Put loco on track segment X5 ish, facing anticlockwise
#  ./measure-speeds /dev/ttyUSB1 2 0 `seq 1 126` |tee santafe.speeds
#                                  ^reverse

# debugging:
# ./measure-speeds /dev/ttyUSB0 1 1 `seq 1 126` 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

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
    set rwy [open $port r+]
    fconfigure $rwy -blocking no -buffering none -encoding binary \
	    -translation binary
    send-now 10
    after 1000
    read -nonewline $rwy
    send-now 11
    fileevent $rwy readable readable
    set buf {}
    set last_fast_speed 0
    set noise {}
    for {set i 0} {$i < 256} {incr i} { append noise \\x [hbytes random 1] }
    foreach f {funcs0to4 funcs5to8} {
	set m [exec ./hostside-old -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
    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 return
	28 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
}

#
# A6 06
# A5 14
#
# X7 02
# X5 04
# X6 0a
# X8 09
#

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 {} {
    global xmit_after xmit_msg funcs_msgs
    set funcs_msg [lindex $funcs_msgs 0]
    set funcs_msgs [lreplace $funcs_msgs 0 0]
    lappend funcs_msgs $funcs_msg
    send-now $xmit_msg$funcs_msg
    set xmit_after [after 10 xmit-now]
}
proc xmit {nmral} {
    global xmit_msg xmit_after detend settle_detend settle_time funcs_msgs
    set msg [eval [list exec ./hostside-old -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
}

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
    if {$last_fast_speed >= 0} {
	run-until -100 {9804 980a}
	set last_fast_speed -1
    }
    run-until -40 {9802 9804}
    run-until-not -40 9804 600
    instruct-speed-for -40 [expr {$speed*$speed}]
    instruct-stop-for 1000
}

proc slow-speed-test {speed} {
    debug "==================== $speed S ===================="
    goto-slow-start-position $speed
    run-until $speed 9804
    timing-start
    run-until $speed 980a
    record-mm-per-s S $speed 231 [timing-finish]
    instruct-stop-for 100
}

proc fast-speed-test {speed} {
    global last_fast_speed
    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 9809
	run-until $speed 9814
	set deficit [expr {$finish - [now-ms]}]
	debug "W $deficit"
	if {$deficit < 0} break
    }
    set last_fast_speed $speed
    set ms 0; set mm 0
    set mslaps 0; set mmlaps 0
    while {$ms < 2000} {
	run-until $speed 9804
	set now [now-ms-click]
	if {$ms} {
	    incr mslaps [timing-finish $now]
	    incr mmlaps 3624
	    debug "++ $mmlaps / $mslaps"
	}
	timing-start $now

	run-until $speed 980a

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

proc speed-test {speed} {
    if {$speed<50} {
	slow-speed-test $speed
    } else {
	fast-speed-test $speed
    }
}

set port [lindex $argv 0]
set loco [lindex $argv 1]
set reverse [lindex $argv 2]

startup

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