# WPS_4.txt, an Acquisifier script for the WPS1 Instrument.
# Copyright (C) 2010 Kevan Hashemi, Open Source Instruments Inc. 
#
# This Acquisifier script obtains wire positions from multiple WPS
# devices. We use two consecutive acquire steps to capture images from
# the two cameras of each WPS. We name each acquire step after the
# camera it uses. The step that acquires from Camera 1 of WPS P0203 must
# be named P0203_1. These names allow the script to look up each
# camera's calibration constants in its own calibration constant
# database.
# 
# For each camera, the script combines the wire edges in the image with
# the camera calibration constants to obtain a plane in mount
# coordinates that must contain the wire center. When the script has the
# two planes from both cameras of a single WPS, it intersects the planes
# and appends the wire position to a result string.
# 
# At the end of the scripts is an acquisifier step that stores the wire
# positions for all the sensors to a file on a single line. Each line
# consists of sets of three elements: name, x, and y. The x and y are in
# mm in mount coordinates, with the z coordinate assumed to be -5 mm.
# The name is something like P0203.
#
# Kevan Hashemi 25-MAY-10, Open Source Instruments Inc.
#

acquisifier:
name: Init
post_processing: {
	# Set up a run results file in the same directory as the script, and clear the
	# run result string.
	set destination [file dirname $config(daq_script)]
	if {![file exists $destination]} {
		error "Results destination directory \"$destination\" does not exist."
	}
	set config(run_results) [file join $destination WPS_Results.txt]
	set config(run_result) "[clock seconds] "
	
	# A database of WPS calibration constants.
	set config(calibration) {
P0195_1 -4.3073 88.4222 -4.1131 -13.6644 93.6896 -4.0386 -1577.685   0.436 -495.197 10.738  1.3
P0195_2 -3.8608 39.3033 -4.3716 -12.7653 33.5628 -4.3823  1562.571  -2.101  573.215 10.594  0.8
P0197_1 -3.7906 89.4672 -3.8999 -12.5781 94.6556 -3.8052 -1575.547  14.727 -537.395 10.205  1.2
P0197_2 -3.3961 38.2920 -4.5059 -12.2748 32.8203 -4.5708  1574.446   7.187  546.390 10.430  1.5
P0198_1 -3.0843 90.1940 -4.2425 -11.9576 95.7197 -4.3450 -1575.744   0.437 -541.792 10.454  1.2
P0198_2 -3.7644 38.9943 -4.6077 -12.7347 33.4335 -4.6656  1567.472  -2.328  551.404 10.554  1.3
P0199_1 -3.0807 90.0282 -4.1893 -11.7775 95.4612 -4.1093 -1573.379  12.464 -554.912 10.255  1.0
P0199_2 -4.2113 39.6984 -4.1309 -13.1938 34.3032 -4.0849  1571.324  -0.426  519.343 10.478  0.7
P0200_1 -3.7885 89.4043 -4.2377 -12.5270 94.9308 -4.2667 -1567.298  10.040 -556.169 10.339  1.6
P0200_2 -4.2151 38.6110 -4.5358 -13.2755 33.1151 -4.5946  1572.835   8.406  525.869 10.597  1.9
P0201_1 -3.7992 89.0629 -4.0823 -12.6996 94.3538 -4.1472 -1569.350  11.913 -532.884 10.354  1.5
P0201_2 -4.1684 38.7802 -4.1355 -13.1010 33.2285 -4.2102  1560.343  15.907  547.316 10.518  1.3
P0202_1 -3.9688 89.2693 -4.3937 -12.8662 94.7569 -4.2628 -1568.482  14.449 -540.951 10.454  1.5
P0202_2 -4.4135 39.0829 -4.2892 -13.4683 33.8120 -4.3727  1573.235   2.048  514.979 10.477  1.5
P0203_1 -3.0524 89.2967 -4.3284 -11.8453 94.7261 -4.3051 -1573.151   7.914 -543.224 10.334  1.9
P0203_2 -3.4665 38.0783 -4.1433 -12.3490 32.5047 -4.2011  1573.072  -6.642  538.407 10.486  1.6
P0222_1 -3.8979 87.9984 -4.4237 -13.0247 93.3574 -4.4730 -1570.209  -7.919 -513.986 10.584  1.0
P0222_2 -4.5453 39.3411 -4.3267 -13.8010 34.0234 -4.4078  1580.291  12.109  508.396 10.675  1.7
P0223_1 -3.8841 87.8976 -4.2084 -12.9706 93.3465 -4.1088 -1571.456  15.510 -556.602 10.596  1.2
P0223_2 -4.5310 40.0608 -4.3034 -13.7758 34.5910 -4.5274  1574.408  -1.478  520.011 10.744  1.5
P0224_1 -3.3551 88.5051 -4.1745 -12.6030 93.9608 -4.2031 -1568.213  -8.750 -551.288 10.737  1.0
P0224_2 -5.4091 40.4620 -4.1222 -14.2126 35.4105 -3.9837  1571.051  20.308  503.579 10.151  1.0
Q0129_1 -7.0421 89.7874 -3.5899 -17.2263 95.8444 -3.8943 -1573.266  33.136 -551.365 11.853  3.3
Q0129_2 -6.9359 37.6035 -4.0286 -17.3163 31.1393 -4.0094  1569.173  62.729  535.665 12.229  4.6
Q0130_1 -5.0285 88.6823 -1.8141 -15.7230 94.9590 -1.3438 -1567.144 -35.132 -510.121 12.409  6.5
Q0130_2 -2.0807 39.9298 -6.0488 -11.8436 34.2353 -6.7620  1568.044  -0.141  652.405 11.325  6.1
Q0131_1 -5.6082 89.6961 -2.9988 -16.1144 96.4313 -2.7459 -1576.227   5.898 -519.091 12.482  3.1
Q0131_2 -2.4564 40.7047 -4.6130 -12.0240 35.1758 -5.2505  1584.573  39.607  663.654 11.069  2.2
Q0132_1 -4.4475 87.6752 -4.0442 -14.6304 93.6529 -3.9048 -1574.944   1.036 -638.629 11.809  3.1
Q0132_2 -4.1223 37.6675 -4.9617 -14.3354 31.3532 -4.6422  1578.667 -23.815  537.598 12.012  3.5
	}
	set config(calibration) [split [string trim $config(calibration)] \n]

	# We will be storing wire plane coordinates in this variable.
	# When the string is empty, we know that this camera is the first
	# of two corresponding to a WPS.
	set config(previous_plane) ""
}
config:
end.

