%% Tangent Stiffness Validation via Finite Differences
% Compares analytical tangent stiffness with numerical (FD) tangent

clear; clc;

% Parameters
E = 2.1e11; A = 0.01; I = 8.333333e-06;
L0 = 3.2 / 20;  % element length for 20-element mesh
alpha0 = 0;      % horizontal element

% Generate a random deformed state (moderate deformation)
rng(42);
d_global = [0.001; -0.0005; 0.1; -0.0008; -0.001; -0.15];
% First node displaced, second node displaced differently

% Fixed DOFs (for full assembly test)
fixed_dofs = [1, 2, 3];

% Compute analytical tangent stiffness
K_analytical = compute_element_tangent(d_global, L0, alpha0, E, A, I);

% Compute numerical tangent stiffness via central differences
eps_fd = 1e-6;
n_dof = 6;
K_numerical = zeros(n_dof, n_dof);

for j = 1:n_dof
    d_plus = d_global;
    d_minus = d_global;
    d_plus(j) = d_plus(j) + eps_fd;
    d_minus(j) = d_minus(j) - eps_fd;

    f_plus = compute_element_force(d_plus, L0, alpha0, E, A, I);
    f_minus = compute_element_force(d_minus, L0, alpha0, E, A, I);

    K_numerical(:, j) = (f_plus - f_minus) / (2 * eps_fd);
end

% Compare
fprintf('=== Tangent Stiffness Validation ===\n\n');
fprintf('Analytical K_T:\n');
disp(K_analytical);
fprintf('\nNumerical K_T (FD):\n');
disp(K_numerical);
fprintf('\nDifference (Analytical - Numerical):\n');
diff_K = K_analytical - K_numerical;
disp(diff_K);
fprintf('\nRelative error (Frobenius norm): %.4e\n', ...
    norm(diff_K, 'fro') / norm(K_numerical, 'fro'));
fprintf('Max absolute error: %.4e\n', max(abs(diff_K(:))));

% Check symmetry
fprintf('\nAsymmetry of analytical K_T: %.4e\n', ...
    norm(K_analytical - K_analytical', 'fro'));
fprintf('Asymmetry of numerical K_T: %.4e\n', ...
    norm(K_numerical - K_numerical', 'fro'));

%% Check force correctness
f_analytical = compute_element_force(d_global, L0, alpha0, E, A, I);
fprintf('\n=== Internal Force Check ===\n');
fprintf('Analytical f_int:\n');
disp(f_analytical');

%% ===== Element Routines =====
function fg = compute_element_force(d_e, L0, alpha0, E, A, I)
    % Unpack
    u1 = d_e(1); v1 = d_e(2); theta1 = d_e(3);
    u2 = d_e(4); v2 = d_e(5); theta2 = d_e(6);

    % Current positions (assume initial at [0,0] and [L0,0])
    x1 = 0 + u1; y1 = 0 + v1;
    x2 = L0 + u2; y2 = 0 + v2;

    % Current chord
    dx = x2 - x1; dy = y2 - y1;
    L = sqrt(dx^2 + dy^2);
    c = dx / L; s = dy / L;
    beta = atan2(dy, dx);

    % Local deformations
    u_bar = L - L0;
    alpha = beta - alpha0;
    theta1_bar = theta1 - alpha;
    theta2_bar = theta2 - alpha;

    % Local forces
    N  = E * A / L0 * u_bar;
    M1 = E * I / L0 * (4 * theta1_bar + 2 * theta2_bar);
    M2 = E * I / L0 * (2 * theta1_bar + 4 * theta2_bar);

    % B matrix
    B = [ -c,  -s, 0,  c,   s, 0;
          -s/L, c/L, 1, s/L, -c/L, 0;
          -s/L, c/L, 0, s/L, -c/L, 1 ];

    % Global force
    fg = B' * [N; M1; M2];
end

function KT = compute_element_tangent(d_e, L0, alpha0, E, A, I)
    % Unpack
    u1 = d_e(1); v1 = d_e(2); theta1 = d_e(3);
    u2 = d_e(4); v2 = d_e(5); theta2 = d_e(6);

    % Current positions
    x1 = 0 + u1; y1 = 0 + v1;
    x2 = L0 + u2; y2 = 0 + v2;

    % Current chord
    dx = x2 - x1; dy = y2 - y1;
    L = sqrt(dx^2 + dy^2);
    c = dx / L; s = dy / L;
    beta = atan2(dy, dx);

    % Local deformations
    u_bar = L - L0;
    alpha = beta - alpha0;
    theta1_bar = theta1 - alpha;
    theta2_bar = theta2 - alpha;

    % Local forces
    N  = E * A / L0 * u_bar;
    M1 = E * I / L0 * (4 * theta1_bar + 2 * theta2_bar);
    M2 = E * I / L0 * (2 * theta1_bar + 4 * theta2_bar);

    % B matrix
    B = [ -c,  -s, 0,  c,   s, 0;
          -s/L, c/L, 1, s/L, -c/L, 0;
          -s/L, c/L, 0, s/L, -c/L, 1 ];

    % Local stiffness
    Kl = [ E*A/L0,      0,          0;
           0,            4*E*I/L0,   2*E*I/L0;
           0,            2*E*I/L0,   4*E*I/L0 ];

    % Material stiffness in global coordinates
    Ke_mat = B' * Kl * B;

    % Geometric stiffness (Crisfield formulation)
    z_vec = [s; -c; 0; -s; c; 0];
    r_vec = [-c; -s; 0; c; s; 0];
    Ke_geo = (N / L) * (z_vec * z_vec') ...
           + ((M1 + M2) / L^2) * (r_vec * z_vec' + z_vec * r_vec');

    % Total tangent
    KT = Ke_mat + Ke_geo;
end