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

clc; clear; close all

%% PARAMETERS

% Environmental parameters
lun_soil.g = 1.62;              % gravity on the Moon [m/s^2]

% Soil parameters (lunar regolith - soft soil)
lun_soil.n = 1;                  % sinkage exponent
lun_soil.kc = 1400;              % Bekker cohesive modulus [N/m^(n+1)]
lun_soil.kphi = 820000;          % Bekker frictional modulus [N/m^(n+2)]

lun_soil.c = 170;                % cohesion [N/m^2]
lun_soil.phi = 40*pi/180;        % internal friction angle [rad]
lun_soil.gamma = 2470;           % soil unit weight [N/m^3]

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

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

lun_soil.c1 = 0.285;
lun_soil.c2 = 0.32;

lun_soil.fr = 0.025;             % rolling resistance coefficient

% Configuration parameters
theta_slope = 0; % [deg]

rover.D_vec = [0.6 0.9 1.1 1.5 2 0.7 1 1.3 1.7];      % wheel diameter [m]
rover.B_vec = [0.4 0.5 0.6 0.7 0.8];                  % wheel width [m]
rover.slip_vec = [0.1 0.15 0.2 0.3 0.4 0.5];         % slip ratio
rover.vel_vec = [0.1 1 3 5 8];                       % velocity [m/s]
rover.mass_vec = [1500 2500 4000 5000 7000];         % vehicle mass [kg]
rover.Nw_vec = [4 6 8 10];                           % number of wheels

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

