% g, cm, sec

addpath('../two_dim_fea');

m = 32; n = 3; router = 5; rinner = 4; thickness = 1;
[points, triangles] = ring_object(m, n, router, rinner);
points = points + [ 0; router ];

npoints = size(points, 2);
ntriangles = size(triangles, 1);
ring = Body(npoints, points, ntriangles, triangles, thickness);

% 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 );
[ lambda_vis, mu_vis ] = Lame_constants( c, nu );
ring = ring.mechanical_parameters(density, lambda, mu);
ring = ring.viscous_parameters(lambda_vis, mu_vis);
ring = ring.calculate_stiffness_matrix;
ring = ring.calculate_damping_matrix;
ring = ring.calculate_inertia_matrix;

% g = 9.8 m/s^2 = 980 cm/s^2
grav = [0; -980];
ring = ring.calculate_gravitational_vector(grav);

mass = 1;

% k = 20000 g cm/s^2 /cm = 20 kg/s^2 = 20 N/m
for i=1:8
    springs(i) = Spring(2e+4, 0.0, rinner);
end
connect = [0:7]*(m/8)*n + 1;

A = zeros(2*npoints+2, 4); b = zeros(4,1);
k = n;   A(2*k-1, 1) = 1; A(2*k, 2) = 1;
k = n-1; A(2*k-1, 3) = 1;
k = n-2; A(2*k-1, 4) = 1;

un_init = zeros(2*npoints,1);
xc_init = [ 0; router ];
q_natural = [ un_init; xc_init ];
options = optimset('Display','iter', 'MaxFunEvals',3*10^5);

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

clf;
%ring.draw;
ring.draw_individual;
draw_mass_and_springs(ring, xc_init, connect);
xlim([-7,11]); ylim([-1,11]); xticks([-6:2:10]); pbaspect([1.8 1.2 1]);
grid on;
drawnow;
saveas(gcf, 'ring_and_springs/ring_and_springs_natural.png');

extensional_forces = [ 0; 0; 0; 0; 0; 0; 0; 0 ];
% N = Kg m/s^2 = 10^3 g 10^2 cm/s^2 = 10^5 g cm/s^2
internal_energy = @(q) internal_energy_params( q, ring, grav, mass, springs, connect, extensional_forces );
[q_deformed, Emin] = fmincon(internal_energy, q_natural, [], [], A', b, [], [], [], options);
un_deformed = q_deformed(1:2*npoints);
xc_deformed = q_deformed(2*npoints+1:2*npoints+2);
clf;
%ring.draw;
disps = reshape(un_deformed, [2, npoints]);
ring.draw_individual(disps);
draw_mass_and_springs(ring, xc_deformed, connect, disps);
xlim([-7,11]); ylim([-1,11]); xticks([-6:2:10]); pbaspect([1.8 1.2 1]);
grid on;
drawnow;
saveas(gcf, 'ring_and_springs/ring_and_springs_deformed_grav.png');
save('ring_and_springs_deformed_grav.mat', 'un_deformed', 'xc_deformed');

extensional_forces = [ -1e+5; 0; 0; 0; 0; 0; 0; 0 ]; filename = 'ring_and_springs/ring_and_springs_deformed_grav_A_5.png';
%extensional_forces = [ 0; -1e+5; 0; 0; 0; 0; 0; 0 ]; filename = 'ring_and_springs/ring_and_springs_deformed_grav_B_5.png';
%extensional_forces = [ 0; -1e+5; 0; 0; 0; -1e+5; 0; 0 ]; filename = 'ring_and_springs/ring_and_springs_deformed_grav_B_5_F_5.png';
%extensional_forces = [ 0; -1e+5; 0; 0; 0; -1e+5; -1e+5; 0 ]; filename = 'ring_and_springs/ring_and_springs_deformed_grav_B_5_F_5_G_5.png';
%extensional_forces = [ -2e+5; 0; 0; 0; 0; 0; 0; 0 ]; filename = 'ring_and_springs/ring_and_springs_deformed_grav_A_2_5.png';
%extensional_forces = [ -12e+4; 0; 0; 0; 0; 0; 0; 0 ]; filename = 'ring_and_springs/ring_and_springs_deformed_grav_A_12_4.png';
% N = Kg m/s^2 = 10^3 g 10^2 cm/s^2 = 10^5 g cm/s^2

internal_energy = @(q) internal_energy_params( q, ring, grav, mass, springs, connect, extensional_forces );
[q_deformed, Emin] = fmincon(internal_energy, q_natural, [], [], A', b, [], [], [], options);
un_deformed = q_deformed(1:2*npoints);
xc_deformed = q_deformed(2*npoints+1:2*npoints+2);
clf;
%ring.draw;
disps = reshape(un_deformed, [2, npoints]);
ring.draw_individual(disps);
draw_mass_and_springs(ring, xc_deformed, connect, disps);
xlim([-7,11]); ylim([-1,11]); xticks([-6:2:10]); pbaspect([1.8 1.2 1]);
grid on;
drawnow;
saveas(gcf, filename);

function energy = internal_energy_params( q, body, grav, mass, springs, connect, extensional_forces )
    persistent npoints xn nsprings;
    if isempty(npoints)
        npoints = body.numNodalPoints;
        for i=1:npoints
            xn = [ xn; body.NodalPoints(i).Coordinates ];
        end
        nsprings = size(springs, 2);
    end
    
    un = q(1:2*npoints);
    xc = q(2*npoints+1:2*npoints+2);    
    disps = reshape(un, [2,npoints]);
    rn = xn + un;
    
    energy = body.total_strain_potential_energy_Green_strain(disps) ...
           + body.total_gravitational_potential_energy(disps, grav);
    
    for i=1:nsprings
        j = connect(i);
        energy = energy + springs(i).internal_energy(xc, rn(2*j-1:2*j), extensional_forces(i));
    end
    energy = energy - mass*grav'*xc;
end
