function [Move,fullPredict] = AbsolutPredict (egoX,egoY,detectCarPoss,clock,closingInfo)
%funkce pro zisk predikce v absolutním systému
% detetectCarPoss(i,:) =[VehiclePossX VehiclePossY]
maxCar=20;
maxClosing=5;
 %% clock + pozice Ega

    persistent prevClock;
    if isempty(prevClock)
        prevClock =  zeros(30,1);
    end
    prevClock = circshift(prevClock, [1, 0]);
    prevClock(1,:) = clock;
    
    persistent prevEgo;
    if isempty(prevEgo)
        prevEgo =  zeros(30,3); %X Y velo
    end
    prevEgo = circshift(prevEgo, [1, 0]);
    egoPredict= zeros(30,2);
    if clock > 0.06
        prevEgo(1,1)=egoX;
        prevEgo(1,2)=egoY;  
        prevEgo(1,3)= carCalcFunction.CalcVelo(prevEgo(:,1:2),prevClock);
        egoAcell = carCalcFunction.CalcAcc(prevEgo(:,3),prevClock);
        % predikce ega
        egoPredict= zeros(30,2);
        for j = 1:30 %
            [egoPredict(j,1), egoPredict(j,2)] = carPrediction.CarPredictRegres2(prevEgo(:,1:2), ...
                prevEgo(:,3), ...
                (j/10), ... %Čas [s]
                0);
        end
    end
    egoPredict=egoPredict(any(egoPredict,2),:);
%% inicializace detectCar 
    persistent detectCar;
    
    if isempty(detectCar)
        detectCar = cell(5, maxCar);  %3. Angle rot 4. pointy k rotaci na predikci 
        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);  %3. Angle rot 4. pointy k rotaci na predikci 
        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 auta

    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  
    chybaPredikce=zeros(1,size(detectCarPoss,1));

 %% získání minulé predikce pro chybu
    prevPred = zeros(2,size(detectCarPoss,1));
    for i=1:size(detectCarPoss,1)
        prevPred(1,i)=detectCar{4,i}(1,1);
        prevPred(2,i)=detectCar{4,i}(1,2);
    end

%% detekovaná vozidla
    detectABS=zeros(size(detectCarPoss,1),2);

    for i = 1:size(detectCarPoss,1)
        detectCar{1,i} = circshift(detectCar{1,i}, [1, 0]);
        detectABS(i,1)=detectCarPoss(i,1) + egoX;
        detectABS(i,2)=detectCarPoss(i,2) + egoY;
        detectCar{1,i}(1,1:2) = detectABS(i,1:2);

      % 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
        
        % ____________________________predikce _______________________________
        figure(i+1) % 
        
        [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] 0,033s 
            1); %pozice v grafu

        [detectCar{4,i}(2,1), detectCar{4,i}(2,2)] = carPrediction.CarPredictRegres2(detectCar{1,i}, ...
            detectCar{2,i}(:), ...
            1, ... %Čas [s]
            3);
        [detectCar{4,i}(3,1), detectCar{4,i}(3,2)] = carPrediction.CarPredictRegres2(detectCar{1,i}, ...
            detectCar{2,i}(:), ...
            3, ... %Čas [s]
            0);
        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

        subplot(2,2,2);
        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')
        hold on
        plot(detectCar{4,i}(1,1), detectCar{4,i}(1,2), 'ro');
        title('Pozice auta',sprintf('Prumerna rychlost: %.1f km/h Predikovana pozice x=%.1f m y=%.1f m',detectCar{2,i}(1)*3.6,detectCar{4,i}(1,1), detectCar{4,i}(1,2)))
        xlabel('X [m]')
        ylabel('Y [m]')
        plot(prevPred(1,i), prevPred(2,i), '*');
        leg=legend('Pozice auta','Predikovana pozice','predchozi predikovana pozice');
        set(leg, 'Position', [.8 .9 .1 .1]);
        hold off
        grid on;

        % výpočet chyby predikce z minulého kroku
        chybaPredikce(1,i)=sqrt((prevPred(1,i)-detectCarPoss(i,1)) ^ 2 + (prevPred(2,i) - detectCarPoss(i,2)) ^ 2) / (mean(detectCar{2,i})*(prevClock(1,1) - prevClock(2,1)));
    end

%% smyčka closingCar
    
     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
                [closedX, closedY] = carPrediction.CarPredictRegres2(closingCar{1,i}, ...
                    closingCar{2,i}(:), ...
                    (prevClock(1,1) - prevClock(2,1)), ...
                    0); %pozice v grafu
                
                closingCar{1,i} = circshift(closingCar{1,i}, [1, 0]);
                closingCar{1,i}(1,1)=closedX;
                closingCar{1,i}(1,2)=closedY;

                % 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
     %% graf
    figure (100)
    subplot(1,1,1)
    plot(prevEgo(any(prevEgo,2),1),prevEgo(any(prevEgo,2),2),'r')
    hold on
    plot(egoPredict(:,1), egoPredict(:,2),'g')
    xlabel('X [m]')
    ylabel('Y [m]')
    title('Lokální absolutní kartézská mapa',sprintf('Pocet detekovaných vozidel v okoli: %2.0f',size(detectCarPoss,1)))
    legend('Vlastní vozidlo','Predikovaná trajektorie');
    xlim([prevEgo(1,1)-80 prevEgo(1,1)+80])
    ylim([prevEgo(1,2)-80 prevEgo(1,2)+80])
    grid on;
    for i = 1:size(detectCarPoss,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]);
        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
    end
    hold off   
     %% zápis hodnot do výstupní matice pro výsledky
    Move = zeros(20,11);
    for i = 1:size(detectCarPoss,1)
        Move(i,1) = detectCar{1,i}(1,1); %X 
        Move(i,2) = detectCar{1,i}(1,2); %Y 
        Move(i,3) = detectCar{2,i}(1);  
        Move(i,4) = detectCar{3,i}(1);  
        Move(i,5)= mean(detectCar{2,i}(any(detectCar{2,i}(1:10,1),2),1),1);   %rychlost
        Move(i,6)= mean(detectCar{3,i}(1:10,1));   %acc
        Move(i,7)=closingCar{1,i}(1,1); %x
        Move(i,8)=closingCar{1,i}(1,2); %Y
        Move(i,9)=mean(closingCar{2,i}(1:10,1)); 
        Move(i,10) = chybaPredikce(1,i);
    end

    fullPredict=zeros(30,8);
    for i=1:2
        fullPredict(:,2*i-1)=detectCar{5,i}(:,1);
        fullPredict(:,2*i)=detectCar{5,i}(:,2);
        fullPredict(:,5)=closingCar{5,1}(:,1);
        fullPredict(:,6)=closingCar{5,1}(:,2);
    end
    if clock > 1
        fullPredict(:,7)=egoPredict(:,1);
        fullPredict(:,8)=egoPredict(:,2);
    end
    fullPredict = fullPredict(any(fullPredict,2),:);
    fullPredict = fullPredict(:,any(fullPredict,1));
end
