clear; clc; close all;

%vstup

% oblouk 1
polomer_1 = 2000;
stred_x1 = 0;
stred_y1 = 0;
pocatecni_uhel1 = pi / 2;
konecny_uhel1 = 1.571 - 0.386;

% oblouk 2
polomer_2 = 432.42;
stred_x2 = 277.1;
stred_y2 = 1367.7;
pocatecni_uhel2 = 2.266;
konecny_uhel2 = 27.49 * pi / 180;

% okno
delka_ab = 300;    
delka_bc = 800;    
puvodni_delka_cd = 160; 
orez_vzdalenost_cd = 34; 
orez_vzdalenost_da = 290; 



posun_hranice_doleva = 99;
doba_trvani = 5;
pocet_kroku = 100;
casovy_vektor = linspace(0, 1, pocet_kroku); 
tolerance = 1e-6; 
odsazeni_popisku = 30; 

figure('Name', 'Animace Kinematiky Okna', 'NumberTitle', 'off');
hold on;
axis equal;
grid on;
xlabel('Osa x [mm]');
ylabel('Osa y [mm]');
title('Animace Pohybu Okna');


uhel1_pro_kresleni = linspace(konecny_uhel1, pocatecni_uhel1, 100);
x_draha1 = stred_x1 + polomer_1 * cos(uhel1_pro_kresleni);
y_draha1 = stred_y1 + polomer_1 * sin(uhel1_pro_kresleni);
plot(x_draha1, y_draha1, 'g-', 'LineWidth', 1.5, 'DisplayName', 'Vodicí dráha A');


uhel2_pro_kresleni = linspace(konecny_uhel2, pocatecni_uhel2, 100);
x_draha2 = stred_x2 + polomer_2 * cos(uhel2_pro_kresleni);
y_draha2 = stred_y2 + polomer_2 * sin(uhel2_pro_kresleni);
plot(x_draha2, y_draha2, 'b-', 'LineWidth', 1.5, 'DisplayName', 'Vodicí dráha B');


x_a_init = stred_x1 + polomer_1 * cos(pocatecni_uhel1);
y_a_init = stred_y1 + polomer_1 * sin(pocatecni_uhel1);
x_b_init_pomocny = stred_x2 + polomer_2 * cos(pocatecni_uhel2);
y_b_init_pomocny = stred_y2 + polomer_2 * sin(pocatecni_uhel2);

% Směrový vektor pro orientaci okna
dx_orient_init = x_b_init_pomocny - x_a_init;
dy_orient_init = y_b_init_pomocny - y_a_init;
delka_orient_init = hypot(dx_orient_init, dy_orient_init);

% Normalizované vektory
u_init = [dx_orient_init, dy_orient_init] / delka_orient_init; % Vektor ve směru AB
p_init = [u_init(2), -u_init(1)]; % Vektor kolmý na AB 

% Výpočet vrcholů původního lichoběžníku
A_init = [x_a_init, y_a_init];
B_init = A_init + u_init * delka_ab;
C_init = B_init + p_init * delka_bc;
D_orig_init = C_init - u_init * puvodni_delka_cd;

% Výpočet rohů D a E
vektor_dc_init = C_init - D_orig_init;
D_init = D_orig_init + (vektor_dc_init / norm(vektor_dc_init)) * orez_vzdalenost_cd;
vektor_da_init = A_init - D_orig_init;
E_init = D_orig_init + (vektor_da_init / norm(vektor_da_init)) * orez_vzdalenost_da;

% Posunutí vrcholů pro vytvoření kolizní hranice
posun_vektor = [-posun_hranice_doleva, 0];
A_hr = A_init + posun_vektor;
B_hr = B_init + posun_vektor;
C_hr = C_init + posun_vektor;
D_hr = D_init + posun_vektor;
E_hr = E_init + posun_vektor;

bod_spojeni_oblouku = [0, polomer_1];
plot([A_hr(1), bod_spojeni_oblouku(1)], [A_hr(2), bod_spojeni_oblouku(2)], 'g-', 'LineWidth', 1.2, 'HandleVisibility', 'off');
hranice_x = [A_hr(1), B_hr(1), C_hr(1), D_hr(1), E_hr(1), A_hr(1)];
hranice_y = [A_hr(2), B_hr(2), C_hr(2), D_hr(2), E_hr(2), A_hr(2)];
plot(hranice_x, hranice_y, 'g-', 'LineWidth', 1.2, 'DisplayName', 'Statická hranice (kolizní)');
segmenty_hranice = { [C_hr; D_hr], [D_hr; E_hr], [E_hr; A_hr] };


