{
}
program p;

{
	calib_calc_1.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=10;
	N_error=N;
	edge_direction=0;
	camera_num=1;
	random_scale=0;
	
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;


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
{			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
		pivot:=xyz_sum(pivot,xyz_scale(xyz_random,random_scale));
		sensor:=xyz_sum(sensor,xyz_scale(xyz_random,random_scale));
	end;
	writeln(string_from_wps_camera(camera));
	
	edge_str:='
{1213122233  1057.77 35.45 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:='
{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:='
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),' ',error(camera):fsr:fsd);

	for i:=1 to 10 do begin
		for j:=1 to 1000 do camera:=step(camera,0.1/i);
		writeln(string_from_wps_camera(camera),' ',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;

	writeln(xyz_length(xyz_difference(camera.pivot,camera.sensor)):fsr:fsd);
end.

{
Nominal Values:
WPS1_1 -4.500 87.400 -5.000 -14.272 93.271 -5.000 -1570.796 0.000 -541.000  0.350
With left edges, skipping points 1 and 9, fitting pivot and sensor x,y and z:
WPS1_1 -3.941 87.439 -4.474 -14.386 93.836 -4.729 -1570.796 0.000 -541.000  0.017
WPS1_1 -4.057 87.175 -4.886 -14.631 93.564 -5.290 -1570.796 0.000 -541.000  0.022
WPS1_1 -4.000 87.360 -4.894 -14.492 93.756 -5.265 -1570.796 0.000 -541.000  0.019
WPS1_1 -4.321 87.431 -4.452 -14.943 93.872 -4.752 -1570.796 0.000 -541.000  0.020
With centers, skipping points 1 and 9, fitting pivot and sensor x,y and z:
WPS1_1 -4.368 87.249 -4.719 -15.094 93.682 -5.114 -1570.796 0.000 -541.000  0.025
WPS1_1 -4.037 87.490 -4.914 -14.531 93.901 -5.281 -1570.796 0.000 -541.000  0.020
WPS1_1 -4.066 87.312 -4.961 -14.637 93.718 -5.374 -1570.796 0.000 -541.000  0.023
WPS1_1 -4.037 87.460 -4.773 -14.548 93.873 -5.117 -1570.796 0.000 -541.000  0.020
With right edges, skipping points 1 and 9, fitting pivot and sensor x,y and z:
WPS1_1 -3.873 87.376 -4.663 -14.366 93.768 -4.976 -1570.796 0.000 -541.000  0.023
WPS1_1 -4.001 87.170 -4.594 -14.634 93.566 -4.955 -1570.796 0.000 -541.000  0.026
WPS1_1 -3.935 87.347 -4.473 -14.465 93.742 -4.761 -1570.796 0.000 -541.000  0.024
WPS1_1 -4.124 87.503 -4.532 -14.685 93.928 -4.828 -1570.796 0.000 -541.000  0.023
WPS1_1 -3.874 87.421 -4.537 -14.360 93.821 -4.822 -1570.796 0.000 -541.000  0.022
With left edges, skipping points 1 and 9, fitting pivot, sensor, and rot x,y and z:
WPS1_1 -3.788 87.129 -4.698 -14.262 93.487 -5.037 -1580.554 17.922 -554.255  0.019
WPS1_1 -3.758 87.460 -4.900 -14.108 93.833 -5.227 -1591.759 12.260 -566.255  0.014
WPS1_1 -4.219 87.455 -4.742 -14.793 93.890 -5.085 -1593.562 -7.492 -551.015  0.018
WPS1_1 -4.096 87.518 -4.818 -14.581 93.935 -5.157 -1575.902 -3.104 -527.636  0.018
With left edges, skipping points 1 and 9, fitting pivot and sensor x,y and z, random start:
WPS1_1 -3.977 87.150 -5.301 -14.517 93.524 -5.799 -1570.796 0.000 -541.000  0.022
WPS1_1 -3.629 87.048 -4.581 -14.064 93.388 -4.891 -1570.796 0.000 -541.000  0.020
WPS1_1 -3.439 87.390 -4.771 -13.675 93.726 -5.044 -1570.796 0.000 -541.000  0.015
WPS1_1 -4.366 87.234 -5.016 -15.063 93.662 -5.480 -1570.796 0.000 -541.000  0.023
WPS1_1 -4.679 87.559 -4.118 -15.416 94.047 -4.352 -1570.796 0.000 -541.000  0.020
With left edges, skipping points 1 and 9, fitting pivot and sensor x and y (not z):
WPS1_1 -3.758 89.519 -5.000 -13.482 96.010 -5.000 -1570.796 0.000 -541.000  0.016
WPS1_1 -3.573 89.268 -5.000 -13.281 95.719 -5.000 -1570.796 0.000 -541.000  0.018
WPS1_1 -3.588 89.351 -5.000 -13.283 95.810 -5.000 -1570.796 0.000 -541.000  0.016
WPS1_1 -3.470 89.258 -5.000 -13.140 95.697 -5.000 -1570.796 0.000 -541.000  0.017
WPS1_1 -3.454 89.234 -5.000 -13.126 95.671 -5.000 -1570.796 0.000 -541.000  0.017
WPS1_1 -3.559 89.231 -5.000 -13.269 95.677 -5.000 -1570.796 0.000 -541.000  0.019
With left edges, skipping points 1 and 9, fitting pivot x and y (not z):
WPS1_1 -3.981 87.037 -5.000 -14.272 93.271 -5.000 -1570.796 0.000 -541.000  0.123
With left edges, skipping points 1 and 9, moving pivot and sensor together in x and y (not z):
WPS1_1 -1.565 85.366 -5.000 -11.337 91.237 -5.000 -1570.796 0.000 -541.000  0.127
With centers, skipping points 1 and 9, fitting pivot and sensor x and y (not z):
WPS1_1 -3.730 89.483 -5.000 -13.461 95.976 -5.000 -1570.796 0.000 -541.000  0.017
WPS1_1 -3.671 89.256 -5.000 -13.414 95.713 -5.000 -1570.796 0.000 -541.000  0.021
WPS1_1 -3.269 89.105 -5.000 -12.907 95.519 -5.000 -1570.796 0.000 -541.000  0.018
WPS1_1 -3.525 89.257 -5.000 -13.216 95.703 -5.000 -1570.796 0.000 -541.000  0.019
WPS1_1 -3.254 89.090 -5.000 -12.888 95.500 -5.000 -1570.796 0.000 -541.000  0.018
With centers, skipping points 1 and 9, fitting pivot and sensor x and y, and rot.z:
WPS1_1 -3.447 89.113 -5.000 -13.145 95.541 -5.000 -1570.796 0.000 -547.600  0.021
WPS1_1 -3.304 89.081 -5.000 -12.958 95.495 -5.000 -1570.796 0.000 -546.503  0.019
WPS1_1 -3.470 89.255 -5.000 -13.149 95.701 -5.000 -1570.796 0.000 -524.774  0.019
WPS1_1 -3.410 89.169 -5.000 -13.093 95.606 -5.000 -1570.796 0.000 -517.119  0.020
WPS1_1 -3.649 89.307 -5.000 -13.383 95.774 -5.000 -1570.796 0.000 -507.663  0.021
WPS1_1 -3.510 89.198 -5.000 -13.194 95.625 -5.000 -1570.796 0.000 -601.494  0.019
With centers, skipping points 1 and 9-12, fitting pivot and sensor x and y (not z):
WPS1_1 -3.828 87.906 -5.000 -14.109 94.335 -5.000 -1570.796 0.000 -541.000  0.013
WPS1_1 -3.830 87.583 -5.000 -14.218 93.994 -5.000 -1570.796 0.000 -541.000  0.017
WPS1_1 -3.886 87.747 -5.000 -14.250 94.177 -5.000 -1570.796 0.000 -541.000  0.015
WPS1_1 -4.088 87.450 -5.000 -14.639 93.884 -5.000 -1570.796 0.000 -541.000  0.021
WPS1_1 -3.983 87.812 -5.000 -14.362 94.253 -5.000 -1570.796 0.000 -541.000  0.013
WPS1_1 -4.114 87.974 -5.000 -14.496 94.438 -5.000 -1570.796 0.000 -541.000  0.011
With centers, skipping points 1 and 9, fitting pivot x and y, sensor x, y, and z:
WPS1_1 -4.213 87.720 -5.000 -14.705 94.163 -5.365 -1570.796 0.000 -541.000  0.019
WPS1_1 -4.095 87.377 -5.000 -14.654 93.787 -5.415 -1570.796 0.000 -541.000  0.022
WPS1_1 -3.930 86.946 -5.000 -14.559 93.311 -5.470 -1570.796 0.000 -541.000  0.027
WPS1_1 -3.710 87.472 -5.000 -14.066 93.847 -5.357 -1570.796 0.000 -541.000  0.019
WPS1_1 -3.884 87.260 -5.000 -14.386 93.640 -5.404 -1570.796 0.000 -541.000  0.022

}