{<pre>}
program p;

{
	Wire Position Sensor Calibration Fitting Program
	Copyright (C) 2009 Kevan Hashemi, Open Source Instruments Inc.
	
	This program is free software; you can redistribute it and/or modify it
	under the terms of the GNU General Public License as published by the Free
	Software Foundation; either version 2 of the License, or (at your option)
	any later version.

	This program is distributed in the hope that it will be useful, but WITHOUT
	ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
	FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
	more details.

	You should have received a copy of the GNU General Public License along with
	this program; if not, write to the Free Software Foundation, Inc., 59 Temple
	Place - Suite 330, Boston, MA  02111-1307, USA. calib_calc_4.pas calls
	routines in our wps.pas library to calculate the error in WPS measurement.
	It takes a set of WPS measurements and compares them to CMM measurements. It
	adjusts camera calibration constants until the root mean square camera error
	is close to a minimum. The program prints the CMM wire positions and the WPS
	three-dimensional error vector at the start of the fitting procedure. It
	prints the camera calibration constants and the rms error during the
	fitting. It prints the error vectors a second time at the end, and last of
	all calculates the ccd-pivot distance using the final parameters.

	At the end of the code are parameters we obtained with various sub-sets of
	the data, and by fitting various sub-sets of the parameters.

	Compile Instructions: Re-name this program p.pas and compile with "make p"
	in LWDAQ/Build. Adjust global constants and re-compile. Input data is
	embedded in the code. Output is to the terminal.

	VERSION NOTES: We add automatic reading of data from files, and automatic
	composure of the device name.
}

uses
	utils,bcam,wps;
	
const
	edge_direction=0;
	random_scale=1;
	num_parameters=9;
	max_num_shrinks=5;
	y_ref=1.220;{mm}
	max_num_points=100;
	
var
	num_points:integer;
	edge_str,wire_str,coord_str:long_string;
	wires:array [1..max_num_points] of wps_wire_type;
	edges:array [1..max_num_points] of wps_edge_type;

{
	Create a random disturbance scaled by random_scale.
}
function disturb:real;
begin
	disturb:=random_scale*(random_0_to_1-0.5);
end;

{
	We obtain the calibration error by comparing each wire position to the line
	implied by each image. We use the camera calibration constants to generate
	the lines from the images. We involve the rotation of the wire images in the
	error calculation by generating two lines for each wire image: one from a
	point in the image towards the top of the CCD and another from a point
	towards the bottom of the CCD. The y_shift parameter tells us how much to
	move up and down the image to choose the two points.
}
function error(v:simplex_vertex_type):real;
const
	y_shift=0.500;{mm}
var
	i:integer;
	sum:real;
	p:xy_point_type;
	c:wps_camera_type;
begin
	c.pivot.x:=v[1];
	c.pivot.y:=v[2];
	c.pivot.z:=v[3];
	c.sensor.x:=v[4];
	c.sensor.y:=v[5];
	c.sensor.z:=v[6];
	c.rot.x:=v[7];
	c.rot.y:=v[8];
	c.rot.z:=v[9];
	sum:=0;
	for i:=1 to num_points do begin
		p:=edges[i].position;
		p.y:=p.y-y_shift;
		p.x:=p.x-sin(edges[i].rotation)*y_shift/cos(edges[i].rotation);
		sum:=sum+sqr(xyz_length(wps_ray_error(p,edge_direction,wires[i],c)));
		p:=edges[i].position;
		p.y:=p.y+y_shift;
		p.x:=p.x+sin(edges[i].rotation)*y_shift/cos(edges[i].rotation);
		sum:=sum+sqr(xyz_length(wps_ray_error(p,edge_direction,wires[i],c)));
	end;
	error:=sqrt(sum/num_points/2);
end;

var 
	mount:kinematic_mount_type;
	i,j,camera_num:integer;
	s:short_string;
	simplex:simplex_type(num_parameters);
	camera:wps_camera_type;
	done:boolean;
	file_name,line,device_name:short_string;
	f:text;
	
