function md=diagnostic(md)
%DIAGNOSTIC - compute the velocity field of a 2d model using MacAyeal solution
%
%   this routine solves the problem using MacAyeal's model. It calculates the velocity
%   field corresponding to the parameters and the geometry given by the model md
%
%   Usage:
%      md=diagnostic(md)

%First check we do have the correct argument number
if ((nargin~=1) || (nargout~=1)),
	macayealdiagnosticusage();
	error('macayealdiagnostic error message: incorrect number of input and output arguments');
end

if ~isa(md,'model'),
	macayealdiagnosticusage();
	error('macayealdiagnostic error message: input argument is not a @model object');
end

%start timing
t1=clock;

%Transfer model fields into matlab variables
x=md.x;
y=md.y;
index=md.elements;index=sort(index,2); %necessary 
nods=md.numberofgrids;
nel=md.numberofelements;
z_thick=md.thickness;
z_surf=md.surface;
z_bed=md.bed;
z_thick_bar=(z_thick(index(:,1))+z_thick(index(:,2))+z_thick(index(:,3)))/3;
rho_ice=md.rho_ice;
rho_water=md.rho_water;
g=md.g;
viscosity_overshoot=md.viscosity_overshoot;
index_icefront=md.segmentonneumann_diag; index_icefront=index_icefront(:,1:2); %we strip the last column, which holds the element number for the boundary segment
nodes_on_boundary=md.gridonboundary;
nodes_on_icefront=zeros(nods,1); nodes_on_icefront(index_icefront)=1;
node_on_dirichlet=md.gridondirichlet_diag;
nodes_on_icesheet=md.gridonicesheet;
element_on_icesheet=md.elementonicesheet;
qcoeff=md.q;
pcoeff=md.p;
drag_type=md.drag_type;
drag=md.drag;
criterion_rel=md.eps_rel;
criterion_abs=md.eps_abs;
yts=md.yts;
B=md.B; B_bar=(B(index(:,1))+B(index(:,2))+B(index(:,3)))/3;
glen_coeff=md.n;

%initialize velocities if any
if (~isnan(md.vx) & ~isnan(md.vy)),
	u=md.vx/yts; v=md.vy/yts;
	velocity_is_present=1;
else
	velocity_is_present=0;
end

