function [predsPoss,fullPredict] = RelPredict(detectCarPossAbsRel,egoIMU,clock,closingInfo)
%funkce pro ziskání predkcí jednotlivých aut 
maxCar=20;
maxClosing=5;
%% persistent promměnné
    persistent prevClock;
    if isempty(prevClock)
        prevClock =  zeros(30,1);
    end
    persistent prevEgo;
    if isempty(prevEgo)
        prevEgo =  zeros(30,7); %X Y VeloX veloY AccX AccY rot
    end

    persistent detectCar;
    
    if isempty(detectCar)
        detectCar = cell(5, maxCar);  
        for i = 1:maxCar
            detectCar{1,i} = zeros(30,2); % přechozí pozice %počet řádků určuje počet předchozích kroků
            detectCar{2,i} = zeros(10,1); %rychlost
            detectCar{3,i} = zeros(10,1); %zrychlení
            detectCar{4,i} = zeros(3,2);  %1. řádek 0,033s 2. řídek 1 s 3. řádek 3 s [X Y]
            detectCar{5,i} = zeros(30,2);% predikce 0:0.1:3 vteřin [X Y]
        end
    end
    persistent closingCar;
    
    if isempty(closingCar)
        closingCar = cell(5, maxClosing);
        for i = 1:maxClosing
            closingCar{1,i} = zeros(30,3); % přechozí pozice %počet řádků určuje počet předchozích kroků
            closingCar{2,i} = zeros(10,1); %rychlost
            closingCar{3,i} = zeros(10,1); %zrychlení
            closingCar{4,i} = zeros(3,2);  %1. řádek 0,033s 2. řídek 1 s 3. řádek 3 s [X Y]
            closingCar{5,i} = zeros(30,2);% predikce 0:0.1:3 vteřin [X Y]
        end
    end