begin
	fsd:=3;
	fsr:=6;

	write('Device Name? ');
	readln(device_name);
	write('Camera Number? ');
	readln(camera_num);
	file_name:='../../../Desktop/WPS/'+device_name+'/Data_'+device_name+'.txt';
	reset(f,file_name);
	readln(f);
	readln(f,num_points);
	readln(f);
	readln(f);
	edge_str:='';
	for i:=1 to num_points do begin
		readln(f,line);
		edge_str:=edge_str+line+crlf;
	end;
	readln(f);
	readln(f);
	wire_str:='';
	for i:=1 to num_points do begin
		readln(f,line);
		wire_str:=wire_str+line+crlf;
	end;
	readln(f);
	readln(f);
	coord_str:='';
	for i:=1 to 3 do begin
		readln(f,line);
		coord_str:=coord_str+line+crlf;
	end;

{
	Read the mount coordinates from coord_str. We use the mount to convert
	between global and wps coordinates.
}
	mount:=read_kinematic_mount(coord_str);
{
	Read all the wire positions, edge positions, and image rotations out of the
	wire and edge strings. If we're going to use only the left or right edges,
	we read only those. If we are going to use the centers, we obtain the image
	centers by taking the average of the left and right edges.
}
	for i:=1 to num_points do begin
		with wires[i] do begin
			radius:=1/16*25.4/2; {one sixteenth inch steel pin}
			position:=wps_from_global_point(read_xyz(wire_str),mount);
			direction:=wps_from_global_vector(read_xyz(wire_str),mount);
		end;
		
		with edges[i] do begin
			position.y:=y_ref;
			if camera_num=2 then begin
				s:=read_word(edge_str);s:=read_word(edge_str);
				s:=read_word(edge_str);s:=read_word(edge_str);
			end;
			side:=edge_direction;
			case side of
				+1:begin
					s:=read_word(edge_str);
					position.x:=read_real(edge_str)/1000;
					rotation:=read_real(edge_str)/1000;
				end;
				0:begin
					s:=read_word(edge_str);
					position.x:=read_real(edge_str)/1000;
					rotation:=read_real(edge_str)/1000;
					position.x:=(position.x+read_real(edge_str)/1000)/2;
					rotation:=(rotation+read_real(edge_str)/1000)/2;
				end;
				-1:begin
					s:=read_word(edge_str);
					s:=read_word(edge_str);
					s:=read_word(edge_str);
					position.x:=read_real(edge_str)/1000;
					rotation:=read_real(edge_str)/1000;
				end;
			end;
		end;
		
		delete(edge_str,1,pos(chr(10),edge_str));
	end;
{
	Start with a nominal set of calibration constants, disturbed by a random
	amount in all parameters.
}
	camera:=nominal_wps_camera(camera_num);
	writeln('nominal:   ',string_from_wps_camera(camera));
	with camera do begin
		id:=device_name+'_'+string_from_integer(camera_num,1);
		case edge_direction of
			+1:id:=id+'_L';
			-1:id:=id+'_R';
		end;
		with pivot do begin
			x:=x+disturb;
			y:=y+disturb;
			z:=z+disturb;
		end;
		with sensor do begin
			x:=x+disturb;
			y:=y+disturb;
			z:=z+disturb;
		end;
		with rot do begin
			x:=x+disturb/10;
			y:=y+disturb/10;
			z:=z+disturb/10;
		end;
	end;
	writeln('disturbed: ',string_from_wps_camera(camera));
{
	Calculate and display the error corresponding to each image edge and its
	cmm-measured wire position. Here we use the point on each edge at y=y_ref.
}	
	for i:=1 to num_points do begin
		write(string_from_xyz(wires[i].position),' ',string_from_xyz(wires[i].direction));
		write(' ',string_from_xyz(wps_ray_error(edges[i].position,edge_direction,wires[i],camera)));
		writeln;
	end;
{
	Construct the fitting simplex, using our starting calibration as the first
	vertex.
}
	with simplex,camera do begin
		vertices[1,1]:=pivot.x;
		vertices[1,2]:=pivot.y;
		vertices[1,3]:=pivot.z;
		vertices[1,4]:=sensor.x;
		vertices[1,5]:=sensor.y;
		vertices[1,6]:=sensor.z;
		vertices[1,7]:=rot.x;
		vertices[1,8]:=rot.y;
		vertices[1,9]:=rot.z;
		construct_size:=random_scale/10;
		done_counter:=0;
		max_done_counter:=10;
	end;
	simplex_construct(simplex,error);	
{
	Run the simplex fit until we reach convergance, as determined by the
	simplex_step routine itself.
}
	done:=false;
	i:=0;
	while not done do begin
		simplex_step(simplex,error);
		done:=(simplex.done_counter>=simplex.max_done_counter);
		inc(i);
		if (i mod 100 = 0) or done then begin
			with simplex,camera do begin
				pivot.x:=vertices[1,1];
				pivot.y:=vertices[1,2];
				pivot.z:=vertices[1,3];
				sensor.x:=vertices[1,4];
				sensor.y:=vertices[1,5];
				sensor.z:=vertices[1,6];
				rot.x:=vertices[1,7];
				rot.y:=vertices[1,8];
				rot.z:=vertices[1,9];		
			end;
			writeln(i:5,' ',
				string_from_wps_camera(camera),' ',
				xyz_separation(camera.sensor,camera.pivot):fsr:fsd,' ',
				error(simplex.vertices[1])*1000:4:1);
		end;
	end;
{
	Calculate and display the errors again.
}	
	for i:=1 to num_points do begin
		write(string_from_xyz(wires[i].position),' ',string_from_xyz(wires[i].direction));
		write(' ',string_from_xyz(wps_ray_error(edges[i].position,edge_direction,wires[i],camera)));
		writeln;
	end;
