clc; clear all;
tsteps = 10000;      % 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;
Dp = 1-D;
Vg = 48;
Iload = 1;

Rout = inf;%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];
  

%% Other definitions
u = [Vg; Iload];

ns = size(A1,1);
pi = size(u,1);
mo = size(C1,1);

t1 = D*Ts;
t2 = (1-D)*Ts;

%% Steady-State Solution by lsim over transient 
% tPeriod = linspace(0,Ts,tsteps);
% 
% t1p = tPeriod(tPeriod<=t1);
% t2p = tPeriod(tPeriod>t1);
% 
% X0 = [0;0];
% nPeriods = 100;
% xs=[];
% 
% for i = 1:nPeriods
%     xs = [xs; lsim(A1,B1,eye(2),0,repmat(u,1,length(t1p)),t1p,X0)];
%     xs = [xs; lsim(A2,B2,eye(2),0,repmat(u,1,length(t2p)),t2p,xs(end,:))];
%     X0 = xs(end,:);
% end
% 
% plot(xs)

% %% Steady-state solution by direct solve
% 
% problemTerm = A1\(expm(A1*t1)-eye(2))*B1*u;
% 
% Xss = (eye(2) - expm(A2*t2)*expm(A1*t1)) \ ...
%     (expm(A2*t2)*(problemTerm) + ...
%     A2\(expm(A2*t2)-eye(2))*B2*u);
% 
% XDTs = expm(A1*t1)*Xss + problemTerm;
% XTs = expm(A2*t2)*XDTs + A2\(expm(A2*t2)-eye(2))*B2*u;
% 
% assert(norm(XTs-Xss) <= 1e-9, 'Steady-state solution failed')

%% Steady-state solution by direct solve

% problemTerm = A1\(expm(A1*t1)-eye(2))*B1*u;
%% Instead calculate with augmented State space

A1tilde = [A1, B1*u;
           zeros(1, ns), 0];
M = expm(A1tilde*t1);
problemTerm = M(1:ns, ns+1);

Xss = (eye(2) - expm(A2*t2)*expm(A1*t1)) \ ...
    (expm(A2*t2)*(problemTerm) + ...
    A2\(expm(A2*t2)-eye(2))*B2*u);

XDTs = expm(A1*t1)*Xss + problemTerm;
XTs = expm(A2*t2)*XDTs + A2\(expm(A2*t2)-eye(2))*B2*u;

assert(norm(XTs-Xss) <= 1e-9, 'Steady-state solution failed')

% tPeriod = linspace(0,Ts,tsteps);
% 
% t1p = tPeriod(tPeriod<=t1);
% t2p = tPeriod(tPeriod>t1);
% X0 = Xss;
% 
% for i = 1:tsteps
%     if tPeriod(i) <= t1
%         xs(i,:) = expm(A1*tPeriod(i))*X0 + A1\(expm(A1*tPeriod(i))-eye(2))*B1*u;
%     else
%          xs(i,:) = expm(A2*(tPeriod(i)-t1))*XDTs + A2\(expm(A2*(tPeriod(i)-t1))-eye(2))*B2*u;
%     end
% end

% plot(xs)

%% Calculate efficiency
% Pout = trapz(tPeriod,Iload*xs(:,2))/Ts;
% Pin = trapz(tPeriod(tPeriod<=t1),Vg*xs(tPeriod<=t1,1))/Ts;
% 
% eff = Pout/Pin