%% OUTPUT MATRIX PREALLOCATION

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 = 29;

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 f = 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(f);
                        final_rover.W_total = rover.W_total_vec(t);

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

                        % 1) CONTACT GEOMETRY
                        contact.Csig = lun_soil.kc ./ final_rover.bw + lun_soil.kphi;

                        hf_vec = linspace(1e-6, final_rover.rw, 200);
                        W_vec = arrayfun(@(h) vertical_load(h, final_rover.rw, final_rover.bw, ...
                            contact.Csig, lun_soil.n, lun_soil.c1, lun_soil.c2, ...
                            final_rover.slip), hf_vec);

                        idx_hmax = find(W_vec >= final_rover.Ww, 1);

                        if isempty(idx_hmax)
                            continue
                        end

                        h_min = hf_vec(1);
                        h_max = hf_vec(idx_hmax);

                        for k = 1:50
                            h_mid = 0.5 * (h_min + h_max);
                            W_mid = vertical_load(h_mid, final_rover.rw, final_rover.bw, ...
                                contact.Csig, lun_soil.n, lun_soil.c1, lun_soil.c2, ...
                                final_rover.slip);

                            if W_mid > final_rover.Ww
                                h_max = h_mid;
                            else
                                h_min = h_mid;
                            end
                        end

                        contact.h_f = 0.5 * (h_min + h_max);

                        contact.theta_f = acos(1 - contact.h_f / final_rover.rw);
                        contact.theta_r = -contact.theta_f;
                        contact.theta_m = (lun_soil.c1 + lun_soil.c2 .* final_rover.slip) * contact.theta_f;
                        contact.h_r = final_rover.rw * (1 - cos(contact.theta_r));
                        contact.theta_all = abs(contact.theta_f) + abs(contact.theta_r);

                        % Contact patch for a rigid cylindrical wheel
                        contact.lwr = sqrt(contact.h_f * (2 * final_rover.rw - contact.h_f)) + ...
                                      sqrt(contact.h_r * (2 * final_rover.rw - contact.h_r));
                        contact.Awr = contact.lwr * final_rover.bw;

                        % Average pressure and equivalent sinkage
                        p_avg = final_rover.Ww / contact.Awr;

                        % 2) QUADRATIC STRESS DISTRIBUTION

                        % Normal stress sigma(theta)
                        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 * (1 - cos(contact.theta_m));
                        contact.h_fm = final_rover.rw * (1 - cos(contact.theta_fm));
                        contact.h_rm = final_rover.rw * (1 - cos(contact.theta_rm));

                        stress.sigma_m = contact.Csig * contact.h_m^lun_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 tau_x(theta)
                        shear_disp.j_mt = final_rover.rw * ((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 * ((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 * ((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 * ((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 * ((contact.theta_f - contact.theta_r) - ...
                            (1 - final_rover.slip) * (sin(contact.theta_f) - sin(contact.theta_r)));

                        stress.taut_m = (lun_soil.c + stress.sigma_m * tan(lun_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_mt) / lun_soil.K));

                        stress.taut_fm = (lun_soil.c + stress.sigma_fm * tan(lun_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_fmt) / lun_soil.K));

                        stress.taut_f = 0;

                        stress.taut_rm = (lun_soil.c + stress.sigma_rm * tan(lun_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_rmt) / lun_soil.K));

                        stress.taut_r = (lun_soil.c + stress.sigma_r * tan(lun_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_rt) / lun_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 tau_y(theta)
                        beta = 5 * pi / 180; % sideslip angle [rad]

                        shear_disp.j_ml = final_rover.rw * (1 - final_rover.slip) * ...
                            (contact.theta_f - contact.theta_m) * tan(beta);

                        shear_disp.j_fml = final_rover.rw * (1 - final_rover.slip) * ...
                            (contact.theta_f - contact.theta_fm) * tan(beta);

                        shear_disp.j_fl = final_rover.rw * (1 - final_rover.slip) * ...
                            (contact.theta_f - contact.theta_f) * tan(beta);

                        shear_disp.j_rml = final_rover.rw * (1 - final_rover.slip) * ...
                            (contact.theta_f - contact.theta_rm) * tan(beta);

                        shear_disp.j_rl = final_rover.rw * (1 - final_rover.slip) * ...
                            (contact.theta_f - contact.theta_r) * tan(beta);

                        stress.taul_m = (lun_soil.c + stress.sigma_m * tan(lun_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_ml) / lun_soil.K));

                        stress.taul_fm = (lun_soil.c + stress.sigma_fm * tan(lun_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_fml) / lun_soil.K));

                        stress.taul_f = (lun_soil.c + stress.sigma_f * tan(lun_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_fl) / lun_soil.K));

                        stress.taul_rm = (lun_soil.c + stress.sigma_rm * tan(lun_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_rml) / lun_soil.K));

                        stress.taul_r = (lun_soil.c + stress.sigma_r * tan(lun_soil.phi)) * ...
                            (1 - exp(-abs(shear_disp.j_rl) / lun_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);

                        % Coefficients ordered as: 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];

                        % FORCE AND MOMENT COMPUTATION
                        [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, final_rover.bw);

                        % RESISTANCE COMPUTATION
                        [out.Rc, out.Rb, out.Rr, out.Rg, out.Rall] = ...
                            Resistence_fun(a_sig_vec, b_sig_vec, c_sig_vec, ...
                                           final_rover.rw, final_rover, ...
                                           lun_soil, contact, theta_slope, ...
                                           contact.h_f);

                        % TRACTION
                        out.Traction = out.Fx + out.Rc;
                        out.DP = out.Traction - out.Rall;

                        % DRIVE TORQUE AND POWER
                        out.T_drw_rw = out.Rall .* final_rover.rw;
                        out.P_drw = out.T_drw_rw .* (final_rover.vel_x ./ final_rover.rw);

                        % RESULTS
                        MATRIX(count, 1) = final_rover.rw * 2;
                        MATRIX(count, 2) = final_rover.bw;
                        MATRIX(count, 3) = final_rover.Nw;
                        MATRIX(count, 4) = final_rover.slip;
                        MATRIX(count, 5) = final_rover.vel_x;
                        MATRIX(count, 6) = final_rover.W_total / lun_soil.g;

                        MATRIX(count, 7) = final_rover.Ww;

                        MATRIX(count, 8) = out.DP;
                        MATRIX(count, 9) = out.Fy;
                        MATRIX(count, 10) = out.Fz;
                        MATRIX(count, 11) = out.Mx;
                        MATRIX(count, 12) = out.My;
                        MATRIX(count, 13) = out.Mz;

                        MATRIX(count, 14) = contact.theta_r * 180 / pi;
                        MATRIX(count, 15) = contact.theta_f * 180 / pi;
                        MATRIX(count, 16) = contact.theta_m * 180 / pi;

                        MATRIX(count, 17) = contact.h_f;

                        MATRIX(count, 18) = stress.sigma_m;
                        MATRIX(count, 19) = stress.taut_m;
                        MATRIX(count, 20) = stress.taul_m;

                        MATRIX(count, 21) = out.Rc;
                        MATRIX(count, 22) = out.Rb;
                        MATRIX(count, 23) = out.Rr;
                        MATRIX(count, 24) = out.Rg;
                        MATRIX(count, 25) = out.Rall;

                        MATRIX(count, 26) = out.T_drw_rw;
                        MATRIX(count, 27) = out.P_drw;

                        MATRIX(count, 28) = p_avg;
                        MATRIX(count, 29) = contact.theta_all;

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

toc

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

%% TABLE CREATION

colNames = {'Diameter [m]', 'Width [m]', 'Number of wheel', ...
    'Slip ratio', 'Velocity [m/s]', 'Vehicle weight [kg]', ...
    'Weight on a wheel [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]', '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]', '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), ...
    'VariableNames', colNames);

%% FIGURE OF MERIT

%% METRICA DA MASSIMIZZARE 

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.FoM = TAB.DP_norm*1/5+TAB.P_norm*1/5;
TAB_sorted_metric= sortrows(TAB, 'FoM', 'descend'); % Optimal configuration

N = 30;   
bestTAB = TAB_sorted_metric(1:N,:);

validNames = matlab.lang.makeValidName(bestTAB.Properties.VariableNames);
bestTAB.Properties.VariableNames = validNames;

results = struct();

for col = 1 : width(bestTAB)
    name = bestTAB.Properties.VariableNames{col};
    results.(name) = unique(bestTAB.(name));   % salva come vettore
end

%% TABLE EXPORT 

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

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

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

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

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

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

