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;          % Output resistor used to prevent A1 from being singular

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

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

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,  -1/Rout];

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
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.
assert(norm(XTs - X0) < 1e-9, 'Steady-State solution failed')     %will throw an error if x0 =/= XTs  
                                  

%% Reconstruct steady-state waveforms using lsim at solve X0
t = linspace(0, Ts, tsteps);

sys1 = ss(A1, B1, C1, D1);          % create state space system during I
t1 = t(t<=D*Ts);                    % get timesteps during I
u1 = repmat(u, 1, length(t1));      % repeat u-vector (constant at each timestep)
[Y1,~,X1] = lsim(sys1,u1,t1,X0);    % use lsim to get back time-domain waveform

sys2 = ss(A2, B2, C2, D2);          % Repeat above for switching interval II
t2 = t(t>D*Ts);
u2 = repmat(u, 1, length(t2));
[Y2,~,X2] = lsim(sys2,u2,t2-t2(1),XDTs);

Y = [Y1; Y2];                       % combine intervals I and II
X = [X1; X2];

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

