path: root/calibration.e

class CALIBRATION
-- This class is able to
-- - build some calibration grid with user assistance,
-- - save calibration grid,
-- - reload calibration grid,
-- - use calibration grid for coordinates conversion.

-- Calibration file format:
-- device_description_file_name
-- [offset_map] x
-- [offset_map] y
-- [parameter_offset] p0 x
-- [parameter_offset] p0 y
-- [parameter_offset] p1 x
-- [parameter_offset] p1 y
-- ...
-- [parameter_offset] pnb_param-1 x
-- [parameter_offset] pnb_param-1 y
--
--
-- offset_map format:
-- nb_point_x nb_point_y
-- offset
-- ...
-- offset (nb_point_x * nb_point_y values)
--
--
-- parameter_offset format:
-- nb_point_x nb_point_y nb_param_values
-- offset
-- ...
-- offset (nb_point_x * nb_point_y * nb_param_values values)
--
	
create {ANY}
	make, load_from

create {CALIBRATION_HANDLER}
	uncalibrated
	
feature {}
	make is
		do
		end

feature {ANY}
	write_to (os: OUTPUT_STREAM) is
		require
			os.is_connected
		local
			i: INTEGER
		do
			os.put_line(device_description_name)
			x_map.write_to(os)
			y_map.write_to(os)
			from
				i := x_parameters_maps.lower
			until
				i > x_parameters_maps.upper
			loop
				x_parameters_maps.item(i).write_to(os)
				y_parameters_maps.item(i).write_to(os)
				i := i + 1
			end
		ensure
			os.is_connected
		end

	load_from (data: INPUT_STREAM) is
		require
			data.is_connected
		local
			i: INTEGER
			po: PARAMETER_OFFSET
			device_file: TEXT_FILE_READ
		do
			data.read_line
			device_description_name := data.last_string.twin
			create device_file.connect_to(device_description_name)
			create device.load_from(device_file)
			device_file.disconnect
			create x_map.load_from(data, device)
			create y_map.load_from(data, device)
			from
				create x_parameters_maps.make(device.nb_parameters)
				create y_parameters_maps.make(device.nb_parameters)
				i := x_parameters_maps.lower
			until
				i > x_parameters_maps.upper
			loop
				create po.load_from(data, device, i + 1)
				x_parameters_maps.put(po, i)
				create po.load_from(data, device, i + 1)
				y_parameters_maps.put(po, i)
				i := i + 1
			end
		end


	convert (x_device, y_device: INTEGER ; params: FAST_ARRAY[INTEGER]) is
			-- Result is available in last_x, last_y
		require
			x_device.in_range(0, device.width)
			y_device.in_range(0, device.height)
			params /= Void implies params.count = device.nb_parameters
		local
			i: INTEGER
		do
			last_x := x_device + x_map.offset_at(x_device, y_device)
			last_y := y_device + y_map.offset_at(x_device, y_device)
			if params /= Void then
				from
					i := x_parameters_maps.lower
				until
					i > x_parameters_maps.upper
				loop
					last_x := last_x + x_parameters_maps.item(i).offset_at(x_device, y_device, params.item(i))
					last_y := last_y + y_parameters_maps.item(i).offset_at(x_device, y_device, params.item(i))
					i := i + 1
				end
			end
			if last_x < 0 then
				last_x := 0
			end
			if last_y < 0 then
				last_y := 0
			end
			if last_x > device.width then
				last_x := device.width
			end
			if last_y > device.height then
				last_y := device.height
			end
		ensure
			last_x.in_range(0, device.width)
			last_y.in_range(0, device.height)
		end

	last_x, last_y: INTEGER
			-- Last conversion result (in device coordinates)

feature {}
	partial_offset (x_device, y_device, param_index: INTEGER) is
			-- Sum offset with parameters set to 0, up to param_index excluded.
			-- Result is available in last_x, last_y
		require
			x_device.in_range(0, device.width)
			y_device.in_range(0, device.height)
			param_index.in_range(0, device.nb_parameters - 1)
		local
			i: INTEGER
		do
			last_x := x_map.offset_at(x_device, y_device)
			last_y := y_map.offset_at(x_device, y_device)
			from
				i := x_parameters_maps.lower
			until
				i >= param_index
			loop
				last_x := last_x + x_parameters_maps.item(i).offset_at(x_device, y_device, 0)
				last_y := last_y + y_parameters_maps.item(i).offset_at(x_device, y_device, 0)
				i := i + 1
			end
		end

