% Define constants
m = 1;        % Mass = 1kg
b = .6;       % Viscous friction coeff.
g = 9.81;     % Gravity
x0=[pi/2; 0]; % Initial state: 90deg, vel=0
maxt = 60;    % Simulate for 1min

f = @(t,x) [ ...
    x(2,:); ...
    -g*sin(x(1,:)) - b*x(2,:)/m ...
];

%% Solve using ode45
[T_ode45, X_ode45] = ode45(f, [0 maxt], x0);
clf
plot(T_ode45, X_ode45(:,1));

%% Solve using forward Euler
h = .08;
x = x0;
t = 0;
nSteps = ceil(maxt/h);
T_fwd = zeros(nSteps, 1);
X_fwd = zeros(nSteps, 2);

for i=1:nSteps
    x = x+h*f(t, x);
    t = t + h;

    T_fwd(i) = t;
    X_fwd(i,:) = x;
end

clf
plot(T_fwd, X_fwd(:,1));


%% Solve using backward Euler
h = .1;
x = x0;
t = 0;
nSteps = ceil(maxt/h);
T_bkw = zeros(nSteps, 1);
X_bkw = zeros(nSteps, 2);

for i=1:nSteps
    % Initial guess
    k = h*f(t, x);

    for it=1:1
        Jq = [1 -h; h*g*cos(x(1) + k(1)) 1+b*h/m];
        q  = k - h*f(t+h,x+k);

        k = k - Jq\q;
    end

    t = t+h;
    x = x+k;

    T_bkw(i) = t;
    X_bkw(i,:) = x;
end

clf
plot(T_bkw, X_bkw(:,1));



%% Plot the vector field

N = 20;
domainX = linspace(-2, 2, N);
domainY = linspace(-6, 6, N);
[meshX,meshY] = meshgrid(domainX, domainY);
meshX = reshape(meshX, 1, N*N);
meshY = reshape(meshY, 1, N*N);

d = f(0, [meshX; meshY]);
clf
hold on
plot(X_ode45(:,1), X_ode45(:,2), 'r')
plot(X_fwd(:,1), X_fwd(:,2), 'g')
plot(X_bkw(:,1), X_bkw(:,2), 'y')
quiver(meshX, meshY, d(1,:), d(2,:));
