clear

load 'test_data.mat'

%% Mesh Details
xmax = 0.5; % Plate width
elmsiz = 0.005; % Grid spacing
gridspace = xmax / elmsiz;
xnodes = (0:elmsiz:xmax);
ynodes = xnodes;

% Boundary elements not included in this calc (2:gridspace)
% Include boundary elements (1:gridspace+1)
[Xall,Yall] = meshgrid(xnodes,ynodes);
[X,Y] = meshgrid(xnodes(1:gridspace+1),ynodes(1:gridspace+1));
X=X-(xmax/2); Y=Y-(xmax/2);

%% Bar co-ordinates
xs = [.000 .025 .050 .075 .100 .125 .150 .175 .200 .225]';
xcoord = xs(1:5,1);

%% Initialise pressure interpolation matrix
p = zeros(size(X,1),size(X,2),size(t,1));


%% Time-shift all data to max values  
for dir=1:4
    [m(:,dir),inx(:,dir)]=max(stressxy(:,:,dir));
    shiftedxy(:,1,dir)=stressxy(:,1,dir);
    for b=2:5
        shift=inx(b,dir)-inx(1,dir);
        for i=1:size(stressxy(:,:,dir),1)-shift                
            shiftedxy(i,b,dir)=stressxy(i+shift,b,dir);
        end
    end
end
shiftedxy(:,:,5)=shiftedxy(:,:,1); %allow easy interplolation between first and last data sets
m(:,5)=m(:,1); inx(:,5)=inx(:,1);

%% Calc surf plot axes
pmin=min(min(min(stressxy)))*(elmsiz^2);
pmax=max(max(max(stressxy)))*(elmsiz^2);
d=zeros(size(X,1),size(X,2));%set data to plot

sump=0;

%% Minimum stress in Hop Bar
lowpass=2e5;

%% Generate 3D interpolated pressure array
for i = 1:size(t,1)
    i
    for ii = 1:size(X,1);
        for jj = 1:size(X,1);
            [theta,vd]=cart2pol(X(ii,jj),Y(ii,jj)); % Calculate polar coordinate of current point
            if vd<max(max(xcoord))
                %Proportional variation with theta
                rot=(theta/(pi()/2))+3; % Calculate which 2 matrices to use - floor & ceil.
                if ceil(rot)-floor(rot)==0
                    pp=interp1(xcoord(:,1),shiftedxy(i,:,rot),vd,'linear');
                else
                    pp=(interp1(xcoord(:,1),shiftedxy(i,:,floor(rot)),vd,'linear')*(ceil(rot)-rot))+...
                    (interp1(xcoord(:,1),shiftedxy(i,:,ceil(rot)),vd,'linear')*(rot-floor(rot)));
                end
                d(ii,jj)=1;
                p(ii,jj,i) = pp*(pp>lowpass||pp<-lowpass)*(elmsiz^2);
                sump=sump+(p(ii,jj,i)*dt);
            end
        end
    end
end

%% Timeshift data based on location
pt=p;
%p=zeros(size(pt));% needed for lower filtered data
p = zeros(size(X,1),size(X,2),size(t,1)); % needed for lower filtered data
for ii = 1:size(X,1);
    for jj = 1:size(X,1);
        %Calculate LOAD based on distance to centre
        [theta,vd]=cart2pol(X(ii,jj),Y(ii,jj)); % Calculate polar coordinate of current point
        rot=(theta/(pi()/2))+3; % Calculate which 2 matrices to use - floor & ceil.
            if ceil(rot)-floor(rot)==0
                reshift=max(0,round(interp1(xcoord(:,1),inx(:,rot),vd,'cubic','extrap'))-inx(1,1));
            else
                reshift=max(0,round(interp1(xcoord(:,1),inx(:,floor(rot)),vd,'cubic','extrap')*(ceil(rot)-rot)+...
                interp1(xcoord(:,1),inx(:,ceil(rot)),vd,'cubic','extrap')*(rot-floor(rot)))-inx(1,1));
            end
            for i=1:size(t(:,1))-reshift
                i
                if i>reshift
                    p(ii,jj,i+reshift)=pt(ii,jj,i);
                end
            end
    end
end

%% Display full plate 
for i = 1:size(t(:,1))
    %Display step
    i
    %Plot loads based on symmetric boundary conditions
    figure(1)
    surf(p(:,:,i))
    axis([0 gridspace 0 gridspace pmin pmax pmin pmax])
    view([1,1,6]);
    pause(0.05)
end
