% genomegalist.m

% function omegalist=getomegalist()

% OMEGALIST = GETOMEGALIST()
%	Prompts user for base calibration filename and number of laser steps
%	between each calibration image.  Then reads all calibration files
%	into a single image and prompts for four rows to extract beams along.
%	From the top two rows, a list lambda_v is extracted, and from the
%	bottom two rows, lambda_h is extracted.  Each pair of lambda_v,lambda_h
%	are intersected and a line is fit to the intersection points which is
%	lambda.  Lambda is used to determine omega_v, which, along with omega_h
%	(which is already known from camera calibration) is used to determine
%	omega for each calibration laser position.  Then, given the specified
%	number of laser steps per calibration image and the calculated values
%	of omega at the calibration positions, an interpolated array of omega
%	values is calculated (and returned) corresponding to omega (of the laser)
%	at each step of the laser.


fprintf(1,'Loading camera calibration data...\n');
load calib_camera;
if ~exist('fc') | ~exist('cc') | ~exist('omega_ground') | ~exist('kc'),
	disp('ERROR - some important variables (fc, cc, kc, omega_ground) couldn''t be loaded from calib_camera - quitting.');
	return;
end


calib_name = input('Calibration image base filename? ','s');

if isempty(calib_name),
	return;
end

[cim,nfiles]=generatecalimage(calib_name);

while 1,
	image(cim);
	colormap(gray);

	disp('Click on the two top rows (along which to extract vertical-intersection beams)...');
	[xtop,ytop] = ginput2(2);

	disp('Click on the two bottom rows (along which to extract horizontal-intersection beams)...');
	[xbot,ybot] = ginput2(2);

	if(min(ybot) > max(ytop)),
		break
	end
	% else
	disp('Invalid points selected - try again');
end


if ytop(1) > ytop(2),
	t=ytop(1);
	ytop(1)=ytop(2);
	ytop(2)=t;
end


if ybot(1) > ybot(2),
	t=ybot(1);
	ybot(1)=ybot(2);
	ybot(2)=t;
end

ybot = floor(ybot);
ytop = floor(ytop);

hold on;
x1=0;
s=size(cim);
x2=s(2)-1;

thresh = 40;

peaktop1 = getpeaklist(cim(ytop(1),:),thresh);
line([x1 x2],[ytop(1) ytop(1)],'Color',[1 0 0]);
y1=ones(1,length(peaktop1))*(ytop(1)-5);
y2=ones(1,length(peaktop1))*(ytop(1)+5);
line([peaktop1 ; peaktop1],[y1 ; y2], 'Color',[1 0 0]);

peaktop2 = getpeaklist(cim(ytop(2),:),thresh);
line([x1 x2],[ytop(2) ytop(2)],'Color',[0 1 0]);
y1=ones(1,length(peaktop2))*(ytop(2)-5);
y2=ones(1,length(peaktop2))*(ytop(2)+5);
line([peaktop2 ; peaktop2],[y1 ; y2], 'Color',[0 1 0]);

peakbot1 = getpeaklist(cim(ybot(1),:),thresh);
line([x1 x2],[ybot(1),ybot(1)],'Color',[0 0 1]);
y1=ones(1,length(peakbot1))*(ybot(1)-5);
y2=ones(1,length(peakbot1))*(ybot(1)+5);
line([peakbot1 ; peakbot1],[y1 ; y2], 'Color',[0 0 1]);

peakbot2 = getpeaklist(cim(ybot(2),:),thresh);
line([x1 x2],[ybot(2),ybot(2)],'Color',[0.7 0.7 0.7]);
y1=ones(1,length(peakbot2))*(ybot(2)-5);
y2=ones(1,length(peakbot2))*(ybot(2)+5);
line([peakbot2 ; peakbot2],[y1 ; y2], 'Color',[0.7 0.7 0.7]);

[intlist,lambda_v,lambda_h] = getintlist(peaktop1, ytop(1), peaktop2, ytop(2), peakbot1, ybot(1), peakbot2, ybot(2), cc, fc, kc);
%plot(xint,yint,'r.');
scrintlist = distort(intlist,kc);
scrintlist = [scrintlist(1,:) * fc(1) + cc(1) ; scrintlist(2,:) * fc(2) + cc(2) ];
plot(scrintlist(1,:),scrintlist(2,:),'r.');

%intlist = comp_distortion([(xint - cc(1)) / fc(1) ; (yint - cc(2)) / fc(2)], kc);
% intlist contains list of intersection points (along lambda) in homogeneous coordinates

% find best-fit line to intersection points
[d,n] = FitQuad(intlist, 0);

% transform line to y=mx+b form (in homogeneous coords)
% d is distance from origin to line
% n is normal to line
% --> n*d is a point on the list
% solve for line in y=mx+b form
m = -n(1)/n(2);
b = n(2)*d - m*n(1)*d;
lambda_i = [m ; -1 ; b];

% now, transform back to screen coords
x1 = -1;
y1 = m * x1 + b;
x2 = 1;
y2 = m * x2 + b;
pt1=distort([x1;y1], kc) .* fc + cc;
pt2=distort([x2;y2], kc) .* fc + cc;
line([pt1(1) pt2(1)],[pt1(2) pt2(2)],'Color',[.8 .8 .8]);

omega_h = omega_ground;
omega_v = omega_h - (dot(omega_h, omega_h) / dot(lambda_i,omega_h)) * lambda_i;

omegalistcoarse = zeros(3,length(peaktop1));

for i=1:length(peaktop1),
	lh = lambda_h(:,i);
	lv = lambda_v(:,i);
	oh = omega_h;
	ov = omega_v;
	A = [ dot(lh,lh) -dot(lh,lv) ; -dot(lh,lv) dot(lv,lv) ];
	b = [ dot(lh,ov-oh) ; dot(lv,oh-ov) ];
	alpha = inv(A) * b;
	o1 = oh + alpha(1) * lh;
	o2 = ov + alpha(2) * lv;
	omegalistcoarse(:,i) = 0.5*(o1 + o2);
end

while 1,
	calib_spacing = input('Spacing (in steps) between each calibration image? ');
	calib_spacing = floor(calib_spacing);
	if(calib_spacing < 1) | (calib_spacing > 100),
		disp(' INVALID - value must be between 1 and 100');
	else
		break;
	end
end

ndx=1;
for i=1:length(peaktop1)-1,
	for j=1:calib_spacing,
		scale = (j-1)/calib_spacing;
		omegalist(:,ndx) = omegalistcoarse(:,i) * (1-scale) + omegalistcoarse(:,i+1) * scale;
		ndx = ndx+1;
	end
end
omegalist(:,ndx)=omegalistcoarse(:,i+1);

fprintf(1,'Saving 3x%d omegalist to ''omlist.mat''.\n',ndx);

save omlist omegalist;

fprintf(1,'MAKEOMEGALIST FINISHED - now use PROC3D to extract 3-D data from a set of files.\n');