function [Ux,Uy] = imFluxoOpticoPorCoincidencia(im0, im1, Sigmafiltro, s)
% im0, im1: imagens correspondente ao frame 0 e 1 respectivamente. 
% Sigmafiltro: parametro que define o tamanho do filtro Gaussiano.
% s: parametro que estabelece cada cuantos pixels sera calculado
% o fluxo optico.

%Determinando as mascara gaussiana e sua derivada
[gFilt,gxFilt] = MaskGaussiana(Sigmafiltro);

%Suavizando os frames para eliminar os possiveis defeitos
im0blur = conv2(conv2(im0,gFilt','same'),gFilt,'same');
im1blur = conv2(conv2(im1,gFilt','same'),gFilt,'same');

%Determinando as derivadas espaciais e temporais
Fx	= conv2(conv2(im1blur,gFilt','same'),gxFilt,'same');
Fy	= conv2(conv2(im1blur,gFilt,'same'),gxFilt','same');
Ft	= im1blur - im0blur;

Fx2 = Fx.^2;
Fy2 = Fy.^2;
Fxy = Fx.*Fy;
Fxt = Fx.*Ft;
Fyt = Fy.*Ft;

[dimy,dimx]=size(im0);
[X,Y] = meshgrid([1:dimx],[1:dimy]);
X2 = X.^2;
Y2 = Y.^2;
XY = X.*Y;

whos
%Determinando os componente da matriz G
G11 = conv2(conv2( X2.*Fx2, gFilt', 'same'), gFilt, 'same');
G12 = conv2(conv2( XY.*Fx2, gFilt', 'same'), gFilt, 'same');
G13 = conv2(conv2( X2.*Fxy, gFilt', 'same'), gFilt, 'same');
G14 = conv2(conv2( XY.*Fxy, gFilt', 'same'), gFilt, 'same');
G15 = conv2(conv2(  X.*Fx2, gFilt', 'same'), gFilt, 'same');
G16 = conv2(conv2(  X.*Fxy, gFilt', 'same'), gFilt, 'same');
G22 = conv2(conv2( Y2.*Fx2, gFilt', 'same'), gFilt, 'same');
G24 = conv2(conv2( Y2.*Fxy, gFilt', 'same'), gFilt, 'same');
G25 = conv2(conv2(  Y.*Fx2, gFilt', 'same'), gFilt, 'same');
G26 = conv2(conv2(  Y.*Fxy, gFilt', 'same'), gFilt, 'same');
G33 = conv2(conv2( X2.*Fy2, gFilt', 'same'), gFilt, 'same');
G34 = conv2(conv2( XY.*Fy2, gFilt', 'same'), gFilt, 'same');
G35 = conv2(conv2(  X.*Fxy, gFilt', 'same'), gFilt, 'same');
G36 = conv2(conv2(  X.*Fy2, gFilt', 'same'), gFilt, 'same');
G44 = conv2(conv2( Y2.*Fy2, gFilt', 'same'), gFilt, 'same');
G46 = conv2(conv2(  Y.*Fy2, gFilt', 'same'), gFilt, 'same');
G55 = conv2(conv2(     Fx2, gFilt', 'same'), gFilt, 'same');
G56 = conv2(conv2(     Fxy, gFilt', 'same'), gFilt, 'same');
G66 = conv2(conv2(     Fy2, gFilt', 'same'), gFilt, 'same');

%Determinando os componente da matriz b
B11 = conv2(conv2(X.*Fxt,gFilt','same'),gFilt,'same') - G11 - G14;
B21 = conv2(conv2(Y.*Fxt,gFilt','same'),gFilt,'same') - G12 - G24;
B31 = conv2(conv2(X.*Fyt,gFilt','same'),gFilt,'same') - G13 - G34;
B41 = conv2(conv2(Y.*Fyt,gFilt','same'),gFilt,'same') - G14 - G44;
B51 = conv2(conv2(   Fxt,gFilt','same'),gFilt,'same') - G15 - G26;
B61 = conv2(conv2(   Fyt,gFilt','same'),gFilt,'same') - G16 - G46;


Ux = zeros( ceil(dimy/s), ceil(dimx/s));
Uy = zeros( ceil(dimy/s), ceil(dimx/s));

cx = 1; 
for x = 1 : s : dimx
        cy = 1;
        for y = 1 : s : dimy
                GG = [G11(y,x) G12(y,x) G13(y,x) G14(y,x) G15(y,x) G16(y,x);
                      G12(y,x) G22(y,x) G14(y,x) G24(y,x) G25(y,x) G26(y,x);
                      G13(y,x) G14(y,x) G33(y,x) G34(y,x) G35(y,x) G36(y,x);
                      G14(y,x) G24(y,x) G34(y,x) G44(y,x) G26(y,x) G46(y,x);
                      G15(y,x) G25(y,x) G35(y,x) G26(y,x) G55(y,x) G56(y,x);
                      G16(y,x) G26(y,x) G36(y,x) G46(y,x) G56(y,x) G66(y,x)];
                BB = [B11(y,x),B21(y,x),B31(y,x),B41(y,x),B51(y,x),B61(y,x)]';
                if( rank(GG) < 6 )
                        Ux(cy,cx) = 0;
                        Uy(cy,cx) = 0;
                else
                        Hest = inv(GG) * BB; 
                        u = [Hest(1) Hest(2);Hest(3) Hest(4)]*[cx;cy]+[Hest(5);Hest(6)] - [cx;cy]; 
                        Ux(cy,cx) = u(1);
                        Uy(cy,cx) = u(2);
                end
                cy = cy + 1;
        end
        cx = cx + 1;
end

if(1)
    fg = figure(2);set(fg,'color','w');
    subplot(121);plot(gFilt);axis('square');
    title('Filtro Gaussiano');
    subplot(122);plot(gxFilt);axis('square');
    title('Filtro Gaussiano Derivado'); 
    
    fg = figure(3);set(fg,'color','w');
    subplot(321);
    imshow(im0,[min(im0(:)) max(im0(:))]); 
    title('Frame 0');
    subplot(322);
    imshow(im1,[min(im1(:)) max(im1(:))]); 
    title('Frame 1');
    subplot(323);
    imshow(im0blur,[min(im0blur(:)) max(im0blur(:))]); 
    title('Filtro pasa-bajo Frame 0');
    subplot(324);
    imshow(im1blur,[min(im1blur(:)) max(im1blur(:))]); 
    title('Filtro pasa-bajo Frame 1');    
    subplot(325);
    imshow(Fx,[min(Fx(:)) max(Fx(:))]); 
    title('derivada horizontal');
    subplot(326);
    imshow(Fy,[min(Fy(:)) max(Fy(:))]); 
    title('derivada vertical');

    [xramp,yramp] = meshgrid(1:s:dimx,1:s:dimy);
    fg = figure(4); set(fg,'color','w');
    imagesc(im0(:,:,1)); colormap gray; 
    hold on;quiver( xramp, yramp, Ux, Uy, 5 );axis equal;hold on;
end


function [gFilt,gxFilt] = MaskGaussiana(sigmaBlur)
%Longitude do filtro em fun玢o do sigma 
gBlurSize = 2 * round(2.5 * sigmaBlur) + 1; 
x = [1:gBlurSize] - round((gBlurSize+1)/2);  
%Filtro de suavizamento
gFilt = exp(- x .* x / (2.0*sigmaBlur*sigmaBlur));
gFilt = gFilt / sum(gFilt(:));
%Filtro da derivada
gxFilt = (-x/sigmaBlur^2) .* gFilt;