clear;home;
IM=double(imread('calle.bmp'));
%IM=double(imread('calle3.bmp'));
figure(1);
subplot(2,1,1);imshow(IM,[]);
[nf nc]=size(IM);
Hnf=round((360))+1;
Hnc=round((round(sqrt(nf^2+nc^2))))+1;
H=zeros(Hnf, Hnc);
tic
for f=1:2:nf
 for c=1:2:nc
  if IM(f,c)==1
   for ff=f:4:nf
    for cc=1:4:nc
	   if IM(ff,cc)==1 & ff~=f & cc~=c
		  %ec recta
		  if cc==c, x0=c;y0=0;
		  else
			 a=(ff-f)/(cc-c);
			 b=ff-a*cc;
			 x0=-a*b/(1+a*a);
			 y0=b-a*a*b/(1+a*a);            
		  end;
      r=sqrt(x0^2+y0^2)+1;
      if x0==0, ang=90;else ang=atan(y0/x0)*180/pi;end;
      if ang<0, ang=360+ang;end;
      r=round(r);ang=round(ang);
  	  H(ang+1,r+1)= H(ang+1,r+1)+1;
     end;%if
    end;%for cc
   end;%for ff
  end;%if
  IM(f,c)=0.5;
 end;%for c
end;%foc f
toc

%Normaliza
H=H*255/(max(max(H)));
figure(2);
colormap(jet);
subplot(2,2,1);image(H);
subplot(2,2,2);mesh(H);


%Se buscan los dos máximos principales
max1=0;
for ang=1:Hnf
   for r=1:Hnc
      if H(ang,r)>max1
         max1=H(ang,r);
         ang1=ang;
         r1=r;
      end;
   end;
end;
%Resultado 1
[ang1 r1]

%Borro Vecindad del Máximo1
dr=20;
da=10;
H(ang1-da:ang1+da,r1-dr:r1+dr)= H(ang1-da:ang1+da,r1-dr:r1+dr)*0;
max2=0;
for ang=1:Hnf
   for r=1:Hnc
      if H(ang,r)>max2
         max2=H(ang,r);
         ang2=ang;
         r2=r;
      end;
   end;
end;
%Resultado 2
[ang2 r2]

%Borro Vecindad del Máximo2
H(ang2-da:ang2+da,r2-dr:r2+dr) = H(ang2-da:ang2+da,r2-dr:r2+dr)*0;
subplot(2,2,3);image(H);


%calcula las 2 rectas
OUT=zeros(nf,nc);

dy=sin(ang1)*r1;
dx=dy/tan((90-ang1)*pi/180);
x0=r1*cos(ang1*pi/180);
y0=r1*sin(ang1*pi/180);
%y=m*x+int
m1=dy/-dx;
int1=y0-m1*x0;

dy=sin(ang2)*r2;
dx=dy/tan((90-ang2)*pi/180);
x0=r2*cos(ang2*pi/180);
y0=r2*sin(ang2*pi/180);
m2=dy/-dx;
int2=y0-m2*x0;

for x=1:nc
    y=m1*x+int1;
    f=round(y);c=round(x);
    if f>0 & f<nf & c>0 & c<nc, OUT(f,c)=1;end;
    y=m2*x+int2;
    f=round(y);c=round(x);
    if f>0 & f<nf & c>0 & c<nc, OUT(f,c)=1;end;
end;
figure(1);
subplot(2,1,2);imshow(OUT,[]);


