% Wheel-soil interaction model
% Integrated nonlinear stress formulation using a quadratic approximation
% of the stress distributions.
% Assumption: cylindrical flexible wheel.

clc; clear; close all

selected_config = 1;

%% PARAMETERS

% Environmental parameters
lunar_soil.g = 1.62;                   % lunar gravity [m/s^2]

% Soil parameters (soft lunar regolith)
lunar_soil.n = 1;                      % sinkage exponent [-]
lunar_soil.kc = 1400;                  % Bekker cohesive modulus [N/m^(n+1)]
lunar_soil.kphi = 820000;              % Bekker frictional modulus [N/m^(n+2)]
               
lunar_soil.c = 170;                    % cohesion [N/m^2]
lunar_soil.phi = 40*pi/180;            % internal friction angle [rad]
lunar_soil.gamma = 2470;               % soil unit weight [N/m^3]

lunar_soil.Nq = 32.23;                 % Bearing capacity friction factor
lunar_soil.Nc = 48.09;                 % Bearing capacity cohesion factor
lunar_soil.Ngamma = 33.27;             % Bearing capacity density factor

lunar_soil.Kc = 33.37;                 % Cohesive modulus of soil deformation
lunar_soil.Kgamma = 72.77;             % Density modulus of soil deformation 
lunar_soil.K = 0.0178;                 % shear deformation modulus [m]

lunar_soil.c1 = 0.285;
lunar_soil.c2 = 0.32;

lunar_soil.fr = 0.025;                 % rolling resistance coefficient [-]

% Configuration parameters
slope_angle_deg = 0;      % angle of slope [deg] (not considered)

% Best configuration
rover.D_vec = 1;          % wheel diameter [m]
rover.B_vec = 0.4;        % wheel width [m]
rover.slip_vec = 0.2;     % slip ratio [-]
rover.vel_vec = 3;        % forward velocity [m/s]
rover.mass_vec = 7000;    % vehicle mass [kg]
rover.Nw_vec = 8;         % number of wheels [-]

rover.W_total_vec = rover.mass_vec .* lunar_soil.g; % total vehicle weight [N]

%% OUTPUT MATRIX SETUP

n_rows = length(rover.D_vec) * length(rover.B_vec) * length(rover.Nw_vec) * ...
    length(rover.slip_vec) * length(rover.vel_vec) * length(rover.W_total_vec);
n_cols = 35;

MATRIX = zeros(n_rows, n_cols);

count=1;
tic

%% ITERATIVE PROCESS

