%% =====================================================================
%  Mappa motore SPM per piccolo veicolo autonomo
%  -------------------------------------------------------------
%  Veicolo: carreggiata 1 m, passo 1 m, massa ~1300 kg, v_max 90 km/h
%  Motore : SPM x4 (uno per ruota). Per motore: P_peak 7.5 kW, P_nom 3.25 kW,
%           T_nom 7.5 Nm, T_peak 15 Nm  (= 1/4 del singolo motore equivalente)
%  Trasm. : motore->ruota 10:1 (omega_motore = 10 * omega_ruota)
%
%  Output : mappa T-omega vs apertura acceleratore alpha
%           mappa P-omega
%           mappa di rendimento eta(n,T)
% =====================================================================
%parametri veicolo da mettere in un altro file

%% ---------------- Parametri veicolo ---------------------------------
m_veh      = 1300;             % [kg]

v_max      = 90/3.6;           % [m/s] = 25
gear_ratio = 10;               % motore -> ruota (riduzione)

r_wheel = 0.2;                                   % [m] allineato a scenario.vehicle8.R0 (plant)
omega_w_max = v_max / r_wheel;                  % [rad/s] ruota
omega_m_max = gear_ratio * omega_w_max;         % [rad/s] motore
n_m_max     = omega_m_max*60/(2*pi);            % [RPM]

% fprintf('=== Cinematica ===\n');
% fprintf(' Raggio ruota          r = %.3f m\n', r_wheel);
% fprintf(' Omega ruota a v_max   = %.1f rad/s (%.0f RPM)\n', ...
%         omega_w_max, omega_w_max*60/(2*pi));
% fprintf(' Omega motore a v_max  = %.1f rad/s (%.0f RPM)\n\n', ...
%         omega_m_max, n_m_max);

%% ---------------- Parametri motore ---------------------------------
P_peak  = 7.5e3;                    % [W]  (1/4: motore per ruota)
P_nom   = 3.25e3;                   % [W]  (1/4)
T_nom   = 7.5;                      % [Nm]  (1/4)
T_peak  = 15;                       % [Nm]  (~2x T_nom). 15 Nm x gear 10 = 150 Nm/ruota -> rimuovere il gain /4 nell'ENGINE

omega_base_peak = P_peak / T_peak;          % corner picco
omega_base_nom  = P_nom  / T_nom;           % corner continuativo
n_base_peak     = omega_base_peak*60/(2*pi);
n_base_nom      = omega_base_nom *60/(2*pi);

% fprintf('=== Punti caratteristici motore ===\n');
% fprintf(' Corner picco : %.0f RPM (%.1f rad/s) @ %.0f Nm\n', ...
%         n_base_peak, omega_base_peak, T_peak);
% fprintf(' Corner nom   : %.0f RPM (%.1f rad/s) @ %.0f Nm\n', ...
%         n_base_nom,  omega_base_nom,  T_nom);
% fprintf(' Velocita max : %.0f RPM\n\n', n_m_max);

%% ---------------- Inviluppi T(omega) e P(omega) --------------------
n_vec     = linspace(0, n_m_max, 400);
omega_vec = n_vec*2*pi/60;

% Inviluppo PICCO: T = min(T_peak, P_peak/omega)
T_env_peak = min(T_peak, P_peak ./ max(omega_vec,eps));
T_env_peak(omega_vec==0) = T_peak;

% Inviluppo CONTINUO: T = min(T_nom, P_nom/omega)
T_env_nom  = min(T_nom, P_nom ./ max(omega_vec,eps));
T_env_nom(omega_vec==0) = T_nom;

P_env_peak = T_env_peak .* omega_vec;
P_env_nom  = T_env_nom  .* omega_vec;

