clc; clear all; close all;
%   Set constants (in mm)
%b=315;
%d=450;
b=1000;
d=1000;
deltaw=0.0872664626;


%   count number of images in imaging folder
c=0;
imgcounter=0;
counter=1;
while c==0
    jpgFileName=strcat('Imaging\images\image', num2str(counter), '.jpg');
    %disp(jpgFileName);
    if exist(jpgFileName, 'file')
        imgcounter=imgcounter+1;
        counter=counter+1;
    else
        c=1;
    end
end

%   print number of images
fprintf('There are: %s images \n', num2str(imgcounter));


depth=0;

for l=0:20
    sum=0;
    w=0;
    totalsum=0;

    %   Load Images
    for c = 1:imgcounter
        %   Create an image filename, and read it in to a variable called
        %   imageData.
        jpgFileName = strcat('Imaging\images\image', num2str(c), '.jpg');
        if exist(jpgFileName, 'file')
            imageData = im2double(imread(jpgFileName));
            imageData=rgb2gray(imageData);
            %   Set Dimensions
            [t_r, t_c, trash]=size(imageData);
        end


        %   Set Columns M(row, columns)= M(j, i)
        for i=1:t_c
            %   Calculate h and r
            %h=(i*d)/(d*cos(w)-i*sin(w));
            h1=i*d
            h2=d*cos(w)-i*sin(w)
            h=h1/h2;

            r1=(h-(l*d*sin(w))/(b*cos(w)-l))
            r2=(1-l/(b*cos(w)))
            r=r1*r2;
            rn=round(r);
            %   Test Code
            disp('i has increased');
            disp(i);
            disp(h);
            disp(r);
            disp(c);
            disp(w);

            %   Set rows
            for j=1:t_r
                %   Temporary Code
                if rn<1 || rn>t_c
                    disp('r is below 1 or > t_c');

                    rn=1;
                else
                Mp(j,rn)=imageData(j,i);

                %   Test Code
                %disp('Rows are running');

                end

            end
        end
        imshow(Mp);
        %pause
        sum=sum+Mp;
        %   Set omega +5 degrees
        w=w+deltaw;
        disp(l);

    end

    %   Calculate M at depth l
    totalsum=sum./imgcounter;

    %   Export image at depth l
    str=num2str(depth);
    imwrite(sum, ['Imaging\images\results\' str '.jpeg']);
    depth=depth+1;
    disp('Got to end of l loop');
end