{
}
program p;

{
	calib_calc_2.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 to 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.
	
}

uses
	utils,bcam,wps;
	
const
	N=11;
	N_error=N;
	edge_direction=1;
	camera_num=2;
	random_scale=1;
	
var
	camera:wps_camera_type;
	mount:kinematic_mount_type;
	edge_str,wire_str,coord_str:long_string;
	i,j:integer;
	wires:array [1..N] of wps_wire_type;
	images:array [1..N] of xy_point_type;
	s:short_string;
	num_steps:integer;


function error(c:wps_camera_type):real;
var
	i:integer;
	sum:real;
begin
	sum:=0;
	for i:=1 to N_error do
		sum:=sum+sqr(xyz_length(wps_ray_error(images[i],edge_direction,wires[i],c)));
	error:=sqrt(sum/N);
end;


function step(c:wps_camera_type;d:real):wps_camera_type;
var 
	ax,ay,sum:real;
	cc:wps_camera_type;
begin
	cc:=c;
	
	with cc do begin
		with pivot do begin
			x:=x+2*(random_0_to_1-0.5)*d;
			y:=y+2*(random_0_to_1-0.5)*d;
{			z:=z+2*(random_0_to_1-0.5)*d;}
		end;
		with sensor do begin
			x:=x+2*(random_0_to_1-0.5)*d;
			y:=y+2*(random_0_to_1-0.5)*d;
			z:=z+2*(random_0_to_1-0.5)*d;
		end;
		with rot do begin
{			z:=arctan((sensor.y-pivot.y)/(sensor.x-pivot.x));}
{			y:=arctan((sensor.z-pivot.z)/(pivot.x-sensor.x));}
{			x:=x+2*(random_0_to_1-0.5)*d/10;}
{			y:=y+2*(random_0_to_1-0.5)*d/10;}
			z:=z+2*(random_0_to_1-0.5)*d/10;
		end;
	end;

	if error(c)>error(cc) then step:=cc
	else step:=c;
end;

var 
	a,b,c:xyz_line_type;
	p:xy_point_type;
	q:xyz_point_type;
	
begin
	fsd:=3;
	fsr:=6;
	
	camera:=nominal_wps_camera(camera_num);
	with camera do begin
		with pivot do begin
			x:=x+(random_0_to_1-0.5)*random_scale;
			y:=y+(random_0_to_1-0.5)*random_scale;
		end;
		with sensor do begin
			x:=x+(random_0_to_1-0.5)*random_scale;
			y:=y+(random_0_to_1-0.5)*random_scale;
			z:=z+(random_0_to_1-0.5)*random_scale;
		end;
	end;
	writeln(string_from_wps_camera(camera));
	
	edge_str:='
{Q0130}
1213110444  2848.77 7.09 3171.98 6.19 2786.24 4.80 3106.39 4.97 23.569 23.871 23.772 23.617
1213110597  2478.01 7.25 2818.34 6.27 2417.89 5.14 2755.74 4.53 23.583 23.875 23.772 23.623
1213110759  2060.03 8.27 2420.36 7.87 2004.63 5.12 2359.88 6.28 23.597 23.894 23.794 23.628
1213110906  1584.59 9.02 1969.78 7.52 1535.46 4.95 1915.77 3.85 23.621 23.923 23.806 23.642
1213111021  1037.09 9.77 1452.90 9.12 996.24 4.95 1405.37 4.34 23.622 23.918 23.807 23.641
1213111132  393.94 10.97 850.16 9.78 363.46 4.40 811.74 5.31 23.632 23.906 23.793 23.651
1213111406  2287.50 6.99 2621.03 7.65 2614.79 4.65 2954.23 5.75 23.651 23.933 23.810 23.653
1213111601  1627.34 8.82 1993.75 8.76 1985.91 4.50 2355.84 4.72 23.655 23.926 23.799 23.663
{1213111709  818.03 9.97 1229.20 9.56 1219.05 5.51 1629.61 4.84 23.661 23.930 23.801 23.668}
1213112060  1342.42 192.58 1684.48 196.03 2734.04 204.63 3108.41 209.43 23.692 23.966 23.821 23.683
1213112208  966.40 190.42 1325.30 193.53 2413.96 200.44 2804.65 204.92 23.706 23.977 23.833 23.695
1213112373  547.82 188.29 945.87 190.45 2060.79 196.07 2481.88 201.43 23.718 23.993 23.829 23.698