feature {}
	file_tools: FILE_TOOLS
	
	uncalibrated(device_file_name: STRING) is
		require
			file_tools.file_exists(device_file_name)
		local
			i: INTEGER
			po: PARAMETER_OFFSET
			device_file: TEXT_FILE_READ
		do
			device_description_name := device_file_name
			create device_file.connect_to(device_file_name)
			create device.load_from(device_file)
			device_file.disconnect
			create x_map.uncalibrated(device)
			create y_map.uncalibrated(device)
			from
				create x_parameters_maps.make(device.nb_parameters)
				create y_parameters_maps.make(device.nb_parameters)
				i := x_parameters_maps.lower
			until
				i > x_parameters_maps.upper
			loop
				create po.uncalibrated(device, i + 1)
				x_parameters_maps.put(po, i)
				create po.uncalibrated(device, i + 1)
				y_parameters_maps.put(po, i)
				i := i + 1
			end
			create reference_points.make(device.nb_parameters)
		end

	reference_points: REFERENCE_POINTS

	rebuild_maps is
		local
			i: INTEGER
		do
 			from
 				i := x_parameters_maps.lower
 			until
 				i > x_parameters_maps.upper
 			loop
 				x_parameters_maps.item(i).reset
 				y_parameters_maps.item(i).reset
 				i := i + 1
 			end
			from
				build_position_maps
				i := x_parameters_maps.lower
			until
				i > x_parameters_maps.upper
			loop
				build_parameter_maps(i)
				i := i + 1
			end
		end

feature {} -- building maps
--	nb_intervals: INTEGER is 100 --*** auto ?
	nb_intervals: INTEGER is 10

	build_position_maps is
		local
			i, j: INTEGER
			xmap, ymap: FAST_ARRAY2[INTEGER]
		do
			from
				xmap := x_map.empty_map(nb_intervals + 1)
				ymap := y_map.empty_map(nb_intervals + 1)
				i := xmap.lower1
			until
				i > xmap.upper1
			loop
				from
					j := xmap.lower2
				until
					j > xmap.upper2
				loop
					xmap.put(build_x_offset_for(i/nb_intervals, j/nb_intervals), i, j)
					ymap.put(build_y_offset_for(i/nb_intervals, j/nb_intervals), i, j)
					j := j + 1
				end
				i := i + 1
			end
		end

	build_x_offset_for(x, y: REAL): INTEGER is
		local
			i, j, v: INTEGER
			continue: BOOLEAN
			left_index, right_index: INTEGER
			left_distance, right_distance: REAL
			left_error, right_error: INTEGER
			tmp, length, dev: REAL
		do
			from
				left_distance := device.nb_parameters * 3
				right_distance := left_distance
				left_index := -1
				right_index := -1
				i := reference_points.count - 1
			until
				i < 0
			loop
				from
					tmp := (reference_points.x_target(i) / device.width - x) * 3
					length := tmp * tmp
					tmp := reference_points.y_target(i) / device.height - y
					length := length + tmp * tmp
					j := device.nb_parameters - 1
					continue := True
				until
					j < 0 or not continue
				loop
					v := reference_points.parameter_value(i, j)
					dev := rate(v, device.min_parameter(j + 1), device.max_parameter(j + 1))
					continue := dev.in_range(0.4, 0.6)
					dev := dev - 0.5
					length := length + dev * dev
					j := j - 1
				end
				if continue then
					if reference_points.x_target(i) / device.width < x then
						if length < left_distance then
							left_index := i
							left_distance := length
						end
					else
						if length < right_distance then
							right_index := i
							right_distance := length
						end
					end
				end
				i := i - 1
			end
			if right_index = -1 then
				Result := reference_points.x_target(left_index) - reference_points.x_measure(left_index)
			elseif left_index = -1 then
				Result := reference_points.x_target(right_index) - reference_points.x_measure(right_index)
			else
				left_distance := left_distance.sqrt
				right_distance := right_distance.sqrt