for i = 1:length(rover.D_vec)
    for j = 1:length(rover.B_vec)
        for u = 1:length(rover.Nw_vec)
            for r = 1:length(rover.slip_vec)
                for iv = 1:length(rover.vel_vec)
                    for t = 1:length(rover.W_total_vec)

                        final_rover.rw = rover.D_vec(i) / 2;
                        final_rover.bw = rover.B_vec(j);
                        final_rover.Nw = rover.Nw_vec(u);
                        final_rover.slip = rover.slip_vec(r);
                        final_rover.vel_x = rover.vel_vec(iv);
                        final_rover.W_total = rover.W_total_vec(t);

                        % Motion
                        motion.omega = final_rover.vel_x / (final_rover.rw * (1 - final_rover.slip));
                        motion.theta = 0; %#ok<NASGU> % flat terrain slope angle [rad]

                        % Weight per wheel
                        final_rover.Ww = Ww_fun(final_rover.Nw, final_rover.W_total);

                        %% 1) CONTACT GEOMETRY

                        contact.Csig = lunar_soil.kc ./ final_rover.bw + lunar_soil.kphi;

                        Kt = 10^6;
                        f = final_rover.Ww / Kt;

                        R = final_rover.rw;
                        f = min(f, 0.12 * R);

                        errFun = @(h) (Fz_soil_subcircle(h, R, final_rover.bw, lunar_soil, contact.Csig, ...
                            lunar_soil.n, lunar_soil.c1, lunar_soil.c2, final_rover.slip, f)) ...
                            - (Kt * f);

                        h_min = 1e-6;
                        h_max = 0.5 * R;

                        if errFun(h_max) < 0
                            h_f = NaN;
                        else
                            h_f = fzero(errFun, [h_min, h_max]);
                        end

                        if isnan(h_f)
                            continue
                        end

                        [Fzs, Rstar] = Fz_soil_subcircle(h_f, R, final_rover.bw, lunar_soil, contact.Csig, ...
                            lunar_soil.n, lunar_soil.c1, lunar_soil.c2, final_rover.slip, f); %#ok<NASGU>

                        final_rover.rw_eff = Rstar;
                        final_rover.delta = f;
                        contact.h_eff = h_f;

                        contact.theta_f = acos(1 - contact.h_eff / final_rover.rw_eff);
                        contact.theta_r = -contact.theta_f;
                        contact.theta_m = (lunar_soil.c1 + lunar_soil.c2 .* final_rover.slip) .* contact.theta_f;
                        contact.h_r = final_rover.rw_eff * (1 - cos(contact.theta_r));

                        contact.theta_all = abs(contact.theta_f) + abs(contact.theta_r);

                        contact.lw = sqrt(contact.h_eff * (final_rover.rw_eff * 2 - contact.h_eff)) + ...
                            sqrt(contact.h_r * (2 * final_rover.rw_eff - contact.h_r));
                        contact.Aw = contact.lw * final_rover.bw;

                        p_avg = final_rover.Ww / contact.Aw;
                        p_crit = (lunar_soil.kc / (min(final_rover.bw, contact.lw)) + lunar_soil.kphi) ^ (1 / (2 * lunar_soil.n + 1)) * ...
                            (3 * final_rover.Ww / ((3 - lunar_soil.n) * min(final_rover.bw, contact.lw) * sqrt(final_rover.rw_eff * 2))) ^ ...
                            (2 * lunar_soil.n / (2 * lunar_soil.n + 1));
                        zw = (p_avg ./ contact.Csig) .^ (1 / lunar_soil.n); %#ok<NASGU>
                        zw_max = (p_crit ./ contact.Csig) .^ (1 / lunar_soil.n); %#ok<NASGU>

                        %% 2) QUADRATIC STRESS DISTRIBUTIONS

                        contact.theta_fm = (contact.theta_f + contact.theta_m) / 2;
                        contact.theta_rm = (contact.theta_r + contact.theta_m) / 2;

                        contact.h_m = final_rover.rw_eff * (1 - cos(contact.theta_m));
                        contact.h_fm = final_rover.rw_eff * (1 - cos(contact.theta_fm));
                        contact.h_rm = final_rover.rw_eff * (1 - cos(contact.theta_rm));

                        stress.sigma_m = contact.Csig * contact.h_m ^ lunar_soil.n;

                        stress.sigma_fm = stress.sigma_m * ...
                            (1 - ((contact.theta_fm - contact.theta_m) / ...
                            (contact.theta_f - contact.theta_m)) ^ 2);
                        stress.sigma_rm = stress.sigma_m * ...
                            (1 - ((contact.theta_rm - contact.theta_m) / ...
                            (contact.theta_r - contact.theta_m)) ^ 2);

                        stress.sigma_f = 0;
                        stress.sigma_r = 0;

                        [a_sigf, b_sigf, c_sigf, a_sigr, b_sigr, c_sigr] = coeff_quad_fun(...
                            contact.theta_f, contact.theta_r, contact.theta_m, ...
                            stress.sigma_m, stress.sigma_fm, stress.sigma_rm, ...
                            stress.sigma_f, stress.sigma_r);

                        % Longitudinal shear stress
                        shear_disp.j_mt = final_rover.rw_eff * ((contact.theta_f - contact.theta_m) - ...
                            (1 - final_rover.slip) * (sin(contact.theta_f) - sin(contact.theta_m)));
                        shear_disp.j_fmt = final_rover.rw_eff * ((contact.theta_f - contact.theta_fm) - ...
                            (1 - final_rover.slip) * (sin(contact.theta_f) - sin(contact.theta_fm)));
                        shear_disp.j_ft = final_rover.rw_eff * ((contact.theta_f - contact.theta_f) - ...
                            (1 - final_rover.slip) * (sin(contact.theta_f) - sin(contact.theta_f)));
                        shear_disp.j_rmt = final_rover.rw_eff * ((contact.theta_f - contact.theta_rm) - ...
                            (1 - final_rover.slip) * (sin(contact.theta_f) - sin(contact.theta_rm)));
                        shear_disp.j_rt = final_rover.rw_eff * ((contact.theta_f - contact.theta_r) - ...
                            (1 - final_rover.slip) * (sin(contact.theta_f) - sin(contact.theta_r)));

                        stress.taut_m = (lunar_soil.c + stress.sigma_m * tan(lunar_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_mt) / lunar_soil.K));
                        stress.taut_fm = (lunar_soil.c + stress.sigma_fm * tan(lunar_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_fmt) / lunar_soil.K));
                        stress.taut_f = 0;
                        stress.taut_rm = (lunar_soil.c + stress.sigma_rm * tan(lunar_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_rmt) / lunar_soil.K));
                        stress.taut_r = (lunar_soil.c + stress.sigma_r * tan(lunar_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_rt) / lunar_soil.K));

                        [a_tautf, b_tautf, c_tautf, a_tautr, b_tautr, c_tautr] = coeff_quad_fun(...
                            contact.theta_f, contact.theta_r, contact.theta_m, ...
                            stress.taut_m, stress.taut_fm, stress.taut_rm, ...
                            stress.taut_f, stress.taut_r);

                        % Lateral shear stress
                        beta = 5 * pi / 180;

                        shear_disp.j_ml = final_rover.rw_eff * (1 - final_rover.slip) * ...
                            (contact.theta_f - contact.theta_m) * tan(beta);
                        shear_disp.j_fml = final_rover.rw_eff * (1 - final_rover.slip) * ...
                            (contact.theta_f - contact.theta_fm) * tan(beta);
                        shear_disp.j_fl = final_rover.rw_eff * (1 - final_rover.slip) * ...
                            (contact.theta_f - contact.theta_f) * tan(beta);
                        shear_disp.j_rml = final_rover.rw_eff * (1 - final_rover.slip) * ...
                            (contact.theta_f - contact.theta_rm) * tan(beta);
                        shear_disp.j_rl = final_rover.rw_eff * (1 - final_rover.slip) * ...
                            (contact.theta_f - contact.theta_r) * tan(beta);

                        stress.taul_m = (lunar_soil.c + stress.sigma_m * tan(lunar_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_ml) / lunar_soil.K));
                        stress.taul_fm = (lunar_soil.c + stress.sigma_fm * tan(lunar_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_fml) / lunar_soil.K));
                        stress.taul_f = (lunar_soil.c + stress.sigma_f * tan(lunar_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_fl) / lunar_soil.K));
                        stress.taul_rm = (lunar_soil.c + stress.sigma_rm * tan(lunar_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_rml) / lunar_soil.K));
                        stress.taul_r = (lunar_soil.c + stress.sigma_r * tan(lunar_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_rl) / lunar_soil.K));

                        [a_taulf, b_taulf, c_taulf, a_taulr, b_taulr, c_taulr] = coeff_quad_fun(...
                            contact.theta_f, contact.theta_r, contact.theta_m, ...
                            stress.taul_m, stress.taul_fm, stress.taul_rm, ...
                            stress.taul_f, stress.taul_r);

                        % Piecewise coefficients: 1 = rear, 2 = front
                        a_sig_vec = [a_sigr, a_sigf];
                        b_sig_vec = [b_sigr, b_sigf];
                        c_sig_vec = [c_sigr, c_sigf];

                        a_tt_vec = [a_tautr, a_tautf];
                        b_tt_vec = [b_tautr, b_tautf];
                        c_tt_vec = [c_tautr, c_tautf];

                        a_tl_vec = [a_taulr, a_taulf];
                        b_tl_vec = [b_taulr, b_taulf];
                        c_tl_vec = [c_taulr, c_taulf];

                        contact.theta0 = [contact.theta_r, contact.theta_m];
                        contact.theta1 = [contact.theta_m, contact.theta_f];

                        %% PLOT normal pressure approximation vs normal pressure real
                        if count == selected_config
                            th = linspace(contact.theta_r, contact.theta_f, 400);

                            sigma_th = evalPiecewiseQuad(th, a_sig_vec, b_sig_vec, c_sig_vec, contact.theta_m);
                            taut_th = evalPiecewiseQuad(th, a_tt_vec, b_tt_vec, c_tt_vec, contact.theta_m);

                            pz_th = sigma_th .* cos(th) + taut_th .* sin(th);

                            pz_max = max(pz_th);
                            pz_eq = zeros(size(th));
                            left = th <= contact.theta_m;

                            pz_eq(left) = pz_max * (th(left) - contact.theta_r) / (contact.theta_m - contact.theta_r);
                            pz_eq(~left) = pz_max * (contact.theta_f - th(~left)) / (contact.theta_f - contact.theta_m);

                            figure; hold on; grid on; box on;
                            plot(th * 180 / pi, pz_th / 1000, 'k', 'LineWidth', 3);
                            plot(th * 180 / pi, pz_eq / 1000, '--r', 'LineWidth', 3);

                            xline(contact.theta_r * 180 / pi, ':k', 'LineWidth', 2);
                            xline(contact.theta_m * 180 / pi, ':k', 'LineWidth', 2);
                            xline(contact.theta_f * 180 / pi, ':k', 'LineWidth', 2);

                            xlabel('$\theta$ [deg]', 'FontSize', 30, 'Interpreter', 'latex');
                            ylabel('$p_{z}$ [kPa]', 'FontSize', 30, 'Interpreter', 'latex');
                            lgd = legend({'$p_z=\sigma\cos\theta+\tau_t\sin\theta$', '$p_{z,\mathrm{eq}}$ (linear approx.)'}, ...
                                'Interpreter', 'latex', 'Location', 'northwest');
                            lgd.FontSize = 30;

                            ax = gca;
                            ax.FontSize = 25;
                            ax.LineWidth = 1;
                            ax.TickLabelInterpreter = "latex";
                        end

                        %% FORCES AND MOMENTS
                        [out.Fx, out.Fy, out.Fz, out.Mx, out.My, out.Mz, out.sigma, out.tau_t, out.tau_l] = ...
                            Force_quad_fun(a_tt_vec, b_tt_vec, c_tt_vec, ...
                            a_tl_vec, b_tl_vec, c_tl_vec, ...
                            a_sig_vec, b_sig_vec, c_sig_vec, ...
                            contact.theta0, contact.theta1, final_rover.rw_eff, final_rover.bw);

                        %% RESISTANCES
                        [out.Rc, out.Rb, out.Rr, out.Rg, out.Rall] = Resistence_fun(...
                            a_sig_vec, b_sig_vec, c_sig_vec, final_rover.rw_eff, ...
                            final_rover, lunar_soil, contact, slope_angle_deg, contact.h_eff);

                        %% TRACTION, DRAWBAR PULL, TORQUE, POWER
                        out.Traction = out.Fx + out.Rc;
                        out.DP = out.Traction - out.Rall;

                        if out.DP < 0
                            continue
                        end

                        out.T_drw_ew = out.Rall .* (final_rover.rw - final_rover.delta);
                        out.P_drw = out.T_drw_ew .* (final_rover.vel_x ./ final_rover.rw);

                        %% THICKNESS AND MASS (Aluminium 7075)
                        E = 72400 * 10^6;
                        rho = 2700;

                        thick = nthroot((1.776 .* final_rover.Ww .* final_rover.rw .^ 3) ./ ...
                            (E .* final_rover.bw .* final_rover.delta), 3);
                        mass = rho .* pi .* (final_rover.rw ^ 2 - (final_rover.rw - thick) ^ 2) .* final_rover.bw;

                        %% PLOTS
                        if count == selected_config
                            th_plot = linspace(contact.theta_r, contact.theta_f, 400);

                            sigma_plot = zeros(size(th_plot));
                            tau_t_plot = zeros(size(th_plot));
                            tau_l_plot = zeros(size(th_plot));

                            for k = 1:numel(th_plot)
                                th = th_plot(k);
                                idx = (th > contact.theta_m) + 1;

                                sigma_plot(k) = a_sig_vec(idx) * th ^ 2 + b_sig_vec(idx) * th + c_sig_vec(idx);
                                tau_t_plot(k) = a_tt_vec(idx) * th ^ 2 + b_tt_vec(idx) * th + c_tt_vec(idx);
                                tau_l_plot(k) = a_tl_vec(idx) * th ^ 2 + b_tl_vec(idx) * th + c_tl_vec(idx);
                            end

                            figure; hold on; grid on; box on;
                            plot(th_plot * 180 / pi, sigma_plot / 1e3, 'Color', [0 0.45 0.74], 'LineWidth', 3);
                            plot(th_plot * 180 / pi, tau_t_plot / 1e3, '--', 'Color', [0.85 0.33 0.10], 'LineWidth', 3);
                            plot(th_plot * 180 / pi, tau_l_plot / 1e3, ':', 'Color', [0.47 0.67 0.19], 'LineWidth', 3);

                            xline(contact.theta_m * 180 / pi, '--', '$\theta_m$', 'Color', [0 0 0], ...
                                'LineWidth', 2, FontSize = 30, Interpreter = 'latex');

                            xlabel('$\theta$ [deg]', FontSize = 30, Interpreter = 'latex');
                            ylabel('Stress [kPa]', FontSize = 30, Interpreter = 'latex');
                            legend('$\sigma$ (normal)', '$\tau_t$ (longitudinal)', '$\tau_l$ (lateral)', ...
                                'Location', 'best', FontSize = 30, Interpreter = 'latex');
                            xlim([-40 40])
                            ylim([0 13])

                            ax = gca;
                            ax.FontSize = 25;
                            ax.LineWidth = 1;
                            ax.TickLabelInterpreter = "latex";
                        end

                        %%

                        if count == selected_config
                            rW = final_rover.rw_eff;
                            bW = final_rover.bw;

                            Ntheta = 14;
                            Ny = 3;

                            theta_plot = linspace(contact.theta_r, contact.theta_f, Ntheta);
                            y_plot = linspace(-bW / 2, bW / 2, Ny);

                            [TH, Y] = meshgrid(theta_plot, y_plot);

                            X = rW * sin(TH);
                            Z = -rW * cos(TH);

                            Nx = sin(TH);  Nz = -cos(TH);
                            Tx = cos(TH);  Tz = sin(TH);

                            stress_piece = @(th, a, b, c) ...
                                (th <= contact.theta_m) .* (a(1) .* th .^ 2 + b(1) .* th + c(1)) + ...
                                (th > contact.theta_m) .* (a(2) .* th .^ 2 + b(2) .* th + c(2));

                            sig_theta = stress_piece(theta_plot, a_sig_vec, b_sig_vec, c_sig_vec);
                            taux_theta = stress_piece(theta_plot, a_tt_vec, b_tt_vec, c_tt_vec);
                            tauy_theta = stress_piece(theta_plot, a_tl_vec, b_tl_vec, c_tl_vec);

                            SIG = repmat(sig_theta, Ny, 1);
                            TAUX = repmat(taux_theta, Ny, 1);
                            TAUY = repmat(tauy_theta, Ny, 1);

                            Lmax_sig = 0.55 * rW;
                            Lmax_taux = 0.45 * rW;
                            Lmax_tauy = 0.45 * (bW / 2);

                            Lsig = Lmax_sig * (SIG / max(SIG(:) + eps));
                            Ltaux = Lmax_taux * (TAUX / max(abs(TAUX(:)) + eps));
                            Ltauy = Lmax_tauy * (TAUY / max(abs(TAUY(:)) + eps));

                            th_cyl = linspace(contact.theta_r - 0.4, contact.theta_f + 0.4, 70);
                            y_cyl = linspace(-bW / 2, bW / 2, 20);
                            [THc, Yc] = meshgrid(th_cyl, y_cyl);
                            Xc = rW * sin(THc);
                            Zc = -rW * cos(THc);

                            figure('Color', 'w'); tiledlayout(1, 3, "TileSpacing", "compact", "Padding", "compact");

                            nexttile; hold on;
                            surf(Xc, Yc, Zc, 'FaceAlpha', 0.10, 'EdgeColor', 'none');
                            surf(X, Y, Z, 'FaceAlpha', 0.35, 'EdgeColor', 'none');
                            quiver3(X, Y, Z, Lsig .* Nx, 0 * Y, Lsig .* Nz, 0, 'LineWidth', 2, 'MaxHeadSize', 1);
                            title('$\sigma$ (normal)', FontSize = 20, Interpreter = 'latex');
                            setup_axes_3d_zoom(rW, X, Y, Z, bW);

                            ax = gca;
                            ax.FontSize = 16;
                            ax.LineWidth = 1;
                            ax.TickLabelInterpreter = "latex";

                            nexttile; hold on;
                            surf(Xc, Yc, Zc, 'FaceAlpha', 0.10, 'EdgeColor', 'none');
                            surf(X, Y, Z, 'FaceAlpha', 0.35, 'EdgeColor', 'none');
                            quiver3(X, Y, Z, Ltaux .* Tx, 0 * Y, Ltaux .* Tz, 0, 'LineWidth', 2, 'MaxHeadSize', 1);
                            title('$\tau_x$ (longitudinal)', FontSize = 20, Interpreter = 'latex');
                            setup_axes_3d_zoom(rW, X, Y, Z, bW);

                            ax = gca;
                            ax.FontSize = 16;
                            ax.LineWidth = 1;
                            ax.TickLabelInterpreter = "latex";

                            nexttile; hold on;
                            surf(Xc, Yc, Zc, 'FaceAlpha', 0.10, 'EdgeColor', 'none');
                            surf(X, Y, Z, 'FaceAlpha', 0.35, 'EdgeColor', 'none');
                            quiver3(X, Y, Z, 0 * X, Ltauy, 0 * Z, 0, 'LineWidth', 2, 'MaxHeadSize', 1);
                            title('$\tau_y$ (lateral)', FontSize = 20, Interpreter = 'latex');
                            setup_axes_3d_zoom(rW, X, Y, Z, bW);

                            ax = gca;
                            ax.FontSize = 16;
                            ax.LineWidth = 1;
                            ax.TickLabelInterpreter = "latex";
                        end

                        %%

                        if count == selected_config
                            rW = final_rover.rw_eff;
                            theta_plot = linspace(contact.theta_r, contact.theta_f, 45);

                            stress_piece = @(th, a, b, c) ...
                                (th <= contact.theta_m) .* (a(1) .* th .^ 2 + b(1) .* th + c(1)) + ...
                                (th > contact.theta_m) .* (a(2) .* th .^ 2 + b(2) .* th + c(2));

                            sig = stress_piece(theta_plot, a_sig_vec, b_sig_vec, c_sig_vec);
                            taux = stress_piece(theta_plot, a_tt_vec, b_tt_vec, c_tt_vec);

                            x = rW * sin(theta_plot);
                            z = -rW * cos(theta_plot);

                            nx = sin(theta_plot);  nz = -cos(theta_plot);
                            tx = cos(theta_plot);  tz = sin(theta_plot);

                            Lsig = 0.45 * rW * (sig / max(sig + eps));
                            Ltaux = 0.35 * rW * (taux / max(abs(taux) + eps));

                            tt = linspace(-pi, pi, 500);
                            xw = rW * sin(tt);
                            zw = -rW * cos(tt);

                            figure('Color', 'w'); tiledlayout(1, 2, "TileSpacing", "compact", "Padding", "compact");

                            nexttile; hold on; axis equal; grid on; box on;
                            title('$\sigma$ on x-z plane', FontSize = 20, Interpreter = "latex");
                            plot(xw, zw, 'k', 'LineWidth', 3);
                            plot(x, z, 'k', 'LineWidth', 4);
                            for k = 1:numel(theta_plot)
                                plot([x(k) x(k) + Lsig(k) * nx(k)], [z(k) z(k) + Lsig(k) * nz(k)], 'b', 'LineWidth', 3);
                            end
                            xline(rW * sin(contact.theta_m), '--', 'Color', [0.4 0.4 0.4], 'LineWidth', 2);
                            xlabel('x [m]', FontSize = 20, Interpreter = "latex");
                            ylabel('z [m]', FontSize = 20, Interpreter = "latex");

                            ax = gca;
                            ax.FontSize = 20;
                            ax.LineWidth = 1;
                            ax.TickLabelInterpreter = "latex";

                            xlim([-0.8 0.8])
                            ylim([-1 1])

                            nexttile; hold on; axis equal; grid on; box on;
                            title('$\tau_x$ on x-z plane', FontSize = 20, Interpreter = "latex");
                            plot(xw, zw, 'k', 'LineWidth', 3);
                            plot(x, z, 'k', 'LineWidth', 4);
                            for k = 1:numel(theta_plot)
                                plot([x(k) x(k) + Ltaux(k) * tx(k)], [z(k) z(k) + Ltaux(k) * tz(k)], 'r', 'LineWidth', 3);
                            end
                            xline(rW * sin(contact.theta_m), '--', 'Color', [0.4 0.4 0.4], 'LineWidth', 2);
                            xlabel('x [m]', FontSize = 20, Interpreter = "latex");
                            ylabel('z [m]', FontSize = 20, Interpreter = "latex");

                            ax = gca;
                            ax.FontSize = 20;
                            ax.LineWidth = 1;
                            ax.TickLabelInterpreter = "latex";

                            xlim([-0.8 0.8])
                            ylim([-1 1])
                        end

                        %%

                        if count == selected_config
                            rW = final_rover.rw_eff;
                            bW = final_rover.bw;

                            theta_plot = linspace(contact.theta_r, contact.theta_f, 60);

                            stress_piece = @(th, a, b, c) ...
                                (th <= contact.theta_m) .* (a(1) .* th .^ 2 + b(1) .* th + c(1)) + ...
                                (th > contact.theta_m) .* (a(2) .* th .^ 2 + b(2) .* th + c(2));

                            tauy_theta = stress_piece(theta_plot, a_tl_vec, b_tl_vec, c_tl_vec);
                            x_theta = rW * sin(theta_plot);

                            Nx = 80;
                            Ny = 25;
                            xg = linspace(min(x_theta), max(x_theta), Nx);
                            yg = linspace(-bW / 2, bW / 2, Ny);
                            [Xg, Yg] = meshgrid(xg, yg); %#ok<NASGU>

                            TAUYg = interp1(x_theta, tauy_theta, xg, 'linear', 'extrap');
                            TAUYg = repmat(TAUYg, Ny, 1);

                            figure('Color', 'w'); hold on; axis equal; box on; grid on;
                            imagesc(xg, yg, TAUYg / 1e3);
                            set(gca, 'YDir', 'normal');
                            colormap parula; cb = colorbar;
                            ylabel(cb, '$\tau_y$ [kPa]', FontSize = 30, Interpreter = "latex");

                            plot([min(x_theta) max(x_theta) max(x_theta) min(x_theta) min(x_theta)], ...
                                [-bW / 2 -bW / 2 bW / 2 bW / 2 -bW / 2], 'k', 'LineWidth', 2);

                            xq = linspace(min(x_theta), max(x_theta), 12);
                            tauq = interp1(x_theta, tauy_theta, xq);
                            L = 0.35 * (bW / 2) * (tauq / max(abs(tauq) + eps));
                            y0 = zeros(size(xq));

                            quiver(xq, y0, 0 * xq, L, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 1);

                            xlabel('x [m]', FontSize = 25, Interpreter = "latex");
                            ylabel('y [m]', FontSize = 25, Interpreter = "latex");

                            ax = gca;
                            ax.FontSize = 30;
                            ax.LineWidth = 1;
                            ax.TickLabelInterpreter = "latex";
                        end

                        %% RESULTS
                        MATRIX(count, 1) = final_rover.rw * 2;
                        MATRIX(count, 2) = final_rover.rw_eff * 2;
                        MATRIX(count, 3) = final_rover.bw;
                        MATRIX(count, 4) = final_rover.Nw;
                        MATRIX(count, 5) = final_rover.slip;
                        MATRIX(count, 6) = final_rover.vel_x;
                        MATRIX(count, 7) = final_rover.W_total / lunar_soil.g;
                        MATRIX(count, 8) = final_rover.Ww;
                        MATRIX(count, 9) = final_rover.delta;

                        MATRIX(count, 10) = out.Traction;
                        MATRIX(count, 11) = out.DP;
                        MATRIX(count, 12) = out.Fy;
                        MATRIX(count, 13) = out.Fz;
                        MATRIX(count, 14) = out.Mx;
                        MATRIX(count, 15) = out.My;
                        MATRIX(count, 16) = out.Mz;

                        MATRIX(count, 17) = contact.theta_r * 180 / pi;
                        MATRIX(count, 18) = contact.theta_f * 180 / pi;
                        MATRIX(count, 19) = contact.theta_m * 180 / pi;

                        MATRIX(count, 20) = contact.h_eff;
                        MATRIX(count, 21) = contact.Aw;

                        MATRIX(count, 22) = stress.sigma_m;
                        MATRIX(count, 23) = stress.taut_m;
                        MATRIX(count, 24) = stress.taul_m;

                        MATRIX(count, 25) = out.Rc;
                        MATRIX(count, 26) = out.Rb;
                        MATRIX(count, 27) = out.Rr;
                        MATRIX(count, 28) = out.Rg;
                        MATRIX(count, 29) = out.Rall;

                        MATRIX(count, 30) = out.T_drw_ew;
                        MATRIX(count, 31) = out.P_drw;

                        MATRIX(count, 32) = thick;
                        MATRIX(count, 33) = mass;

                        MATRIX(count, 34) = p_avg;
                        MATRIX(count, 35) = contact.theta_all;

                        count = count + 1;
                    end
                end
            end
        end
    end
