classdef fgslin_test < meq_test
  % Tests of fgslin: linearized FGS
  %
  % [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.
  
  properties (TestParameter)
    tok = {'ana'};
    shot = {2,82}
    t = {1};
  end
  
  properties
    verbose = 1;
    tol = 1e-11
  end
  
  methods (Test, TestTags = {'fgs'})
    function test_fgs_linearization(testCase,tok,shot,t)
      % Test FGS linearization vs full FGS
      [L,~,LY] = fgs(tok,shot,t,'tolF',testCase.tol,'mkryl',50,...
        'bfct',@bfabmex,'bfp',[1 1],...
        'agfitfct',@meqfit3,'agfitp',[true,false,true],... % settings for FBT
        'agcon',{'Ip','bp'}...  % settings for FGS
        );
     
      testCase.assertTrue(LY.isconverged,'fgs did not converge')
      
      [dIydIe,dIydCo,dFxdIe,~] = fgslin(L,LY);
      LX0 = LY;
      Le = fge(tok,shot,t,'insrc',L.P.insrc); Mey = Le.Mey;
      
      ia = 1:L.G.na; iu = L.G.na+(1:L.G.nu);
      dIydIa = dIydIe(:,ia);
      dIydIu = dIydIe(:,iu);
      dFxdIa = dFxdIe(:,ia);
      dFxdIu = dFxdIe(:,iu);
      
      %% check w.r.t. Ia perturbation
      
      LX = LX0;
      dIa = 0.01*ones(size(LX.Ia));
      LX.Ia = LX.Ia + dIa;
      LY1 = fgst(L,LX);
      
      numval = LY1.Iy-LY.Iy;
      linval = reshape(dIydIa*dIa,L.nzy,L.nry);
      testCase.verifyEqual(Mey*numval(:),Mey*linval(:),'AbsTol',testCase.tol*10,...
        sprintf('residual too high for dIy/dIa'));
      
      if testCase.verbose
        subplot(331); imagesc(L.ry,L.zy,numval); ylabel('Ia pert pert'); title('num')
        subplot(332); imagesc(L.ry,L.zy,linval);title('lin');
        subplot(333); imagesc(L.ry,L.zy,numval-linval); title('num-lin')
      end
      
      % also check dFxdIa (for shape control)
      numval = LY1.Fx - LY.Fx;
      linval = reshape(dFxdIa*dIa,L.nzx,L.nrx);
      testCase.verifyEqual(numval(:),linval(:),'AbsTol',testCase.tol*10,...
        sprintf('residual too high for dFx/dIa'));
      
      %% check w.r.t. Iu perturbation
      
      LX = LX0;
      dIu = 1e-3*randn(size(LX.Iu));
      LX.Iu = LX.Iu + dIu;
      LY1 = fgst(L,LX);
      
      numval = LY1.Iy-LY.Iy;
      linval = reshape(dIydIu*dIu,L.nzy,L.nry);
      
      testCase.verifyEqual(Mey*numval(:),Mey*linval(:),'AbsTol',testCase.tol*10,...
        sprintf('residual too high for dIy/dIa'));
      
      if testCase.verbose
        subplot(334); imagesc(L.ry,L.zy,numval); ylabel('Iu pert')
        subplot(335); imagesc(L.ry,L.zy,linval);
        subplot(336); imagesc(L.ry,L.zy,numval-linval);
      end
      
      % also check dFx_dIu (for shape control)
      numval = LY1.Fx - LY.Fx;
      linval = reshape(dFxdIu*dIu,L.nzx,L.nrx);
      testCase.verifyEqual(numval(:),linval(:),'AbsTol',testCase.tol*10,...
        sprintf('residual too high for dFx/dIu'));
      
      %% check con
      
      L.P.debug=0; % silence these repetitive calls
      % check w.r.t. separate agcon perturbation
      for iC = 1:size(L.agconc,1)
        %%
        LX = LX0;
        mycon = func2str(L.agconc{iC,1});
        myfield = L.agconc{iC,3}; 
        ii = L.agconc{iC,4};

        % perturb constraint value
        con0 = LX.(myfield)(ii);
        if con0, dcon = 1e-6*con0; else, dcon = 1e-6; end
        LX.(myfield)(ii) = LX.(myfield)(ii)+dcon;
        
        fprintf('icon=%d: %s(%d)\n',iC,myfield,ii);
        
        LY1 = fgst(L,LX,'usepreconditioner',1);
        
        linval = reshape(dIydCo(:,iC)*dcon,L.nzy,L.nry);
        numval = LY1.Iy - LY.Iy;
        
        testCase.verifyEqual(Mey*numval(:),Mey*linval(:),...
          'AbsTol',testCase.tol*10,sprintf('Tolerance exceeded for dIy/d%s',mycon));
        
        if testCase.verbose
          subplot(337); imagesc(L.ry,L.zy,numval); ylabel('co pert')
          subplot(338); imagesc(L.ry,L.zy,linval);
          subplot(339); imagesc(L.ry,L.zy,numval-linval);
          drawnow
        end
      end
    end
  end
end