%COMP 256 Assignment 3
%Adrian Ilie

function res=hough(data,threshold,disctheta,discd);

%data=datagen(sigma,inliers);

if nargin<2
    threshold=0.01;
end
if nargin<3
    disctheta=180;
end
if nargin<4
    discd=200;
end

%discretize Hough space
h=zeros(discd,disctheta);

for theta=1:disctheta
    for d=-discd/2+1:discd/2
        a=sin(theta*pi/disctheta);
        b=cos(theta*pi/disctheta);
        for l=1:100
           dist=abs(a*data(1,l)+b*data(2,l)+d/discd*2);
           if dist<=threshold
               h(d+discd/2,theta)=h(d+discd/2,theta)+1;
           end
        end
    end
end
figure(3);
imagesc(h);

%find the maximum
[mx,ix]=max(h);
[my,iy]=max(mx);
b=iy;
a=ix(b);
theta=b*pi/disctheta;
d=(a-discd/2)/(discd/2);

figure(1);
hold on;
drawline(theta,d,'-r');

res=[sin(theta);cos(theta);d];