end

toc

if exist('hWB', 'var') && ishandle(hWB)
    waitbar(1, hWB, 'Completed!');
    pause(0.2);
    close(hWB);
end

MATRIX(count:end, :) = [];

%% OUTPUT TABLE

colNames = {'Diameter [m]', 'Substitute Diameter [m]', 'Width [m]', 'Number of wheel', ...
    'Slip ratio', 'Velocity [m/s]', 'Vehicle weight [kg]', ...
    'Weight on a wheel [N]', 'Deflection of the wheel [m]', 'Max traction [N]', ...
    'Drawbar pull [N]', 'Fy Lateral force [N]', 'Fz Soil vertical reaction [N]', ...
    'Mx Overturning moment [N*m]', 'My Rolling resistant moment [N*m]', ...
    'Mz Self-aligning moment [N*m]', 'Entry contact angle [deg]', 'Exit contact angle [deg]', ...
    'Maximum stress angle [deg]', 'Maximum sinkage [m]', 'Contact patch [m^2]', ...
    'Maximum sigma [N/m]', 'Maximum tx [N/m]', 'Maximum ty [N/m]', ...
    'Soil compaction resistance [N]', 'Bulldozing resistance [N]', ...
    'Rolling resistance [N]', 'Gravitational resistance [N]', ...
    'Total Motion resistance [N]', 'Drive torque [N*m]', 'Drive power [W]', ...
    'Thickness [m]', 'Mass of wheel [kg]', 'Average pressure [Pa]', 'Total contact angle [deg]'};

