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; %il
        0,  -1/Rout];%vout

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;   %vds1
     -ron, 1;   %vds2
      0,   1;   %vout
      1,   0];  %ig

D1 = [0, 0;
      1, 0;
      0, 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;
      0,  0];

D2 = [1, 0;
      0, 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')

%% 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')

%% Plot over one period in steady-state
% 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 from reconstructed waveforms
% Pout = trapz(tPeriod,Iload*xs(:,2))/Ts;
% Pin = trapz(tPeriod(tPeriod<=t1),Vg*xs(tPeriod<=t1,1))/Ts;
% 
% eff = Pout/Pin


%% Solve Augmented System (Augment #1)
A1tilde = [A1, B1*u;
           zeros(1, ns), 0];
A2tilde = [A2, B2*u;
           zeros(1, ns), 0];

%xtilde = [x, 1];

basis = null(eye(ns+1) - expm(A2tilde*t2)*expm(A1tilde*t1));
Xsstilde = basis/basis(end);
Xss = Xsstilde(1:ns);

%% Solve Augmented System (Augment #2)
A1a = [A1, B1;
       zeros(pi, ns+pi)];
A2a = [A2, B2;
       zeros(pi, ns+pi)];

%xa = [x, u];

basis = null(eye(ns+pi) - expm(A2a*t2)*expm(A1a*t1));

k = basis(ns+1:end,:)\u;

Xssa = basis*k;
Xss = Xssa(1:ns);



%% Use further augmentation to calculate integrals of states/outputs
       %ns pi ns mo
A1b = [A1, B1, zeros(ns, ns+mo)  ;          %ns
       zeros(pi, 2*ns+pi+mo);               %pi
       eye(ns), zeros(ns, ns+pi+mo);        %ns
       C1, D1, zeros(mo, ns+mo)];        %mo

A2b = [A2, B2, zeros(ns, ns+mo)  ;          %ns
       zeros(pi, 2*ns+pi+mo);               %pi
       eye(ns), zeros(ns, ns+pi+mo);        %ns
       C2, D2, zeros(mo, ns+mo)];        %mo

% Xb = [X; u; \int{x}; \int{y}];

X0b = [Xss; u; zeros(ns,1); zeros(mo,1)];

Xfb = expm(A2b*t2)*expm(A1b*t1)*X0b;

intXs = Xfb(ns+pi+1:ns+pi+ns);
intYs = Xfb(2*ns+pi+1:end);

avgXs = intXs/Ts;
avgYs = intYs/Ts;

%% Calculate efficiency directly
Pout = Iload*avgYs(3);
Pin = Vg*avgYs(4);

eff = Pout/Pin



%% Re-expand to get time-domain waveforms
tPeriod = linspace(0,Ts,tsteps);

t1p = tPeriod(tPeriod<=t1);
t2p = tPeriod(tPeriod>t1);

xsb = zeros(tsteps,ns*2+pi+mo);
ys= zeros(tsteps,mo);

for i = 1:tsteps
    if tPeriod(i) <= t1
        xsb(i,:) =expm(A1b*tPeriod(i))*X0b;
        ys(i,:) = C1*xsb(i,1:ns)' + D1*u;
        XDTsb = xsb(i,:);
    else
         xsb(i,:) = expm(A2b*(tPeriod(i)-t1))*XDTsb';
         ys(i,:) = C2*xsb(i,1:ns)' + D2*u;
    end
end

figure(1)
subplot(5,1,1)
plot(tPeriod,xsb(:,1));
hold on;
plot(tPeriod,xsb(:,2));
legend('i_L', 'v_{out}')

subplot(5,1,2)
plot(tPeriod,xsb(:,3));
hold on;
plot(tPeriod,xsb(:,4));
legend('V_g', 'I_{load}')

subplot(5,1,3)
plot(xsb(:,ns+pi+1:ns+pi+ns));
legend('\int{i_L}', '\int{v_{out}}')

subplot(5,1,4)
plot(ys);
legend('{v_{ds1}}', '{v_{ds2}}', '{v_{out}}', '{i_g}')

subplot(5,1,5)
plot(xsb(:,end-mo:end));
legend('\int{v_{ds1}}', '\int{v_{ds2}}', '\int{v_{out}}', '\int{i_g}')

