vwait-based motor/channel

set mydir [file dirname [info script]]
lappend auto_path $mydir
package require AsynCA
package require snit
package require ukaz

source [file join $mydir ddlparser.tcl]
 
DDLParser ddl [ddl_preprocess /home/cgollwit/Programmieren/Messprogramm3/ddl/kmc-include.ddl]

proc getPV {devname attrib} {
	string trim [ddl get device $devname attributes $attrib PV]
}


proc connectPV {devname attrib} {
	variable conntrigger
	set myPV [getPV $devname $attrib]
	if {$myPV eq {}} { return }
	
	lassign [AsynCA::connectwait $myPV] pv cmd
	return $cmd
}

snit::type motor {
	
	component gotopv
	component actpospv
	component stoppv
	component offpv 
	variable actpos
	variable movedone
	variable myname
	component movedonepv

	# store the moving state of all motors
	typevariable movingmotors {}

	typevariable allmotors {}

	typemethod movingmotors {} {
		dict keys [dict filter $movingmotors value 1]
	}

	typemethod waitforall {} {
		# wait until all motors are complete with moving
		while {[llength [$type movingmotors]] > 0} {
			vwait [mytypevar movingmotors]
		}
	}

	typemethod motorpositions {} {
		# read all motors and return dict
		set result {}
		dict for {m cmd} $allmotors {
			# lazy serial way. Could be sped up with AsynCA::readmultiple
			dict set result $m [$cmd read]
		}
		return $result
	}

	constructor {name} {
		install gotopv using connectPV $name gotoPos
		install actpospv using connectPV $name actPos
		install stoppv using connectPV $name stop
		install offpv using connectPV $name offset

		#install movedonepv using ddl get device $name attributes moveDone PV

		set myname $name

		if {$gotopv eq {}} {
			error "Don't know how to move >$name<"
		}
		
		if {$actpospv eq {}} {
			error "Don't know how to read the actPos of >$name<"
		}

		dict set allmotors $name $self

	}

	destructor {
		catch {$gotopv destroy}
		catch {$actpospv destroy}
		catch {$stoppv destroy}
	}	

	method putcallback {where cmd} {
		$gotopv put $where -command $cmd
	}

	method goto {where} {
		$gotopv put $where -command [mymethod moveready]
		dict set movingmotors $myname 1
	}
	
	method moveready {args} {
		dict set movingmotors $myname 0
	}

	method ismoving {} {
		dict get $movingmotors $myname
	}

	method read {} {
		$actpospv get -command [mymethod readback]
		vwait [myvar actpos]
		return $actpos
	}

	method readback {pos meta} {
		set actpos $pos
	}

	method wait {} {
		while {[$self ismoving]} {
			vwait [mytypevar movingmotors]
		}
	}

	method stop {} {
		if {$stoppv eq {}} {
			return -code error "Don't know how to stop"
		}
		$stoppv put 1
	}

	method define {pos1 {pos2 {}}} {
		# move the offset such that currenjt pos1 is equal to pos2
		if {$pos2 eq {}} {
			set pos2 $pos1
			set pos1 [$self read]
		}
		
		set curroff [AsynCA::read $offpv]
		set newoff [expr {$curroff+($pos2-$pos1)}]
		AsynCA::putwait $offpv $newoff

	}

	delegate method monitor to actpospv
}


snit::type detector {
	variable triggerpv
	variable scanmodepv 

	constructor {name} {
		set triggerpv [connectPV $name trigger]
		set scanmodepv [connectPV $name scan]
	}

	destructor {
		catch {$scanmodepv delete}
		catch {$triggerpv delete}
	}

	method trigger {args} {
		if {$triggerpv ne {}} {
			puts "Trigger device"
			$triggerpv put 1 {*}$args
			return 1
		} else {
			puts "Can't trigger"
			return 0
		}
	}

	method scanmode {mode args} {
		if {$scanmodepv ne {}} {
			$scanmodepv put $mode {*}$args
		}
	}
}