{Q0131, Point 1 corrected left edge}
{
1213122233  3207.15 14.23 3207.15 14.23 2890.82 26.29 3207.15 25.40 23.566 23.867 23.741 23.628
1213122405  2830.11 13.34 3165.46 13.77 2539.18 26.83 2872.31 26.47 23.565 23.847 23.740 23.623
1213122539  2407.60 12.94 2761.36 12.78 2143.87 27.82 2496.53 27.21 23.580 23.873 23.749 23.631
1213122677  1929.90 12.47 2305.53 12.20 1696.11 28.97 2072.15 27.84 23.596 23.884 23.762 23.628
1213122794  1381.98 11.27 1785.82 11.49 1181.24 30.43 1586.50 28.95 23.598 23.880 23.750 23.629
1213122919  741.33 9.07 1183.85 10.55 579.06 32.09 1022.30 30.80 23.608 23.899 23.769 23.634
1213123194  2636.01 13.04 2967.15 14.20 2737.72 26.43 3076.52 25.66 23.628 23.916 23.782 23.651
1213123355  1971.85 12.09 2332.69 11.94 2138.31 27.88 2507.42 27.21 23.640 23.923 23.775 23.646
1213123478  1163.28 10.12 1565.42 10.85 1408.58 29.86 1818.23 28.84 23.648 23.932 23.778 23.654
1213124105  1697.46 181.41 2043.35 184.94 2808.67 214.36 3188.04 216.86 23.690 23.981 23.810 23.676
1213124241  1323.53 177.32 1687.26 181.15 2503.66 211.55 2899.96 214.94 23.697 23.987 23.825 23.689
1213124369  909.66 173.54 1294.94 177.07 2167.19 208.88 2582.30 212.00 23.698 23.974 23.831 23.690
}
	';
	wire_str:='
{Q0130}
111.7920 7.7853 44.3442 0.99999324 0.00021539 -0.00367112                                       
111.8133 3.7750 44.3878 0.99999323 0.00008940 -0.00367757                                       
111.8351 -0.2258 44.4332 0.99999328 0.00007317 -0.00366411                                       
111.8578 -4.2300 44.4751 0.99999326 0.00006007 -0.00367086                                       
111.8801 -8.2284 44.5172 0.99999323 0.00005921 -0.00367798                                       
111.9021 -12.2322 44.5619 0.99999328 0.00004369 -0.00366548                                       
112.8199 3.7627 43.3839 0.99999335 -0.00000660 -0.00364560                                        
112.8535 -2.2392 43.4484 0.99999335 -0.00002084 -0.00364782                                        
{112.8535 -2.2392 43.4484 0.99999335 -0.00002084 -0.00364782 }                                       
113.6592 -0.2862 40.5197 0.97818151 0.00219896 0.20774046                                      
113.6759 -3.2832 40.5519 0.97818112 0.00219834 0.20774231                                      
113.6930 -6.3024 40.5828 0.97818194 0.00219319 0.20773847                                      

{Q0131}
{
111.8021 7.7840 44.2651 0.99993495 0.00036492 0.01139999                                    
111.8222 3.7758 44.3113 0.99993474 0.00023516 0.01142185                                      
111.8442 -0.2246 44.3548 0.99993473 0.00022720 0.01142255                                      
111.8664 -4.2263 44.3975 0.99993472 0.00020959 0.01142395                                      
111.8890 -8.2264 44.4387 0.99993490 0.00021781 0.01140822                                      
111.9103 -12.2318 44.4842 0.99993469 0.00018295 0.01142754                                      
112.8273 3.7636 43.2978 0.99993451 0.00016862 0.01144311                                      
112.8607 -2.2369 43.3619 0.99993456 0.00015286 0.01143879                                      
112.8607 -2.2369 43.3619  0.99993456 0.00015286 0.01143879                                    
113.6510 -0.2865 40.5312 0.97794118 0.00234389 0.20886730                                      
113.6683 -3.2870 40.5635 0.97794144 0.00234277 0.20886612                                      
113.6841 -6.2866 40.5963 0.97793874 0.00233801 0.20887879                                      
}
	';
	coord_str:='
{Q0130}
118.0668 -43.2519 -18.6473                                      
45.1356 -64.4028 -17.6246                                      
44.9564 -22.4162 -18.0720                                      

