%COMP 256 Assignment 2
%Adrian Ilie

function m=disparity(im1, im2, hs, vs, dispr)
%computes disparity map between im1 and im2
%using a window size of hsXvs
%and zmncc for a disparity range dispr

[h,w,d]=size(im1);

%compute gray levels - luminance
im1=double(im1);
im2=double(im2);
l1=0.299*im1(:,:,1)+0.587*im1(:,:,2)+0.114*im1(:,:,3);
l2=0.299*im2(:,:,1)+0.587*im2(:,:,2)+0.114*im2(:,:,3);

%precompute variance
var1=zeros(h,w);
m1=zeros(h,w);
var2=zeros(h,w);
m2=zeros(h,w);
for i=vs+1:h-vs-1
    for j=hs+1:w-hs-1
        submat1=l1(i-vs:i+vs,j-hs:j+hs);
        m1(i,j)=mean(submat1(:));
        dif1=submat1(:)-m1(i,j);
        var1(i,j)=sum(dif1.*dif1);
        submat2=l2(i-vs:i+vs,j-hs:j+hs);
        m2(i,j)=mean(submat2(:));
        dif2=submat2(:)-m2(i,j);
        var2(i,j)=sum(dif2.*dif2);
    end
end

zmncc=zeros(h-2*vs-1,w-2*hs-1,dispr);

for d=1:dispr
    d
    for i=vs+1:h-vs-1
        for j=hs+1:w-hs-1-d
            submat1=l1(i-vs:i+vs,j-hs:j+hs);
            dif1=submat1(:)-m1(i,j);
            submat2=l2(i-vs:i+vs,j-hs+d:j+hs+d);
            dif2=submat2(:)-m2(i,j+d);
            cov=sum(dif1.*dif2);
            if (var1(i,j)*var2(i,j+d)~=0)
                zmncc(i,j,d)=cov/sqrt(var1(i,j)*var2(i,j+d));
            end
        end
    end
end

%[mat,m]=max(zmncc,[],3);
m=zmncc;