%                   University of Notre Dame
%              Aerospace and Mechanical Engineering
%
%                ME 469: Introduction to Robotics
%
%                       Homework 5 Solutions
%
% B. Goodwine with help from  Fickel, Rogers and Veldhuizen.
% Fall 1998 

% ----------------------------------
% Problem 1 (just save with xv)
% ----------------------------------


% ----------------------------------
% Problem 2 (histogram):
% ----------------------------------

load img1.pgm;
hist(img1,256)
title('Histogram for Image 1')
pause;

load img2.pgm;
hist(img2,256)
title('Histogram for Image 2')
pause;


% ----------------------------------
% Problem 3 (threshholding):
% ----------------------------------

fid = fopen('img1a.pgm','w');

for i=1:22454,
	for j=1:17,
		i
		if img1(i,j)>80
		  fprintf(fid, '%4.4g', 255);
		  img1a(i,j)=255;
		else
		  fprintf(fid, '%4.3g', 0);
		  img1a(i,j)=0;
		end

	end
fprintf(fid,'\n');
end
fclose(fid)

fid = fopen('img2a.pgm','w');

for i=1:16497,
	for j=1:17,
		i
		if img2(i,j)>140
		  fprintf(fid, '%4.4g', 255);
		  img2a(i,j)=255;
		else
		  fprintf(fid, '%4.3g', 0);
		  img2a(i,j)=0;
		end

	end
fprintf(fid,'\n');
end
fclose(fid)


% ----------------------------------
% Problem 4 (signature analysis)
% ----------------------------------

img1vec = reshape(img1a',1,381718);

% NOTE the transpose   ^
% the command reshape takes the elements *column-wise*...
%
% From 'help reshape'...
%
% RESHAPE Change size.
%    RESHAPE(X,M,N) returns the M-by-N matrix whose elements
%    are taken columnwise from X.  An error results if X does
%    not have M*N elements.

pv1(0:678)=0;
ph1(0:563)=0;
pd1(0:1241)=0;
pe1(0:1241)=0;
for i=1:678
	for j=1:563
		i
		if img1vec(1,(j-1)*678 + i) >= 254
		        pv1(i)=pv1(i)+1;
			ph1(j)=ph1(j)+1;
			pd1(i+j)=pd1(i+j)+1;
			pe1(j-i+678)=pe1(j-i+678)+1;
		end
	end
end

img2vec = reshape(img2a',1,280449);
pv2(0:615)=0;
ph2(0:456)=0;
pd2(0:1071)=0;
pe2(0:1071)=0;

plot(pv1);
pause;
plot(ph1);
pause;
plot(pd1);
pause;
plot(pe1);

for i=1:615
	for j=1:456
		i
		if img2(1,(j-1)*615 + i)==0
			pv2(i)=pv2(i)+1;
			ph2(j)=ph2(j)+1;
			pd2(i+j)=pd2(i+j)+1;
			pe2(j-i+615)=pd2(j-i+615)+1;
		end
	end
end

plot(pv2);
pause;
plot(ph2);
pause;
plot(pd2);
pause;
plot(pe2);


% ----------------------------------
% Problem 5 (connected components):
% ----------------------------------

clear CON

i=1.0;
j=1.0;

                                                 % first need to resize the matrix correctly          
for l=1:22454
	for m=1:17
		CON(j,i)=img1a(l,m);
		i=i+1.0;
		if i==679                        % need to change to number of columns +1
			i=1.0;	
			j=j+1.0;
		end	
	end
end

u=size(CON);
ni=u(2);	                                 % should be 678
nj=u(1);	                                 % should be 563


disp('CONNECTED COMPONENTS')

n=1;
ss=1;
iter=0;


for j=1:nj
	for i=1:ni
		if CON(j,i) < 0.5
			CON(j,i) = n;
			n=n+ 1;
		else
			CON(j,i)=ni*nj+1;        % make all nonzero very large
		end
	end	
end

val=CON;
valprv=val;

while ss>0.0
	ss=0.0;
	for i=2:ni-1                               % top to bottom, left to right
		for j=2:nj-1
			if val(j,i)~=ni*nj+1       % if the pixel is in an object, then
				                   % find the smallest of all its neighbors
				val(j,i)=min([val(j,i+1), val(j-1,i), val(j,i-1), val(j+1,i)]);
			end
		end
	end

	for q=2:ni-1                               % bottom to top, right to left
		for r=2:nj-1
			i=ni+1-q;
			j=nj+1-r;
			if val(j,i)~=ni*nj+1
				val(j,i)=min([val(j,i+1), val(j-1,i), val(j,i-1), val(j+1,i)]);
			end
		end
	end

	ss=ss+sum(abs(valprv(j,i)-val(j,i)));
	valprv(j,i)=val(j,i);
	iter=iter+1;
	clc
	disp(iter)
	disp(ss)
end

disp('zeroing')


for j=1:nj
	for i=1:ni
		if val(j,i)==2.0
			val(j,i)=255.0;
		else
			val(j,i)=0.0;
		end
	end
end



disp('SAVING TO FILE')

fid = fopen('F14con.pgm','w');
      fprintf(fid,'%2.3g %2.3g %2.3g %2.3g %2.3g %2.3g %2.3g %2.3g %2.3g %2.3g %2.3g %2.3g %2.3g %2.3g %2.3g %2.3g %2.3g\n',val);
      fclose(fid);