%% clean up
clear
close all

%% load data
exampleCase = 'SCENARIO';
% Load Mesh
fid=fopen([exampleCase,'.grid'],'r');
num=fscanf(fid,'%i',2);
np=num(1);
nt=num(2);
ptmp=fscanf(fid,'%f',[2,np]);
ttmp=fscanf(fid,'%f',[3,nt]);
fclose(fid);
% Change for Matlab
nodes=ptmp';
triangles=ttmp';
clear ptmp ttmp
% Load Dirichlet boundary conditions
DirichletNodes = load([exampleCase,'.DirichletBC']);
% Load Neumann boundary conditions
NeumannSegments = [];
if exist([exampleCase,'.nonzeroNeumannBC'],'file')
    NeumannSegments = load([exampleCase,'.nonzeroNeumannBC']);
end
% Load volume forces
f = zeros(2*np,1);
if exist([exampleCase,'.f'],'file')
    f = [zeros(np,1);-load([exampleCase,'.f'])];
end
% Load elastic parameters
%params = load([exampleCase,'.parameters']);
params = ones(size(triangles,1),2);
if exist([exampleCase,'.lm'],'file')
    params = load([exampleCase,'.lm']);
end
% Set lambda and mu from file:
lambda = params(:,1);
mu = params(:,2);
% or set manually...

%% plot
% Plot mesh
subplot(1,2,1);
triplot(triangles,nodes(:,1),nodes(:,2));
axis equal;

%% mass and stiffness matrix assembly
[L,M] = StiffMassMat(nodes,triangles,lambda,mu);

%% compute rhs
rhs = M*f;
for i = 1:size(NeumannSegments,1)
    segmentIntegral = norm(nodes(NeumannSegments(i,1),:)-nodes(NeumannSegments(i,2),:),2)/2*NeumannSegments(i,3:4);
    rhs(NeumannSegments(i,1)) = rhs(NeumannSegments(i,1)) + segmentIntegral(1);
    rhs(NeumannSegments(i,2)) = rhs(NeumannSegments(i,2)) + segmentIntegral(1);
    rhs(np+NeumannSegments(i,1)) = rhs(np+NeumannSegments(i,1)) + segmentIntegral(2);
    rhs(np+NeumannSegments(i,2)) = rhs(np+NeumannSegments(i,2)) + segmentIntegral(2);
end

%% encode homogeneous Dirichlet conditions
for i = [0,np]
    L(i+DirichletNodes,:) = 0;
    L(:,i+DirichletNodes) = 0;
    L(i+DirichletNodes,i+DirichletNodes) = eye(length(DirichletNodes));
    rhs(i+DirichletNodes) = 0;
end

%% solve system
sol = L\rhs;

%% display result
displacedNodes = nodes + reshape(sol,size(nodes));
% amplify deformation for the "earth" scenario:
% displacedNodes = nodes + 20000000.0 .* reshape(sol,size(nodes));
subplot(1,2,2);
triplot(triangles,displacedNodes(:,1),displacedNodes(:,2));
hold on;
plot(displacedNodes(DirichletNodes,1),displacedNodes(DirichletNodes,2),'r.');
for i = 1:size(NeumannSegments,1)
    center = (displacedNodes(NeumannSegments(i,1),:)+displacedNodes(NeumannSegments(i,2),:))/2;
    arrowEnd = center + .1*NeumannSegments(i,3:4);
    coords = [center;arrowEnd]';
    axis equal;
    plot(coords(1,:),coords(2,:),'k');
end