{Q0131}
{
118.0663 -43.2519 -18.6495                                      
45.1359 -64.4029 -17.6254                                      
44.9566 -22.4167 -18.0723
}
	';  
	
	mount:=read_kinematic_mount(coord_str);
	
	
	for i:=1 to N 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 images[i] do begin
			y:=1.220; {mm}
			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;
			case edge_direction of
				+1:begin
					s:=read_word(edge_str);
					x:=read_real(edge_str)/1000;
				end;
				0:begin
					s:=read_word(edge_str);
					x:=read_real(edge_str)/1000;
					s:=read_word(edge_str);
					x:=(x+read_real(edge_str)/1000)/2;
				end;
				-1:begin
					s:=read_word(edge_str);
					s:=read_word(edge_str);
					s:=read_word(edge_str);
					x:=read_real(edge_str)/1000;
				end;
			end;
		end;
		
		edge_str:=delete_to_mark(edge_str,chr(10));
	end;

	case edge_direction of
		+1:writeln('Camera ',camera_num,' using left edges.');
		0:writeln('Camera ',camera_num,' using center line.');
		-1:writeln('Camera ',camera_num,' using right edges.');
	end;
	
	for i:=1 to N do begin
		write(string_from_xyz(wires[i].position),' ',string_from_xyz(wires[i].direction));
		write(' ',string_from_xyz(wps_ray_error(images[i],edge_direction,wires[i],camera)));
		writeln;
	end;

	writeln(string_from_wps_camera(camera),' ',
		xyz_length(xyz_difference(camera.pivot,camera.sensor)):fsr:fsd,' ',
		error(camera):fsr:fsd);

	for i:=1 to 200 do begin
		for j:=1 to 1000 do begin
			camera:=step(camera,0.001);
		end;
		writeln(i:3,' ',string_from_wps_camera(camera),' ',
			xyz_length(xyz_difference(camera.pivot,camera.sensor)):fsr:fsd,' ',
			error(camera):fsr:fsd);
	end;

	for i:=1 to N do begin
		write(string_from_xyz(wires[i].position),' ',string_from_xyz(wires[i].direction));
		write(' ',string_from_xyz(wps_ray_error(images[i],edge_direction,wires[i],camera)));
		writeln;
	end;
end.

