%declare an empty depth matrix
run 'kinect_test'
% close all;
% clc;
% clear all;
depth=zeros(480,640);
%extract the depth values from frameDataDepth
for i=1:480
    for j=1:640
        depth(i,j)=depthFrameData(i,j,1,2);
    end
end

%flip image to correct it
depth=fliplr(depth);

% Filter out the floor 
h=300; %height of kinect

theta=pi*(22-(480-269)*(22/240))/180;
z=(h/tan(theta));
for i=240:480
    for j=1:640
        theta=pi*(22-(480-i)*(22/240))/180;
        if depth(i,j)< (h/tan(theta))+150
            depth(i,j)=0;
        end
    end
end
%filter out unwated data far (more then 10 cm) from the nearest obstacle
pos=nonzeros(depth);
closest=min(pos);
averclos=mean(pos);
fdepth=zeros(480,640);

for i=1:480
    for j=1:640
        if abs(closest-depth(i,j))<500
            fdepth(i,j)=depth(i,j);
        else
            fdepth(i,j)=0;
        end
    end
end

%convert to int 8
res=int8(mod(depth, 256));
% compare the original and filtered image
close all;
imshow(depth);
% imsharpen(A)
figure;
imshow(res);
figure;
imshow(fdepth);



%Find start and end row of obstacle
% clc;
A=find(fdepth);
%find row of each non zero value and take average to find the midheight of
%obstacle
b=[];
for k=1:length(A)
    if mod(A(k),480)>0
    row=mod(A(k),480);
    else
    row=480;
    end
    b(k)=row;
    
end

averow=ceil(mean(b)); 

%find obstacle distance

averdepth=mean2(nonzeros(fdepth));
obstdepth=closest
%find opening width at the midheight of the obstacle
counter=0;
width=[];
jcount=[];
d=1;
%this finds the width of different openings and their right end pixel located at midheight of
%obstacle
for j=2:640
    if fdepth(averow,j)==0 
        counter=counter+1;
    elseif fdepth(averow,j)~=0 && fdepth(i,j-1)==0
            jcount(d)=j;
            width(d)=counter;
            d=d+1;
            counter=0;
    end
end 
size(width);

% this finds the max width and its right end pixel
space=max(width);
[width, jmax]=max(width);
rightp=jcount(jmax);
%calculate start and end of space to find width in mm.
Pr=rightp-320;
Pl=rightp-space-320;
thetar=atan(Pr*tan(degtorad(28))/320);
thetal=atan(Pl*tan(degtorad(28))/320);
Xr=tan(thetar)*averclos
Xl=tan(thetal)*averclos
xspace=Xr-Xl

%calculate how much the robot has to move to the side to align with the max
%opening
move=(Xr+Xl)/2
