% g, cm, sec

addpath('../two_dim_fea');

% E = 0.1 MPa; c = 0.04 kPa s; rho = 1 g/cm^2
Young = 1.0*1e+6; c = 0.4*1e+3; nu = 0.48; density = 1.00;
[ lambda, mu ] = Lame_constants( Young, nu );

m = 5; n = 5;
[ points, triangles ] = rectangular_object( m, n, 10, 10 );
thickness = 1;

npoints = size(points,2);
ntriangles = size(triangles,1);
elastic = Body(npoints, points, ntriangles, triangles, thickness);
elastic = elastic.mechanical_parameters(density, lambda, mu);
elastic = elastic.calculate_stiffness_matrix;

Atrans = zeros(19, 2*npoints);
Atrans(1:10,1:10) = eye(10);
R_9_10 = zeros(9,10);
R_9_10(1,1) = 1;
R_9_10(2,3) = 1;
R_9_10(3,5) = 1;
R_9_10(4,7) = 1;
R_9_10(5,9) = 1;
R_9_10(6,2) = 1; R_9_10(6,6) = -1;
R_9_10(7,4) = 1; R_9_10(7,6) = -1;
R_9_10(8,8) = 1; R_9_10(8,6) = -1;
R_9_10(9,10) = 1; R_9_10(9,6) = -1;
Atrans(11:19, (2*npoints-10+1):(2*npoints)) = R_9_10;
A = Atrans';

% 10.0 N --> fpush = 10.0*1e+5;
%fpush = -10.0*1e+5;
force = [];
disp = [];

filename = 'push_test/push_test';

figure('position', [0, 0, 800, 800]);
set(0,'defaultAxesFontSize',32);
set(0,'defaultTextFontSize',32);

clf;
elastic.draw_individual;
xlim([-2,12]);
ylim([-2,12]);
grid on;
pbaspect([1 1 1]);
saveas(gcf, strcat(filename, '_natural.png'), 'png');

for fpushscaled = 0.0:(-1.0):-20.0
    f = zeros(2*npoints,1);
    f(46) = fpushscaled * 1e+5;
    
    mat = [ elastic.Stiffness_Matrix, -A;
                             -Atrans, zeros(19,19)];
    vec = [ f; zeros(19,1) ];
    sol = mat \ vec;

    clf;
    elastic.draw(reshape(sol(1:50), [2,npoints]));
    xlim([-2,12]);
    ylim([-2,12]);
    grid on;
    pbaspect([1 1 1]);
    saveas(gcf, strcat(filename, '_', num2str(fpushscaled, '%3d'), '.png'), 'png');    
    
    force = [force; fpushscaled];
    disp = [disp; sol(46)];
end

clf;
elastic.draw(reshape(sol(1:50), [2,npoints]));
xlim([-2,12]);
ylim([-2,12]);
grid on;
pbaspect([1 1 1]);
saveas(gcf, strcat(filename, '_deformed.png'), 'png');

clf;
plot(-disp, -force);
xlabel('displacement');
ylabel('force');
xlim([0,1.0]);
ylim([0,20]);
grid on;
pbaspect([1 1 1]);
