% This function uses the dislocation density and fundamental function to
% derive the stresses resulting from both the bilateral and corrective
% terms
function [Stress]=FindStresses(parameter,d)
%% Read parameter
N    = parameter.N;                            % Number of Gauss along slip zone
ID   = parameter.ID;                           % Logical vector identifying the equation to be removed
t    = parameter.t;                            % Collocation points on slip line
s    = parameter.s;                            % Integration points on slip line
W    = parameter.W;                            % Weights on slip line
om   = parameter.om;                           % Fundamental function
Phi  = parameter.Phi;                          % SIE solution vector
f    = parameter.f;                            % Coefficient of friction
lam1 = parameter.lam1;                         % Mode I eigenvalue
lam2 = parameter.lam2;                         % Mode II eigenvalue
g1   = parameter.g1;                           % Mode I shear multiplier
g2   = parameter.g2;                           % Mode II shear multiplier
%% Coordinates and displacements
Stress.x = [flipud(d./2.*(t+1)); linspace(d+1E-12,2*d,N)'];        % x-coordinate used throughout, normalised by d0
B    = Phi.*om';                        % Dislocation density
slip = -cumtrapz(d./2.*(s+1),B);        % Slip displacements
Stress.CSlip = [flipud(slip); zeros(length(Stress.x)-N,1)];             % Slip displacement vector matched to x-coordinate
%% Bilateral stresses
Stress.Nb =    Stress.x.^(lam1-1) +    Stress.x.^(lam2-1);          % Bilateral normal traction
Stress.Sb = g1*Stress.x.^(lam1-1) + g2*Stress.x.^(lam2-1);          % Bilateral shear traction
%% Corrective stresses
[Kxyy,Kxxy,~,~] = WedgeKernels(2.*Stress.x./d-1,s,parameter,d);       % Wedge kernels for dislocations on slip line observed along x-coordinate
Stress.Nd = pi.*W.*Kxyy*Phi;                                        % Normal corrective stress
Stress.Sd = pi.*W.*(Kxxy+1./(2.*Stress.x./d-1-s))*Phi;                     % Shear corrective stress
%% Corrected stresses
Stress.N = Stress.Nb + Stress.Nd;                                   % Corrected normal stress
Stress.S = Stress.Sb + Stress.Sd;                                   % Corrected shear stress
end
% Kernelfunctions 
%
% Kernels for cracks evolving from apex of 3/4 plane
% -> origin at the surface
% -> dislocation at y=0 (hard coded)

% Date: 08/10/20   
% 
function [Fxyy,Fxxy,Fyyy,Fyxy] = WedgeKernels(t,s,parameter,d)

% Parameter
N     = parameter.N;
theta = parameter.theta;
n     = parameter.n;
facl  = parameter.facl;
alpha = parameter.alpha;

%------------- Prepare numerical integration for free surfaces ------------
idx = 1:n;                                     % Index up to number of points
sn  = cos(pi.*2.*idx./(2*n+1));                % Integration points
tn  = cos(pi.*(2.*idx'-1)./(2*n+1));           % Collocation points
Wn  = 2.*(1-sn)./(2*n+1);                      % Weights

x  = d./2.*(t+1);
xi = d./2.*(s+1);

a  = xi;                                       % Radius of dislocation
l  = facl*d;                                   % Length of free surfaces

t = 1-2.^(1-2.*x./l);
s = 1-2.^(1-2.*a./l);

%--------------------------------------------------------------------------
%------------------------------ Calculation -------------------------------
%--------------------------------------------------------------------------

% Assemble matrix A
A = zeros(4*n,4*n);
[Krtt,Krrt,Kttt,Ktrt] = PolarKernels_norm(tn,alpha,sn,alpha);
A(1:n    ,1:4:end)    = pi.*Wn.*Krtt;
A(1:n    ,2:4:end)    = pi.*Wn.*Kttt;
A(n+1:2*n,1:4:end)    = pi.*Wn.*Krrt;
A(n+1:2*n,2:4:end)    = pi.*Wn.*Ktrt;
[Krtt,Krrt,Kttt,Ktrt] = PolarKernels_norm(tn,alpha,sn,-alpha);
A(1:n    ,3:4:end)    = pi.*Wn.*Krtt;
A(1:n    ,4:4:end)    = pi.*Wn.*Kttt;
A(n+1:2*n,3:4:end)    = pi.*Wn.*Krrt;
A(n+1:2*n,4:4:end)    = pi.*Wn.*Ktrt;
[Krtt,Krrt,Kttt,Ktrt] = PolarKernels_norm(tn,-alpha,sn,alpha);
A(2*n+1:3*n,1:4:end)  = pi.*Wn.*Krtt;
A(2*n+1:3*n,2:4:end)  = pi.*Wn.*Kttt;
A(3*n+1:4*n,1:4:end)  = pi.*Wn.*Krrt;
A(3*n+1:4*n,2:4:end)  = pi.*Wn.*Ktrt;
[Krtt,Krrt,Kttt,Ktrt] = PolarKernels_norm(tn,-alpha,sn,-alpha);
A(2*n+1:3*n,3:4:end)  = pi.*Wn.*Krtt;
A(2*n+1:3*n,4:4:end)  = pi.*Wn.*Kttt;
A(3*n+1:4*n,3:4:end)  = pi.*Wn.*Krrt;
A(3*n+1:4*n,4:4:end)  = pi.*Wn.*Ktrt;

% Right hand side for radial and tangential dislocation
Fr = zeros(4*n,N);
Ft = zeros(4*n,N);
[Krtt,Krrt,Kttt,Ktrt] = PolarKernels_norm(tn,alpha,s,theta);               % Kernel functions
Fr(1:n,:)             = -Krtt./(l./2./((1-s).*log(2)));                    % Normal traction at pos half-line
Fr(n+1:2*n,:)         = -Krrt./(l./2./((1-s).*log(2)));                    % Shear  traction at pos half-line
Ft(1:n,:)             = -Kttt./(l./2./((1-s).*log(2)));                    % Normal traction at pos half-line
Ft(n+1:2*n,:)         = -Ktrt./(l./2./((1-s).*log(2)));                    % Shear  traction at pos half-line
[Krtt,Krrt,Kttt,Ktrt] = PolarKernels_norm(tn,-alpha,s,theta);              % Kernel functions
Fr(2*n+1:3*n,:)       = -Krtt./(l./2./((1-s).*log(2)));                    % Normal traction at neg half-line
Fr(3*n+1:4*n,:)       = -Krrt./(l./2./((1-s).*log(2)));                    % Shear  traction at neg half-line
Ft(2*n+1:3*n,:)       = -Kttt./(l./2./((1-s).*log(2)));                    % Normal traction at neg half-line
Ft(3*n+1:4*n,:)       = -Ktrt./(l./2./((1-s).*log(2)));                    % Shear  traction at neg half-line


[Krttp,Krrtp,Ktttp,Ktrtp] = PolarKernels_norm(t,theta,sn,alpha);
[Krttn,Krrtn,Ktttn,Ktrtn] = PolarKernels_norm(t,theta,sn,-alpha);

%---------------------------- Radial direction ----------------------------
phi = A\Fr;

% Extract different distributions of dislocations
phirp = phi(1:4:end,:);           % Density at pos free surf in rad direction
phitp = phi(2:4:end,:);           % Density at pos free surf in tan direction
phirn = phi(3:4:end,:);           % Density at neg free surf in rad direction
phitn = phi(4:4:end,:);           % Density at neg free surf in tan direction

%-------------------------------- Kernels ---------------------------------
% Fxxx = pi*(Wn.*Krrrp*phirp + Wn.*Ktrrp*phitp + Wn.*Krrrn*phirn + Wn.*Ktrrn*phitn)*c/2;
Fxyy = pi*(Wn.*Krttp*phirp + Wn.*Ktttp*phitp + Wn.*Krttn*phirn + Wn.*Ktttn*phitn)*d/2;
Fxxy = pi*(Wn.*Krrtp*phirp + Wn.*Ktrtp*phitp + Wn.*Krrtn*phirn + Wn.*Ktrtn*phitn)*d/2;

%-------------------------- Tangential direction --------------------------
phi = A\Ft;

% Extract different distributions of dislocations
phirp = phi(1:4:end,:);           % Density at pos free surf in rad direction
phitp = phi(2:4:end,:);           % Density at pos free surf in tan direction
phirn = phi(3:4:end,:);           % Density at neg free surf in rad direction
phitn = phi(4:4:end,:);           % Density at neg free surf in tan direction

%-------------------------------- Kernels ---------------------------------
%-------------------------------- Kernels ---------------------------------
% Fyxx = pi*(Wn.*Krrrp*phirp + Wn.*Ktrrp*phitp + Wn.*Krrrn*phirn + Wn.*Ktrrn*phitn)*c/2;
Fyyy = pi*(Wn.*Krttp*phirp + Wn.*Ktttp*phitp + Wn.*Krttn*phirn + Wn.*Ktttn*phitn)*d/2;
Fyxy = pi*(Wn.*Krrtp*phirp + Wn.*Ktrtp*phitp + Wn.*Krrtn*phirn + Wn.*Ktrtn*phitn)*d/2;

end
% Kernelfunctions 
%
% Kernels for dislocations near a notch
% -> origin at the tip of the notch
% -> dislocation at r = a and theta = phi
% Checked against StressTrans_finalDan.m
%
% Date: 23/09/20   
% 
% function [Krrr,Krtt,Krrt,Ktrr,Kttt,Ktrt] = PolarKernels_norm(t,theta,s,phi)
function [Krtt,Krrt,Kttt,Ktrt] = PolarKernels_norm(t,theta,s,phi)


% Krrr = (s-1).^(-1).*(log((-2).*(s-1).^(-1)).^2-2.*cos(theta-1.*phi) ...
%   .*log((-2).*(s-1).^(-1)).*log((-2).*(t-1).^(-1))+log((-2).*( ...
%   t-1).^(-1)).^2).^(-2).*(2.*cos(theta-1.*phi).*log((-2).*(s-1) ...
%   .^(-1)).^3-1.*(2+cos(2.*(theta-1.*phi))).*log((-2).*(s-1).^(-1) ...
%   ).^2.*log((-2).*(t-1).^(-1))+log((-2).*(t-1).^(-1)).^3).* ...
%   sin(theta-1.*phi);
  
Krtt = (s-1).^(-1).*(log((-2).*(s-1).^(-1)).^2-2.*cos(theta-1.*phi) ...
  .*log((-2).*(s-1).^(-1)).*log((-2).*(t-1).^(-1))+log((-2).*( ...
  t-1).^(-1)).^2).^(-2).*((4+cos(2.*(theta-1.*phi))).*log((-2).*(( ...
  -1)+s).^(-1)).^2.*log((-2).*(t-1).^(-1))+log((-2).*(t-1).^( ...
  -1)).^3-2.*cos(theta-1.*phi).*log((-2).*(s-1).^(-1)).*(log((-2) ...
  .*(s-1).^(-1)).^2+2.*log((-2).*(t-1).^(-1)).^2)).*sin(theta-1 ...
  .*phi);
  
Krrt = (s-1).^(-1).*(log((-2).*(s-1).^(-1))-1.*cos(theta-phi).* ...
  log((-2).*(t-1).^(-1))).*(log((-2).*(s-1).^(-1)).^2-2.* ...
  cos(theta-phi).*log((-2).*(s-1).^(-1)).*log((-2).*(t-1).^( ...
  -1))+log((-2).*(t-1).^(-1)).^2).^(-2).*(cos(2.*(theta-phi)).* ...
  log((-2).*(s-1).^(-1)).^2+log((-2).*(t-1).^(-1)).*((-2).* ...
  cos(theta-phi).*log((-2).*(s-1).^(-1))+log((-2).*(t-1).^(-1) ...
  )));
  
% Ktrr = (1/2).*(s-1).^(-1).*(log((-2).*(s-1).^(-1)).^2-2.*cos(theta+( ...
%   -1).*phi).*log((-2).*(s-1).^(-1)).*log((-2).*(t-1).^(-1))+log( ...
%   (-2).*(t-1).^(-1)).^2).^(-2).*(2.*log((-2).*(s-1).^(-1)).^3+ ...
%   ((-7).*cos(theta-1.*phi)+cos(3.*(theta-1.*phi))).*log((-2).*(s-1).^( ...
%   -1)).^2.*log((-2).*(t-1).^(-1))+6.*log((-2).*(s-1).^(-1)).* ...
%   log((-2).*(t-1).^(-1)).^2-2.*cos(theta-1.*phi).*log((-2).*((-1) ...
%   +t).^(-1)).^3);
  
Kttt = (1/2).*(s-1).^(-1).*(log((-2).*(s-1).^(-1)).^2-2.*cos(theta+( ...
  -1).*phi).*log((-2).*(s-1).^(-1)).*log((-2).*(t-1).^(-1))+log( ...
  (-2).*(t-1).^(-1)).^2).^(-2).*(2.*log((-2).*(s-1).^(-1)).^3+ ...
  (-1).*(5.*cos(theta-1.*phi)+cos(3.*(theta-1.*phi))).*log((-2).*(s-1) ...
  .^(-1)).^2.*log((-2).*(t-1).^(-1))+2.*(1+2.*cos(2.*(theta-1.*phi)) ...
  ).*log((-2).*(s-1).^(-1)).*log((-2).*(t-1).^(-1)).^2-2.* ...
  cos(theta-1.*phi).*log((-2).*(t-1).^(-1)).^3);
  
Ktrt = (-1).*(s-1).^(-1).*log((-2).*(t-1).^(-1)).*(log((-2).*((-1)+ ...
  s).^(-1)).^2-2.*cos(theta-1.*phi).*log((-2).*(s-1).^(-1)).*log( ...
  (-2).*(t-1).^(-1))+log((-2).*(t-1).^(-1)).^2).^(-2).*(cos( ...
  2.*(theta-1.*phi)).*log((-2).*(s-1).^(-1)).^2+log((-2).*(t-1) ...
  .^(-1)).*((-2).*cos(theta-1.*phi).*log((-2).*(s-1).^(-1))+log((-2) ...
  .*(t-1).^(-1)))).*sin(theta-1.*phi);

end