clc;close all;clear

%############################################
%###### Preprocessing of the raw image ######
%############################################
I=imread('Image.jpg'); % Read image from database
% I=rgb2gray(I); % Convert a color image to gray image
imshow(I)
loc=ginput(2)
loc=round(loc)
I=I(loc(3):loc(4),loc(1):loc(2));
I=imresize(I,[500,500]); % Changing the image size to 500*500 pixels


%%
Width_Lane = widthfind(I)

%%
%<<<<<<<<<<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>>>>>>>
%<< creating VPP of SubImages by overlaping >>
%<<<<<<<<<<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>>>>>>>
w=100; % The width of each SubImage
Step_overlap=30; % Degree of overlap between SubImages 
overlap=1-Step_overlap/100; % The amount of overlap
S_vpp=[];
for i=1:size(I,1)/w/overlap
    if i==round(size(I,1)/(w*overlap))
       II=I((i-1)*w*(overlap)+1 : end,:); 
    else
       II=I((i-1)*w*(overlap)+1 : (i-1)*w*(overlap)+w,:);
    end
    S_vpp(i,:)=sum(II); % Including VPP signal of SubImages
end

%%
%<<<<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>>>>>>
%<<<Remove additional peak of signal>>>
%<<<<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>>>>>>

P=ceil(Width_Lane/4); % P Equal to half of Averaged Range - width of Lane is 40 pixels
for j=1:size(S_vpp,1)
    S_vpp1=S_vpp(j,:);
    % S_vpp is signal that removed noise
    for i=P+1:size(S_vpp,2)-P
        S_vpp(j,i)=sum(S_vpp1(i-P:i+P))/21;
    end
end
% plot(S_vpp1,'r','LineWidth',2)
% hold on
% plot(S_vpp,'b','LineWidth',2)

%%
PLans={};
SlopeThreshold=0.0005;
AmpThreshold=0;
smoothwidth=2;
peakgroup=5;
smoothtype=5;
for ii=1:size(S_vpp,1)
    y=S_vpp(ii,:);
    x=1:size(y,2);
    P=[];
    P=peakfind(x,y,SlopeThreshold,AmpThreshold,smoothwidth,peakgroup,smoothtype);
%     g=sum(P);
%     meanarea=g(5)/size(P,1);
%     for j=1:size(P,1)
%         if 1-(abs(meanarea-P(j,5))/meanarea)<0.15
%             P(j,:)=[];
%         end
%     end
%   Removeing the redundant local maxima in Both sides of Picture
    
    P( ( P(:,2)<Width_Lane/3 | P(:,2)>(size(S_vpp,2)-Width_Lane/3) ),:)=[];    
    PLans{ii}=P;
end

 %%
 StartPartition=1;
% S_hpp=sum(I,2);
% SlopeThreshold=0.001;
% AmpThreshold=0;
% smoothwidth=2;
% peakgroup=5;
% smoothtype=5;
% P=peakfind(1:size(S_hpp,1),S_hpp,SlopeThreshold,AmpThreshold,smoothwidth,peakgroup,smoothtype);
% [M N]=sort(P(:,3));
% w=100; % The width of each SubImage
% Step_overlap=30; % Degree of overlap between SubImages 
% for i=1:size(I,1)/w/overlap
%     if P(N(end-1),2)>=(i-1)*w*(overlap)+1  &  P(N(end-1),2)<(i-1)*w*(overlap)+w
%         StartPartition=i;
%     end
% end

%% creating matrix of local maxima
local_maxima=Remov_locmax(PLans{1,StartPartition}(:,2),Width_Lane)';
local_maxima=[local_maxima;zeros(size(PLans,2)-StartPartition,size(local_maxima,2))];
%...............
% for i=StartPartition:-1:2
%     P1=Remov_locmax(PLans{1,i}(:,2),Width_Lane);
%     P2=Remov_locmax(PLans{1,i-1}(:,2),Width_Lane);
% 
%     for ii=1:size(P1,1)
%         [M N]=min(abs(P1(ii)-P2));
%         local_maxima(i-1,N)=P2(N);
%     end
% end
%...............
for i=StartPartition+1:size(PLans,2)
    P1=local_maxima(i-1,:)';
    P2=Remov_locmax(PLans{1,i}(:,2),Width_Lane);

    for ii=1:size(P2,1)
        [M N]=min(abs(P1-P2(ii)));
        if M < Width_Lane/2
            local_maxima(i,N)=P2(ii);
        end
    end
    s=local_maxima;
    %...................
    P3=local_maxima(i,:,1)';
    for iii=1:size(P2,1)
        if sum((P2(iii)-P3)==0)~=1
            [M1 N1]=min(abs(P2(iii)-P3));
            if Width_Lane/2 < M1 < Width_Lane
                if P2(iii)<local_maxima(i,N1,1)
                    local_maxima(i,N1-1,1)=P2(iii);
                elseif P2(iii)>local_maxima(i,N1,1)
                    local_maxima(i,N1+1,1)=P2(iii);
                end
            end
        end
    end
end

%%
% Separation of Lane
w=100; % The width of each SubImage
Step_overlap=30; % Degree of overlap between SubImages
local_maxima(:,:,2)=zeros(size(local_maxima));
for i=1:size(I,1)/w/overlap
    if i==round(size(I,1)/(w*overlap))
        local_maxima(i,:,2)=round(((i-1)*w*(overlap)+1 + size(I,1))/2);
    else
       local_maxima(i,:,2)=round(((i-1)*w*(overlap)+1 + (i-1)*w*(overlap)+w)/2);
    end
end
%....................
% curve fitting
Lanes={};
imshow(I)
hold on
for i=1:size(local_maxima,2)
    xylane=[local_maxima(:,i,2) , local_maxima(:,i,1)];
    bwxy=(xylane(:,2)==0);
    xylane(bwxy,:)=[];
    if size(xylane,1)<2
        warning([num2str(i),'th lane from left side is incorect'])
    else
        [row, col] = createFit(xylane(:,1), xylane(:,2),size(I,1));
        Lane=[];
        for i2=1:500
            Lane= [Lane;I(row(i2), col(i2)-fix(Width_Lane/2):col(i2)+fix(Width_Lane/2))];
        end
        Lanes{i,1}=Lane';
%         Lanes{i}=I(row , col-fix(Width_Lane/2):col+fix(Width_Lane/2));
        plot(col,row,'.r','MarkerSize',5)
        plot(xylane(:,2),xylane(:,1),'*g','MarkerSize',5)
    end
end
for k=1:size(Lanes,1)
    figure;imshow(Lanes{k,1})
end