rolna_A_plot = plot(NaN, NaN, 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r', 'DisplayName', 'Rolna A');
rolna_B_plot = plot(NaN, NaN, 'mo', 'MarkerSize', 8, 'MarkerFaceColor', 'm', 'DisplayName', 'Rolna B');
okno_plot = plot(NaN, NaN, 'k-', 'LineWidth', 1.5, 'DisplayName', 'Sklo okna');
pruseciky_plot = plot(NaN, NaN, 'o', 'MarkerSize', 8, 'LineWidth', 1.5, 'Color', [1 0.5 0], 'MarkerFaceColor',[1 0.5 0], 'DisplayName', 'Průsečíky');

% body ABCDE
popisek_A = text(NaN, NaN, 'A', 'FontWeight', 'bold', 'HorizontalAlignment', 'center');
popisek_B = text(NaN, NaN, 'B', 'FontWeight', 'bold', 'HorizontalAlignment', 'center');
popisek_C = text(NaN, NaN, 'C', 'FontWeight', 'bold', 'HorizontalAlignment', 'center');
popisek_D = text(NaN, NaN, 'D', 'FontWeight', 'bold', 'HorizontalAlignment', 'center');
popisek_E = text(NaN, NaN, 'E', 'FontWeight', 'bold', 'HorizontalAlignment', 'center');

% Nastavení rozsahu os a legendy
xlim([-1500, 2500]);
ylim([-500, 3000]);
legend('Location', 'northeastoutside');

fprintf('Start animace...\n');
byla_nalezena_kolize = false;

for i = 1:pocet_kroku
    
    aktualni_uhel1 = pocatecni_uhel1 + (konecny_uhel1 - pocatecni_uhel1) * casovy_vektor(i);
    aktualni_uhel2 = pocatecni_uhel2 + (konecny_uhel2 - pocatecni_uhel2) * casovy_vektor(i);
    x_a = stred_x1 + polomer_1 * cos(aktualni_uhel1);
    y_a = stred_y1 + polomer_1 * sin(aktualni_uhel1);
    x_b = stred_x2 + polomer_2 * cos(aktualni_uhel2);
    y_b = stred_y2 + polomer_2 * sin(aktualni_uhel2);
    
    set(rolna_A_plot, 'XData', x_a, 'YData', y_a);
    set(rolna_B_plot, 'XData', x_b, 'YData', y_b);
    
    
    dx_orient = x_b - x_a;
    dy_orient = y_b - y_a;
    delka_orient = hypot(dx_orient, dy_orient);
    
    vsechny_pruseciky_kroku = [];
    
    if delka_orient > tolerance
        u = [dx_orient, dy_orient] / delka_orient;
        p = [u(2), -u(1)];
        
        A = [x_a, y_a];
        B = A + u * delka_ab;
        C = B + p * delka_bc;
        D_orig = C - u * puvodni_delka_cd;
        
        vektor_dc = C - D_orig;
        D = D_orig + (vektor_dc / norm(vektor_dc)) * orez_vzdalenost_cd;
        vektor_da = A - D_orig;
        E = D_orig + (vektor_da / norm(vektor_da)) * orez_vzdalenost_da;
        
     
        okno_x = [A(1), B(1), C(1), D(1), E(1), A(1)];
        okno_y = [A(2), B(2), C(2), D(2), E(2), A(2)];
        set(okno_plot, 'XData', okno_x, 'YData', okno_y);
        
        
        if mod(i, 10) == 0 
            plot(okno_x, okno_y, '-', 'Color', [0.8 0.8 0.8], 'LineWidth', 1, 'HandleVisibility', 'off');
        end
        
       
        set(popisek_A, 'Position', A + [0, odsazeni_popisku]);
        set(popisek_B, 'Position', B + [odsazeni_popisku, 0]);
        set(popisek_C, 'Position', C + [0, -odsazeni_popisku]);
        set(popisek_D, 'Position', D + [-odsazeni_popisku, 0]);
        set(popisek_E, 'Position', E + [-odsazeni_popisku, 0]);
        
        
        pohybujici_se_segmenty = { [A; B], [B; C], [C; D], [D; E], [E; A] };
        
        % Kontrola kolize s vodicí lištou 1 (oblouk)
        for j = 1:length(pohybujici_se_segmenty)
            seg = pohybujici_se_segmenty{j};
            pruseciky = findPrusecik(seg(1,:), seg(2,:), [stred_x1, stred_y1], polomer_1, konecny_uhel1, pocatecni_uhel1, tolerance);
            if ~isempty(pruseciky)
                if hypot(pruseciky(1) - A(1), pruseciky(2) - A(2)) > tolerance
                    vsechny_pruseciky_kroku = [vsechny_pruseciky_kroku; pruseciky];
                end
            end
        end

        for j = 1:length(pohybujici_se_segmenty)
            seg_pohyb = pohybujici_se_segmenty{j};
            for k = 1:length(segmenty_hranice)
                seg_hranice = segmenty_hranice{k};
                prusecik = najdiPrusecikUseckaUsecka(seg_pohyb(1,:), seg_pohyb(2,:), seg_hranice(1,:), seg_hranice(2,:), tolerance);
                if ~isempty(prusecik)
                    vsechny_pruseciky_kroku = [vsechny_pruseciky_kroku; prusecik];
                end
            end
        end
        
    end
    
    % změna barvy okna a vykreslení průsečíků 
    if ~isempty(vsechny_pruseciky_kroku)
        set(okno_plot, 'Color', 'r');
        set(pruseciky_plot, 'XData', vsechny_pruseciky_kroku(:,1), 'YData', vsechny_pruseciky_kroku(:,2));
        byla_nalezena_kolize = true;
      
        if size(vsechny_pruseciky_kroku, 1) > 0 && (i == 1 || isempty(findobj(pruseciky_plot, 'XData', NaN)))
             fprintf('Krok %d: Detekována kolize v bodě (%.2f, %.2f)\n', i, vsechny_pruseciky_kroku(1,1), vsechny_pruseciky_kroku(1,2));
        end
    else
        set(okno_plot, 'Color', 'k');
        set(pruseciky_plot, 'XData', NaN, 'YData', NaN);
    end
    
    drawnow;
    pause(0.01);
end

hold off;
fprintf('\nAnimace dokončena.\n');

if byla_nalezena_kolize
    fprintf('Během animace byly detekovány kolize (oranžově).\n');
else
    fprintf('Během animace nebyly detekovány žádné kolize.\n');
end

function body = findPrusecik(p1, p2, stred, polomer, uhel_start, uhel_end, tol)
    body = [];
    x1 = p1(1); y1 = p1(2);
    x2 = p2(1); y2 = p2(2);
    cx = stred(1); cy = stred(2);
    
    dx = x2 - x1;
    dy = y2 - y1;
    A = dx^2 + dy^2;
    B = 2 * (dx * (x1 - cx) + dy * (y1 - cy));
    C = (x1 - cx)^2 + (y1 - cy)^2 - polomer^2;
    
    diskriminant = B^2 - 4*A*C;
    if diskriminant < -tol
        return; 
    end
    
    t_hodnoty = [];
    if abs(diskriminant) < tol
        t_hodnoty = -B / (2*A);
    else
        sqrt_d = sqrt(diskriminant);
        t_hodnoty = [(-B + sqrt_d) / (2*A); (-B - sqrt_d) / (2*A)];
    end
    
    for i = 1:length(t_hodnoty)
        t = t_hodnoty(i);
        if t >= -tol && t <= 1 + tol
          
            ix = x1 + t * dx;
            iy = y1 + t * dy;

            uhel = atan2(iy - cy, ix - cx);

            uhel_start_norm = mod(uhel_start, 2*pi);
            uhel_end_norm = mod(uhel_end, 2*pi);
            uhel_norm = mod(uhel, 2*pi);
            
            na_oblouku = false;
            if uhel_start_norm <= uhel_end_norm
                if uhel_norm >= uhel_start_norm - tol && uhel_norm <= uhel_end_norm + tol
                    na_oblouku = true;
                end
            else 
                if uhel_norm >= uhel_start_norm - tol || uhel_norm <= uhel_end_norm + tol
                    na_oblouku = true;
                end
            end
            
            if na_oblouku
                body = [body; ix, iy];
            end
        end
    end
end

function bod = najdiPrusecikUseckaUsecka(p1, p2, p3, p4, tol)
    bod = [];
    x1 = p1(1); y1 = p1(2); x2 = p2(1); y2 = p2(2);
    x3 = p3(1); y3 = p3(2); x4 = p4(1); y4 = p4(2);
    
    jmenovatel = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
    if abs(jmenovatel) < tol
        return; 
    end
    
    t_citatel = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
    u_citatel = -((x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3));
    
    t = t_citatel / jmenovatel;
    u = u_citatel / jmenovatel;
    
    if t >= -tol && t <= 1 + tol && u >= -tol && u <= 1 + tol
        
        bod = [x1 + t * (x2 - x1), y1 + t * (y2 - y1)];
    end
end