tsteps = 1000;      % Number of time-steps per period

%% Buck Boost Example
ron = 1e-3;
RL = 50e-3;
Co = .5e-6;
L = .3e-6;

fs = 1e6;
Ts = 1/fs;

D = 0.25;
Vg = 48;
Iload = 1;

% Rout = 100e3;          % No output resistor -> A1 is singular

K = [L, 0;
    0, Co];

%% Define state space representations during each subinterval
% Subinterval I
A1p = [-(ron+RL), 0;
        0,  0];

B1p = [1, 0;
       0, -1];

A1 = K\A1p;             % "\" computes the inverse of a matrix multiplied by another matrix/vector
B1 = K\B1p;             % results in faster computation that inv(K)*A1p by not actually forming inv(K)
   
C1 = [ron, 0;
     -ron, 1;
      0,   1];

D1 = [0, 0;
      1, 0;
      0, 0];
  
% Subinterval II
A2p = [-(ron+RL), -1;
        1,  0];

B2p = [0, 0;
       0, -1];

A2 = K\A2p;
B2 = K\B2p;

C2 = [ron, 1;
     -ron, 0;
      0,   1];

D2 = [1, 0;
      0, 0;
      0, 0];
  
%% Steady-State Solution - Normal Approach
u = [Vg; Iload];

ns = length(A1);
pi = length(u);

I = eye(ns);
eA1 = expm(A1*D*Ts);        % state transition matrix during I
eA2 = expm(A2*(1-D)*Ts);    % state transition matrix during II

X0 = (I-eA2*eA1)\( ...
    eA2*(A1\(eA1-I))*B1*u + A2\(eA2-I)*B2*u   );
%       &uarr;  Note: parenthesis necessary to make sure the "\" doesnt take inverse of (eA2*A1)

XDTs = eA1*X0 + A1\(eA1-I)*B1*u;  % Re-apply formula to find x(t=D*Ts)

XTs = eA2*XDTs + A2\(eA2-I)*B2*u;  % Re-apply formula to find x(t=D*Ts)

%double-check to ensure steady-state was found.  Note: WILL fail in this example
try
    assert(norm(XTs - X0) < 1e-9, 'Steady-State solution failed')    
catch
    display("Normal approach didn't work due to A1 being singular.");
    display(X0);
end
                                    

%% Use Augmented State Space to calculate only the offending term
Aa1 = [A1, B1*u; zeros(1,ns+1)];       %Form Augmented matrix
eAa1 = expm(Aa1*D*Ts);

assert(norm(eAa1(1:2,1:2) - eA1) < 1e-9)    %Double-check upper-left matrix is eA1
offendingTerm = eAa1(1:ns,ns+1);                %Extract convolution integral term

% Replace term with singular A with Augmented State Space result.
X0 = (I-eA2*eA1)\( ...
    eA2*offendingTerm + A2\(eA2-I)*B2*u   );

XDTs = eA1*X0 + offendingTerm;  % Re-apply formula to find x(t=D*Ts)

XTs = eA2*XDTs + A2\(eA2-I)*B2*u;  % Re-apply formula to find x(t=D*Ts)

%Check solution (should pass here)
assert(norm(XTs - X0) < 1e-9, 'Augmented Steady-State solution failed');  

%% Reconstruct steady-state waveforms using direct solution (With Aug SS)
t = linspace(0, Ts, tsteps);
X = zeros(2,length(t));                 %Pre-allocate
Y = zeros(3,length(t));                 %Pre-allocate
for i = 1:1:length(t)
    ti = t(i);
    if(ti<= D*Ts)
        eAa1 = expm(Aa1*ti);
        offendingTerm = eAa1(1:ns,ns+1);            %Recalculate at the current time
        X(:,i) = expm(A1*ti)*X0 + offendingTerm;
        Y(:,i) = C1*X(:,i) + D1*u;
    else
        X(:,i) = expm(A2*(ti-D*Ts))*XDTs + A2\(expm(A2*(ti-D*Ts))-eye(2))*B2*u;
        Y(:,i) = C2*X(:,i) + D2*u;
    end
end

%% Plot x(t) and y(t)
figure(1);
subplot(2,1,1)
plot(t*1e6, X, 'LineWidth', 3)
legend('{\it i_L} [A]', '{\it v_c} [V]');
ylabel('{\bf x}({\it{t}})')

subplot(2,1,2)
plot(t*1e6, Y, 'LineWidth', 3)
legend('{\it v_{ds1}} [V]', '{\it v_{ds2}} [V]', '{\it v_c} [V]');
xlabel('time [{\mu}sec]')
ylabel('{\bf y}({\it{t}})')

%% Calculate efficiency using time-domain waveforms
ig = X(1,:).*(t<=D*Ts);
vo = X(2,:);

Pin = trapz(t,Vg*ig)/Ts;
Pout = trapz(t,vo*Iload)/Ts;

eta = Pout/Pin






