clear

% create 2d hanning window for center weighting
hfilth = hanning(320);
hfiltv = hanning (240);
for i=1:320
        for j = 1:240
                hanmatrix(j,i) = hfilth(i) * hfiltv(j);
        endfor
endfor

%focus loop
%focusresult
bestfocus = 0;
bestfocusresult = 0;
start_pos =73;
end_pos = 248;
focus_step = 10;

index = 0;

while (focus_step > 1)

focus_pos = start_pos;
commandstring = strcat("./autofocus ",num2str(focus_pos));
system(commandstring);

while (focus_pos < end_pos)

index = index+1;

system ("./xcaplive -tvchannel 1");

commandstring = strcat("./autofocus ",num2str(focus_pos+focus_step));
system(commandstring);
printf("loading ppm\n");
[R G B] = loadpnm("file2.PPM");
printf("greying rgb\n");
grey = rgb2grey(R,G,B);
printf("filtering rgb\n");
greyfilt = grey.*hanmatrix;
%imshow(equalize64(greyfilt));
printf("ffting rgb\n");
greyfft = fft2(greyfilt, 240,320);
printf("abssfing rgb\n");
absfft = abs(greyfft);

 momentsum = 0;
 sum = 0;
 printf("counting\n");
for u = 1:120
        for v = 1:160
                momentsum = momentsum + absfft(u,v)*(u^2+v^2)^(1/2);
                sum = sum+absfft(u,v);
        endfor
endfor

absfftcom = momentsum/sum;
focusresult(index,1) = focus_pos;
focusresult(index,2) = absfftcom;
if (absfftcom > bestfocusresult)
        bestfocus = focus_pos;
        bestfocusresult = absfftcom;
endif
focus_pos = focus_pos + focus_step;

printf("done\n");
endwhile

start_pos = bestfocus - focus_step;
end_pos = bestfocus + focus_step;
focus_step = floor(focus_step /2);

endwhile

gplot (focusresult);
commandstring = strcat("./autofocus ",num2str(bestfocus));
system(commandstring);