TAB = table(MATRIX(:, 1), MATRIX(:, 2), MATRIX(:, 3), MATRIX(:, 4), ...
    MATRIX(:, 5), MATRIX(:, 6), MATRIX(:, 7), MATRIX(:, 8), MATRIX(:, 9), MATRIX(:, 10), ...
    MATRIX(:, 11), MATRIX(:, 12), MATRIX(:, 13), MATRIX(:, 14), MATRIX(:, 15), ...
    MATRIX(:, 16), MATRIX(:, 17), MATRIX(:, 18), MATRIX(:, 19), MATRIX(:, 20), ...
    MATRIX(:, 21), MATRIX(:, 22), MATRIX(:, 23), MATRIX(:, 24), MATRIX(:, 25), ...
    MATRIX(:, 26), MATRIX(:, 27), MATRIX(:, 28), MATRIX(:, 29), MATRIX(:, 30), ...
    MATRIX(:, 31), MATRIX(:, 32), MATRIX(:, 33), MATRIX(:, 34), MATRIX(:, 35), 'VariableNames', colNames);

%% FIGURE OF MERIT

TAB.DP_norm = (TAB.("Drawbar pull [N]") - min(TAB.("Drawbar pull [N]"))) ./ ...
    (max(TAB.("Drawbar pull [N]")) - min(TAB.("Drawbar pull [N]")));
