% Locomotion system parametric study:
% rigid/elastic cylindrical wheel behavior on lunar regolith.

% Required external functions on the MATLAB path:
%   Ww_fun, H_fun, Rall_fun, DP_fun, T_P_fun

clc; clear; close all

%% PARAMETERS

% Environmental parameters
g = 1.62; % Lunar gravity [m/s^2]

% Soil parameters (lunar regolith, soft soil)
soil.n = 1;                  % Sinkage exponent [-]
soil.kc = 1400;              % Bekker cohesive modulus [N/m^(n+1)]
soil.k_phi = 820000;         % Bekker frictional modulus [N/m^(n+2)]
soil.c = 170;                % Soil cohesion [N/m^2]
soil.phi = 40*pi/180;        % Internal friction angle [rad]
soil.gamma = 2470;           % Soil specific weight [N/m^3]
soil.Nq = 32.23;             % Bearing capacity friction factor
soil.Nc = 48.09;             % Bearing capacity cohesion factor
soil.Ngamma = 33.27;         % Bearing capacity density factor
soil.Kc = 33.37;             % Cohesive modulus of soil deformation   
soil.Kgamma = 72.77;         % Density modulus of soil deformation  
soil.K = 0.0178;             % Shear deformation modulus [m]

% Vehicle parameter
fr = 0.025;                  % Rolling resistance coefficient [-]

% Configuration vectors
D_vec = [0.6 0.9 1.1 1.5 2 0.7 1 1.3 1.7];      % Wheel diameter [m]
B_vec = [0.4 0.5 0.6 0.7 0.8];                  % Wheel width [m]
Nw_vec = [4 6 8 10];                            % Number of wheels [-]
Type_vec = ["rigid", "elastic"];                % Wheel type [-]
slope_vec = 0;                                  % Terrain slope [deg]
vel_vec = [0.1 1 3 5 8];                        % Vehicle speed [m/s]
slip_vec = [0.1 0.15 0.2 0.3 0.4 0.5];          % Slip ratio [-]
mass_vec = [1500 2500 4000 5000 7000];          % Vehicle mass [kg]
W_total_vec = mass_vec .* g;                    % Total vehicle weight [N]
def_vec = [0.001 0.002 0.003 0.004 0.005];      % Wheel deflection [m]

%% OUTPUT PREALLOCATION

numRows = length(D_vec) * length(B_vec) * length(Nw_vec) * length(Type_vec) * ...
    length(slope_vec) * length(vel_vec) * length(W_total_vec) * ...
    length(slip_vec) * length(def_vec);

numOutputColumns = 26;
MATRIX = zeros(numRows, numOutputColumns);
COL = strings(numRows, 1);
count = 1;

%% PARAMETRIC STUDY

tic