default:
name: WPS_Init
instrument: WPS
default_post_processing: {
	# Check to make sure that both edges of the wire are visible. This
	# we do by checking to see if there are at least $mp pixels in the
	# wire edge.
	if {![LWDAQ_is_error_result $result]} {
		set mp 100
		if {([lindex $result 3] < $mp) || ([lindex $result 9] < $mp)} {
			set result "ERROR: Wire is out of bounds."
		}
	}

	if {![LWDAQ_is_error_result $result]} {
		# Calculate the wire center and its rotation.
		set x [format %6.5f [expr ([lindex $result 7] + [lindex $result 1]) / 2000]]
		set rot [format %6.5f [expr ([lindex $result 8] + [lindex $result 2]) / 2]]

		# Extract the reference height on the image from the instrument
		# parameters analysis_reference_um, which is in the WPS instrument's
		# info array.
		set ref_y [format %.6f [expr 1.0 * $iinfo(analysis_reference_um) / 1000]]
		
		# Extract the calibration constants for this camera from the database
		# stored in config(calibration). We use the step name to find the
		# correct constants.
		set constants [lindex $config(calibration) [lsearch $config(calibration) "$name *"]]

		# Calculate the plane containing the camera pivot point, the center
		# of the wire image, and therefore the center of the wire itself. We use
		# the camera calibration constants and the wps_wire_plane routine to
		# determine a point in this plane and a normal vector. For the point, 
		# the routine chooses the camera pivot point.
		set plane [lwdaq wps_wire_plane "$x $ref_y" $rot $constants]
	} {
		set plane "0 0 0 1 0 0"
	}

	if {$config(previous_plane) != ""} {
		set wire [lwdaq xyz_plane_plane_intersection $config(previous_plane) $plane]
		set point [lwdaq xyz_line_plane_intersection $wire "0 0 -5 0 0 1"]
		set direction [lrange $wire 3 5]
		append config(run_result) "[string range $name 0 4]\
			[format %.5f [lindex $point 0]]\
			[format %.5f [lindex $point 1]] "
		set config(previous_plane) ""	
	} {
		set config(previous_plane) $plane
	}
}
config:
	daq_adjust_flash 1
	daq_ip_addr 129.64.37.88
	analysis_reference_um 1220
end.

acquire:
name: P0203_1
instrument: WPS
result: None
time: 0
config:
	daq_flash_seconds 0.2
	daq_device_element 1
	daq_driver_socket 1
	daq_mux_socket 7
end.

acquire:
name: P0203_2
instrument: WPS
result: None
time: 0
config:
	daq_flash_seconds 0.2
	daq_device_element 2
	daq_driver_socket 1
	daq_mux_socket 7
end.

acquisifier:
name: Store
post_processing: {
	LWDAQ_print $info(text) $config(run_result) blue
	LWDAQ_print $config(run_results) $config(run_result)
}
config:
end.