%% 
%  Author: Peter Polcz (ppolcz@gmail.com) 
%  Created on 2023. December 06. (2023a)
%

Color_1 = [0 0.4470 0.7410];
Color_2 = [0.8500 0.3250 0.0980];
Color_3 = [0.9290 0.6940 0.1250];
Color_4 = [0.4940 0.1840 0.5560];
Color_5 = [0.4660 0.6740 0.1880];
Color_6 = [0.3010 0.7450 0.9330];
Color_7 = [0.6350 0.0780 0.1840];

%%

R = 1.79e3;
C1 = 10e-9;
C2 = 100e-9;
L = 18e-3;

alpha = C2/C1;
beta = C2*R^2/L;

Ga = -0.758e-3;
Gb = -0.409e-3;

a = R*Ga;
b = R*Gb;
h = @(x) b*x + 2\(a-b)*(abs(x+1) - abs(x-1));

f = @(t,x) [
    alpha*(x(2)-x(1)-h(x(1)))
    x(1)-x(2)+x(3)
    -beta*x(2)
    ];

x0 = [1;1;1]/10;

T = 500;
[t,x] = ode89(f,[0,T],x0);

View = [41.4637 17.2302];
if exist("ax1","var") && isvalid(ax1)
    View = ax1.View;
end

fig = figure(12);
Tl = tiledlayout(3,2,"TileSpacing","compact","Padding","compact");
ax1 = nexttile([3,1]);
hold on;

plot3(x(:,1),x(:,2),x(:,3),'Color',Color_1)

Shift = [ 10 , 0 , 0 ];

Pl = plot3(0,0,0,'Color',Color_5);

Tl = title('tau = 0');

rng(3)
sigma = randperm(numel(t));
Idx = sort(sigma(1:10));

tPeaks = t(Idx);

MaxTau = 1;
ldx = tPeaks < 3*MaxTau | tPeaks > T - 3*MaxTau;
Idx(ldx) = [];
tPeaks(ldx) = [];

N = nan(numel(tPeaks),1);
Mc = plot3(N,N,N,'.-','Color',Color_3,'LineWidth',1.5,'MarkerSize',25);

c = @(M) M(:);

plot3(0,0,0,'.','MarkerSize',40,'Color',[0,0,0]);
plot3(Shift(1),Shift(2),Shift(3),'.','MarkerSize',40,'Color',[0,0,0]);

R = diag([1,0.3,1,1])*roty(-0.3)*rotx(0.98+pi)*roty(0.4)*rotz(0.95);
PeakDelay = 0;

view(View)
axis vis3d

xlabel('x')
ylabel('y')
grid on

ldx = 3*MaxTau < t & t < 3*MaxTau+60;
tPl = t(ldx);

ax2 = nexttile;
plot(tPl,interp1(t,x(:,1),tPl));
grid on, box on, xlim(tPl([1,end]))
Leg = legend('$x(t)$',Interpreter="latex",FontSize=15);

ax3 = nexttile; hold on
PlY = plot(tPl,interp1(t,x(:,1),tPl));
plot(tPl,interp1(t,x(:,2),tPl));
grid on, box on, xlim(tPl([1,end]))
Leg = legend('$x(t-\tau)$','$y(t)$',Interpreter="latex",FontSize=15);

ax4 = nexttile; hold on
PlZ = plot(tPl,interp1(t,x(:,1),tPl));
plot(tPl,interp1(t,x(:,3),tPl));
grid on, box on, xlim(tPl([1,end]))
Leg = legend('$x(t-2\tau)$','$z(t)$',Interpreter="latex",FontSize=15);

y = x(:,1);

for tau = 0.3
% for tau = 0:0.02:MaxTau
   
    t_d1 = t - tau;
    y_d1 = interp1(t,y,t_d1);

    t_d2 = t - 2*tau;
    y_d2 = interp1(t,y,t_d2);

    ldx = ~isnan(y_d1 + y_d2);

    r = [y(ldx,1) , y_d1(ldx) , y_d2(ldx)];
    r_Tr = r * R(1:3,1:3)' + Shift;

    Pl.XData = r_Tr(:,1);
    Pl.YData = r_Tr(:,2);
    Pl.ZData = r_Tr(:,3);
    Tl.String = sprintf('tau = %.3f',tau);

    rPeaks = interp1(t(ldx)+PeakDelay,r_Tr,tPeaks);

    Mc.XData = c([x(Idx,1) rPeaks(:,1) N]');
    Mc.YData = c([x(Idx,2) rPeaks(:,2) N]');
    Mc.ZData = c([x(Idx,3) rPeaks(:,3) N]');

    PlY.YData = interp1(t(ldx),r(:,2),PlY.XData);
    PlZ.YData = interp1(t(ldx),r(:,3),PlZ.XData);

    % 3*MaxTau < t & t < 3*MaxTau+100

    drawnow
    pause(0.1)
    % keyboard
end