for i = 1:length(D_vec)
    for j = 1:length(B_vec)
        for u = 1:length(Nw_vec)
            for m = 1:length(Type_vec)
                for r = 1:length(slope_vec)
                    for f = 1:length(vel_vec)
                        for t = 1:length(W_total_vec)
                            for h = 1:length(def_vec)
                                for tr = 1:length(slip_vec)

                                    dw = D_vec(i);
                                    bw = B_vec(j);
                                    Nw = Nw_vec(u);
                                    Type = Type_vec(m);
                                    teta = slope_vec(r);
                                    vel = vel_vec(f);
                                    W_total = W_total_vec(t);
                                    deltaw = def_vec(h);
                                    slip = slip_vec(tr);

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

                                    if Type == "rigid"
                                        deltaw = 0;
                                    end

                                    % 1) Sinkage
                                    Csig = soil.kc / bw + soil.k_phi;

                                    if Type == "rigid"
                                        zw = (3 * Ww * cosd(teta) / ((3 - soil.n) * (soil.kc + bw * soil.k_phi) * sqrt(dw))) ^ (2 / (2 * soil.n + 1));
                                        L = dw / 2 * acos(1 - 2 * zw / dw);
                                        Aw = L * bw;
                                        pw = Ww / Aw;
                                    else
                                        zw = (3 * Ww * cosd(teta) / ((3 - soil.n) * (soil.kc + bw * soil.k_phi) * sqrt(dw))) ^ (2 / (2 * soil.n + 1));

                                        tolz = 1e-6;
                                        for it = 1:30
                                            z_eff = zw + deltaw;
                                            L = dw / 2 * acos(1 - 2 * z_eff / dw);
                                            Aw = bw * L;
                                            pw = Ww / Aw;

                                            zw_new = (pw / Csig) ^ (1 / soil.n);

                                            if abs(zw_new - zw) < tolz
                                                zw = zw_new;
                                                break
                                            end
                                            zw = zw_new;
                                        end
                                    end

                                    J = slip * L;

                                    % 2) Soil thrust and traction
                                    Fmax = (soil.c * Aw + Ww * cosd(teta) * tan(soil.phi)) * ...
                                        (1 - soil.K / J * (1 - exp(-J / soil.K)));

                                    H = H_fun(Aw, soil, Ww);

                                    % 3) Motion resistance
                                    teta_inc = 0;
                                    [Rc, flag, Rb, Rr, Rg, Ro, Rall] = Rall_fun(Type, bw, pw, zw, Ww, teta_inc, dw, soil, fr, soil.n);

                                    % 4) Drawbar pull
                                    DP = DP_fun(Fmax, Rall);

                                    % 5) Drive torque and power per wheel
                                    [T_drw, P_drw] = T_P_fun(Type, Rall, dw, deltaw, vel);

                                    % 6) Wheel thickness and mass (Aluminum 7075)
                                    E = 72400 * 10^6;   % Young's modulus [Pa]
                                    rho = 2700;         % Density [kg/m^3]
                                    rw = dw / 2;        % Wheel radius [m]

                                    th = nthroot((1.776 * Ww * dw / 2^3) / (E * bw * deltaw), 3);
                                    if th == inf
                                        th = 0;
                                    end
                                    mass = rho * pi * (rw^2 - (rw - th)^2) * bw;

                                    % Stress
                                    sigma = pw;
                                    tau = Fmax / Aw;

                                    % Store results
                                    MATRIX(count, 1) = dw;
                                    MATRIX(count, 2) = bw;
                                    MATRIX(count, 3) = Nw;
                                    MATRIX(count, 4) = teta;
                                    MATRIX(count, 5) = vel;
                                    MATRIX(count, 6) = W_total / g;
                                    MATRIX(count, 7) = Ww;
                                    MATRIX(count, 8) = deltaw;
                                    COL(count, 1) = Type;

                                    MATRIX(count, 9) = Fmax;
                                    MATRIX(count, 10) = Rc;
                                    MATRIX(count, 11) = Rb;
                                    MATRIX(count, 12) = Rr;
                                    MATRIX(count, 13) = Rg;
                                    MATRIX(count, 14) = Ro;
                                    MATRIX(count, 15) = Rall;
                                    MATRIX(count, 16) = H;
                                    MATRIX(count, 17) = pw;
                                    MATRIX(count, 18) = DP;
                                    MATRIX(count, 19) = T_drw;
                                    MATRIX(count, 20) = P_drw;
                                    MATRIX(count, 21) = zw;
                                    MATRIX(count, 22) = th;
                                    MATRIX(count, 23) = mass;
                                    MATRIX(count, 24) = slip;
                                    MATRIX(count, 25) = sigma;
                                    MATRIX(count, 26) = tau;

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

toc

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

%% RESULT TABLE

colNames= {'Type of wheel','Diameter [m]','Width [m]','Number of wheel', ...
    'Total contact angle [deg]', 'Velocity [m/s]','Vehicle weight [kg]',...
    'Weight on a wheel [N]','Deflection of the wheel [m]','Slip ratio','Max traction [N]',...
    'Soil compaction resistance [N]','Bulldozing resistance [N]',...
    'Rolling resistance [N]','Gravitational resistance [N]','Obstacle resistance [N]',...
    'Total Motion resistance [N]','Soil thrust [N]', 'Average ground pressure [N]', ...
    'Drawbar pull [N]','Drive torque [N*m]','Drive power [W]','Maximum sinkage [m]',...
    'Thickness [m]','Mass of wheel [kg]', 'Sigma [Pa]', 'tau x [Pa]'};

TAB = table(COL, MATRIX(:,1), MATRIX(:,2), MATRIX(:,3), MATRIX(:,4), ...
    MATRIX(:,5), MATRIX(:,6), MATRIX(:,7), MATRIX(:,8), MATRIX(:,24), ...
    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(:,25), MATRIX(:,26), '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]")));

m = TAB.("Mass of wheel [kg]");

TAB.M_norm= (max(TAB.("Mass of wheel [kg]"))-TAB.("Mass of wheel [kg]"))/...
    (max(TAB.("Mass of wheel [kg]"))-min(m(m > 0)));

TAB.FoM = TAB.DP_norm*1/3+TAB.P_norm*1/3+TAB.M_norm*1/3;
TAB_sorted_metric= sortrows(TAB, 'FoM', 'descend'); % Optimal configuration


%% SAVE  TABLE

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

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

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

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

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

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