-- 				io.put_string(once "x = "); io.put_real(x); io.put_string(once "  y = "); io.put_real(y); io.put_new_line
-- 				io.put_string(once "ld = "); io.put_real(left_distance); io.put_character(' '); io.put_integer(left_index);io.put_new_line
-- 				io.put_string(once "rd = "); io.put_real(right_distance); io.put_character(' '); io.put_integer(right_index); io.put_new_line
				left_error := reference_points.x_target(left_index) - reference_points.x_measure(left_index)
				right_error := reference_points.x_target(right_index) - reference_points.x_measure(right_index)
-- 				io.put_string(once "le = "); io.put_real(left_error); io.put_new_line
-- 				io.put_string(once "re = "); io.put_real(right_error); io.put_new_line
				Result := ((right_error*left_distance + left_error*right_distance) / (left_distance + right_distance)).force_to_integer_32
-- 				io.put_string(once "R = "); io.put_integer(Result); io.put_new_line; io.put_new_line
			end
		end

	build_y_offset_for(x, y: REAL): INTEGER is
		local
			i, j, v: INTEGER
			continue: BOOLEAN
			up_index, down_index: INTEGER
			up_distance, down_distance: REAL
			up_error, down_error: INTEGER
			tmp, length, dev: REAL
		do
			from
				up_distance := device.nb_parameters * 3
				down_distance := up_distance
				up_index := -1
				down_index := -1
				i := reference_points.count - 1
			until
				i < 0
			loop
				from
					tmp := reference_points.x_target(i) / device.width - x
					length := tmp * tmp
					tmp := (reference_points.y_target(i) / device.height - y) * 3
					length := length + tmp * tmp
					j := device.nb_parameters - 1
					continue := True
				until
					j < 0 or not continue
				loop
					v := reference_points.parameter_value(i, j)
					dev := rate(v, device.min_parameter(j + 1), device.max_parameter(j + 1))
					continue := dev.in_range(0.4, 0.6)
					dev := dev - 0.5
					length := length + dev * dev
					j := j - 1
				end
				if continue then
					if reference_points.y_target(i) / device.height < y then
						if length < up_distance then
							up_index := i
							up_distance := length
						end
					else
						if length < down_distance then
							down_index := i
							down_distance := length
						end
					end
				end
				i := i - 1
			end
			if down_index = -1 then
				Result := reference_points.y_target(up_index) - reference_points.y_measure(up_index)
			elseif up_index = -1 then
				Result := reference_points.y_target(down_index) - reference_points.y_measure(down_index)
			else
				up_distance := up_distance.sqrt
				down_distance := down_distance.sqrt
				up_error := reference_points.y_target(up_index) - reference_points.y_measure(up_index)
				down_error := reference_points.y_target(down_index) - reference_points.y_measure(down_index)
				Result := ((down_error*up_distance + up_error*down_distance) / (up_distance + down_distance)).force_to_integer_32
			end
		end

	param_division: INTEGER is 4
	
	build_parameter_maps(param: INTEGER) is
		require
			param.in_range(0, device.nb_parameters - 1)
		local
			i, j, k: INTEGER
			xmap, ymap: FAST_ARRAY3[INTEGER]
			hstep, vstep: INTEGER
			correction_x, correction_y: INTEGER
		do
			hstep := x_map.horizontal_step.to_integer_32
			vstep := y_map.vertical_step.to_integer_32
			xmap := x_parameters_maps.item(param).empty_map(nb_intervals + 1, param_division + 1)
			ymap := y_parameters_maps.item(param).empty_map(nb_intervals + 1, param_division + 1)
			from
				i := xmap.lower1
			until
				i > xmap.upper1
			loop
				from
					j := xmap.lower2
				until
					j > xmap.upper2
				loop
					from
						k := xmap.lower3
						partial_offset(i * hstep, j * vstep, param)
						correction_x := last_x
						correction_y := last_y
					until
						k > xmap.upper3
					loop
						xmap.put(build_x_offset_for_parameter(i/nb_intervals, j/nb_intervals, k / param_division, param, correction_x), i, j, k)
						ymap.put(build_y_offset_for_parameter(i/nb_intervals, j/nb_intervals, k / param_division, param, correction_y), i, j, k)
						k := k + 1
					end
					j := j + 1
				end
				i := i + 1
			end
		end

	build_x_offset_for_parameter(x, y, z: REAL; index: INTEGER; cumulated_correction: INTEGER): INTEGER is
		local
			i, j, v: INTEGER
			continue: BOOLEAN
			left_index, right_index: INTEGER
			left_distance, right_distance: REAL
			left_error, right_error: INTEGER
			tmp, length, dev: REAL
		do
			from
				left_distance := device.nb_parameters * 3
				right_distance := left_distance
				left_index := -1
				right_index := -1
				i := reference_points.count - 1
			until
				i < 0
			loop
				from
					tmp := reference_points.x_target(i) / device.width - x
					length := tmp * tmp
					tmp := reference_points.y_target(i) / device.height - y
					length := length + tmp * tmp
					j := device.nb_parameters - 1
					continue := True
				until
					j < 0 or not continue
				loop
					v := reference_points.parameter_value(i, j)
					dev := rate(v, device.min_parameter(j + 1), device.max_parameter(j + 1))
					if j = index then
						continue := dev.in_range(z - 0.15, z + 0.15)
						dev := (dev - z) * 3
					else
						continue := dev.in_range(0.4, 0.6)
						dev := dev - 0.5
					end
					length := length + dev * dev
					j := j - 1
				end
				if continue then
					if reference_points.x_target(i) / device.width < x then
						if length < left_distance then
							left_index := i
							left_distance := length
						end
					else
						if length < right_distance then
							right_index := i
							right_distance := length
						end
					end
				end
				i := i - 1
			end
			if right_index /= -1 then
				if left_index /= -1 then
					left_distance := left_distance.sqrt
					right_distance := right_distance.sqrt
					left_error := reference_points.x_target(left_index) - reference_points.x_measure(left_index)
					right_error := reference_points.x_target(right_index) - reference_points.x_measure(right_index)
					Result := ((right_error*left_distance + left_error*right_distance) / (left_distance + right_distance)).force_to_integer_32 - cumulated_correction
				else
					Result := reference_points.x_target(right_index) - reference_points.x_measure(right_index) - cumulated_correction
				end
			else
				if left_index /= -1 then
					Result := reference_points.x_target(left_index) - reference_points.x_measure(left_index) - cumulated_correction
				else
					Result := 0
				end
			end
		end

	build_y_offset_for_parameter(x, y, z: REAL; index: INTEGER; cumulated_correction: INTEGER): INTEGER is
		do
		end

	rate(value, min, max: INTEGER): REAL is
		require
			value.in_range(min, max)
		do
			Result := (value - min) / (max - min)
		ensure
			Result.in_range(0, 1)
		end

feature {CALIBRATION_HANDLER}
	add_measure(x_target, y_target: INTEGER; x_measure, y_measure: INTEGER; parameters_values: FAST_ARRAY[INTEGER]) is
		require
			device.valid_x(x_target)
			device.valid_y(y_target)
			device.valid_x(x_measure)
			device.valid_y(y_measure)
			device.valid_parameters(parameters_values)
		do
			reference_points.add_point(x_target, y_target, x_measure, y_measure, parameters_values)
			rebuild_maps
		end

	x_map, y_map: OFFSET_MAP

	x_parameters_maps, y_parameters_maps: FAST_ARRAY[PARAMETER_OFFSET]

	device: DEVICE_DESCRIPTION

feature {}
	device_description_name: STRING

invariant
	(x_parameters_maps /= Void) = (y_parameters_maps /= Void)
	
	x_parameters_maps /= Void implies x_parameters_maps.count = y_parameters_maps.count

	device.nb_parameters = 0 implies x_parameters_maps = Void

	device.nb_parameters /= 0 implies x_parameters_maps.count = device.nb_parameters
end