%% ============================================================
%  Post-processing Simulink – versione per struttura nota
% ============================================================
R_CS=24.75;
out.R_CS = R_CS; 
clearvars -except out
R_CS = out.R_CS;

clc; close all;
format shortEng
try
    time = squeeze(out.tout(:));  % [nStepsx1]
catch
    time = squeeze(out.time(:)); % Fallback se 'tout' non esiste
end

%% --- 1. Rimodellamento diretto dei campi principali ---
nSteps = numel(time);
F_b   = squeeze(permute(out.F_b,   [3,2,1]));   % [nSteps x 9]
F_TS  = squeeze(permute(out.F_TS,  [3,2,1]));   % [nSteps x 9]
x_FS  = squeeze(permute(out.x_FS,  [3,1,2]));   % [nSteps x 200]
y_FS  = squeeze(permute(out.y_FS,  [3,1,2]));   % [nSteps x 200]
x_e   = squeeze(permute(out.x_e,   [3,2,1]));   % [nSteps x 9]
y_e   = squeeze(permute(out.y_e,   [3,2,1]));   % [nSteps x 9]

theta_CS = squeeze(out.theta_CS);               % [nSteps x 1]
C_sx  = squeeze(permute(out.C_sx,  [3,1,2]));   % [nSteps x 202]
C_dx  = squeeze(permute(out.C_dx,  [3,1,2]));   % [nSteps x 202]
F_dx  = squeeze(permute(out.F_dx,  [3,1,2]));   % [nSteps x 200]
F_sx  = squeeze(permute(out.F_sx,  [3,1,2]));   % [nSteps x 200]
Q     = squeeze(permute(out.Q,     [3,2,1]));   % [nSteps x 9 x 200] o [nSteps x 200 x 9]
x_i      = squeeze(permute(out.x_i, [3,2,1])); % [nSteps x 9]
y_i      = squeeze(permute(out.y_i, [3,2,1])); % [nSteps x 9]

theta_WG = squeeze(out.theta_WG);
omega_WG = squeeze(out.omega_WG.Data);
omega_CS= squeeze(out.thetad_CS);
% ---------------

% Se Q ha più di 2 dimensioni, lascia così (usato per mappe e forze distribuite)
szQ = size(Q);
fprintf('Q dimensioni: [%s]\n', num2str(szQ));
% Controllo dimensioni variabili principali
fprintf('C_dx: %d colonne, C_sx: %d colonne\n', size(C_dx,2), size(C_sx,2));

if length(time) < length(theta_CS)
    fprintf('ATTENZIONE: length(time) (%d) < length(theta_CS) (%d).\n', length(time), length(theta_CS));
    fprintf('Estrapolo un campione di tempo.\n');
    
    if length(time) > 1
        
        dt = time(2) - time(1); 
    else
        dt = 1e-7; % Valore di fallback se il tempo ha 0 o 1 elementi

    
    time(end+1) = time(end) + dt;
    nSteps = nSteps + 1; % Aggiorno nSteps per coerenza
    fprintf('Nuova lunghezza time: %d\n', nSteps);
    end
end

%% --- 3. Crea struttura ridotta (stesso nome: "out") ---
out = struct( ...
    'time',       time, ...
    'x_FS',       x_FS, ...
    'y_FS',       y_FS, ...
    'x_e',        x_e, ...
    'y_e',        y_e, ...
    'F_b',        F_b, ...
    'F_TS',       F_TS, ...
    'theta_CS',   theta_CS, ...
    'R_CS',       R_CS, ...
    'C_sx',       C_sx, ...
    'C_dx',       C_dx, ...
    'F_dx',       F_dx, ...
    'F_sx',       F_sx, ...
    'Q',          Q, ...
    'x_i',        x_i, ...
    'y_i',        y_i, ...
    'theta_WG',   theta_WG, ...
    'omega_WG',   omega_WG, ...
    'omega_CS', omega_CS );

