%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Namira Group %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%% www.nn4e.com %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 09360703858  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%% nn4e@aol.com %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  @nimanamia  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @nn4enamira  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


clc; clear all; close all;
addpath(genpath('flow_code'));
%% LOADING AND INITIAL PROCESSING OF THE STEREO PAIR

% Load test images
database = cell(2,1);
file = dir('Test_Images/packard/*.jpg');
NF = length(file);
for k = 1 : NF
  database{k,1} = imread(fullfile('Test_Images/packard', file(k).name));
end

im1 = database{1,1};
im2 = database{2,1};

%Scaling down the original images by a factor scaleOrg
scaleOrg = 0.5;
im1_small = imresize(im1,scaleOrg);
im2_small = imresize(im2,scaleOrg);

%Transformed to a Gray scale image
im1_color = im1_small;
im1_small = rgb2gray(im1_small);
im2_small = rgb2gray(im2_small);

%Display bottom and top images
figure(1);
imshow(im1_small);
title('Bottom image')
figure(2);
imshow(im2_small);
title('Top image')

%% DISPARITY AND DEPTH ESTIMATION

%Disparity
%Obtaining the optical flow (refer to /flow_code for more details)
uv = estimate_flow_interface(im1_small, im2_small, 'classic+nl-fast');
Ver = uv(:,:,2);


imVer = mat2gray(Ver);
figure(3);
imshow(Ver,[]);
title('Vertical disparity')
Ver(Ver<0.01) = 0.01;
figure(4);
imshow(Ver,[]);
title('Vertical disparity with -ve values saturated')

%Depth calculation
baseline = 0.135;
depth = disparity2depth(Ver,baseline);
depth1 = depth;

%thresholding the depth range
thres = 50;
depth1(depth1>thres) = thres;
figure(5);
imshow(depth1,[])
title('Depth Map')


%% OBSTACLE DETECTION (OD)

%Chopping to the required field of view for Obstacle detection
azRange = [179,-180];
altRange = [30, -30];
[depth_chop] = chopSphImage(depth1,azRange,altRange,'b');

%scaling down the image for faster OD
scale = 0.75;
d_ds = imresize(depth_chop,scale);
figure(6)
imshow(d_ds,[])
title('Chopped Depth')

%OD function
[Obstacles,Obs_point] = OD(d_ds,azRange,altRange);
figure(7);
imshow(Obstacles)
title('Obstacle areas')

%Filtering
medIm = medfilt2(Obstacles);
medD = medfilt2(d_ds,[5,5]);
figure(8);
imshow(medIm)
title('After Median filtering')
SE = strel('square',5);
ObstaclesClose = imcomplement(medIm);
closeIm = imclose(ObstaclesClose, SE);
closeIm = imcomplement(closeIm);
figure(9);
imshow(closeIm)
title('After Closing operation')

% Polar plot for motion planning
figure(10);
obstacleMap(closeIm,medD,azRange,altRange,30)
title('Polar Plot')

%Overlaying obstacles on the original image
resizeObstacleBW = imresize(closeIm, 1/(scale));
depthresize = imresize(d_ds, 1/scale);
Bin = resizeObstacleBW;
Bin(depthresize>=(thres-0.2))=0;
R   = 1;  % Value in range [0, 1]
G   = 1;
B   = 0;
RGB = cat(3, Bin * R, Bin * G,Bin * B);
im1_chopped = chopSphImage(im1_color,azRange,altRange,'c');

figure(12)
imshow(im1_chopped); hold on;
h = imshow(RGB); hold off;
alpha_data = Bin.*0.6;
set(h, 'AlphaData', alpha_data);


%% SAVE RESULTS
disp_chopped = chopSphImage(Ver,azRange,altRange,'b');
imwrite(mat2gray(im1_chopped),'results/packard/original.png')
imwrite(mat2gray(disp_chopped),'results/packard/disparity.png')
imwrite(mat2gray(d_ds),'results/packard/depth.png')
imwrite(mat2gray(Obstacles),'results/packard/OD.png')
imwrite(mat2gray(closeIm),'results/packard/OD_pp.png')



