function md=control(md)
%CONTROL - launch a control method using MacAyeal solution
%
%   the routine is used for a control method. It determines the most adapted viscosity
%   field so that the calculated velocity field is as close as possible to an observed velocity field
%
%   Usage:
%      md=control(md)

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

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

%Check that control is done on flow law, and not drag (not supported yet):
%if ~strcmp(md.control_type,'B')
%	error('macayealcontrol error message: only ''B'' inversion supported yet');
%end

%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;
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_dirichlet=md.gridondirichlet_diag;
nodes_on_icefront=zeros(nods,1); nodes_on_icefront(index_icefront)=1;
nodes_on_iceshelf=md.gridoniceshelf;

criterion=md.eps_rel;
yts=md.yts;
tolx=md.tolx; 
maxiter=md.maxiter(1);
if strcmp(md.control_type,'B')
	B_ini=md.B;
	B_to_p=10^-3*yts^(-1/3);
else
	drag_coeff_ini=md.drag;
end
glen_coeff=md.n;
vx_obs=md.vx_obs/md.yts; %From m/a to m/s
vy_obs=md.vy_obs/md.yts;
nsteps=md.nsteps;

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

%Building shape functions and derivative operators
[alpha, beta, gamma, area]=shape(index,x,y,nel,nods);

[matrix_bar, matrix_xbar, matrix_ybar]=...
   bar_maker(nel,nods,index,alpha,beta);

%initialize some data
create_el2nod_matrices
old_direction=zeros(nods,1);

%setup some fake distributions.
z_thick_bar=matrix_bar*z_thick;
z_surf_bar=matrix_bar*z_surf;
z_surf_xbar=matrix_xbar*z_surf;
z_surf_ybar=matrix_ybar*z_surf;
z_surf_x=el2nod\(el2nodRhs*z_surf_xbar);
z_surf_y=el2nod\(el2nodRhs*z_surf_ybar);
z_bed_bar=matrix_bar*z_bed;
z_bed_xbar=matrix_xbar*z_bed;
z_bed_ybar=matrix_ybar*z_bed;

%data_elements and data_nodes are the elements and nodes where 
%we have observations
data_elements=[1:nel]';
data_nodes=ones(nods,1);

%initialize misfit between model velocity and observed velocity
J=zeros(nsteps,1);

%Weighting: one areas where we don't want the control method to optimize 
%the misfit, we can set weighting to 0.
weighting=ones(nods,1);

%optimization parameters for the matlab functoin fminbnd
options=optimset('fminbnd');
options=optimset(options,'Display','iter');
options=optimset(options,'MaxFunEvals',maxiter);
options=optimset(options,'MaxIter',100);
options=optimset(options,'TolX',tolx);

%build useful matrices.
setup_control

%setup initial drag paramter distribution to startup the optimization.
drag_type=md.drag_type;
qcoeff=md.q;
pcoeff=md.p;

if strcmp(md.control_type,'B')
	%setup initial flow law paramter distribution to startup the optimization.
	B_bar_ini=(B_ini(index(:,1))+B_ini(index(:,2))+B_ini(index(:,3)))/3; 
	B=B_ini;  %variables used in optimziation are B and B_bar. 
	B_bar=B_bar_ini;
else
	B_bar=(md.B(index(:,1))+md.B(index(:,2))+md.B(index(:,3)))/3;
	if (drag_type~=2), error('md.drag_type must be 2 for control methods'); end
		drag_coeff_bar_ini=(drag_coeff_ini(index(:,1))+drag_coeff_ini(index(:,2))+drag_coeff_ini(index(:,3)))/3; 
		drag_coeff=drag_coeff_ini;  %variables used in optimziation are drag_coeff and drag_coeff_bar. 
		drag_coeff_bar=drag_coeff_bar_ini;
end

%check that the model is not a pure ice shelf (no friction on ice shelves)
if strcmp(md.control_type,'drag') & (length(find(drag_coeff))==0),
    disp(sprintf('\n      No drag pure for ice shelves => STOP'))
    return
end

%nu_bar=10^14*ones(nel,1);  %initial element viscosity distribution.
nu_bar=viscosity(index,nel,alpha,beta,[],[],B_bar,glen_coeff);

%%%AK velfinder;    %forward model that determines first velocity field used 
%to start optimization.
disp('calculating the velocity with the initial parameters')
c_velfinder;

