% 長さが変化する単振り子
global len;
global amp;
global omega;
global mass;
global grav;
global nu;

%len = 2.0;
len = 6.0;
amp = 1.0;
omega = 2*pi/4;
mass = 0.01;
grav = 9.8;
nu = 2000;

%interval=0:0.1:10; % 固定ステップ
interval = [0,40]; % 可変ステップ
thetainit = pi/6; % 初期角度
qinit = [len*sin(thetainit); len*(1-cos(thetainit)); 0; 0];
[time, q] = ode45(@pendulum_variable_Cartesian, interval, qinit);

set(0,'defaultAxesFontSize',16);
set(0,'defaultTextFontSize',16);

plot(time,q(:,1),time,q(:,2),'--');
set(0,'defaultAxesFontSize',16);
set(0,'defaultTextFontSize',16);
%title('position x and y');
xlabel('time');
ylabel('position');
legend('x', 'y');
grid on;
saveas(gcf, 'pendulum-variable-position.eps', 'eps');
fprintf("t-x t-y グラフ\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;

plot(time,q(:,3),time,q(:,4),'--');
set(0,'defaultAxesFontSize',16);
set(0,'defaultTextFontSize',16);
%title('velocity v_x and v_y');
xlabel('time');
ylabel('velocity');
legend('v_x', 'v_y');
grid on;
saveas(gcf, 'pendulum-variable-velocity.eps', 'eps');
fprintf("t-vx t-vy グラフ\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;

theta = atan2(q(:,1),len-q(:,2));
plot(time,theta);
set(0,'defaultAxesFontSize',16);
set(0,'defaultTextFontSize',16);
%title('angle \theta');
xlabel('time');
ylabel('angle');
legend('\theta');
grid on;
saveas(gcf, 'pendulum-variable-computed-angle.eps', 'eps');
fprintf("t-角度 グラフ\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;

constraint = sqrt(power(q(:,1),2)+power(q(:,2)-len,2))-(len + amp*sin(omega*time));
plot(time,constraint);
set(0,'defaultAxesFontSize',16);
set(0,'defaultTextFontSize',16);
ylim([-0.001,0.001]);
%title('constraint R');
xlabel('time');
ylabel('constraint');
legend('R');
grid on;
saveas(gcf, 'pendulum-variable-computed-constraint.eps', 'eps');
fprintf("t-制約 グラフ\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;

plot(q(:,1),q(:,2));
set(0,'defaultAxesFontSize',16);
set(0,'defaultTextFontSize',16);
xlim([-4,4]);
ylim([-2,4]); %ylim([-2,3]);
yticks([-2 0 2 4]);
pbaspect([1 6/8 1]);
xlabel('x');
ylabel('y');
grid on;
saveas(gcf, 'pendulum-variable-path.eps', 'eps');
fprintf("軌跡\n");
fprintf("一時停止：何かのキーを押してください\n");
pause;