TAB.P_norm = (max(TAB.("Drive power [W]")) - TAB.("Drive power [W]")) ./ ...
    (max(TAB.("Drive power [W]")) - min(TAB.("Drive power [W]")));
TAB.M_norm = (max(TAB.("Mass of wheel [kg]")) - TAB.("Mass of wheel [kg]")) ./ ...
    (max(TAB.("Mass of wheel [kg]")) - min(TAB.("Mass of wheel [kg]")));

TAB.FoM = TAB.DP_norm / 3 + TAB.P_norm / 3 + TAB.M_norm / 3;
TAB_sorted_metric = sortrows(TAB, 'FoM', 'descend'); %#ok<NASGU>


%% SAVE  TABLE

baseFolder = fileparts(fileparts(mfilename('fullpath')));
folderName = fullfile(baseFolder, 'Comparison models');

if ~exist(folderName, 'dir')
    mkdir(folderName)
end

fileName = fullfile(folderName, 'TAB_quad_flessibile.mat');

if isfile(fileName)
    S = load(fileName, 'TAB_quad_flessibile');
    TAB_quad_flessibile = S.TAB_quad_flessibile;

    [TAB_quad_flessibile, TAB] = alignTablesForAppend(TAB_quad_flessibile, TAB);
    TAB_quad_flessibile = [TAB_quad_flessibile; TAB];
    TAB_quad_flessibile = unique(TAB_quad_flessibile, 'rows', 'stable');
else
    TAB_quad_flessibile = TAB;
end

TAB_quad_flessibile = sortrows(TAB_quad_flessibile, TAB_quad_flessibile.Properties.VariableNames);
save(fileName, 'TAB_quad_flessibile');

%% EXPORT TABLES

writetable(TAB, 'tabella_quadratic_locB.xlsx')

%% GLOBAL LATEX STYLE

set(groot, 'defaultTextInterpreter', 'latex');
set(groot, 'defaultAxesTickLabelInterpreter', 'latex');
set(groot, 'defaultLegendInterpreter', 'latex');