%% --- 4. Salva su file .mat (versione 7.3 per grandi dimensioni) ---
% Ottieni il timestamp corrente
timestamp = datestr(now, 'yyyymmdd_HHMMSS');
filename = ['output_' timestamp '.mat'];  % nome file di output
save(filename, 'out', '-v7.3');
fprintf('\n Dati salvati correttamente in "%s".\n', filename);
fprintf('Variabili salvate in out:\n');
disp(fieldnames(out));

%% ============================================================
% --- 2. Plot delle forze F_b ---
figure('Name', 'Forze Sfere F_b');
plot(time, F_b, '.-','LineWidth',1.2);
xlabel('Tempo [s]'); ylabel('F_b [N]');
title('Evoluzione F_b (9 sfere)');
grid on;
legend(arrayfun(@(i) sprintf('Sfera %d',i), 1:9, 'UniformOutput', false));

%% ============================================================
% --- 3. Plot delle forze F_TS ---
figure('Name', 'Forze Sensori F_TS');
plot(time, F_TS, '.-','LineWidth',1.2);
xlabel('Tempo [s]'); ylabel('F_{TS} [N]');
title('Evoluzione F_{TS}');
grid on;
legend(arrayfun(@(i) sprintf('Settore %d',i), 1:9, 'UniformOutput', false));

%% ============================================================
% --- 4. Traiettorie settori (x_e, y_e) e Punti WG (x_i, y_i) ---
figure('Name', 'Traiettorie Settori e Punti WG');
hold on; grid on; axis equal;
colors = lines(9);
h_plots = []; % Handle per la legenda
for i = 1:9
    % Traiettorie settori (x_e, y_e)
    h1 = plot(x_e(:,i), y_e(:,i), '-o', 'Color', colors(i,:), 'MarkerSize', 6);
    % Traiettorie punti WG (x_i, y_i)
    h2 = plot(x_i(:,i), y_i(:,i), ':x', 'Color', colors(i,:), 'MarkerSize', 8);
    
    % Salva solo il primo handle per la legenda
    if i == 1
        h_plots(1) = h1;
        h_plots(2) = h2;
    end
end
xlabel('x [mm]'); ylabel('y [mm]');
title('Traiettorie Settori (o) e Punti WG (x)');
legend(h_plots, {'Settori (x_e, y_e)', 'Punti WG (x_i, y_i)'}, 'Location', 'best');
hold off;

%% ============================================================
% --- 5. Traiettorie denti (x_FS, y_FS) ---
figure('Name', 'Traiettorie Denti');
hold on; grid on; axis equal;
idx_teeth = 1:4:200;
colors = lines(length(idx_teeth));
for k = 1:length(idx_teeth)
    plot(x_FS(1:1000,idx_teeth(k)), y_FS(1:1000,idx_teeth(k)), '.-', 'Color', colors(k,:), 'DisplayName', sprintf('Dente %d', idx_teeth(k)));
end
xlabel('x_{FS} [mm]'); ylabel('y_{FS} [mm]');
title('Traiettorie denti');
legend show; hold off;

figure('Name', 'Traiettorie Denti Selezionati');

% --- Dati ---
% Denti specificati dall'utente
teeth_to_plot = [25, 75, 125, 180];


     num_samples = 1500; 

% --- Creazione Subplot ---
for i = 1:length(teeth_to_plot)
    current_tooth_idx = teeth_to_plot(i);
    
    % Attiva la posizione corretta del subplot (2x2)
    subplot(2, 2, i);
    
    % Disegna il grafico per quel singolo dente
    plot(x_FS(1:num_samples, current_tooth_idx), ...
         y_FS(1:num_samples, current_tooth_idx), ...
         '.-', 'MarkerSize', 4); % Aggiunto MarkerSize per visibilità
         
    % Impostazioni Assi
    hold on; grid on; axis equal;
    
    % Aggiungi etichette e titolo
    xlabel('x_{FS} [mm]'); 
    ylabel('y_{FS} [mm]');
    title(sprintf('Dente %d', current_tooth_idx));
    hold off;
end

