classdef carCalcFunction < matlab.System
    % soubor funkcí pro vypočty predikce

    methods(Static)
        function velocity = CalcVelo(inMatPoss,clock)
            % rychlost
                lastNum = size(inMatPoss(any(inMatPoss(:,1:2),2),1:2),1);
                
                switch lastNum
                    case 1
                        velocity=0;
                    case 2
                        velocity = sqrt((inMatPoss(1,1)-inMatPoss(2,1)) ^ 2 + (inMatPoss(1,2) - inMatPoss(2,2)) ^ 2)/ (clock(1,1) - clock(2,1)); 
                    case 3
                        velocity = sqrt((inMatPoss(1,1)-inMatPoss(3,1)) ^ 2 + (inMatPoss(1,2) - inMatPoss(3,2)) ^ 2)/ (clock(1,1) - clock(3,1)); 
                    case 4
                        velocity = sqrt((inMatPoss(1,1)-inMatPoss(4,1)) ^ 2 + (inMatPoss(1,2) - inMatPoss(4,2)) ^ 2)/ (clock(1,1) - clock(4,1)); 
                    case 5
                        velocity = sqrt((inMatPoss(1,1)-inMatPoss(5,1)) ^ 2 + (inMatPoss(1,2) - inMatPoss(5,2)) ^ 2)/ (clock(1,1) - clock(5,1)); 
                    case 6
                        velocity = sqrt((inMatPoss(1,1)-inMatPoss(6,1)) ^ 2 + (inMatPoss(1,2) - inMatPoss(6,2)) ^ 2)/ (clock(1,1) - clock(6,1)); 
                    case 7
                        velocity = sqrt((inMatPoss(1,1)-inMatPoss(7,1)) ^ 2 + (inMatPoss(1,2) - inMatPoss(7,2)) ^ 2)/ (clock(1,1) - clock(7,1)); 
                    case 8
                        velocity = sqrt((inMatPoss(1,1)-inMatPoss(8,1)) ^ 2 + (inMatPoss(1,2) - inMatPoss(8,2)) ^ 2)/ (clock(1,1) - clock(8,1)); 
                    case 9
                        velocity = sqrt((inMatPoss(1,1)-inMatPoss(9,1)) ^ 2 + (inMatPoss(1,2) - inMatPoss(9,2)) ^ 2)/ (clock(1,1) - clock(9,1)); 
                    otherwise
                        velocity = sqrt((inMatPoss(1,1)-inMatPoss(10,1)) ^ 2 + (inMatPoss(1,2) - inMatPoss(10,2)) ^ 2)/ (clock(1,1) - clock(10,1)); 
                end
        end


        function velocity = CalcVeloDet(inMatPoss,clock)
            % rychlost
                lastNum = size(inMatPoss(any(inMatPoss(:,1:2),2),1:2),1);
                if lastNum<=1
                    velocity=0;
                    return
                end
                inMatPoss=inMatPoss(any(inMatPoss(:,1:2),2),1:2);
                if lastNum>=1 &&  lastNum<20
                        velocity = sqrt((inMatPoss(1,1)-inMatPoss(lastNum,1)) ^ 2 + (inMatPoss(1,2) - inMatPoss(lastNum,2)) ^ 2)/ (clock(1,1) - clock(lastNum,1));
                else
                        velocity = sqrt((inMatPoss(1,1)-inMatPoss(20,1)) ^ 2 + (inMatPoss(1,2) - inMatPoss(20,2)) ^ 2)/ (clock(1,1) - clock(20,1)); 
                end
        end


        function [veloX,veloY] = CalcVeloXY(inMatPoss,clock)
            % rychlostvXY
                lastNum = size(inMatPoss(any(inMatPoss(:,1:2),2),1:2),1);
                switch lastNum
                    case 2
                        veloX = (inMatPoss(1,1) - inMatPoss(2,1)) / (clock(1,1) - clock(2,1)); 
                        veloY = (inMatPoss(1,2) - inMatPoss(2,2)) / (clock(1,1) - clock(2,1)); 
                    case 3
                        veloX = (inMatPoss(1,1) - inMatPoss(3,1)) / (clock(1,1) - clock(3,1)); 
                        veloY = (inMatPoss(1,2) - inMatPoss(3,2)) / (clock(1,1) - clock(3,1)); 
                    case 4
                        veloX = (inMatPoss(1,1) - inMatPoss(4,1)) / (clock(1,1) - clock(4,1)); 
                        veloY = (inMatPoss(1,2) - inMatPoss(4,2)) / (clock(1,1) - clock(4,1)); 
                    case 5
                        veloX = (inMatPoss(1,1) - inMatPoss(5,1)) / (clock(1,1) - clock(5,1)); 
                        veloY = (inMatPoss(1,2) - inMatPoss(5,2)) / (clock(1,1) - clock(5,1)); 
                    case 6
                        veloX = (inMatPoss(1,1) - inMatPoss(6,1)) / (clock(1,1) - clock(6,1)); 
                        veloY = (inMatPoss(1,2) - inMatPoss(6,2)) / (clock(1,1) - clock(6,1)); 
                    case 7
                        veloX = (inMatPoss(1,1) - inMatPoss(7,1)) / (clock(1,1) - clock(7,1)); 
                        veloY = (inMatPoss(1,2) - inMatPoss(7,2)) / (clock(1,1) - clock(7,1)); 
                    case 8
                        veloX = (inMatPoss(1,1) - inMatPoss(8,1)) / (clock(1,1) - clock(8,1)); 
                        veloY = (inMatPoss(1,2) - inMatPoss(8,2)) / (clock(1,1) - clock(8,1));  
                    case 9
                        veloX = (inMatPoss(1,1) - inMatPoss(9,1)) / (clock(1,1) - clock(9,1)); 
                        veloY = (inMatPoss(1,2) - inMatPoss(9,2)) / (clock(1,1) - clock(9,1)); 
                    otherwise
                        veloX = (inMatPoss(1,1) - inMatPoss(10,1)) / (clock(1,1) - clock(10,1)); 
                        veloY = (inMatPoss(1,2) - inMatPoss(10,2)) / (clock(1,1) - clock(10,1)); 
                end
        end
        
        function accel = CalcAcc(inMatVelo,prevClock)
             % zrychlení
                lastNumAcc = size(inMatVelo(any(inMatVelo(:,1),2),1),1);
                switch lastNumAcc
                    case 1
                        accel=0;%
                    case 2
                        accel = (inMatVelo(1,1)-inMatVelo(2,1)) / (prevClock(1,1) - prevClock(2,1));
                    case 3
                        accel = (inMatVelo(1,1)-inMatVelo(3,1)) / (prevClock(1,1) - prevClock(3,1));
                    case 4
                        accel = (inMatVelo(1,1)-inMatVelo(4,1)) / (prevClock(1,1) - prevClock(4,1));
                    case 5
                        accel = (inMatVelo(1,1)-inMatVelo(5,1)) / (prevClock(1,1) - prevClock(5,1));
                    case 6
                        accel = (inMatVelo(1,1)-inMatVelo(6,1)) / (prevClock(1,1) - prevClock(6,1));
                    case 7
                        accel = (inMatVelo(1,1)-inMatVelo(7,1)) / (prevClock(1,1) - prevClock(7,1));
                    case 8
                        accel = (inMatVelo(1,1)-inMatVelo(8,1)) / (prevClock(1,1) - prevClock(8,1));
                    case 9
                        accel = (inMatVelo(1,1)-inMatVelo(9,1)) / (prevClock(1,1) - prevClock(9,1));
                    otherwise
                        accel = (inMatVelo(1,1)-inMatVelo(10,1)) / (prevClock(1,1) - prevClock(10,1));
                end
        end
        
        function accel = CalcAccDet(inMatVelo,prevClock)
             % zrychlení 
                inMatVelo=inMatVelo(any(inMatVelo(:,1:2),2),1:2);
                lastNumAcc = size(inMatVelo(:,1));
                if lastNumAcc<=1
                    accel=0;
                    return
                end

                if lastNumAcc>=1 &&  lastNumAcc<10
                        accel = (inMatVelo(1,1)-inMatVelo(lastNumAcc,1)) / (prevClock(1,1) - prevClock(lastNumAcc,1)); 
                else
                        accel = (inMatVelo(1,1)-inMatVelo(10,1)) / (prevClock(1,1) - prevClock(10,1));
                end
        end

        function velocity = CalcVeloActual(inMatPoss,clock)
            % rychlost
            velocity = sqrt((inMatPoss(1,1)-inMatPoss(2,1)) ^ 2 + (inMatPoss(1,2) - inMatPoss(2,2)) ^ 2)/ (clock(1,1) - clock(2,1)); 
        end

        function accel = CalcAccActual(inMatVelo,prevClock)
            accel = (inMatVelo(1,1)-inMatVelo(2,1)) / (prevClock(1,1) - prevClock(2,1));
        end

        function [orientCar, deltaX , deltaY] = WorldRot(inMatPoss,step)

            deltaX = (inMatPoss(1,1) - inMatPoss(step,1));  
            deltaY = (inMatPoss(1,2) - inMatPoss(step,2));
        
            if deltaX >= 0 && sign(deltaY) == 1 && not(deltaY == 0) %0°-lim90°
                orientCar = atand(deltaX/deltaY);
            elseif deltaY == 0 && sign(deltaX) == 1  %90°
                orientCar = 90;
            elseif sign(deltaX) == 1 && deltaY <= 0 && not(deltaX == 0)
                orientCar = -atand(deltaY/deltaX) + 90;
            elseif deltaX == 0 && sign(deltaY) == -1
                orientCar = 180;
            elseif deltaX <= 0 && sign(deltaY) == -1 && not(deltaY == 0)
                orientCar = atand(deltaX/deltaY) + 180;
            elseif deltaY == 0 && sign(deltaX) == -1
                orientCar = 270;
            else 
                orientCar = -atand(deltaY/deltaX) + 270;
            end
        end


        function [xCirc, yCirc] = CircleTrajectory(velocity,accel,time)
            if velocity > 1
                radius = velocity*(time) + (accel/2)*((time)^2); 
                %radius = max(radius);
            else
                radius=0;
            end
            th = 0:pi/50:2*pi;
            xCirc = radius * cos(th); 
            yCirc = radius * sin(th);
        end
        
        function [xCirc, yCirc] = Circle(radius,centerX,centerY)
            th = 0:pi/50:2*pi;
            xCirc = radius * cos(th) + centerX; %
            yCirc = radius * sin(th) + centerY; %
        end

        function [x_intersect, y_intersect] = IntersectTwoPoly(x1,y1,x2,y2)
            % Copyright 2020 The MathWorks, Inc.
            x_intersect = [];
            y_intersect = [];
            
            for i = 1:length(x1) - 1
                for j = 1:length(x2) - 1
                    x1_1 = x1(i);
                    y1_1 = y1(i);
                    x1_2 = x1(i + 1);
                    y1_2 = y1(i + 1);
                    
                    x2_1 = x2(j);
                    y2_1 = y2(j);
                    x2_2 = x2(j + 1);
                    y2_2 = y2(j + 1);
                    
                    denom = (x1_2 - x1_1) * (y2_2 - y2_1) - (x2_2 - x2_1) * (y1_2 - y1_1);
                    if denom == 0
                        continue;
                    end
                    
                    ua = ((x2_2 - x2_1) * (y1_1 - y2_1) - (y2_2 - y2_1) * (x1_1 - x2_1)) / denom;
                    ub = ((x1_2 - x1_1) * (y1_1 - y2_1) - (y1_2 - y1_1) * (x1_1 - x2_1)) / denom;
                    
                    if ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1
                        x_intersect = x1_1 + ua * (x1_2 - x1_1);
                        y_intersect = y1_1 + ua * (y1_2 - y1_1);
                    end
                end
            end
        end

        function [x_rotated, y_rotated] = RotPoints(x_points,y_points,angle)
            %otéčení bodů - kladný uhel otáčí protisměru hodin
            x_rotated = x_points*cosd(angle)-y_points*sind(angle);
            y_rotated = x_points*sind(angle)+y_points*cosd(angle);
        end

        function longitude = GeoLong (longitudeSet,x)
            % výpočet zeměpisné délky z kartézského
            earthRadius = 6378000;         %metres
           
            longitude = longitudeSet + atand(x/earthRadius);
        end
        function latitude = GeoLat (latitudeSet,y)
            % výpočet zeměpisné výšky z kartézského
            earthRadius = 6378000;         %metry
            latitude = latitudeSet + atand(y/earthRadius);
        end
    end
end
