clear; close; clc
% Achse 1: r1 = 40 cm
v1 = 0.7;    % rad/s
a1 = 0.5;    % rad/s²
b1 = 0.3;    % rad/s²

% Achse 2: r2 = 30 cm
v2 = 0.3;    % rad/s
a2 = 0.3;    % rad/s²
b2 = 0.4;    % rad/s²

% Ausgangsplots (mit hold on)
% Start               Ziel              Matrix mit Wegpunkten, 2.Zeile
S = [-0.4 0.3];  Z = [0.4 -0.3];   load('points.mat');

h = plot([S(1) Z(1)], [S(2) Z(2)],'b-',[S(1) Z(1)], [S(2) Z(2)],'rx'); grid on; hold on; axis([-0.65 0.45 -0.5 0.5]); dist_blue_line = sqrt((Z(1)-S(1))^2 + (Z(2)-S(2))^2  );
Ax1_OriginP1 = [-0.182388 -0.306506]; Ax1_OriginP2 = [-0.182388 0.093494];

plotStartAx1 = plot([Ax1_OriginP1(1) Ax1_OriginP2(1)], [Ax1_OriginP1(2) Ax1_OriginP2(2)],'r-',[Ax1_OriginP1(1) Ax1_OriginP2(1)], [Ax1_OriginP1(2) Ax1_OriginP2(2)],'ro'); plotStartAx1(1).LineWidth = 1.8; dist_Axis1 = sqrt((Ax1_OriginP2(1)-Ax1_OriginP1(1))^2 + (Ax1_OriginP2(2)-Ax1_OriginP1(2))^2  );
Ax2_OriginP1 = [-0.182388 0.093494];  Ax2_OriginP2 = [-0.4 0.3];

plotStartAx2 = plot([Ax2_OriginP1(1) Ax2_OriginP2(1)], [Ax2_OriginP1(2) Ax2_OriginP2(2)],'g-',[Ax2_OriginP1(1) Ax2_OriginP2(1)], [Ax2_OriginP1(2) Ax2_OriginP2(2)],'go'); plotStartAx2(1).LineWidth = 1.8; dist_Axis2 = sqrt((Ax2_OriginP2(1)-Ax2_OriginP1(1))^2 + (Ax2_OriginP2(2)-Ax2_OriginP1(2))^2  );

%%%%%%%%%%%%%%%%%%%%%%%%
% Interpolation
p1 = p([1 2], :); p2 = p([2 3], :); s1 = spline(p1(:, 1), p1(:, 2)); s2 = spline(p2(:, 1), p2(:, 2));

x1 = p1(1,1):sign(p1(end, 1)-p1(1, 1))*min(abs(diff(p1(:, 1))))/10:p1(end, 1); x2 = p2(1,1):sign(p2(end, 1)-p2(1, 1))*min(abs(diff(p2(:, 1))))/10:p2(end, 1); x2(end) = p2(end, 1);
y1 = ppval(s1, x1); y2 = ppval(s2, x2);

plot(x1, y1, 'w');
plot(p1(:, 1), p1(:, 2), 'go');

plot(x2, y2, 'w');
plot(p2(:, 1), p2(:, 2), 'go');

%%%%%%%%%%%%
% Make robot arm rotate by connecting points, see figure