%% odstranění zmizelého vozidla
    firstZeroCollClosing = 1;
    for i = 1:size(closingCar, 2) 
        if mean(closingCar{1,i},'all')== 0 % porovnání průměru matice s nulou 
            firstZeroCollClosing = i; % uložení indexu sloupce
            break; 
        end
    end

    %____________odstranění zmizelého auta __________
    if ~isempty(closingInfo)
        for i=1:size(closingInfo,1) 
            j=closingInfo(i,1);
            l=closingInfo(i,2);
            m=closingInfo(i,3);
            n=closingInfo(i,4); %hard delete
            if n>0
                for c=1:5
                    for k=n:(maxCar-1)
                        detectCar{c,k} = detectCar{c,k+1}; %posunutí buněk o jedno doleva od pozice podle closing
                    end
                end
                detectCar{1,maxCar} = zeros(30,2); %nulovaní posledního
                detectCar{2,maxCar} = zeros(10,1);
                detectCar{3,maxCar} = zeros(10,1);
                detectCar{4,maxCar} = zeros(3,2);
                detectCar{5,maxCar} = zeros(30,2); 
            end

            if j>0 %přesuň do closing a smaž exist
                closingCar{1,firstZeroCollClosing}(:,1:2)=detectCar{1,j}(:,1:2);
                closingCar{2,firstZeroCollClosing}=detectCar{2,j};
                closingCar{3,firstZeroCollClosing}=detectCar{3,j};
                closingCar{4,firstZeroCollClosing}=detectCar{4,j};
                closingCar{5,firstZeroCollClosing}=detectCar{5,j};

                for c=1:5
                    for k=j:(maxCar-1)
                        detectCar{c,k} = detectCar{c,k+1}; %posunutí buněk o jedno doleva od pozice podle closing
                    end
                end
                detectCar{1,maxCar} = zeros(30,2); %nulovaní posledního
                detectCar{2,maxCar} = zeros(10,1);
                detectCar{3,maxCar} = zeros(10,1);
                detectCar{4,maxCar} = zeros(3,2);
                detectCar{5,maxCar} = zeros(30,2); 
                firstZeroCollClosing=firstZeroCollClosing+1;
            end

            if l>0 %přesuň do exist a smaž closing
                detectCar{1,m}(:,1:2)= closingCar{1,l}(:,1:2);
                detectCar{2,m}= closingCar{2,l};
                detectCar{3,m}= closingCar{3,l};
                detectCar{4,m}= closingCar{4,l};
                detectCar{5,m}= closingCar{5,l};
                for c=1:5
                    for k = l:(maxClosing-1)
                        closingCar{c,k} =  closingCar{c,k+1};
                    end
                end
                closingCar{1,maxClosing} = zeros(30,3); 
                closingCar{2,maxClosing} = zeros(10,1); 
                closingCar{3,maxClosing} = zeros(10,1); 
                closingCar{4,maxClosing} = zeros(3,2);  
                closingCar{5,maxClosing} = zeros(30,2);
                firstZeroCollClosing=firstZeroCollClosing-1;
            end
        end
    end  
 %% získání minulé predikce pro chybu
    prevPred = zeros(2,size(detectCarPossAbsRel,1));
    for i=1:size(detectCarPossAbsRel,1)
        prevPred(1,i)=detectCar{4,i}(1,1);
        prevPred(2,i)=detectCar{4,i}(1,2);
    end

    %% inicializace rychlosti and natočení auta (nezačíná se od nulové rychlosti 
    if clock > 0.05 && clock < 0.070
        prevEgo(1,3)=14*sind(egoIMU(1,4));
        prevEgo(1,4)=14*cosd(egoIMU(1,4));
        prevEgo(1,7)=egoIMU(1,4);
    end
    %% zpracování pohybu Ega / prevEgo =  X Y VeloX veloY AccX AccY / IMU = aX aY omegaZ magnetic
    %uložení nového času
    prevClock = circshift(prevClock, [1, 0]);
    prevClock(1,:) = clock;
        
    dt=prevClock(1,1)-prevClock(2,1);
   
    accelEgoFront=egoIMU(1,1);
    accelEgoLateral=egoIMU(1,2);
    omegaZ=egoIMU(1,3);
    magnetic=egoIMU(1,4);
    
    prevEgo = circshift(prevEgo, [1, 0]);
    
    fi_Z= omegaZ*dt;
    prevEgo(1,7)=prevEgo(2,7)+fi_Z;

    accelEgoX=accelEgoFront*sind(prevEgo(1,7))+(accelEgoLateral)*sind(prevEgo(1,7)-90);
    accelEgoY=accelEgoFront*cosd(prevEgo(1,7))+(accelEgoLateral)*cosd(prevEgo(1,7)-90);
    prevEgo(1,5)=accelEgoX;
    prevEgo(1,6)=accelEgoY;
    
    egoVeloNewX = prevEgo(2,3)+accelEgoX*dt;
    egoVeloNewY = prevEgo(2,4)+accelEgoY*dt;
    prevEgo(1,3) = egoVeloNewX;
    prevEgo(1,4) = egoVeloNewY;

    deltaEgoX=((accelEgoX/2)*(dt^2))+egoVeloNewX*dt;
    deltaEgoY=((accelEgoY/2)*(dt^2))+egoVeloNewY*dt;
    prevEgo(1,1)=0;
    prevEgo(1,2)=0;

    
    prevEgo(2:end,1)=prevEgo(2:end,1)-deltaEgoX;
    prevEgo(2:end,2)=prevEgo(2:end,2)-deltaEgoY;
    %% predikce ega
    egoPredict= zeros(30,2);
    egoVelo= sqrt(egoVeloNewX^2 + egoVeloNewY^2);
    for j = 1:30 %
        [egoPredict(j,1), egoPredict(j,2)] = carPrediction.CarPredictRegres2(prevEgo, ...
            egoVelo, ...
            (j/10), ... %Čas [s]
            0);
    end

    %% zpracování jednotlivých detekovaných vozidel
    
    for i=1:size(detectCarPossAbsRel,1)

        %posun všech hodnot podle Ega
        elmntsNum=size(detectCar{1,i}(any(detectCar{1,i}(:,1),2),1),1);
        detectCar{1,i}(1:elmntsNum,1) = detectCar{1,i}(1:elmntsNum,1)-deltaEgoX;
        detectCar{1,i}(1:elmntsNum,2) = detectCar{1,i}(1:elmntsNum,2)-deltaEgoY;

        detectCar{1,i} = circshift(detectCar{1,i}, [1, 0]);
        %rotace bodu do rel XY s nulou Ega
        [x_rotated, y_rotated] = carCalcFunction.RotPoints(detectCarPossAbsRel(i,1),detectCarPossAbsRel(i,2),(-prevEgo(1,7)+90));

        detectCar{1,i}(1,1)=x_rotated;
        detectCar{1,i}(1,2)=y_rotated;

        %% rychlost
        if ~(detectCar{1,i}(2,1) == 0)      
            detectCar{2,i} = circshift(detectCar{2,i}, [1, 0]);
            velo_act = carCalcFunction.CalcVeloDet(detectCar{1,i},prevClock);

            if size(detectCar{2,i}(any(detectCar{2,i}(:,1),2),1),1) >= 2
                detectCar{2,i}(1) = velo_act*1+detectCar{2,i}(2,1)*0;
            else
                detectCar{2,i}(1) = velo_act;
            end
        else
            detectCar{4,i}(1,1)=NaN;
            detectCar{4,i}(1,2)=NaN;
            break
        end
        %% zrychlení
        if ~(detectCar{2,i}(2,1) == 0)
            detectCar{3,i} = circshift(detectCar{3,i}, [1, 0]);
            acc_act = carCalcFunction.CalcAcc(detectCar{2,i}(:,1),prevClock);

            if size(detectCar{3,i}(any(detectCar{3,i}(:,1),2),1),1) >= 2
                detectCar{3,i}(1) = acc_act*0.3+detectCar{3,i}(2,1)*0.7;
            else
                detectCar{3,i}(1) = acc_act;
            end
        else
            detectCar{4,i}(1,1)=NaN;
            detectCar{4,i}(1,2)=NaN;
            break
        end


        %figure(i+21) 
        [detectCar{4,i}(1,1), detectCar{4,i}(1,2)] = carPrediction.CarPredictRegres2(detectCar{1,i}, ...
            detectCar{2,i}(:), ...
            (prevClock(1,1) - prevClock(2,1)), ... %%Čas [s] 1/32s 
            0); %pozice v grafu
        for j = 1:30 %
            [detectCar{5,i}(j,1), detectCar{5,i}(j,2)] = carPrediction.CarPredictRegres2(detectCar{1,i}, ...
                detectCar{2,i}(:), ...
                (j/10), ... %Čas [s]
                0);
        end
        
    end

%% smyčka closingCar pouze zavřené připsání bodu nebo smazání
    if firstZeroCollClosing > 1
        i=1;
        while i <= firstZeroCollClosing-1
        closingCar{1,i}(1,3)=closingCar{1,i}(2,3)+1;
            if closingCar{1,i}(1,3) > 5
                for c= 1:5
                    for k = i:(maxClosing-1)
                         closingCar{c,k} =  closingCar{c,k+1};
                    end
                end
                    closingCar{1,maxClosing} = zeros(30,3); 
                    closingCar{2,maxClosing} = zeros(10,1); 
                    closingCar{3,maxClosing} = zeros(10,1); 
                    closingCar{4,maxClosing} = zeros(3,2);  
                    closingCar{5,maxClosing} = zeros(30,2);
    
                firstZeroCollClosing=firstZeroCollClosing-1; 
            else
                %posun všech hodnot podle Ega
                elmntsNum=size(closingCar{1,i}(any(closingCar{1,i}(:,1),2),1),1);
                closingCar{1,i}(1:elmntsNum,1) = closingCar{1,i}(1:elmntsNum,1)-deltaEgoX;
                closingCar{1,i}(1:elmntsNum,2) = closingCar{1,i}(1:elmntsNum,2)-deltaEgoY;

                [x_pred, y_pred] = carPrediction.CarPredictRegres2(closingCar{1,i}, ...
                    closingCar{2,i}(:), ...
                    (prevClock(1,1) - prevClock(2,1)), ... %%Čas [s] 1/32s 
                    0); %pozice v grafu

                closingCar{1,i} = circshift(closingCar{1,i}, [1, 0]);
                closingCar{1,i}(1,1)=x_pred;
                closingCar{1,i}(1,2)=y_pred;
        
                          % rychlost
                if ~(closingCar{1,i}(2,1) == 0)      
                    closingCar{2,i} = circshift(closingCar{2,i}, [1, 0]);
                    veloC_act = carCalcFunction.CalcVeloDet(closingCar{1,i},prevClock);
        
                    if size(closingCar{2,i}(any(closingCar{2,i}(:,1),2),1),1) >= 2
                        closingCar{2,i}(1) = veloC_act*1+closingCar{2,i}(2,1)*0;
                    else
                        closingCar{2,i}(1) = veloC_act;
                    end
                else
                    closingCar{4,i}(1,1)=NaN;
                    closingCar{4,i}(1,2)=NaN;
                    break
                end
        
                % zrychlení
                if ~(closingCar{2,i}(2,1) == 0)
                    closingCar{3,i} = circshift(closingCar{3,i}, [1, 0]);
                    accC_act = carCalcFunction.CalcAcc(closingCar{2,i}(:,1),prevClock);
        
                    if size(closingCar{3,i}(any(closingCar{3,i}(:,1),2),1),1) >= 2
                        closingCar{3,i}(1) = accC_act*0.3+closingCar{3,i}(2,1)*0.7;
                    else
                        closingCar{3,i}(1) = accC_act;
                    end
                else
                    closingCar{4,i}(1,1)=NaN;
                    closingCar{4,i}(1,2)=NaN;
                    break
                end
                for j = 1:30
                [closingCar{5,i}(j,1), closingCar{5,i}(j,2)] = carPrediction.CarPredictRegres2(closingCar{1,i}, ...
                    closingCar{2,i}(:), ...
                    (j/10), ... %Čas [s]
                    0);
                end
                i=i+1;
            end
        end
    end
    
    %% vystupní
    predsPoss=zeros(20,12);
    for i=1:size(detectCarPossAbsRel,1)
        predsPoss(i,1)=detectCar{1,i}(1,1);
        predsPoss(i,2)=detectCar{1,i}(1,2);
        predsPoss(i,3)=detectCar{2,i}(1);  
        predsPoss(i,4)=detectCar{3,i}(1);  
        predsPoss(i,7)=mean(detectCar{2,i}(any(detectCar{2,i}(1:10,1),2),1),1);   %rychlost
        predsPoss(i,8)=mean(detectCar{3,i}(1:10,1));   %acc
        predsPoss(i,9)=closingCar{1,i}(1,1);
        predsPoss(i,10)=closingCar{1,i}(1,2);
        predsPoss(i,11)=mean(closingCar{2,i}(any(closingCar{2,i}(1:10,1),2),1),1);
        predsPoss(i,12)=egoVelo;
    end

    fullPredict=zeros(30,6);
    for i=1:2
        fullPredict(:,2*i-1)=detectCar{5,i}(:,1);
        fullPredict(:,2*i)=detectCar{5,i}(:,2);
    end
    fullPredict(:,5)=closingCar{5,1}(:,1);
    fullPredict(:,6)=closingCar{5,1}(:,2);

    fullPredict = fullPredict(any(fullPredict,2),:);
    fullPredict = fullPredict(:,any(fullPredict,1));

%% graf
    figure (102)
    subplot(1,1,1)
    plot(prevEgo(:,1),prevEgo(:,2),'r')
    hold on
    plot(egoPredict(:,1), egoPredict(:,2),'g')
    xlabel('X [m]')
    ylabel('Y [m]')
    xlim([-80 80])
    ylim([-80 80])
    title('Lokální relativní kartézská mapa',sprintf('Pocet detekovaných vozidel v okoli: %2.0f ',size(detectCarPossAbsRel,1)))
    legend('Vlastní vozidlo','Predikovaná trajektorie')
    grid on;   
    for i = 1:size(detectCarPossAbsRel,1)
        plot(movmean(detectCar{1,i}(any(detectCar{1,i}(:,1),2),1),[2 0],1),movmean(detectCar{1,i}(any(detectCar{1,i}(:,2),2),2),[2 0],1),'b')
        plot(detectCar{5,i}(:,1), detectCar{5,i}(:,2), 'g');
        legend('Vlastní vozidlo','Predikované trajektorie','Detekované vozidla');
        %set(leg, 'Position', [.8 .9 .1 .1]);
    end
    if firstZeroCollClosing > 1
        for i = 1:(firstZeroCollClosing-1)
            plot(movmean(closingCar{1,i}(any(closingCar{1,i}(:,1),2),1),[2 0],1),movmean(closingCar{1,i}(any(closingCar{1,i}(:,2),2),2),[2 0],1),'k')
            plot(closingCar{5,i}(:,1), closingCar{5,i}(:,2), 'g');
            legend('Vlastní vozidlo','Predikovaná trajektorie','Detekované vozidla','','Zmizelé vozidlo');
        end
    end
    hold off

end
