%close all
%clear all
%clc
global g m M l A f
% Cup trajectory
% x(t) = A/2 * sin(2*pi*f*t + pi/2)

% Mechanical parameters
g = 9.81;
m = 0.6;
M = 2.4;
l = 0.45;
f = 0.6546;
A = 0.1755;
th0 = 0.789;
dth0 = 0.;

% Nominal movement and simulation times
tsim = 20.;

% Define the simulation time vector with sampling frequency of 1000 Hz
sfrq = 100;
tvec0 = [0:1:sfrq*tsim]/sfrq;

% Define the initial state vector
th_p0 = th0;
th_s0 = dth0;
xx0 = [th_p0, th_s0 ];

% Use ode45 to solve state equations
[tvec, xvec] = ode45('cup_task_model', tvec0, xx0);

% Extract states
th_p = xvec(:,1);
th_s = xvec(:,2);

x_p = zeros(length(tvec0),1);
x_s = zeros(length(tvec0),1);
x_a = zeros(length(tvec0),1);
th_a = zeros(length(tvec0),1);
F = zeros(length(tvec0),1);
Fball = zeros(length(tvec0),1);
for k=1:length(tvec0)
    x_p(k) = A/2. * sin(2*pi*f*tvec(k) + pi/2.);
    x_s(k) = A/2. * 2*pi*f * cos(2*pi*f*tvec(k) + pi/2.);
    x_a(k) = -A/2. *  (2*pi*f)^2 * sin(2*pi*f*tvec(k) + pi/2.); 
    th_a(k) = -1./l * (x_a(k) * cos(th_p(k)) + g * sin(th_p(k)));
    F(k) = (m+M) * x_a(k) + m*l * ( th_a(k) * cos(th_p(k)) - th_s(k)^2 * sin(th_p(k)));
    Fball(k) = m*l * ( - th_a(k) * cos(th_p(k)) + th_s(k)^2 * sin(th_p(k)));
end;

figure('Position', [100, 100, 1400, 400]);

subplot(1,3,1);
plot(tvec0, x_p);
%title('Cart Position X');
xlabel('Time (s)');
ylabel('Cart Position X (m)');

subplot(1,3,2);
plot(tvec0, th_p);
%title('Pendulum Angle \theta');
xlabel('Time (s)');
ylabel('Pendulum Angle \theta (rad)');

subplot(1,3,3);
plot(tvec0, F);
%title('Interaction Force F_{inter}');
xlabel('Time (s)');
ylabel('Interaction Force F_{inter} (N)');

