% POP:
syms alfa z l2
l1=1; h=0.10;
xSQ = l1*cos(alfa)+l2*cos(alfa);
ySQ = l1*sin(alfa)+l2*sin(alfa);
zSQ = h+z;
[alfa1 z1 l21]=solve(xSQ==-0.8, ySQ==0.8, zSQ==1.0, [alfa z l2])
[alfa2 z2 l22]=solve(xSQ==0.6, ySQ==-0.7, zSQ==1.0, [alfa z l2])

real([alfa1 z1 l21])
real([alfa2 z2 l22])

alfa1= -0.785398163;
l21=-1.63137085;
z1=0.9;

alfa2=-0.86217;
l22=-1.42195;
z2=0.9;

SA = alfa2 - alfa1
SR = l22 - l21
SB = z2 - z1

vang = 0.5

vr = sign(SR) * vang
va = sign(SA) * vang
vb = sign(SB) * vang

to = 0;
tz = 5;
ts = 60;

Tdr = SR / vr - tz
Tda = SA / va - tz * -1
Tdb = SB - tz

t = [0,10,20,30,40]

Vgam=(vr/tz).*(heaviside(t-to).*(t-to)-heaviside(t-to-tz).*(t-to-tz)-heaviside(t-to-tz-Tdr).*(t-to-tz-Tdr)+heaviside(t-to-2*tz-Tdr).*(t-to-2*tz-Tdr))

Valf=(va/tz).*(heaviside(t-to).*(t-to)-heaviside(t-to-tz).*(t-to-tz)-heaviside(t-to-tz-Tda).*(t-to-tz-Tda)+heaviside(t-to-2*tz-Tda).*(t-to-2*tz-Tda))

Vbet=(vb/tz).*(heaviside(t-to).*(t-to)-heaviside(t-to-tz).*(t-to-tz)-heaviside(t-to-tz-Tdb).*(t-to-tz-Tdb)+heaviside(t-to-2*tz-Tdb).*(t-to-2*tz-Tdb))

figure;
plot(t, Vgam, 'r', 'LineWidth', 2, 'DisplayName', 'Vgam');
hold on;
plot(t, Valf, 'g', 'LineWidth', 2, 'DisplayName', 'Valf');
plot(t, Vbet, 'b', 'LineWidth', 2, 'DisplayName', 'Vbet');
xlabel('Time');
ylabel('Voltage');
title('Voltage Functions');
legend('Location', 'best');
grid on;
hold off;


plotuj(l1,l21,h,alfa1,z1,l1,l22,h,alfa2,z2,4);

function res = plotuj(l1, l2, h, alfa, z, l11, l21, h1, alfa1, z1, i)

    Z = [0, h, h+z, h+z, h+z];

    Y = [0,
         0, 
         0,
         l1*sin(alfa),
         l1*sin(alfa)+l2*sin(alfa)];

    X = [0,
         0, 
         0,
         l1*cos(alfa),
         l1*cos(alfa)+l2*cos(alfa)];

    Z1 = [0, h1, h1+z1, h1+z1, h1+z1];

    Y1 = [0,
         0, 
         0,
         l11*sin(alfa1),
         l11*sin(alfa1)+l21*sin(alfa1)];

    X1 = [0,
         0, 
         0,
         l11*cos(alfa1),
         l11*cos(alfa1)+l21*cos(alfa1)];

    zeroes = [0,0,0,0,0];

    figure(i);
    plot3(X,Y,Z);
    hold on;
    plot3(X1,Y1,Z1);
end