classdef rzp_legacy_test < meq_test
  %% Test class for rzpl
  % Test the old (in the rzip repository of codes) versus new rzip model
  % matrices.
  %
  % [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.
  
  properties
    verbosity = 0; % Make plot 1 to make the plots

    tok       = 'tcv';
    selu      = 'v';
    nu        = 256; 
    rtol      = 1e-3;    %tolerance on the elements relative difference
    atol      = 1.5e-8;  %tolerance on the elements absolute difference
    
    P_args = {'selx','X','iterq',20,'cde','OhmTor_rigid','agcon',{{'Ip','bp','li'}},'lin',true};
    
    old_rzp, new_rzp;

    %number of eigen to compare
    n_eig = 4;
  end
  
  properties (ClassSetupParameter)
    shot = struct('pos_triang', 61400, 'neg_triang', 69393);
    t    = struct('pos_triang', 1    , 'neg_triang', 0.5  );
  end
  
  methods (TestClassSetup, ParameterCombination = 'sequential')
    function setup_output(testCase,shot,t)
      %% Setup the output for the comparison between the old and new rzip
      
      [L,LX] = rzp(testCase.tok,shot,t,'selu',testCase.selu,'nu',testCase.nu,testCase.P_args{:});

      % Call to the repository where the old rzip code is placed. 
      old_rzp_dir = '/home/codes/rzip';
      assert(logical(exist(old_rzp_dir,'dir')),...
        ['Path ',old_rzp_dir,' doesn''t exist']);
      addpath(old_rzp_dir);
      add_rzip_paths
      
      [sys_old,sysinfo] = make_rzip_model(testCase.selu,testCase.nu,...
        'plasmasource',{L,LX},...
        'shot',LX.shot,...
        'time',LX.t,...
        'full_output',true);
      
      LX.Rp = sysinfo.Rp;
      LX.Ia(contains(L.G.dima,'G_001')) = 0; %no current in the coil G
      L.Re(startsWith(L.G.dima,'G_001')) = 0.032; % change to hard-coded G coil resistance
      L = rzpl(L,LX);
      
      [sys_new,ind_new] = fgess(L);
      
      if testCase.verbosity
        meqplott(L,LX)
      end 
          
      %% Set up the permutations used to multiply sysinfo
      % The order of the active coils between the old and new rzip is
      % different. The matrices ipermute, II and III are used to multiply
      % the matrices of the old rzip to recover the same ordering.
      iEn  = startsWith( L.G.dima,'E'  ); iEo  = startsWith( sysinfo.G.dima,'E'  );
      iFn  = startsWith( L.G.dima,'F'  ); iFo  = startsWith( sysinfo.G.dima,'F'  );
      iGn  = startsWith( L.G.dima,'G'  ); iGo  = startsWith( sysinfo.G.dima,'G'  );
      iOHn = startsWith( L.G.dima,'OH' ); iOHo = startsWith( sysinfo.G.dima,'OH' );
      
      ipermute            = zeros( L.G.na  );    
      ipermute(iEn,iEo)   = eye( sum(iEn)  );
      ipermute(iFn,iFo)   = eye( sum(iFn)  );
      ipermute(iGn,iGo)   = eye( sum(iGn)  );
      ipermute(iOHn,iOHo) = eye( sum(iOHn) );
      
      II       = -blkdiag( ipermute,eye(size(sysinfo.Asp,1)-size(ipermute,1))   );
      III      = blkdiag(  ipermute,eye(size(sysinfo.Asp,1)-size(ipermute,1)-1) );
      
      %% Extract the quantities for the new rzip
      %Compute and store quantities useful for the comparison of the old
      %rzip
      testCase.new_rzp.L      = L;
      testCase.new_rzp.ss     = sys_new;
      testCase.new_rzp.ss_ind = ind_new;
      testCase.new_rzp.Iy     = LX.Iy; %store Iy in the linearisation points
      
      %Esp dxdt = Asp x + Vsp u
      iCoIp = contains(testCase.new_rzp.L.agconc(:,3), 'Ip');
      
      testCase.new_rzp.Esp = [testCase.new_rzp.L.Mee + testCase.new_rzp.L.lin.Xee, testCase.new_rzp.L.lin.Xeo(:,iCoIp);testCase.new_rzp.L.lin.Mpe + testCase.new_rzp.L.lin.Xpe,testCase.new_rzp.L.lin.Xpo(:,iCoIp)];
      testCase.new_rzp.Asp = diag([testCase.new_rzp.L.Re; LX.Rp(1)]);
      testCase.new_rzp.Vsp = [eye(testCase.new_rzp.L.G.na); zeros(testCase.new_rzp.L.G.nu + 1,testCase.new_rzp.L.G.na)];
      
      %% Compute the quantities useful for comparison form the old rzip  
      %Static matrix calculation
      testCase.old_rzp.drdrMxa = sysinfo.G.dmdrdrxa(:,1:19)*ipermute';
      testCase.old_rzp.drdzMxa = sysinfo.G.dmdrdzxa(:,1:19)*ipermute';
      testCase.old_rzp.dzdzMxa = sysinfo.G.dmdzdzxa(:,1:19)*ipermute';
      testCase.old_rzp.drdrMxu = sysinfo.G.dmdrdrxv;
      testCase.old_rzp.drdzMxu = sysinfo.G.dmdrdzxv;
      testCase.old_rzp.dzdzMxu = sysinfo.G.dmdzdzxv;
      testCase.old_rzp.Mee = III*sysinfo.matrices.M11*III';
      testCase.old_rzp.Re  = [ipermute*sysinfo.G.Ra(1:testCase.new_rzp.L.G.na); sysinfo.G.Rv];
      testCase.old_rzp.Mey = [sysinfo.G.Mxa(testCase.new_rzp.L.lxy(:),1:testCase.new_rzp.L.G.na)*ipermute' sysinfo.G.Mxv(testCase.new_rzp.L.lxy(:),:)]';
      testCase.old_rzp.rry = sysinfo.G.rx(testCase.new_rzp.L.lxy(:));
      testCase.old_rzp.zzy = sysinfo.G.zx(testCase.new_rzp.L.lxy(:));
      testCase.old_rzp.Re(contains(testCase.new_rzp.L.G.dima,'G')) = 0.032; %old rzp hardcoded
      
      %Plasma parameters calculation
      testCase.old_rzp.Ia  = ipermute*sysinfo.pcurrents.Ia;
      testCase.old_rzp.Iu  = sysinfo.pcurrents.Iu;
      testCase.old_rzp.Ip  = sysinfo.pcurrents.IP0; 
      testCase.old_rzp.Iy  = sysinfo.pcurrents.Ipx0(testCase.new_rzp.L.lxy(:));
      testCase.old_rzp.rIp = sysinfo.pcurrents.R0*sysinfo.pcurrents.IP0;
      testCase.old_rzp.zIp = sysinfo.pcurrents.z0*sysinfo.pcurrents.IP0;
      testCase.old_rzp.bp  = sysinfo.pcurrents.betap;
      
      %Matrix calculated in fgel
      %dFdIy
      testCase.old_rzp.dFdIy = [eye(numel(testCase.new_rzp.Iy));...
                                zeros(L.ng+L.np,numel(testCase.new_rzp.Iy))];
      %dFdag
      dFydIp = -sysinfo.pcurrents.Ipx0(testCase.new_rzp.L.lxy(:))'./sysinfo.pcurrents.IP0;
      dFydRc = -sysinfo.pcurrents.dIpdR0(testCase.new_rzp.L.lxy(:))';
      dFydZc = -sysinfo.pcurrents.dIpdz0(testCase.new_rzp.L.lxy(:))';
      dFpdIp = 1;
      dFpdRc = 0;
      dFpdZc = 0;
      dFrdIp = sysinfo.matrices.M34*sysinfo.pcurrents.IP0;
      dFrdRc = sysinfo.pcurrents.IP0^2*sysinfo.matrices.M33;
      dFrdZc = sysinfo.pcurrents.IP0^2*sysinfo.matrices.M32;
      dFzdIp = sysinfo.matrices.M24*sysinfo.pcurrents.IP0;
      dFzdRc = sysinfo.pcurrents.IP0^2*sysinfo.matrices.M23;
      dFzdZc = sysinfo.pcurrents.IP0^2*sysinfo.matrices.M22; 
      testCase.old_rzp.dFdag = [dFydIp dFydRc dFydZc;...
                                dFpdIp dFpdRc dFpdZc;...
                                dFrdIp dFrdRc dFrdZc;...
                                dFzdIp dFzdRc dFzdZc;...
                                zeros(L.np,L.ng)];
      %dFdIe
      testCase.old_rzp.dFdIe = [zeros(numel(testCase.new_rzp.Iy),numel(testCase.new_rzp.L.lin.Ie)); zeros(1,numel(testCase.new_rzp.L.lin.Ie));...
                                sysinfo.pcurrents.IP0*sysinfo.matrices.M31*III'                   ;... 
                                sysinfo.pcurrents.IP0*sysinfo.matrices.M21*III'                   ;...
                                zeros(L.np,L.ne)                                                  ];
      %dFdCo and dFdIp
      dFydCo = zeros(numel(testCase.new_rzp.Iy),3);
      dFpdCo = [-1, 0, 0];
      dFrdCo = [0, mu0*(sysinfo.pcurrents.IP0.^2)/2, mu0*(sysinfo.pcurrents.IP0.^2)/4];
      dFzdCo = zeros(1,3);
      testCase.old_rzp.dFdCo = [dFydCo;...
                                dFpdCo;...
                                dFrdCo;...
                                dFzdCo;...
                                zeros(L.np,L.nC)];
      testCase.old_rzp.dFdIp = zeros(numel(testCase.new_rzp.Iy)+L.ng+L.np,testCase.new_rzp.L.np);
      if testCase.new_rzp.L.icde %in case of cde, update dFdIp
        iagconIp = contains(testCase.new_rzp.L.agconc(:,3),'Ip');
        testCase.old_rzp.dFdIp = testCase.old_rzp.dFdCo(:,iagconIp);
        testCase.old_rzp.dFdCo(:,iagconIp) = 0; 
      end
      % Order the force operator F according to L.ind
      iF = [L.ind.iy,L.ind.ig,L.ind.ip]; % index into F
      testCase.old_rzp.dFdIy(iF,:) = testCase.old_rzp.dFdIy; 
      testCase.old_rzp.dFdag(iF,:) = testCase.old_rzp.dFdag; 
      testCase.old_rzp.dFdIe(iF,:) = testCase.old_rzp.dFdIe;
      testCase.old_rzp.dFdCo(iF,:) = testCase.old_rzp.dFdCo; 
      testCase.old_rzp.dFdIp(iF,:) = testCase.old_rzp.dFdIp; 
      
      %Matrix calculated in fgselim: 
      testCase.old_rzp.dagdIe = [zeros(1,L.ne); ...
                                 flipud(-(sysinfo.pcurrents.IP0^2*sysinfo.matrices.M_22)\(sysinfo.pcurrents.IP0*sysinfo.matrices.M_21(:,1:L.ne)*III'))];
      testCase.old_rzp.dagdCo = [1, 0, 0; ...
                                 flipud(-(sysinfo.pcurrents.IP0^2*sysinfo.matrices.M_22)\(sysinfo.pcurrents.IP0*sysinfo.matrices.M_21(:,end))) -testCase.old_rzp.dFdag(L.ind.ig(end-1:end),2:3)\testCase.old_rzp.dFdCo(L.ind.ig(end-1:end),end-1:end)];
      testCase.old_rzp.dIydIe = -testCase.old_rzp.dFdag(L.ind.iy,:)*testCase.old_rzp.dagdIe;
      testCase.old_rzp.dIydCo = -testCase.old_rzp.dFdag(L.ind.iy,:)*testCase.old_rzp.dagdCo;
      
      %Xee, Xeo (,Xpe, Xpo)
      testCase.old_rzp.Xee = [zeros(L.ne,1),...
                              fliplr(sysinfo.pcurrents.IP0*III*sysinfo.matrices.M_12(1:L.ne,:))]*testCase.old_rzp.dagdIe;
      testCase.old_rzp.Xeo = [III*sysinfo.matrices.M_11(1:end-1,end),...
                              zeros(L.ne,2)] + [zeros(L.ne,1) fliplr(sysinfo.pcurrents.IP0*III*sysinfo.matrices.M_12(1:end-1,:))] * testCase.old_rzp.dagdCo;
      testCase.old_rzp.Xpe =  sysinfo.pcurrents.IP0*[sysinfo.matrices.M43 sysinfo.matrices.M42]*testCase.old_rzp.dagdIe(2:3,:);
      testCase.old_rzp.Xpo = [sysinfo.matrices.M44, sysinfo.matrices.M43*sysinfo.pcurrents.IP0,...
                                sysinfo.matrices.M42*sysinfo.pcurrents.IP0]*testCase.old_rzp.dagdCo;
      
      %Esp dxdt = Asp x + Vsp u
      testCase.old_rzp.Asp = -II*sysinfo.Asp*II';
      testCase.old_rzp.Esp =  II*sysinfo.Esp*II';
      testCase.old_rzp.Vsp = -II*sysinfo.Vsp*ipermute'; 
      
      %A and B
      testCase.old_rzp.A =  II*sys_old.A*II';
      testCase.old_rzp.B = -II*sys_old.B*ipermute';
    end
  end
  
  methods (Test, TestTags = {'rzp'})
    function rzp_G_entries(testCase)
      %% Test the static matrices and the grid points
      tol = {'drdrMxa','drdrMxu','drdzMxa','drdzMxu','dzdzMxa','dzdzMxu'};
      for ii=1:numel(tol)
        st = sprintf('%s new vs old tolerance exceeded', tol{ii});
        testCase.verifyEqual(testCase.new_rzp.L.G.(tol{ii}),testCase.old_rzp.(tol{ii}),'AbsTol',testCase.atol,'RelTol',testCase.rtol,st)
      end
      
      tol = {'Mee','Re','Mey'};
      for ii=1:numel(tol)
        st = sprintf('%s new vs old tolerance exceeded', tol{ii});
        testCase.verifyEqual(testCase.new_rzp.L.(tol{ii}),testCase.old_rzp.(tol{ii}),'AbsTol',testCase.atol,'RelTol',testCase.rtol,st)
      end
      
      tol = {'rry','zzy'};
      for ii=1:numel(tol)
        st = sprintf('%s new vs old tolerance exceeded', tol{ii});
        testCase.verifyEqual(testCase.new_rzp.L.(tol{ii})(:),testCase.old_rzp.(tol{ii}),'AbsTol',testCase.atol,'RelTol',testCase.rtol,st)
      end
    end
    
    function rzp_linearisation_point(testCase)
      %% Test the linearisation equilibrium points    
      L_ = testCase.new_rzp.L;
      L_.lin.Iy = testCase.new_rzp.Iy(:)';
      
      tol = {'Ia','Iu','Ip','Iy','rIp','zIp','bp'};
      for ii=1:numel(tol)
        st = sprintf('%s new vs old tolerance exceeded', tol{ii});
        testCase.verifyEqual(L_.lin.(tol{ii}),testCase.old_rzp.(tol{ii}),'AbsTol',testCase.atol,'RelTol',testCase.rtol,st)
      end
    end
    
    function rzp_linear_force(testCase)
      %% Test the linearisation of the force operator  
      tol = {'dFdIy','dFdag','dFdIe','dFdCo','dFdIp'};
      for ii=1:numel(tol)
        st = sprintf('%s new vs old tolerance exceeded', tol{ii});
        testCase.verifyEqual(testCase.new_rzp.L.lin.(tol{ii}),testCase.old_rzp.(tol{ii}),'AbsTol',testCase.atol,'RelTol',testCase.rtol,st)
      end
    end
    
    function rzp_singular_perturbation(testCase)
      %% Test singular perturbation        
      tol = {'dagdIe','dagdCo','dIydIe','dIydCo'};
      for ii=1:numel(tol)
        st = sprintf('%s new vs old tolerance exceeded', tol{ii});
        testCase.verifyEqual(testCase.new_rzp.L.lin.(tol{ii}),testCase.old_rzp.(tol{ii}),'AbsTol',testCase.atol,'RelTol',testCase.rtol,st)
      end

      tol = {'Xee','Xeo'};
      for ii=1:numel(tol)
        st = sprintf('%s new vs old tolerance exceeded', tol{ii});
        testCase.verifyEqual(testCase.new_rzp.L.lin.(tol{ii}),testCase.old_rzp.(tol{ii}),'AbsTol',testCase.atol,'RelTol',testCase.rtol,st)
      end
      
      if testCase.new_rzp.L.icde && strcmp(testCase.new_rzp.L.P.cde,'OhmTor_rigid')
        tol = {'Xpe','Xpo'};
        for ii=1:numel(tol)
          st = sprintf('%s new vs old tolerance exceeded', tol{ii});
          testCase.verifyEqual(testCase.new_rzp.L.lin.(tol{ii}),testCase.old_rzp.(tol{ii}),'AbsTol',testCase.atol,'RelTol',testCase.rtol,st)
        end
      end
    end
    
    function rzp_ss_matrix(testCase)
      %% Test ss matrices  
      %Compare Esp dxdt = Asp x + Vsp u 
      tol = {'Asp','Esp','Vsp'};
      for ii=1:numel(tol)
        st = sprintf('%s new vs old tolerance exceeded', tol{ii});
        testCase.verifyEqual(testCase.new_rzp.(tol{ii}),testCase.old_rzp.(tol{ii}),'AbsTol',testCase.atol,'RelTol',testCase.rtol,st)
      end 
      
      %Compare A and B matrices 
      new.A = testCase.new_rzp.ss.A; 
      new.B = testCase.new_rzp.ss.B(:,testCase.new_rzp.ss_ind.iSIa); %in the old rzip, only the input voltages Va were considered
      tol = {'A','B'};
      % The matrix Esp is ill conditioned, therefore the absolute tolerance
      % for this test has been increased to 1.5e-6
      for ii=1:numel(tol)
        st = sprintf('%s new vs old tolerance exceeded', tol{ii});
        testCase.verifyEqual(new.(tol{ii}),testCase.old_rzp.(tol{ii}),'AbsTol',1.5e-6,'RelTol',testCase.rtol,st)
      end
      
      %Make plots
      if testCase.verbosity
        clf;
        subplot(231)
        imagesc(log(abs(Asp)))
        subplot(232)
        imagesc(log(abs(II*sysinfo.Asp*II')))
        subplot(233)
        imagesc(log(abs(II*sysinfo.Asp*II' + Asp))); colorbar;

        subplot(234)
        imagesc(log(abs(Esp)))
        subplot(235)
        imagesc(log(abs(II*sysinfo.Esp*II')))
        subplot(236)
        imagesc(log(abs(II*sysinfo.Esp*II'+ Esp))); colorbar
        
        subplot(234)
        imagesc(log(abs(Vsp)))
        subplot(235)
        imagesc(log(abs(II*sysinfo.Vsp)))
        subplot(236)
        imagesc(log(abs(II*sysinfo.Vsp+ Vsp))); colorbar
      end
    end
    
    function rzp_eigenvalues(testCase)
      %% Test eigenvalues
      %Calculate the eigenvalues of matrix Esp and A
      eigen_Esp = [esort(eig( testCase.new_rzp.Esp  )), esort(eig( testCase.old_rzp.Esp ))];
      eigen_A   = [esort(eig( testCase.new_rzp.ss.A )), esort(eig(  testCase.old_rzp.A  ))];
      
      testCase.verifyEqual( eigen_Esp(1:testCase.n_eig,2), eigen_Esp(1:testCase.n_eig,1), 'RelTol', testCase.rtol, 'AbsTol', testCase.atol, sprintf('Eigen of Esp new vs old rel tolerance exceeded'));
      testCase.verifyEqual( eigen_A(1:testCase.n_eig,2)  , eigen_A(1:testCase.n_eig,1)  , 'RelTol', testCase.rtol, 'AbsTol', testCase.atol, sprintf('Eigen of A new vs old rel tolerance exceeded')  );
    end
    
  end
  
end
