function md=macayealcontrol(md)
%MACAYEALCONTROL - 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=macayealcontrol(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 ''flow'' law 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);
B_ini=md.B;
drag_coeff_ini=md.drag;
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;
B_to_p=10^-3*yts^(-1/3);

%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 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;

%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,'basal') & (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;

%check that the model is not a pure ice shelf (no friction on ice shelves)
if strcmp(md.control_type,'basal') & (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,vx_obs,vy_obs,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{1},'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 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);pause(1);

       %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 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);pause(1);


       % 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 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);pause(1);

       %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.cont_J=J;
    md.cont_parameters=B;

elseif strcmp(md.control_type,'basal'),
    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 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);pause(1);

        %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 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);pause(1);


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

       %during optimization, bounds on drag variations can vary. Here, they are less
       %strict in the first iteration. 
       if niteration<=2,
          upperbound=2;
          lowerbound=-2;
       else
          upperbound=1;
          lowerbound=-1;
       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. 
       plotmodel(md,'data',drag_coeff,'title',['Drag at iteration ' num2str(niteration)],'caxis',[0 1000],'colorbar','on','figure',3);pause(1);

       %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');