snit::type channel {
	variable value
	variable scanmodepv
	variable triggerpv
	component valuepv
	component mama

	typevariable detectors {}
	
	constructor {name} {
		set valuepv [connectPV $name value]
		if {$valuepv eq {}} { error "Don't know how to read >$name<" }
		
		set triggerpv [connectPV $name trigger]
		set scanmodepv [connectPV $name scan]
		
		set mamaname [ddl get device $name mama]
		puts "$name has $mamaname"

		if {$mamaname ne {}} {
			if {[dict exists $detectors $mamaname]} {
				install mama using dict get $detectors $mamaname
			} else {
				install mama using detector $mamaname $mamaname
				dict set detectors $mamaname $mama
			}
		}

	}

	destructor {
		catch {$triggerpv delete}
		catch {$scanmodepv delete}
		catch {$valuepv delete}
	}

	method trigger {args} {
		if {$triggerpv eq {}} {
			if {$mama eq {}} {
				puts "Trigger ignored (can't trigger)"
				return 0
			} else {
				puts "Trigger mama $mama"
				return [$mama trigger {*}$args]
			}
		} else {
			puts "Trigger me"
			$triggerpv put 1 {*}$args
			return 1
		}
	}

	method scanmode {mode args} {
		if {$scanmodepv eq {}} {
			if {$mama ne {}} {
				$mama scanmode $mode {*}$args
			} else {
				return -code error "Can't set scanmode"
			}
		} else {
			$scanmodepv put $mode {*}$args
		}
	}

	method getcallback {cmd} {
		$valuepv get -command $cmd
	}
	
	method read {} {
		$valuepv get -command [mymethod readback]
		vwait [myvar value]
		return $value
	}

	method readback {val meta} {
		set value $val
	}

	method getmama {} {
		return $mama
	}
	
	delegate method monitor to valuepv
	
	typevariable pending {}
	typevariable abort
	typevariable results
	typevariable resultvar
	

	typemethod readall {timeout args} {
		# trigger all detectors, wait for the result
		# should detect if the same mama is to be triggered
		set resultvar {}

		foreach c $args {
			# might be easier with coroutines ?
			if {![$c trigger -command [mytypemethod triggerready $c]]} {
				# if the channel can't be triggered, start an asynchronous get immediately
				$c getcallback [mytypemethod valueready $c]
			}
			dict set pending $c 1
			dict set resultvar $c {}
		}

		set abort false	
		set timeoutid [after $timeout [mytypemethod cancelreading]]

		while {([llength [$type pendingchannels]]>0) && !$abort} {
			vwait [mytypevar pending]
		}

		if {!$abort} { 
			after cancel $timeoutid
		}

		return [dict values $resultvar]
	}
	
	typemethod cancelreading {} {
		# signal the loop to break
		puts "Timeout fired"
		set pending $pending
		set abort true
	}

	typemethod triggerready {c val meta} {
		puts "$c updated, $pending"
		if {[dict get $pending $c]==1} {
			#puts "$c marked done"
			$c getcallback [mytypemethod valueready $c]
		}
	}

	typemethod valueready {c val meta} {
		puts "Got update $c $val"
		dict set pending $c 0
		dict set resultvar $c $val
	}

	typemethod pendingchannels {} {	
		dict keys [dict filter $pending value 1]
	}


}

# service routines for typical scanning procedures
proc setup_protocol {template} {
	variable ftemplate $template
	variable lastfnr 0
}

proc newprotofile {args} {
	set opts {Comment {} Motor {} Detector Value}
	set opts [dict merge $opts $args]

	variable protofd
	closeprotofile

	variable ftemplate
	variable lastfnr

	incr lastfnr
	
	while {true} {
		set fname [format $ftemplate $lastfnr]
		if {![file exists $fname]} { break }
		incr lastfnr
	}
	
	set protofd [open $fname w]
	fconfigure $protofd -buffering line

	dict with opts {
		if {$Comment ne {}} {
			puts $protofd "# Comment = $Comment"
		}
		if {$Motor ne {}} {
			puts $protofd "# Plot:"
			puts $protofd "#   Motor = $Motor"
			puts $protofd "#   Detector = $Detector"
		}
	}

	puts $protofd "# MotorPositions:"
	dict for {m v} [motor motorpositions] {
		puts $protofd "#   $m = $v"
	}

	return $protofd
}

proc closeprotofile {} {
	variable protofd
	if {[info exists protofd]} {
		catch {close $protofd}
		unset protofd
	}
}


proc read_signal {detspec} {
	set detectors [split $detspec /]
	set result [channel readall 5000 {*}$detectors]
	puts "$detectors, [llength $detectors]"
	switch [llength $detectors] {
		1 { return [lindex $result 0] }
		2 { 
			lassign $result nom denom
			if { $denom != 0.0 } {
				return [expr {$nom/$denom}]
			} else {
				return NaN
			}
		}
	}
	error "Wrong detspec"
}


set flcounter 0
proc flyscan {motor to detspec} {
	set top [toplevel .fl[incr ::flcounter]]
	set graph [ukaz::graph $top.g]
	pack $graph -expand yes -fill both
	$graph set xlabel $motor
	$graph set ylabel $detspec

	set data {}
	set plotid [$graph plot $data with linespoints]
	$motor goto $to
	while {[$motor ismoving]} {
		set value [read_signal $detspec]
		puts $value
		lappend data [$motor read] $value
		$graph update $plotid data $data
	}
}

proc xrangeN {a b steps} {
	set result [list $a]
	for {set i 1} {$i<$steps} {incr i} {
		lappend result [expr {$a+($b-$a)/double($steps-1.0)*$i}]
	}
	return $result
}


