% g, cm, sec
% sub1 : hard   sub2 : soft

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 );
[ lambda2, mu2 ] = Lame_constants( 0.1*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);

sub_hard = [ 1:8, 17:24 ];  filename = 'push_test_layered/push_test_layered_1_8';
%sub_hard = [ 9:16, 25:32 ];  filename = 'push_test_layered/push_test_layered_9_16';
%sub_hard = [ 1:4:29, 3:4:31 ];  filename = 'push_test_layered/push_test_layered_1_4_29';
%sub_hard = [ 2:4:30, 4:4:32 ];  filename = 'push_test_layered/push_test_layered_2_4_30';
%sub_hard = [ 1:32 ];  filename = 'push_test_layered/push_test_layered_1_32';
%sub_hard = [ ];  filename = 'push_test_layered/push_test_layered_';

sub_soft = setdiff(1:32, sub_hard);

elastic = elastic.define_subregion(sub_hard);
elastic = elastic.subregion_mechanical_parameters(density, lambda, mu);
elastic = elastic.subregion_color( [0.85 0.85 0.85] );

elastic = elastic.define_subregion(sub_soft);
elastic = elastic.subregion_mechanical_parameters(density, lambda2, mu2);
elastic = elastic.subregion_color( [0.95 0.95 0.95] );

elastic = elastic.calculate_stiffness_matrix;
K = elastic.Stiffness_Matrix;

A = elastic.constraint_matrix([1:5,21:25]);
zr = [0;0]; ex = [1;0]; ey = [0;1];
d = [zr; zr; zr; zr; zr; ey; ey; ey; ey; ey];

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

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

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

for fpushscaled = 0.0:1.0:20.0
    fpush = fpushscaled * 1e+5;
    mat = [ K, zeros(2*npoints,1), -A;
            zeros(1,2*npoints), 0, -d';
            -A', -d, zeros(20,20) ];
    vec = [ zeros(2*npoints,1); fpush; zeros(20,1) ];
    sol = mat \ vec;

    force = [force; fpushscaled];
    disp = [disp; sol(2*npoints+1)];
end

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

save(strcat(filename, '.mat'), 'force', 'disp');
