classdef (SharedTestFixtures={mexm_fixture}) ipmh_test < meq_test
  % Tests for IPMH
  %
  % [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.
  
  properties
    L,Yd,Ie0,Jh0,sIp,Ie,Jh;
    errtol = 1e-3;
    chitol = 1e-3;
    maxiter = 50;
    verbosity = 0;
  end
  
  properties(ClassSetupParameter)
    shot = struct('limited',1,'diverted',2);
  end
  
  properties(TestParameter)
    method = {'ipmhmex','ipmhmexm'};
  end
    
  methods(TestClassSetup)
    function ipm1TestSetup(testCase,shot)
            
      s = warning('off','MATLAB:nearlySingularMatrix');
      testCase.addTeardown(@() warning(s));
      
      testCase.assumeTrue(~isempty(which('lsqlin')), 'lsqlin missing, need optimization toolbox for fbtt')
      if ~isempty(which('optimoptions')) % optimoptions not available in Octave
        opt = optimoptions('lsqlin','algorithm','interior-point','display','off',...
                           'MaxIterations',testCase.maxiter,'StepTolerance',testCase.errtol);
      else
        opt = [];
      end
      
      [L,LX] = lih('ana',shot,1,'nelem',16,'tolh',testCase.errtol,'iterh',testCase.maxiter,'selu','n'); %#ok<*PROP,*PROPLC>
      
      sIp = sign(LX.Ip);
      
      Xd = [LX.Ff;LX.Bm;LX.Ia;LX.Iu;LX.Ft;LX.Ip]; % fmautp
      Yd = L.Wd.*Xd;
      
      C = [L.Wde,L.Wdh];
      d = Yd;
      
      LB = -Inf(L.ne+L.nh,1);
      UB =  Inf(L.ne+L.nh,1);
      if sIp > 0
        LB(L.ne+(1:L.nh)) = 0;
      else
        UB(L.ne+(1:L.nh)) = 0;
      end
      
      Ie0 = [LX.Ia;LX.Iu];
      Jh0 = LX.Ip/L.nh*ones(L.nh,1);
      x0 = [Ie0;Jh0];
      
      x = lsqlin(C,d,[],[],[],[],LB,UB,x0,opt);
      Ie = x(1:L.ne);
      Jh = x(L.ne+1:end);
      
      testCase.L  = L;
      testCase.Yd = Yd;
      testCase.Ie0 = Ie0;
      testCase.Jh0 = Jh0;
      testCase.sIp = sIp;
      testCase.Ie  = Ie;
      testCase.Jh  = Jh;
    end
  end
  
  methods(Test, TestTags = {'Unit'})
    function ipmhTest(testCase,method)

      L = testCase.L;
      Yd = testCase.Yd;
      Ie0 = testCase.Ie0;
      Jh0 = testCase.Jh0;
      sIp = testCase.sIp;
      
      [Ie,Jh,ST] = feval(method,L.Ahd,L.Aed,L.Ahe,L.Aeh,L.Ahh,L.uAhh,Yd,Ie0,Jh0,sIp,L.P.iterh,max(L.P.Ipmin/L.nh,abs(Jh0(1)))*L.P.tolh);

      Zd = L.Wde*Ie + L.Wdh*Jh;
      chih = sqrt(sum((Zd-Yd).^2)/L.nd); % chi of current fit
      
      Ie_ = testCase.Ie;
      Jh_ = testCase.Jh;
      
      Zd_ = L.Wde*Ie_ + L.Wdh*Jh_;
      chih_ = sqrt(sum((Zd_-Yd).^2)/L.nd);
      
      
      testCase.verifyTrue(logical(ST)  , 'ipmhmex procedure returned an error')
      testCase.verifyTrue(all(sIp*Jh>0), 'Jh does not respect sign constraint')
      testCase.verifyTrue(abs(chih./chih_ - 1) < testCase.chitol, 'ipmh fit is worse than reference fit (lsqlin) by more than tolerance')
      % NOTE, not checking difference in Jh, Ie since methods sometimes
      % yield different answers but with similar fit agreement.
      
      if testCase.verbosity
        clf;
        ax = subplot(1,2,1);
        hold(ax,'on');
        plot(ax,Yd-Zd,1:L.nd);
        Zd = L.Wde*testCase.Ie + L.Wdh*testCase.Jh;
        plot(ax,Yd-Zd_,1:L.nd,'--');
        ax.YDir = 'reverse';
        ylabel(ax,'Meas index');
        title(ax,'Residual (fmautp)');
        
        ax = subplot(1,2,2);
        hold(ax,'on');
        [~,h]=contour(ax,L.ry,L.zy,reshape(L.Tyh*Jh,L.nzy,L.nry),'Linestyle','-');
              contour(ax,L.ry,L.zy,reshape(L.Tyh*Jh_,L.nzy,L.nry),h.LevelList,'Linestyle','--');
        title(ax,'Current distribution')
      end
    end
  end 
end