end.

{
The numbers are: pivot.x, y, z (mm), sensor.x, y, z (mm), rot.x, y, z (rad), pivot-ccd (mm), error (mm).

We observe convergance after roughly 5000 iterations.
P0195_A_1 -4.3892 88.4846 -4.4033 -13.7607 93.7637 -4.3873 -1577.741 -0.008 -493.211 10.756  0.001
P0195_A_1 -4.3892 88.4846 -4.4033 -13.7607 93.7637 -4.3873 -1577.741 -0.008 -493.211 10.756  0.001
P0195_A_1 -4.3892 88.4846 -4.4033 -13.7607 93.7637 -4.3873 -1577.741 -0.008 -493.211 10.756  0.001
P0195_A_1 -4.3892 88.4846 -4.4033 -13.7607 93.7637 -4.3873 -1577.741 -0.008 -493.211 10.756  0.001
P0195_A_2 -3.8191 39.3077 -4.6536 -12.7098 33.5709 -4.7217 1562.642 -1.805 573.494 10.581  0.001
P0195_A_2 -3.8191 39.3077 -4.6536 -12.7098 33.5709 -4.7217 1562.642 -1.805 573.494 10.581  0.001
P0195_A_2 -3.8191 39.3077 -4.6536 -12.7098 33.5709 -4.7217 1562.642 -1.805 573.494 10.581  0.001
P0195_A_2_R -3.8946 39.2691 -4.6543 -12.7987 33.5273 -4.7227 1562.478 -1.475 574.337 10.595  0.001
P0195_A_2_R -3.8946 39.2691 -4.6543 -12.7987 33.5273 -4.7227 1562.478 -1.475 574.337 10.595  0.001
P0195_A_2_L -3.7944 39.3162 -4.6558 -12.6775 33.5812 -4.7238 1562.817 -2.127 572.635 10.574  0.001
P0195_A_2_L -3.7944 39.3162 -4.6558 -12.6775 33.5812 -4.7238 1562.817 -2.127 572.635 10.574  0.001
P0195_A_1_L -4.4128 88.4975 -4.3324 -13.7863 93.7793 -4.3018 -1577.741 0.529 -489.213 10.759  0.002
P0195_A_1_L -4.4128 88.4975 -4.3324 -13.7863 93.7793 -4.3018 -1577.741 0.529 -489.213 10.759  0.002
P0195_A_1_L -4.4128 88.4975 -4.3324 -13.7863 93.7793 -4.3018 -1577.741 0.529 -489.213 10.759  0.002
P0195_A_1_R -4.4230 88.5022 -4.4704 -13.7975 93.7812 -4.4684 -1577.715 -0.594 -497.244 10.759  0.001
P0195_A_1_R -4.4230 88.5022 -4.4704 -13.7975 93.7812 -4.4684 -1577.715 -0.594 -497.244 10.759  0.001
P0195_A_1 -4.3892 88.4846 -4.4033 -13.7607 93.7637 -4.3873 -1577.741 -0.008 -493.211 10.756  0.001
P0195_B_1 -4.3073 88.4222 -4.1131 -13.6644 93.6896 -4.0386 -1577.685 0.436 -495.197 10.738  0.001
P0195_A_2 -3.8191 39.3077 -4.6536 -12.7098 33.5709 -4.7217  1562.642 -1.805 573.494 10.581  0.001
P0195_B_2 -3.8608 39.3033 -4.3716 -12.7653 33.5628 -4.3823  1562.571 -2.101 573.215 10.594  0.001

Now we have the number of iterations to completion first.
 3925 P0200_A_1 -3.7766 89.3969 -4.2359 -12.5124 94.9219 -4.2645 -1567.300 10.051 -556.381 10.336  0.002
 6333 P0200_A_1 -3.7885 89.4043 -4.2376 -12.5270 94.9308 -4.2667 -1567.298 10.040 -556.169 10.339  0.002
 6756 P0200_A_1 -3.7885 89.4043 -4.2376 -12.5270 94.9308 -4.2667 -1567.298 10.040 -556.169 10.339  0.002
 7001 P0200_A_1 -3.7885 89.4043 -4.2376 -12.5270 94.9308 -4.2667 -1567.298 10.040 -556.169 10.339  0.002
 5123 P0200_A_2 -4.2149 38.6111 -4.5357 -13.2753 33.1152 -4.5946 1572.835 8.406 525.873 10.597  0.002
 4088 P0200_A_2 -4.2149 38.6111 -4.5357 -13.2753 33.1152 -4.5946 1572.835 8.406 525.873 10.597  0.002
 4675 P0197_A_1 -3.7906 89.4672 -3.8999 -12.5781 94.6556 -3.8052 -1575.547 14.727 -537.395 10.205  0.001
 5188 P0197_A_1 -3.7906 89.4672 -3.8999 -12.5781 94.6556 -3.8052 -1575.547 14.727 -537.395 10.205  0.001
 5969 P0197_A_2 -3.3961 38.2920 -4.5059 -12.2748 32.8203 -4.5708 1574.446 7.187 546.390 10.430  0.002
 6263 P0197_A_2 -3.3961 38.2920 -4.5059 -12.2748 32.8203 -4.5708 1574.446 7.187 546.390 10.430  0.002

We switch to reporting the error in um.
 7114 P0201_A_1 -3.7992 89.0629 -4.0823 -12.6996 94.3538 -4.1472 -1569.350 11.913 -532.884 10.354  1.5
 5550 P0201_A_1 -3.7992 89.0629 -4.0823 -12.6996 94.3538 -4.1472 -1569.350 11.913 -532.884 10.354  1.5
 5201 P0201_A_2 -4.1684 38.7802 -4.1355 -13.1010 33.2285 -4.2102 1560.343 15.907 547.316 10.518  1.3
 5209 P0201_A_2 -4.1684 38.7802 -4.1355 -13.1010 33.2285 -4.2102 1560.343 15.907 547.316 10.518  1.3
 4954 P0203_A_1 -3.1318 88.7801 -3.9966 -11.9431 94.2143 -3.9305 -1573.359 6.314 -539.410 10.353  1.3
 5472 P0203_A_1 -3.1318 88.7801 -3.9966 -11.9431 94.2143 -3.9305 -1573.359 6.314 -539.410 10.353  1.3

Here we see failure of the fit convergeance criteria:
 1735 P0203_A_2 -4.9933 40.3333 -5.3482 -13.9693 34.8070 -5.6867 1573.123 -7.387 529.836 10.546  2.5
 1677 P0203_A_2 -5.5491 40.0640 -6.4503 -14.6505 34.4762 -7.0214 1573.211 -7.124 519.995 10.695  5.1
 1929 P0203_A_2 -7.9753 38.6603 -4.2451 -17.6159 32.7615 -4.3478 1573.471 -6.121 480.605 11.303  8.3
 
We must increase the max_done_counter to 20 to get the following:
 9860 P0203_A_2 -4.7610 40.4305 -4.4217 -13.6844 34.9267 -4.5646 1573.078 -7.620 533.945 10.485  1.2
10000 P0203_A_2 -4.7610 40.4305 -4.4217 -13.6844 34.9267 -4.5646 1573.078 -7.620 533.945 10.485  1.2
 6500 Q0132_A_1 -4.4475 87.6752 -4.0442 -14.6304 93.6529 -3.9048 -1574.944 1.036 -638.629 11.809  3.1
 6712 Q0132_A_1 -4.4475 87.6752 -4.0442 -14.6304 93.6529 -3.9048 -1574.944 1.036 -638.629 11.809  3.1
 5525 Q0132_A_2 -4.1223 37.6675 -4.9617 -14.3354 31.3532 -4.6422 1578.667 -23.815 537.598 12.012  3.5
 6391 Q0132_A_2 -4.1223 37.6675 -4.9617 -14.3354 31.3532 -4.6422 1578.667 -23.815 537.598 12.012  3.5
 3880 P0202_A_1 -3.9688 89.2693 -4.3937 -12.8662 94.7569 -4.2628 -1568.482 14.449 -540.951 10.454  1.5
 5447 P0202_A_1 -3.9688 89.2693 -4.3937 -12.8662 94.7569 -4.2628 -1568.482 14.449 -540.951 10.454  1.5
 6734 P0202_A_2 -4.4135 39.0829 -4.2892 -13.4683 33.8120 -4.3727 1573.235 2.048 514.979 10.477  1.5
 4190 P0202_A_2 -4.4135 39.0829 -4.2892 -13.4683 33.8120 -4.3727 1573.235 2.048 514.979 10.477  1.5
 
Re-calibrate P0198 and P0203. Had to increase max_done_counter to 100 for P0198_B_1
 7812 P0198_B_1 -3.0843 90.1940 -4.2425 -11.9576 95.7197 -4.3450 -1575.744 0.437 -541.792 10.454  1.2
33124 P0198_B_2 -3.7644 38.9943 -4.6077 -12.7347 33.4335 -4.6656 1567.472 -2.328 551.404 10.554  1.3
 6259 P0198_B_2 -3.7644 38.9943 -4.6077 -12.7347 33.4335 -4.6656 1567.472 -2.328 551.404 10.554  1.3
 7063 P0203_B_1 -3.0524 89.2967 -4.3284 -11.8453 94.7261 -4.3051 -1573.151 7.914 -543.224 10.334  1.9
 5519 P0203_B_2 -3.4665 38.0783 -4.1433 -12.3490 32.5047 -4.2011 1573.072 -6.642 538.407 10.486  1.6
 
WPS1-B Calibrations.
 4846 P0222_A_1 -3.8979 87.9984 -4.4237 -13.0247 93.3574 -4.4730 -1570.209 -7.919 -513.986 10.584  1.0
 6623 P0222_A_2 -4.5453 39.3411 -4.3267 -13.8010 34.0234 -4.4078 1580.291 12.109 508.396 10.675  1.7
 7198 P0223_A_1 -3.8841 87.8976 -4.2084 -12.9706 93.3465 -4.1088 -1571.456 15.510 -556.602 10.596  1.2
 6571 P0223_A_2 -4.5310 40.0608 -4.3034 -13.7758 34.5910 -4.5274 1574.408 -1.478 520.011 10.744  1.5
 6696 P0224_A_1 -3.3551 88.5050 -4.1745 -12.6030 93.9608 -4.2031 -1568.213 -8.750 -551.288 10.737  1.0
 7252 P0224_A_2 -5.4091 40.4620 -4.1222 -14.2126 35.4105 -3.9837 1571.051 20.308 503.579 10.151  1.0


 }