classdef carPrediction < carCalcFunction
    % funkce predikcí
    %
    % This template includes the minimum set of functions required
    % to define a System object with discrete state.

    methods(Static)
        function [x_pred, y_pred] = CarPredictRegres2(inMatPoss,velo,time,i)%,accel
                %funkce pro získání predikované pozice
                inMatPoss=inMatPoss(any(inMatPoss,2),:);
                cont = size(inMatPoss(any(inMatPoss(:,1),2),1),1);
                if cont < 29
                    if time*10 > cont || isempty(inMatPoss)  
                        x_pred=NaN;
                        y_pred=NaN;
                        return
                    end
                end
                movedPoints=zeros(size(inMatPoss,1),2);
                movMeanPoints=zeros(size(inMatPoss,1),2);
                x_pred = [];
                y_pred = [];
                
                movMeanPoints(:,1)=movmean(inMatPoss(:,1),[0 2],1);
                movMeanPoints(:,2)=movmean(inMatPoss(:,2),[0 2],1);
                

                movedPoints(:,1) = movMeanPoints(:,1)-movMeanPoints(1,1); %posunutí X do 0
                movedPoints(:,2) = movMeanPoints(:,2)-movMeanPoints(1,2); %posunutí Y do 0
            
                %__________rotace do X ________ 
                [forRotY,~,~] = carCalcFunction.WorldRot(movMeanPoints,size(movMeanPoints,1));
                forRot=forRotY-90;

                rotatedPointsX = movedPoints(:,1)*cosd(forRot)-movedPoints(:,2)*sind(forRot);
                rotatedPointsY = movedPoints(:,1)*sind(forRot)+movedPoints(:,2)*cosd(forRot);
                
                %_____________________proložení________________________fitnutí v x+
                carFitCurve = polyfit(rotatedPointsX,rotatedPointsY,2);
                %posunutí do nuly pomoci  posledního členu
                
                if cont > 0 && cont < 10
                    carFitCurve(1,1)=carFitCurve(1,1)/2;
                    carFitCurve(1,2)=carFitCurve(1,2)/2;
                elseif cont > 10 && cont < 20
                    carFitCurve(1,1)=carFitCurve(1,1)/1.3;
                    carFitCurve(1,2)=carFitCurve(1,2)/1.3;
                end
                carFitCurve(1,3)=0;
                
                velo=velo(any(velo,2),:);
                if size(velo,1) > 9
                     meanVelo=mean(velo(1:10,1),"all","omitnan");
                else
                    meanVelo=mean(velo,"all","omitnan");
                end

                if meanVelo > 1
                     x1 = 0:0.1:(meanVelo*(time)*2);% + (meanAccel/2)*((time)^2))*2; 

                elseif meanVelo <= 1
                    x_pred=movedPoints(1,2);
                    y_pred=movedPoints(1,2);
                    return;
                else
                    x1=0:0.1:1;  
                end


                y1 = polyval(carFitCurve, x1);
            
                %_____________________kružnice_______________________
                [xCirc, yCirc] = carCalcFunction.CircleTrajectory(meanVelo,0,time);%meanAccel
                
                %____________________nejbližší body____________________  
                [x_notRotPred, y_notRotPred] = carCalcFunction.IntersectTwoPoly(x1,y1,xCirc, yCirc);
             
                %_________________rotace a posunutí zpět______________
                if ~isempty(x_notRotPred)
                    x_pred = (x_notRotPred*cosd(-forRot) - y_notRotPred*sind(-forRot))+inMatPoss(1,1); 
                    y_pred = (x_notRotPred*sind(-forRot) + y_notRotPred*cosd(-forRot))+inMatPoss(1,2);
                else
                    x_pred = NaN;
                    y_pred = NaN;
                end

                if i > 0  
                    subplot(2,2,i);
                    plot(rotatedPointsX,rotatedPointsY)
                    hold on
                    plot(x1,y1,'g')
                    plot(xCirc, yCirc, 'k');
                    plot(x_notRotPred, y_notRotPred, 'ro');
                    title('Body pro predikci',sprintf('Body orotovane do -x, Cas predikce: %.3f s',time))
                    xlabel('X [m]')
                    ylabel('Y [m]')
                    hold off
                    grid on;
                end
        end
        
        function y_DistPred = PredictDistFromMat(matDistFromLid)
        %funkce pro predikovaní vzdálensti pomocí predchozích hodnot pomocí regrese