proc xrange {a b step} {
	if {$step == 0.0} {
		return -code error "Zero stepsize"
	}
	
	if {($a < $b)} {
		set step [expr {abs($step)}]
		set sign 1.0
	} else {
		set step [expr {-abs($step)}]
		set sign -1.0
	}

	set epsilon 1e-10
	
	set i 1
	set x $a
	set result {}
	while {$x*$sign < $b*$sign*(1+$epsilon) } {
		lappend result $x
		set x [expr {$a+$i*$step}]
		incr i
	}
	return $result
}

proc pause {ms} {
	after $ms {set _ 1}
	vwait ::_
}

proc stepscanN {motor detspec a b N args} {
	stepscan_list $motor $detspec [xrangeN $a $b $N] {*}$args
}

proc stepscan {motor detspec a b step} {
	stepscan_list $motor $detspec [xrange $a $b $step] {*}$args
}

proc stepscan_list {motor detspec list args} {
	# parse extra args
	set stdopt {-wait 0}
	set opts [dict merge $stdopt $args]
	
	if {[dict size $opts] != [dict size $stdopt]} {
		set wrongopt [dict filter $args script {k v} {expr ![dict exists $stdopt $k]}]
		return -code error "Wrong options $wrongopt"
	}

	set wtime [dict get $opts -wait]

	set top [toplevel .gr[incr ::grcounter]]
	set graph [ukaz::graph $top.g]
	pack $graph -expand yes -fill both
	$graph set xlabel $motor
	$graph set ylabel $detspec

	set data {}
	set plotid [$graph plot $data with linespoints]
	
	# skip first point
	$motor goto [lindex $list 0]
	$motor wait
	
	if {$wtime != 0} { pause $wtime }

	read_signal $detspec

	set fd [newprotofile Motor $motor Detector $detspec]
	puts $fd "# $motor $detspec Time"

	set t0 [clock microseconds]
	foreach pos $list {
		$motor goto $pos
		$motor wait
		if {$wtime != 0} { pause $wtime }
		
		set value [read_signal $detspec]
		set rbv [$motor read]
		set time [expr {([clock microseconds]-$t0)/1e6}]
		lappend data $rbv $value
		$graph update $plotid data $data
		puts $fd "$rbv $value $time"
	}
	
	closeprotofile
	return $data
}


proc FindCenter {fdata {thresh 0.5}} {

	# compute min/max position 
	set maxx 0; set maxy -Inf; set miny +Inf; set ind 0
	foreach {x y} $fdata {
		if {$y > $maxy} { 
			set maxy $y
			set maxx $x
			set maxi $ind
		}
		if {$y < $miny} {
			set miny $y
		}
		incr ind
	}
	
	set indmax $ind

	# go from max position to the left and right
	# until we hit the threshold
	set rightx {}
	set leftx {}
	set ythresh [expr {$miny*$thresh + $maxy*(1.0-$thresh)}]
	set xold $maxx; set yold $maxy

	for {set ind $maxi} {$ind < $indmax} {incr ind 1} {
		set xcur [lindex $fdata [expr {2*$ind}]]
		set ycur [lindex $fdata [expr {2*$ind+1}]]
		if {$ycur < $ythresh} {
			# interpolate for position 
			set rightx [expr {$xold + ($xcur-$xold)*double($ythresh-$yold)/double($ycur-$yold)}]
			break
		}
		set xold $xcur
		set yold $ycur
	}
	
	set emergencyx1 $xcur

	set xold $maxx; set yold $maxy
	for {set ind $maxi} {$ind >= 0} {incr ind -1} {
		set xcur [lindex $fdata [expr {2*$ind}]]
		set ycur [lindex $fdata [expr {2*$ind+1}]]
		if {$ycur < $ythresh} {
			# interpolate for position 
			set leftx [expr {$xold + ($xcur-$xold)*double($ythresh-$yold)/double($ycur-$yold)}]
			break
		}
		set xold $xcur
		set yold $ycur
	}
	
	set emergencyx2 $xcur

	set cx [expr {($emergencyx1+$emergencyx2)/2}]

	if {$leftx != {} && $rightx != {}} {
		set width [expr {$rightx-$leftx}]
		set cx [expr {($leftx+$rightx)/2}]
	} else {
		puts stderr "Warning: No center found - faking it"
	}

	puts "Center: $cx"
	return $cx

}

proc derive {fdata {sign 1.0}} {
	set fdata [lsort -real -stride 2 -unique $fdata]
	set result {}
	set rest [lassign $fdata oldx oldy]
	foreach {x y} $rest {
		set dx [expr {$oldx-$x}]
		set dy [expr {$oldy-$y}]
		lappend result [expr {($x+$oldx)/2.0}] [expr {(double($dy)/$dx)*$sign}]
		set oldx $x
		set oldy $y
	}
	
	return $result
}