{
NOTE: The first number on a line is the number of iterations divided by 100k.

Q0131-1. Skip points 1 and 9. With wire centers, fitting pivot x, y, sensor x, y, z and rot.z, random 
starting values for the fitted position parameters: +-0.5 mm.
100 WPS1_A_1 -3.670 88.223 -5.000 -13.737 94.619 -5.228 -1570.796 0.000 -600.018 11.929  0.006
100 WPS1_A_1 -3.359 88.123 -5.000 -13.331 94.485 -5.208 -1570.796 0.000 -589.225 11.830  0.006
100 WPS1_A_1 -4.161 88.575 -5.000 -14.350 95.059 -5.225 -1570.796 0.000 -579.979 12.079  0.006
100 WPS1_A_1 -3.935 88.497 -5.000 -14.040 94.944 -5.212 -1570.796 0.000 -592.025 11.989  0.005
100 WPS1_A_1 -3.982 88.420 -5.000 -14.136 94.870 -5.230 -1570.796 0.000 -589.273 12.031  0.007
100 WPS1_A_1 -3.905 88.513 -5.000 -13.989 94.956 -5.208 -1570.796 0.000 -597.021 11.968  0.005
100 WPS1_A_1 -3.903 88.329 -5.000 -14.047 94.763 -5.236 -1570.796 0.000 -591.058 12.014  0.007
100 WPS1_A_1 -3.886 88.397 -5.000 -14.007 94.835 -5.227 -1570.796 0.000 -588.820 11.998  0.006
100 WPS1_A_1 -3.476 88.040 -5.000 -13.514 94.402 -5.239 -1570.796 0.000 -605.646 11.887  0.007
200 WPS1_A_1 -3.584 88.505 -5.000 -13.529 94.909 -5.175 -1570.796 0.000 -606.290 11.831  0.003
200 WPS1_A_1 -3.784 88.620 -5.000 -13.784 95.056 -5.178 -1570.796 0.000 -600.542 11.893  0.004
200 WPS1_A_1 -3.574 88.499 -5.000 -13.517 94.902 -5.174 -1570.796 0.000 -605.987 11.827  0.003
200 WPS1_A_1 -3.593 88.514 -5.000 -13.541 94.920 -5.174 -1570.796 0.000 -604.993 11.834  0.003
200 WPS1_A_1 -3.439 88.430 -5.000 -13.343 94.812 -5.171 -1570.796 0.000 -609.380 11.784  0.003
200 WPS1_A_1 -3.776 88.645 -5.000 -13.766 95.082 -5.173 -1570.796 0.000 -599.515 11.886  0.004
200 WPS1_A_1 -3.417 88.386 -5.000 -13.323 94.763 -5.176 -1570.796 0.000 -610.756 11.783  0.003
200 WPS1_A_1 -3.987 88.754 -5.000 -14.040 95.224 -5.178 -1570.796 0.000 -593.488 11.956  0.004
200 WPS1_A_1 -3.627 88.519 -5.000 -13.587 94.929 -5.177 -1570.796 0.000 -605.338 11.846  0.003
200 WPS1_A_1 -3.793 88.617 -5.000 -13.799 95.055 -5.180 -1570.796 0.000 -599.658 11.899  0.004
1000 WPS1_A_1 -3.570 88.568 -5.000 -13.492 94.976 -5.164 -1570.796 0.000 -606.059 11.812  0.003
1000 WPS1_A_1 -3.653 88.627 -5.000 -13.595 95.049 -5.162 -1570.796 0.000 -602.571 11.836  0.003
1000 WPS1_A_1 -3.390 88.456 -5.000 -13.264 94.834 -5.162 -1570.796 0.000 -611.560 11.756  0.003

Q0131-1. Skip point 1 and 9. Use left edges.
200 WPS1_A_1 -3.872 88.542 -5.000 -13.942 94.994 -5.195 -1570.796 0.000 -574.585 11.962  0.004
200 WPS1_A_1 -3.565 88.375 -5.000 -13.551 94.781 -5.187 -1570.796 0.000 -584.397 11.865  0.004

Q0131-1. Skip point 9. Correct left edge of Point 1. Use left edges.
200 WPS1_A_1 -3.792 88.476 -5.000 -13.848 94.915 -5.199 -1570.796 0.000 -594.528 11.943  0.005
200 WPS1_A_1 -3.660 88.361 -5.000 -13.691 94.776 -5.202 -1570.796 0.000 -599.985 11.908  0.005

Q0131-2. Skip point 9. Use centers.
200 WPS1_A_2 -4.935 39.241 -5.000 -15.188 33.310 -5.677 1570.796 0.000 579.758 11.864  0.006

Q0131-2. Skip point 9. Use left edges.
200 WPS1_A_2 -4.694 39.316 -5.000 -14.891 33.400 -5.674 1570.796 0.000 565.562 11.808  0.006

Q0130-1. Skip point 9. Use left edges.
200 WPS1_A_1 -3.839 88.157 -5.000 -14.179 94.272 -5.250 -1570.796 0.000 -539.313 12.015  0.004

Q0130-2. Skip point 9. Use left edges.
200 WPS1_A_2 -4.726 38.535 -5.000 -15.252 32.428 -5.425 1570.796 0.000 537.062 12.177  0.006
}