%             PredDist= zeros(1,1);
                   
            distPoints = matDistFromLid(any(matDistFromLid(:,1),2),1);            
            xForPoints =  0:-1:-(size(distPoints,1)-1);            
            distFitCurve = polyfit(xForPoints(1:size(distPoints,1)), distPoints,2);           
            y_DistPred = polyval(distFitCurve, 1);            
            

%             if ~isempty(y_DistPred)           
%                 PredDist(1,1)=y_DistPred;
%             else
% %                 PredDist(1,1)=NaN;
% %                     figure(41);
% %                     plot(xForPoints,matDistFromLid,'b')
% %                     hold on
% %                     title('vzdálenost')
% %                     plot(1, y_DistPred, 'ro');
% %                     hold off
% %                     grid on;
%             end
        end


        function y_AnglePred = PredictAngleFromMat(matAngle)
        %funkce pro predikovaní uhlu predchozích hodnot pomocí regrese 2.
            %y_AnglePred= zeros(1,1);     
            anglePoints = matAngle(any(matAngle(:,1),2),1);

            if max(anglePoints) - min(anglePoints) > 180
                % vyskytuje se přeskakování mezi 0 a 360 stupni 
                for i = 1 : size(anglePoints,1)
                    if anglePoints(i,1) < 0
                        anglePoints(i,1)=anglePoints(i,1)+360;
                    elseif anglePoints(i,1) > 360
                        anglePoints(i,1)=anglePoints(i,1)-360;
                    end
                end
            end

            xForPoints =  0:-1:-(size(anglePoints,1)-1);
            angleFitCurve = polyfit(xForPoints(1:size(anglePoints,1)), anglePoints,2);      
            y_AnglePred = polyval(angleFitCurve, 1);
            
            if y_AnglePred < 0
                y_AnglePred=y_AnglePred+360;
            elseif y_AnglePred > 360
                y_AnglePred=y_AnglePred-360;
            end

%             figure(71);
%             plot(xForPoints,anglePoints,'r')
%             hold on
%             title('uhel')
%             plot(1, y_AnglePred, 'ro');
%             hold off
%             grid on;
        end

        function [x_pred, y_pred] = CarPredictRotateLastLine(inMatPoss,carWorldAngle,velo,accel,AngleDetDiff,time)
                %nebyla použitá- není upravená  
                movMeanPoints(:,1)=movmean(inMatPoss(:,1),[0 2],1);
                movMeanPoints(:,2)=movmean(inMatPoss(:,2),[0 2],1);

                for n = 1:3            %vytvoření přímky 
                    rotatedPointsX(n,1) = (movMeanPoints(1,1) + carWorldAngle(1,2)*n); 
                    rotatedPointsY(n,1) = (movMeanPoints(1,2) + carWorldAngle(1,3)*n); 
                end
       
                rotatedPointsX  = circshift(rotatedPointsX , [1, 0]); %posunutí hodnot v matici o řádek
                rotatedPointsY  = circshift(rotatedPointsY , [1, 0]); %posunutí hodnot v matici o řádek
                

                rotatedPointsX (1,1)  = inMatPoss(1,1); %přiřazení nulové hodnoty X
                rotatedPointsY (1,1)  = inMatPoss(1,2);   %přiřazení nulové hodnoty Y
                
                forRot = -(AngleDetDiff); % positivní směr je proti hodinám 
                
                for n = 1:size(rotatedPointsX,1) % rotate 
                    for i = (1+n):size(rotatedPointsX,1)
                        rotatedPointsX(i,1) = (rotatedPointsX(i,1) - rotatedPointsX(n,1))*cosd(forRot) - (rotatedPointsY(i,1)- rotatedPointsY(n,1))*sind(forRot) + rotatedPointsX(n,1); %rotace X bodů
                        rotatedPointsY(i,1) = (rotatedPointsX(i,1) - rotatedPointsX(n,1))*sind(forRot) + (rotatedPointsY(i,1)- rotatedPointsY(n,1))*cosd(forRot) + rotatedPointsY(n,1);  
                    end
                end

                [xCirc, yCirc] = carPrediction.CircleTrajectory(velo,accel,time);
                [x_pred, y_pred] = carPrediction.IntersectTwoPoly(x1,y1,xCirc, yCirc);               
            end
    end
end
