function sdot = eval_ebike_rhs(t, s, w, p)
% EVAL_EBIKE_RHS - Returns the time derivative of the states, i.e. evaluates
% the right hand side of the explicit ordinary differential equations.
%
% Syntax: sdot = eval_rhs(t, s, r, p)
%
% Inputs:
%   t - Scalar value of time, size 1x1.
%   s - State vector at time t, size 5x1: [theta, omega, i, x, e]'.
%   w - Function that returns the road angle input.
%   p - Constant parameter structure with 14 constants: m, R, Cr, Cd, rho,
%       A, g, J, bm, Kt, L, Rw, X, H.
% Outputs:
%   sdot - Time derivative of the states at time t, size mx1.

% unpack the states
theta = s(1);  % rotation angle of the wheel [rad]
omega = s(2);  % angular rate of the wheel [rad/s]
i = s(3);  % motor current [A]
x = s(4);  % x distance traveled [m]
e = s(5);  % energy [J]

% unpack the constant parameters
m = p.m;
R = p.R;
Cr = p.Cr;
Cd = p.Cd;
rho = p.rho;
A = p.A;
g = p.g;
J = p.J;
bm = p.bm;
Kt = p.Kt;
L = p.L;
Rw = p.Rw;

% calculate the road angle using the provided input function
r = w(t, s, p);

% unpack the input vector
V = r(1);
alpha = r(2);

% calculate some intermediate quantities for evaluating the ODEs
v = omega*R;  % v-omega relationship from no-slip tires
Fd = 1/2*rho*Cd*A*v^2;  % drag force [N]
Fr = Cr*m*g*cos(alpha);  % rolling resistant [N]
Fg = m*g*sin(alpha);  % gravity force [N]

% evaluate the four 1st order explicit ordinary differential equations
thetadot = omega;
omegadot = (Kt*i - bm*omega - Fd*R - Fr*R - Fg*R) / (2*J + m*R^2);
idot = (-Rw*i - Kt*omega + V)/L;
xdot = v*cos(alpha);
edot = V*i;

% pack the state derivatives in a vector
sdot = [thetadot; omegadot; idot; xdot; edot];

end

function r = eval_ebike_input(t, s, p)
% EVAL_EBIKE_INPUT - Returns the angle of the sinusoidal road at time t and
% the voltage applied to the throttle.
%
% Syntax: r = eval_ebike_input(t, s, p)
%
% Inputs:
%   t - Scalar value of time, size 1x1.
%   s - State vector at time t, size 5x1: [theta, omega, i, x, e]'.
%   p - Constant parameter structure with 14 constants: m, R, Cr, Cd, rho,
%       A, g, J, bm, Kt, L, Rw, X, H.
% Outputs:
%   r - Input vector at time t, size 2x1, [voltage, alpha]

% unpack the necessary states
x = s(4);  % x distance traveled [m]

% unpack the necessary constants
X = p.X;  % distance between sinusoidal road peaks [m]
H = p.H;  % amplitude of sinusoidal road peaks [m]

% voltage is constant with respect to time
voltage = p.V;  % voltage [V]

% calculate the road angle at this x location on the sine curve road
wr = 1/2/pi/X;  % road angular change per unit distance traveled [rad/m]
slope = H*wr*cos(wr*x);  % road slope at location x [m/m]
alpha = atan(slope);  % road angle at locatoin x [rad]

r = [voltage, alpha];

end

% This m file simulates the linear motion of a simple electric bicycle model
% traveling over a 2D sinusoidal road. The sinusoidal road is defined by its
% amplitude, H, and the horizontal distance corresponding to the "period" of
% the road. The states are wheel angle theta, wheel angular rate omega,
% motor current i, and horizontal distance traveled x. The no-slip
% constraint provides that the forward speed v is equal to the angular rate
% time the wheel speed.

% define numerical constants
p.A = 0.5;  % frontal area [m^2]
p.Cd = 1.1;  % coefficient of drag [unitless]
p.Cr = 0.005;  % coefficient of rolling resistance [unitless]
p.H = 3;  % amplitude of sine curve road [m]
p.J = 0.24;  % wheel moment of inertia [kg m^2]
p.Kt = 1.5;  % motor torque constant [N*m/A]
p.L = 0.5;  % motor inductance [H]
p.R = 0.3;  % wheel radius [m]
p.Rw = 1;  % motor winding resistance [Ohm]
p.V = 50;  % throttle voltage amplitude [V]
p.X = 5;  % x distance for one period of sine curve road [m]
p.bm = 0.2;  % hub friction coefficient [N*m*s]
p.g = 9.81;  % acceleration due to gravity [m/s^2]
p.m = 100;  % mass of bike and rider [kg]
p.rho = 1.2;  % density of air [kg/m^3]

% create an anoymous function to pass in inputs and parameters, provide the
% input function that creates a constant voltage and sinusoidal road
rhs = @(t, s) eval_ebike_rhs(t, s, @eval_ebike_input, p);

% integrate the dynamic equations over 60 seconds with the initinal
% condition at zero
[t, res] = ode45(rhs, linspace(0, 60), [0, 0, 0, 0, 0]);

% extract the states from the integration results
theta = res(:, 1);
omega = res(:, 2);
i = res(:, 3);
x = res(:, 4);
e = res(:, 5);

% plot the variables of interest versus time
figure(1);
plot(t, 180/pi*theta);
ylabel('\theta [deg]');
xlabel('Time [s]');

figure(2);
plot(t, 180/pi*omega);
ylabel('\omega [deg/s]');
xlabel('Time [s]');

figure(3);
plot(t, i);
ylabel('i [A]');
xlabel('Time [s]');

figure(4);
plot(t, x);
ylabel('x [m]');
xlabel('Time [s]');

figure(5);
plot(t, omega*p.R);
ylabel('v [m/s]');
xlabel('Time [s]');

figure(6);
plot(t, p.V*i)
xlabel('Time [s]');
ylabel('Power [W]')

figure(7);
plot(t, e)
xlabel('Time [s]');
ylabel('Energy [J]')