% --- Titolo Globale ---
% Aggiunge un titolo principale sopra tutti i subplot
sgtitle('Traiettorie Dettagliate per Denti Selezionati');

%% ============================================================
% --- 6. Plot Cinematica Input/Output e Rapporto --- 
figure('Name', 'Analisi Cinematica Input-Output'); % Nome figura aggiornato

% --- 1. Angolo Input (WG) ---
subplot(2,2,1); % Modificato in layout 2x2
hold on; grid on;
plot(time, theta_WG, 'b.-', 'DisplayName', '\theta_{WG} (Input)');
xlabel('Tempo [s]'); ylabel('Angolo [rad]');
title('Angolo Input (WG)');
legend show;
hold off;

% --- 2. Velocità Input (WG) ---
subplot(2,2,2); % Modificato in layout 2x2
hold on; grid on;
plot(time, omega_WG, 'r.-', 'DisplayName', '\omega_{WG} (Input)');
xlabel('Tempo [s]'); ylabel('Velocità [rad/s]');
title('Velocità Input (WG)');
legend show;
hold off;

% --- 3. Angolo Output (CS) ---
subplot(2,2,3); % Modificato in layout 2x2
hold on; grid on;
plot(time, theta_CS, 'g.-', 'DisplayName', '\theta_{CS} (Output)');
xlabel('Tempo [s]'); ylabel('Angolo [rad]');
title('Angolo Output (CS)');
legend show;
hold off;

% --- 4. Velocità Output (CS) ---
subplot(2,2,4); % Modificato in layout 2x2
hold on; grid on;
plot(time, omega_CS, 'm.-', 'DisplayName', '\omega_{CS} (Output)');
xlabel('Tempo [s]'); ylabel('Velocità [rad/s]');
title('Velocità Output (CS)');
legend show;
hold off;

% --- Collega gli assi X di tutti i subplot della *prima* figura ---
all_axes_fig1 = findobj(gcf, 'Type', 'axes');
linkaxes(all_axes_fig1, 'x');


%% --- FIGURA 2: Rapporto di Riduzione (Separata) --- 
figure('Name', 'Rapporto di Riduzione');
hold on; grid on;
% Calcolo rapporto angoli, escludendo il primo punto per evitare 0/0 (NaN)
% (nSteps è definito nella Sezione 1 dello script)
if nSteps > 1
    idx_valid_theta = 2:nSteps; 
    ratio_theta = theta_WG(idx_valid_theta) ./theta_CS(idx_valid_theta) ;
    plot(time(idx_valid_theta), ratio_theta, 'k.-');
else
    text(0.5, 0.5, 'Dati insuff. per calcolo rapporto', 'HorizontalAlignment', 'center');
end

ylim([60, 130]);

title('Rapporto Angoli (\theta_{WG} /\theta_{CS} )');
xlabel('Tempo [s]');
ylabel('Rapporto');
hold off;


%% ============================================================
% --- 7. Plot Forze F_sx (campione) ---
figure('Name', 'Forze F_sx e F_dx');
hold on; grid on;
idx_200 = 1:20:200;
colors = lines(length(idx_200));
for k = 1:length(idx_200)
    plot(time, F_sx(:, idx_200(k)), '.-', 'Color', colors(k,:), 'DisplayName', sprintf('Dente %d', idx_200(k)));
end
xlabel('Tempo [s]'); ylabel('F_{m} [N]');
title('Evoluzione F_{sx} e F_{dx} ');
legend show; 

% ============================================================
% --- 8. Plot Forze F_dx (campione) ---

hold on; grid on;
idx_200 = 1:20:200;
colors = lines(length(idx_200));
for k = 1:length(idx_200)
    plot(time, F_dx(:, idx_200(k)), '.-', 'Color', colors(k,:), 'DisplayName', sprintf('Dente %d', idx_200(k)));
end

legend show; hold off;

%% ============================================================
% --- 9 & 10. Plot Contatto C_sx e C_dx  ---
figure('Name', 'Contatto C_sx e C_dx (Totale)');
hold on; % Attiva la sovrapposizione
grid on;