if strcmp(md.control_type,'B'),
    for niteration=1:nsteps,

       disp(['   Step #' num2str(niteration) ' on ' num2str(nsteps)]);

       % Compute search direction -
       [u,v,adjointu,adjointv,direction]= ...      
          grad_J_flow(Rhs,S,F,P,P0,area,specified_velocity,nods,vx_obs,vy_obs,...
          index,x,y,nel,rho_ice,g,weighting,alpha,beta,z_thick_bar,B_bar);

		 if md.plot==1,
			 if niteration==1
				 scrsz = get(0,'ScreenSize');
				 figure('Position',[scrsz(3)/3 scrsz(4)*1.8/3 scrsz(3)/3 scrsz(4)/3])
			 end
			 plotmodel(md,'data',sqrt(u.^2+v.^2)*yts,'title','Modeled velocity',...
				 'data',sqrt(adjointu.^2+adjointv.^2)*yts,'title','Adjoint vectors','colorbar#all', 'on',...
				 'data',sqrt(vx_obs.^2+vy_obs.^2)*yts,'title','Observed velocity',...
				 'data',100*abs(sqrt(vx_obs.^2+vy_obs.^2)-sqrt(u.^2+v.^2))./sqrt(vx_obs.^2+vy_obs.^2),'title','Relative misfit','caxis#4',[0 100],'figure',1);drawnow;
		 end

       %Keep track of u and v to use in objectivefunction_C: 
       u_objective=u;
       v_objective=v;

       %Orthogonalize direction
       direction=real(direction);
       direction=direction/sqrt(direction'*direction);

       % rough orthagonalization
       direction=direction-(direction'*old_direction)*old_direction; 
       old_direction=direction;

		 %visualize direction 
		 if md.plot==1,
			 if niteration==1
				 scrsz = get(0,'ScreenSize');
				 figure('Position',[10 scrsz(4)*1/3 scrsz(3)/3 scrsz(4)/3])
			 end              
			 plotmodel(md,'data',direction,'title','Orthogonal direction','figure',2);drawnow;
		 end


       % normalize direction to 10^7, so that when variations on B are computed
       %they will be significant. 
       if abs(max(direction))>abs(min(direction))
          direction=10^7*direction/abs(max(direction));
       else
          direction=10^7*direction/abs(min(direction));
       end

       %during optimization, bounds on B variations can vary. Here, they are less
       %strict in the first iteration. 
       if niteration<=2,
          upperbound=20;
          lowerbound=0;
       else
          upperbound=10;
          lowerbound=0;
       end

       %search the multiplicative constant to the direction, that will 
       %be used to modify B.

       search_constant=fminbnd('objectivefunction_C_flow',lowerbound,upperbound, ...
          options,B,glen_coeff,direction,Rhs,S,F,P,specified_velocity,nods,nel,vx_obs, ...
          vy_obs,u_objective, v_objective, index,alpha, beta, area,weighting,rowD,colD,rowDshort,colDshort,valueuu,valuevv,valueuv,matrix_bar,criterion);


        %update value of B
        B_old=B;
        B_new=B+search_constant*direction;
        B=B_new;
        pos=find(B<0);B(pos)=-B(pos);

       %Average B over elements: 
       B_bar=(B(index(:,1))+B(index(:,2))+B(index(:,3)))/3;

       %visualize new distribution. 
		 if md.plot==1,
			 if niteration==1
				 scrsz = get(0,'ScreenSize');
				 figure('Position',[scrsz(3)*1.97/3 scrsz(4)*1/3 scrsz(3)/3 scrsz(4)/3])
			 end
			 plotmodel(md,'data',B*B_to_p,'title',['B at iteration ' num2str(niteration)],'caxis',[200 900],'colorbar','on','figure',3);drawnow;
		 end

       %evaluate new misfit. 
       %@@@AK load F_file   %this file was created in objectivefunction_C, and is reloaded
       %here to win some computation time.
       load F_file

       J(niteration)=objectivefunction_C_flow(0,B,glen_coeff,direction,Rhs,S,F,P,specified_velocity,nods,nel,vx_obs, ...
          vy_obs,u_objective, v_objective, index,alpha, beta, area,weighting,rowD,colD,rowDshort,colDshort,valueuu,valuevv,valueuv,matrix_bar,criterion);
      disp(J(niteration));

      %do a backup every 5 iterations
      if(mod(niteration,5)==0),
          save temporary_control_results_flow B B_bar u v J direction
      end
    end

	 %Load results onto md:
	 md.results.control.step=1;
	 md.results.control.time=0;
    md.results.control.J=J;
	 md.results.control.parameter=B;
	 md.results.control.vx=u;
	 md.results.control.vy=v;
	 md.results.control.vel=sqrt(u.^2+v.^2);

elseif strcmp(md.control_type,'drag'),
    for niteration=1:nsteps,

        disp(['   Step #' num2str(niteration) ' on ' num2str(nsteps)]);

        % Compute search direction -
        [u,v,adjointu,adjointv,direction]= ...
            grad_J_drag(Rhs,S,F,P,P0,area,specified_velocity,nods,vx_obs,vx_obs,...
            index,x,y,nel,rho_ice,g,weighting,alpha,beta,z_thick_bar,drag_coeff,drag_coeff_bar);

		  if md.plot==1,
			  if niteration==1
				  scrsz = get(0,'ScreenSize');
				  figure('Position',[scrsz(3)/3 scrsz(4)*1.8/3 scrsz(3)/3 scrsz(4)/3])
			  end
			  plotmodel(md,'data',sqrt(u.^2+v.^2),'title','Modeled velocity',...
				  'data',sqrt(adjointu.^2+adjointv.^2),'title','Adjoint vectors','colorbar', 'all',...
				  'data',sqrt(vx_obs.^2+vy_obs.^2),'title','Observed velocity',...
				  'data',100*abs(sqrt(vx_obs.^2+vy_obs.^2)-sqrt(u.^2+v.^2))./sqrt(vx_obs.^2+vy_obs.^2),'title','Relative misfit','caxis#3',[0 100],'figure',1);drawnow;
		  end

        %Keep track of u and v to use in objectivefunction_C:
        u_objective=u;
        v_objective=v;

		  %Orthogonalize direction
		  direction=real(direction);
		  direction=direction/sqrt(direction'*direction);
		  pos=find(nodes_on_iceshelf);
		  direction(pos)=0;


		  % rough orthagonalization
		  direction=direction-(direction'*old_direction)*old_direction; 
		  old_direction=direction;

		  %visualize direction 
		  if md.plot==1,
			  if niteration==1
				  scrsz = get(0,'ScreenSize');
				  figure('Position',[10 scrsz(4)*1/3 scrsz(3)/3 scrsz(4)/3])
			  end 
			  plotmodel(md,'data',direction,'title','Orthogonal direction','figure',2);drawnow;
		  end


		  % normalize direction to 50, so that when variations on drag are computed
		  %they will be significant. 
		  direction=50*direction/max(abs(direction));

		  %during optimization, bounds on drag variations can vary. Here, they are less
		  %strict in the first iteration. 
		  if niteration<=2,
			  upperbound=2;
			  lowerbound=0;
		  else
			  upperbound=1;
			  lowerbound=0;
		  end

		  %search the multiplicative constant to the direction, that will 
		  %be used to modify drag.

		  search_constant=fminbnd('objectivefunction_C_drag',lowerbound,upperbound, ...
			  options,B,glen_coeff,drag_coeff,direction,Rhs,S,F,P,specified_velocity,nods,nel,vx_obs, ...
			  vy_obs,u_objective, v_objective, index,alpha, beta, area,weighting,rowD,colD,rowDshort,colDshort,valueuu,valuevv,valueuv,matrix_bar,criterion);

		  %update value of drag
		  drag_coeff_old=drag_coeff;
		  drag_coeff_new=drag_coeff+search_constant*direction;
		  drag_coeff=drag_coeff_new;

		  if length(find(drag_coeff<0))~=0,
			  disp(sprintf('\n      Some basal drag coefficient negative => STOP'))
			  break
		  end

		  %Average drag over elements: 
		  if niteration==1
			  scrsz = get(0,'ScreenSize');
			  figure('Position',[scrsz(3)*1.97/3 scrsz(4)*1/3 scrsz(3)/3 scrsz(4)/3])
		  end
		  drag_coeff_bar=(drag_coeff(index(:,1))+drag_coeff(index(:,2))+drag_coeff(index(:,3)))/3;

		  %visualize new distribution. 
		  if md.plot==1,
			  plotmodel(md,'data',drag_coeff,'title',['Drag at iteration ' num2str(niteration)],'caxis',[0 1000],'colorbar','on','figure',3);drawnow;
		  end

		  %evaluate new misfit. 
		  %@@@AK load F_file   %this file was created in objectivefunction_C, and is reloaded
       %here to win some computation time.
       load F_file

       J(niteration)=objectivefunction_C_drag(0,B,glen_coeff,drag_coeff,direction,Rhs,S,F,P,specified_velocity,nods,nel,vx_obs, ...
          vy_obs,u_objective, v_objective, index,alpha, beta, area,weighting,rowD,colD,rowDshort,colDshort,valueuu,valuevv,valueuv,matrix_bar,criterion);
       disp(J(niteration));
       
       %do a backup every 5 iterations
       if(mod(niteration,5)==0),
           save temporary_control_results_drag drag_coeff drag_coeff_bar u v J direction
       end
    end
end

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