%% ---------------- Mappa T-omega vs pedale acceleratore -------------
% Strategia: T_richiesta(alpha, omega) = alpha * T_env_peak(omega)
% (mappatura lineare; per veicoli reali si usa spesso una curva
% progressiva con dead-band e compensazione di velocita').
alpha_vec = 0:0.1:1;
T_map = (alpha_vec(:)) .* T_env_peak;       % righe: alpha, colonne: omega
P_map = T_map .* omega_vec;

%% ---------------- Mappa di rendimento eta(n,T) ---------------------
n_grid = linspace(50, n_m_max, 200);
T_grid = linspace(0.5, T_peak, 150);
[NN,TT] = meshgrid(n_grid, T_grid);
WW = NN*2*pi/60;            % rad/s
PP = TT.*WW;                % potenza meccanica [W]

% Modello perdite (sintetico, parametri tarati per eta_max ~93-94%)
%   - Joule (rame) ~ T^2
%   - Ferro        ~ omega^1.6
%   - Meccaniche   ~ omega
%   - Elettronica  ~ costante
P_loss_cu  = 0.9   * TT.^2;
P_loss_fe  = 0.013 * (WW.^1.6);
P_loss_mec = 0.22  * WW;
P_loss_pe  = 50;
P_loss     = P_loss_cu + P_loss_fe + P_loss_mec + P_loss_pe;

eta = PP ./ (PP + P_loss);
eta(PP <= 0) = NaN;

% Maschera punti fuori dall'inviluppo di picco
T_env_grid = min(T_peak, P_peak ./ max(WW,1e-3));
eta(TT > T_env_grid) = NaN;

%% ---------------- Motor-curve plots exported as PDF --------------------
% The four motor-characteristic plots are regenerated as HIDDEN figures
% ('Visible','off', so they do not pop up on screen) and exported as PDF
% in <project>\..\latex_tesi_claude\Images\motor_maps
% The figures are closed right after the export.
try
    motor_pdf_dir = fullfile(fileparts(which('motor_map')), '..', ...
        'latex_tesi_claude', 'Images', 'motor_maps');
    if ~exist(motor_pdf_dir, 'dir')
        mkdir(motor_pdf_dir);
    end

    cmap_mm    = turbo(numel(alpha_vec));
    eta_levels = [70 80 85 88 90 91 92 93 93.5 94];

    %% PLOT 1 : torque-speed vs accelerator opening
    f_mm1 = figure('Name','Motor T-n vs alpha','Color','w', ...
        'Visible','off','Position',[80 80 900 520]);
    hold on; grid on; box on;
    for i = 1:numel(alpha_vec)
        plot(n_vec, T_map(i,:), 'Color', cmap_mm(i,:), 'LineWidth', 1.6, ...
             'DisplayName', sprintf('\\xi = %3d%%', round(alpha_vec(i)*100)));
    end
    plot(n_vec, T_env_nom, 'k--', 'LineWidth', 2, 'DisplayName','T_{nom}');
    xlabel('Motor speed [RPM]'); ylabel('Torque [Nm]');
    title('SPM torque-speed map vs accelerator opening');
    legend('Location','eastoutside'); xlim([0 n_m_max]); ylim([0 T_peak*1.05]);
    exportgraphics(f_mm1, fullfile(motor_pdf_dir,'motor_torque_speed.pdf'));
    close(f_mm1);

    %% PLOT 2 : power-speed vs accelerator opening
    f_mm2 = figure('Name','Motor P-n vs alpha','Color','w', ...
        'Visible','off','Position',[120 120 900 520]);
    hold on; grid on; box on;
    for i = 1:numel(alpha_vec)
        plot(n_vec, P_map(i,:)/1000, 'Color', cmap_mm(i,:), 'LineWidth', 1.6, ...
             'DisplayName', sprintf('\\xi = %3d%%', round(alpha_vec(i)*100)));
    end
    plot(n_vec, P_env_nom/1000, 'k--', 'LineWidth', 2, 'DisplayName','P_{nom}');
    xlabel('Motor speed [RPM]'); ylabel('Power [kW]');
    title('SPM power-speed map vs accelerator opening');
    legend('Location','eastoutside'); xlim([0 n_m_max]); ylim([0 P_peak/1000*1.1]);
    exportgraphics(f_mm2, fullfile(motor_pdf_dir,'motor_power_speed.pdf'));
    close(f_mm2);

    %% PLOT 3 : efficiency map (T vs n)
    f_mm3 = figure('Name','Motor efficiency T-n','Color','w', ...
        'Visible','off','Position',[160 160 900 560]);
    contourf(NN, TT, eta*100, eta_levels, 'ShowText','on');
    colormap(parula); cb = colorbar; cb.Label.String = 'Efficiency [%]';
    clim([70 94]);
    hold on;
    plot(n_vec, T_env_peak, 'r-',  'LineWidth', 2.5);
    plot(n_vec, T_env_nom,  'k--', 'LineWidth', 2);
    text(n_base_peak*1.05, T_peak*0.95, 'peak envelope',       'Color','r');
    text(n_base_nom *1.05, T_nom *0.92, 'continuous envelope', 'Color','k');
    xlabel('Motor speed [RPM]'); ylabel('Torque [Nm]');
    title('SPM efficiency map \eta(n,T)');
    exportgraphics(f_mm3, fullfile(motor_pdf_dir,'motor_efficiency_Tn.pdf'));
    close(f_mm3);

    %% PLOT 4 : efficiency map (P vs n)
    f_mm4 = figure('Name','Motor efficiency P-n','Color','w', ...
        'Visible','off','Position',[200 200 900 560]);
    PP_kW = PP/1000;
    contourf(NN, PP_kW, eta*100, eta_levels, 'ShowText','on');
    colormap(parula); cb = colorbar; cb.Label.String = 'Efficiency [%]';
    clim([70 94]);
    hold on;
    plot(n_vec, P_env_peak/1000, 'r-',  'LineWidth', 2.5);
    plot(n_vec, P_env_nom /1000, 'k--', 'LineWidth', 2);
    xlabel('Motor speed [RPM]'); ylabel('Mechanical power [kW]');
    title('SPM power-speed map with efficiency islands');
    ylim([0 P_peak/1000*1.05]);
    exportgraphics(f_mm4, fullfile(motor_pdf_dir,'motor_efficiency_Pn.pdf'));
    close(f_mm4);

    fprintf('[motor_map] 4 motor-curve PDFs saved in:\n    %s\n', motor_pdf_dir);
catch ME_plot
    warning('motor_map:plotPdf', ...
        'Motor-curve PDFs not generated: %s', ME_plot.message);
end

%% ---------------- Salvataggio struct utile -------------------------
motor.r_wheel        = r_wheel;
motor.gear_ratio     = gear_ratio;
motor.T_peak         = T_peak;
motor.T_nom          = T_nom;
motor.P_peak         = P_peak;
motor.P_nom          = P_nom;
motor.omega_base_pk  = omega_base_peak;
motor.omega_max      = omega_m_max;
motor.n_vec          = n_vec;
motor.alpha_vec      = alpha_vec;
motor.T_pedal_map    = T_map;       % [length(alpha) x length(n)]
motor.P_pedal_map    = P_map;
motor.eff_grid_n     = n_grid;
motor.eff_grid_T     = T_grid;
motor.eff_map        = eta;
motor.T_env_peak     = T_env_peak;
motor.T_env_nom      = T_env_nom;

% Funzioni di lookup pronte all uso (interpolazione)
motor.f_T_pedal = @(alpha,n) alpha .* interp1(n_vec, T_env_peak, n, 'linear', 0);
motor.f_eta     = @(n,T)     interp2(n_grid, T_grid, eta, n, T, 'linear', NaN);
%% ---------------- Export per Simulink Lookup Table ------------------

% Breakpoints per Simulink
mot_n_bp = n_vec;                  % [rpm]
mot_alpha_bp = alpha_vec * 100;    % [%] da 0 a 100

% Tabelle per Simulink
% Convenzione scelta:
% righe    = rpm
% colonne = acceleratore %
mot_T_table = T_map.';             % [length(n_vec) x length(alpha_vec)] [Nm]
mot_P_table = P_map.';             % [length(n_vec) x length(alpha_vec)] [W]

% Variante potenza in kW
mot_P_table_kW = mot_P_table / 1000;

% Controllo dimensioni
% disp('=== Dimensioni tabelle per Simulink ===')
% fprintf('mot_n_bp       : %d punti\n', length(mot_n_bp));
% fprintf('mot_alpha_bp   : %d punti\n', length(mot_alpha_bp));
% fprintf('mot_T_table    : %d x %d\n', size(mot_T_table,1), size(mot_T_table,2));
% fprintf('mot_P_table    : %d x %d\n', size(mot_P_table,1), size(mot_P_table,2));

% Salvataggio opzionale
% save('motor_map_simulink.mat', ...
%      'mot_n_bp', ...
%      'mot_alpha_bp', ...
%      'mot_T_table', ...
%      'mot_P_table', ...
%      'mot_P_table_kW');
%%
% fprintf('Struct ''motor'' salvato nel workspace.\n');
% fprintf('Esempio: motor.f_T_pedal(0.5, 3000)  -> coppia richiesta a 50%% pedale, 3000 RPM\n');
% fprintf('         motor.f_eta(4000, 25)        -> rendimento a 4000 RPM, 25 Nm\n');