% Calcola e plotta C_sx
C_sx_total = sum(C_sx, 2);
plot(time, C_sx_total, 'b.-', 'LineWidth', 1.5, 'DisplayName', 'C_{sx} Totale');

% Calcola e plotta C_dx
C_dx_total = sum(C_dx, 2);
plot(time, C_dx_total, 'r.-', 'LineWidth', 1.5, 'DisplayName', 'C_{dx} Totale');

% Etichette e Titolo
xlabel('Tempo [s]'); 
ylabel('Somma [Nm]'); % Etichetta Y generica
title('Evoluzione C_{sx} e C_{dx} TOTALE'); 

% Aggiungi legenda
legend show;

hold off; % Rilascia la sovrapposizione
%% ============================================================
% --- 11. Animazione del sistema --- 
figure('Name','Animazione Riduttore Armonico','NumberTitle','off','Color','w');
hold on; grid on; axis equal;
xlabel('x [mm]'); ylabel('y [mm]');
title('Animazione:denti FS (blu), Settori (giallo), CS (rosso)');

% setup grafico
h_teeth = scatter(nan, nan, 15, 'filled', 'MarkerFaceColor', [0.2 0.2 0.8]); % Denti FS
h_sectors = plot(nan, nan, 'sy', 'MarkerSize',8, 'MarkerFaceColor','y'); % Settori
h_CS = plot(nan, nan, 'k-', 'LineWidth',1.3); % Linea CS
h_CS_start = plot(nan, nan, 'ro', 'MarkerFaceColor', 'r', 'MarkerSize', 6); % Punto per theta_CS (Rosso)

% --- AGGIUNTE (basate sulla nuova interpretazione) ---
% Handle per i 9 punti del WG (x_i, y_i)
h_xi_pts = plot(nan, nan, 'bo', 'MarkerSize', 6, 'MarkerFaceColor', 'b'); 
% Handle per il PRIMO punto del WG (magenta, per theta_WG)
h_xi_start = plot(nan, nan, 'mo', 'MarkerFaceColor', 'm', 'MarkerSize', 6); 
% ---------------

% limiti asse
R_FS = max([abs(x_FS(:)); abs(y_FS(:))]);
% R_WG = max([abs(x_i(:)); abs(y_i(:))]);
R = R_CS*1.3;
if isempty(R) || R==0 || isnan(R), R = 1; end % Evita limiti [0 0] se i dati sono vuoti
xlim([-R R]); ylim([-R R]);
t_txt = text(0, -1.15*R, '', 'HorizontalAlignment','center','FontSize',12);

for k = 1:2000
    % aggiorna denti e settori
    set(h_teeth, 'XData', x_FS(k,:), 'YData', y_FS(k,:));
    set(h_sectors, 'XData', x_e(k,:), 'YData', y_e(k,:)); 
    
    % --- AGGIUNTE ---
    % Aggiorna i 9 punti del WG (blu)
  %  set(h_xi_pts, 'XData', x_i(k,:), 'YData', y_i(k,:));
    % Aggiorna il primo punto del WG (magenta)
  %  set(h_xi_start, 'XData', x_i(k,1), 'YData', y_i(k,1));
    % ---------------

    % disegna circular spline ruotata di theta_CS
    r = out.R_CS;
    th = linspace(0, 2*pi, 200);
    xc = r*cos(th);
    yc = r*sin(th);
    c = cos(theta_CS(k)); s = sin(theta_CS(k));
    xrot = c*xc - s*yc;
    yrot = s*xc + c*yc;
    
    % Aggiorna la linea della circular spline
    set(h_CS, 'XData', xrot, 'YData', yrot);
    % Aggiorna la posizione del punto rosso (theta_CS)
    set(h_CS_start, 'XData', xrot(1), 'YData', yrot(1)); 
   
    % aggiorna tempo
    set(t_txt, 'String', sprintf('t = %.2e s', time(k)));
    
    drawnow;
    pause(0.0001);
end

% ============================================================

disp(' Elaborazione completata con successo.');