%average of p and q over the grids (size nel->nods)
pcoeff_grid=zeros(nods,1);
qcoeff_grid=zeros(nods,1);
for i=1:nods
    %1: find the elements that contain the grid i
    neighbors_gridi=[];
    for j=1:3
        neighbors_gridi=[neighbors_gridi find(index(:,j)==i)'];
    end
    numberofneighbors_gridi=length(neighbors_gridi);
    %2 retrieve the value of p and q over each of these elements. The average is
    %plugged into dbx_grid
    qcoeff_grid(i)=sum(qcoeff(neighbors_gridi))/numberofneighbors_gridi;
    pcoeff_grid(i)=sum(pcoeff(neighbors_gridi))/numberofneighbors_gridi;
end

%Build length_icefront and normal_icefront:
[length_icefront,normal_icefront]=buildicefrontnormal(x,y,index_icefront);

%Start building areas
aire=zeros(md.numberofelements,1);

for n=1:nel
	aire(n)=1/2 * det([1 1 1;x(index(n,:))';y(index(n,:))']);
end

aire=abs(aire); % if index is sorted from its original value, then aire could be negative

alpha=zeros(nel,3);
beta=zeros(nel,3);
gamma=zeros(nel,3);

for n=1:nel
	X=inv([x(index(n,:)) y(index(n,:)) ones(3,1)]);
	alpha(n,:)=X(1,:);
	beta(n,:)=X(2,:);
	gamma(n,:)=X(3,:);
end

clear X


%Do once and for all the initial computation of matrix-locations:
row_location=zeros(nel*3*3,1);
col_location=zeros(nel*3*3,1);
row_location_AD=zeros(nel*6,1);
col_location_AD=zeros(nel*6,1);

count=-nel+1;

for i=1:3
	for j=1:3
			count=count+nel;
			row_location(count:count+nel-1)=index(:,i);
			col_location(count:count+nel-1)=index(:,j);
	end
end

count=-nel+1;

for i=1:3
	for j=i:3
			count=count+nel;
			row_location_AD(count:count+nel-1)=index(:,i);
			col_location_AD(count:count+nel-1)=index(:,j);
	end
end

permanent_pieces_of_A=zeros(nel*6,1);
permanent_pieces_of_B=zeros(nel*3*3,1);
permanent_pieces_of_C=zeros(nel*3*3,1);
permanent_pieces_of_D=zeros(nel*6,1);

count=-nel+1;

for i=1:3
	for j=i:3
			count=count+nel;
			permanent_pieces_of_A(count:count+nel-1)= z_thick_bar .* aire ...
				.*(2*alpha(:,i).*alpha(:,j) + 1/2*beta(:,i).*beta(:,j));				
				% This loop structure works only when index is sorted
			permanent_pieces_of_D(count:count+nel-1)= z_thick_bar .* aire ...
				.*(2*beta(:,i).*beta(:,j) + 1/2*alpha(:,i).*alpha(:,j));
	end
end

count=-nel+1;

for i=1:3
	for j=1:3
			count=count+nel;

			permanent_pieces_of_B(count:count+nel-1)= z_thick_bar .* aire ...
			.*(alpha(:,j).*beta(:,i) + 1/2*beta(:,j).*alpha(:,i));

			permanent_pieces_of_C(count:count+nel-1)= z_thick_bar .* aire ...
			.*(beta(:,j).*alpha(:,i) + 1/2*alpha(:,j).*beta(:,i));
	end
end

% Step 3 -- Set up right-hand side of the problem. 

% (Note to myself: to avoid vector dependency problem, yet still vectorize,
% I treat the Rhs vector as a sparse matrix

Rhs_x=zeros(nel*27,1);
Rhs_y=zeros(nel*27,1);
Rhs_y=ones(nel*27,1);
row_rhs=zeros(nel*27,1);

count=-nel+1;
for i=1:3
	for n=1:3
		for m=1:3
			count=count+nel;

			Rhs_x(count:count+nel-1)= -rho_ice * g  * ...
			 z_thick(index(:,n)).*z_surf(index(:,m)) ...
			.* aire(:) .* alpha(:,m) * ( (n==i)/6 + (n~=i)/12 );

			Rhs_y(count:count+nel-1)= -rho_ice * g  * ...
			 z_thick(index(:,n)).*z_surf(index(:,m)) ...
			.* aire(:) .* beta(:,m) .* ( (n==i)/6 + (n~=i)/12 );

			row_rhs(count:count+nel-1)=index(:,i);

		end
	end
end

Rhs=full([sparse(row_rhs,ones(nel*27,1),Rhs_x,nods,1)
   sparse(row_rhs,ones(nel*27,1),Rhs_y,nods,1)]);

	for k=1:2
		for l=1:2
			for j=1:2
				Rhs(index_icefront(:,k))=Rhs(index_icefront(:,k)) + ...
					(rho_ice*g/2*z_thick(index_icefront(:,l)).*z_thick(index_icefront(:,j)) ...
					+ rho_water*g/2*(md.gridoniceshelf(index_icefront(:,k))) ...
					.*(-min(0,z_bed(index_icefront(:,l))).*min(0,z_bed(index_icefront(:,j))) ...
					+min(0,z_thick(index_icefront(:,l))+z_bed(index_icefront(:,l)))...
					.*min(0,z_thick(index_icefront(:,j))+z_bed(index_icefront(:,j)))))...
					.*normal_icefront(:,1).*length_icefront(:) ...
					/(4*(l==k & j==k) + 12*(l~=k | j~=k) );

				Rhs(index_icefront(:,k)+nods)=Rhs(index_icefront(:,k)+nods) + ...
					(rho_ice*g/2*z_thick(index_icefront(:,l)).*z_thick(index_icefront(:,j)) ...
					+ rho_water*g/2*(md.gridoniceshelf(index_icefront(:,k))) ...
					.*(-min(0,z_bed(index_icefront(:,l))).*min(0,z_bed(index_icefront(:,j))) ...
					+min(0,z_thick(index_icefront(:,l))+z_bed(index_icefront(:,l)))...
					.*min(0,z_thick(index_icefront(:,j))+z_bed(index_icefront(:,j)))))...
					.*normal_icefront(:,2).*length_icefront(:) ...
					/(4*(l==k & j==k) + 12*(l~=k | j~=k) );
		end
	end
end

clear Rhs_x Rhs_y row_rhs

% Step 4 -- Create the parsing matrix to "wring out"
%           the known boundary conditions

% kinematic condition (u,v=0) specified at non-ice-front boundaries;
num_specified= 2*sum(node_on_dirichlet);
row=zeros(2*nods-num_specified,1);
col=zeros(2*nods-num_specified,1);
value=zeros(2*nods-num_specified,1);
count=0;
for n=1:nods
   if(~node_on_dirichlet(n))
   		count=count+1;
		row(count)=count;
		col(count)=n;
		value(count)=1;
	end
end
for n=1:nods
   if (~node_on_dirichlet(n))
         count=count+1;
		row(count)=count;
		col(count)=nods+n;
		value(count)=1;
	end
end

P=sparse(row,col,value,2*nods-num_specified,2*nods);

specified_velocity=zeros(2*nods,1);
pos=find(node_on_dirichlet);
specified_velocity(pos)=md.dirichletvalues_diag(pos,1)/md.yts;
specified_velocity(nods+pos)=md.dirichletvalues_diag(pos,2)/md.yts;


% ######## an attention grabbing break ########
%      Iterate on flow law till converged

converged_yet=0;
loop=0;

u_old=zeros(nods,1);
v_old=zeros(nods,1);
convergence_count=1;

while (~converged_yet)

	if (loop>(100))
		warning('Maximum Viscosity Iterations  Reached.')
		break;
	end;
	loop=loop+1;

	%Compute viscosity (as in ICE and CIELO)
	if (convergence_count==1),
		%Initialize viscosity
		if ~velocity_is_present;
			nu_bar=viscosity(index,nel,alpha,beta,{},{},B_bar,glen_coeff);
		else
			nu_bar=viscosity(index,nel,alpha,beta,u,v,B_bar,glen_coeff);
			nu_bar(find(nu_bar<=0))=-nu_bar(find(nu_bar<=0));
			u_old=u; v_old=v;
		end
	elseif (convergence_count==2),
		nu_bar_oldvalue=nu_bar;
		nu_bar=viscosity(index,nel,alpha,beta,u,v,B_bar,glen_coeff);
		nu_bar=nu_bar + viscosity_overshoot*(nu_bar-nu_bar_oldvalue);
		nu_bar(find(nu_bar<=0))=-nu_bar(find(nu_bar<=0));
		u_old=u; v_old=v;
	else
		nu_bar_oldvalue=viscosity(index,nel,alpha,beta,u_old,v_old,B_bar,glen_coeff);
		nu_bar=viscosity(index,nel,alpha,beta,u,v,B_bar,glen_coeff);
		nu_bar=nu_bar + viscosity_overshoot*(nu_bar-nu_bar_oldvalue);
		nu_bar(find(nu_bar<=0))=-nu_bar(find(nu_bar<=0));
		u_old=u; v_old=v;
	end

	% Step 4 -- Set up stress-balance matrix  (whos solution is the velocity
	% field):

	matrix_value_A=zeros(nel*6,1);
	matrix_value_B=zeros(nel*3*3,1);
	matrix_value_C=zeros(nel*3*3,1);
	matrix_value_D=zeros(nel*6,1);

	count=-nel+1;

	for i=1:3
		for j=i:3
				count=count+nel;

				matrix_value_A(count:count+nel-1)=nu_bar .* ...
				  permanent_pieces_of_A(count:count+nel-1);

				matrix_value_D(count:count+nel-1)=nu_bar .* ...
				  permanent_pieces_of_D(count:count+nel-1);
		end
	end

	count=-nel+1;

	for i=1:3
		for j=1:3
				count=count+nel;

				matrix_value_B(count:count+nel-1)=nu_bar .* ...
				  permanent_pieces_of_B(count:count+nel-1);

				matrix_value_C(count:count+nel-1)=nu_bar .* ...
				  permanent_pieces_of_C(count:count+nel-1);
		end
	end

	A=sparse(row_location_AD,col_location_AD,matrix_value_A,nods,nods);
	A=A+triu(A,1)';
	B=sparse(row_location,col_location,matrix_value_B,nods,nods);
	C=sparse(row_location,col_location,matrix_value_C,nods,nods);
	D=sparse(row_location_AD,col_location_AD,matrix_value_D,nods,nods);
	D=D+triu(D,1)';

	F=[A C
	B D];

	%Now, take care of the basal friction if there is any: to make things easier, we translate u = k *Neff^(-q)* sigma^p into 
	% sigma= drag^2 * Neff ^(r) * u^s with r=q/p and s=1/p : */
	 
	if  (drag_type==2), 
		%compute coeffs: 
		rcoeff=qcoeff_grid./pcoeff_grid;
		scoeff=1./pcoeff_grid;
		
		%initialization of basal drag stiffness
		Dragoperator=spalloc(2*nods,2*nods,0);

		if loop~=1,
			
			%retrieve the velocity magnitude
			velocity_mag=sqrt(solution(1:nods).^2+solution(nods+1:2*nods).^2);
		
			%Computation of the effective pressure
			Neff=g*(rho_ice*z_thick+rho_water*z_bed);

			%If effective pressure becomes negative, sliding becomes unstable (Paterson 4th edition p 148). This is because 
			%the water pressure is so high, the ice sheet elevates over its ice bumps and slides. But the limit behaviour 
			%for this should be an ice shelf sliding (no basal drag). Therefore, for any effective pressure Neff < 0, we should 
			%replace it by Neff=0 (ie, equival it to an ice shelf)*/
			pos=find(Neff<0);
			Neff(pos)=0;
			
			%Basal drag coefficient: Tau_x=-alpha^2 u, Tau_y=-alpha^2 v (See
			%MacAyeal)
			alpha2=(drag.^2).*(Neff.^rcoeff).*(velocity_mag.^(scoeff-1));

			%stiffness due to basal drag 
			%initialization
			count=0;
			value=zeros(nel,27);
			row=zeros(nel,27);
			col=zeros(nel,27);

			for m=1:3
				for k=1:3
				for l=1:3
					if ( (m==k) + (m==l) + (l==k) )==3
					fac=1/10;
					elseif ( (m==k) + (m==l) + (l==k) )==1
					fac=1/30;
					else
					fac=1/60;
					end

					count=count+1;
					row(:,count)=index(:,m);
					col(:,count)=index(:,k);
					value(:,count)=fac*aire(:).* ...
					(alpha2(index(:,l)));%.*element_on_icesheet(index(:,l));

				end
				end
			end

			Dragoperator=[sparse(row,col,value,nods,nods) spalloc(nods,nods,0)
				spalloc(nods,nods,0) sparse(row,col,value,nods,nods)];
			end

		F=F+Dragoperator;  %plug into the global stiffness matrix
		
	end

	Rhs_parsed=P*(Rhs - F*specified_velocity);
	F=P*F*P';

	% Step 5 -- Solve the problem:

	% Digression: if need be clear up some memory:

	clear   matrix_value_A ...
	matrix_value_B matrix_value_C matrix_value_D A B C D 

	% We can use either the LU or the Cholesky decomposition, but the
	% Cholesky decomposition is twice as efficient as LU for symmetric
	% definite positive matrix
	if md.debug,
		disp(sprintf('%s%g','      condition number of stiffness matrix: ',condest(F)));
	end
	solution=Solver(F,Rhs_parsed,md.solver_type);

	%Add spcs to the calculated solution
	solution=P'*solution + specified_velocity;

	%Recover solution vector
	u=solution(1:nods);
	v=solution(nods+1:2*nods);

	%Test for direct shooting convergence
	if (convergence_count>1 | velocity_is_present),

		ug=[u_old;v_old];
		nug=norm(ug,2);
		dug=[u; v]-[u_old; v_old];
		ndug=norm(dug,2);
		relative_change=ndug/nug;

		%Figure out if viscosity converged
		if relative_change<criterion_rel,
			if md.debug, disp(sprintf('%s %g %s %g %s','   Convergence criterion: norm(du)/norm(u)=',relative_change,' < ',criterion_rel,' m/yr'));end
			converged_yet=1;
		else
			if md.debug, disp(sprintf('%s %g %s %g %s','   Convergence criterion: norm(du)/norm(u)=',relative_change,' > ',criterion_rel,' m/yr'));end
			converged_yet=0;
		end

		if ~isnan(criterion_abs),
			change=max(abs(dug))*yts;
			if change<criterion_abs
				if md.debug, disp(sprintf('%s %g %s %g','   Convergence criterion: max(du)=',change,' < ',criterion_abs));end
			else
				if md.debug, disp(sprintf('%s %g %s %g','   Convergence criterion: max(du)=',change,' > ',criterion_abs));end
				converged_yet=0;
			end
		end
	else 
		converged_yet=0;
	end

	convergence_count=convergence_count+1;
		
end % This end statement terminates the "while" command way above

%Load results onto md:
md.vx=u*yts;
md.vy=v*yts;
md.vel=sqrt(md.vx.^2+md.vy.^2);

%stop timing
t2=clock;

disp(sprintf('\n%s\n',['solution converged in ' num2str(etime(t2,t1)) ' seconds']));

function macayealdiagnosticusage();
disp('md=macayealdiagnostic(md)');
disp('   where md is a structure of class @model');