function [full_state_dot,body_rates,body_accel] = fcn_eom_template(g,m,J,J_inv,layout,thrusts,full_state)
%
% ----------------------------------------------------------------------- %
% DESCRIPTION:
% This function is a template for implementing the full equations of motion
% in one function.
%
%
% INPUT ARGUMENTS:
% m
% > This is the mass of the vehicle, in units of kilograms
%
% g
% > This is the acceleration due to gravity (typically 9.81), in units of
%   meters per second squared
%
% J
% > This is the moment of inertia matrix of the vehicle (it should be a
%   3-by3 matrix), the units of each element is kilogram meters squared
%
% J_inv
% > This is the inverse of the momement of inertia matrix, it is not
%   compute in this script to reduce the simulation run time slightly
%
% layout
% > This is a matrix that specifies the layout via the following format:
% nrotor_vehicle_layout = [...
%       x_1  ,  x_2  ,  ...  ,  x_{N-1}  ,  x_{N}  ;...
%       y_1  ,  y_2  ,  ...  ,  y_{N-1}  ,  y_{N}  ;...
%       c_1  ,  c_2  ,  ...  ,  c_{N-1}  ,  c_{N}   ...
%   ];
% where:
%   > x_i   is the x coordinate of propeller "i" relative to the CoG
%   > y_i   is the y coordinate of propeller "i" relative to the CoG
%   > c_i   is the thurst to torque coefficient of the propeller, with the
%           sign used to indicate the propeller's direction of rotation,
%           +ve indicates clock-wise propeller rotation.
%
% thrusts
% > This is a colulmn vector with length equal to the number of propellers
%   on the N-rotor vehicle, and each element represents the thrust force in
%   Newtons being produced by the respective propeller
%
% full_state
% > This is a vector of length 12 and is order as:
%   full_state[1:3]     the x,y,z posiiton of the body frame origin
%                       expressed in inertial frame coordinates, in units
%                       of [meters].
%
%   full_state[4:6]     the x,y,z linear velocity of the body frame origin
%                       expressed in inertial frame coordinates, in units
%                       of [meters per second].
%
%   full_state[7:9]     the roll, pitch, and yaw intrinsic Euler angles
%                       respectively, in units of [radians].
%
%   full_state[10:12]   the angular velocity of the roll, pitch, and yaw
%                       intrinsic Euler angles respectively, in units of
%                       [radians per second].
%
%
% RETURN ARGUMENTS:
% full_state_dot
% > This is a vector of length 12 and is ordered as:
%   full_state_dot[1:3]
%      - This is position dot and is taken directly as full_state[4:6]
%
%   full_state_dot[4:6]
%      - This is the position double dot and is computed using the equation
%        of motion for transaltion, it has units of [meters per second
%        squared]
%
%   full_state_dot[7:9]
%      - This is the angular velocity of the intrinsic Euler angles and is
%        taken directly as full_state[10:12]
%
%   full_state_dot[10:12]
%      - This is the angular accelerations of the intrinsic Euler angles
%        and is computed using the equation of motion for Euler angle
%        rotations, it has units of [radians per second squared]
%
% body_rates
% > This is a vector of length 3, each element represents the angular
%   velocity of the body frame as measured about the body frame's
%   coordinate axes, ordered as:
%   body_rates = [ w_x ; w_y ; w_z ];
%   In other words, this is exactly what is measured by a typical
%   strap-down Inertial Measurement Unit (IMU) that is attached to the body
%   of most all flying vehicles. It has units of [radians per second];
%
% body_accel
% > This is a vector of length 3, each element represents the linear
%   accelerations of the body frame as measured along the body frame's
%   coordinate axes, i.e., this is exctly what is measured by a typical
%   strap-down Inertial Measurement Unit (IMU). It is computed directly
%   from the "pos_double_dot" by applying the rotation matrix from inertial
%   to body frame. It has units of [meters per second squared].
%
% ----------------------------------------------------------------------- %


% Extract the components of the full state
pos      =  full_state(1:3);
pos_dot  =  full_state(4:6);
psi      =  full_state(7:9);
psi_dot  =  full_state(10:12);


% Extract each Euler angles
roll   =  psi(1);  % Roll  (gamma is the symbol used in the script)
pitch  =  psi(2);  % Pitch (beta  is the symbol used in the script)
yaw    =  psi(3);  % Yaw   (alpha is the symbol used in the script)


% Extract each Euler anglular rates
roll_dot   =  psi_dot(1);  % Roll  angular velocity
pitch_dot  =  psi_dot(2);  % Pitch angular velocity
yaw_dot    =  psi_dot(3); % Yaw   angular velocity, not needed





%% --------------------------------------------------------------------- %%
%% STUDENTS SHOULD EDIT THE FOLLOWING TO COMPLETE THE EQUATIONS OF MOTION
%  NOTE: Only the "R_B2I" and "T" matrices are already completed.


% Compute the rotation matrix from body frame to inertial frame:
R_B2I = [...
        cos(yaw)*cos(pitch)  ,...
            cos(yaw)*sin(pitch)*sin(roll)-sin(yaw)*cos(roll)  ,...
            cos(yaw)*sin(pitch)*cos(roll)+sin(yaw)*sin(roll)   ...
        ;...
        sin(yaw)*cos(pitch)  ,...
            sin(yaw)*sin(pitch)*sin(roll)+cos(yaw)*cos(roll)  ,...
            sin(yaw)*sin(pitch)*cos(roll)-cos(yaw)*sin(roll)   ...
        ;...
        -sin(pitch)  ,...
            cos(pitch)*sin(roll)  ,...
            cos(pitch)*cos(roll)   ...
    ];


% Compute the T matrix:
T = [...
        1  ,  0          ,  -sin(pitch)            ;...
        0  ,  cos(roll)  ,   sin(roll)*cos(pitch)  ;...
        0  , -sin(roll)  ,   cos(roll)*cos(pitch)   ...
    ];


% Compute the T inverse matrix:
T_inv = [...
        0  ,  0  ,   0  ;...
        0  ,  0  ,   0  ;...
        0  ,  0  ,   0   ...
    ];


% Compute the time derivative of the T matrix:
T_dot = [...
        0  ,  0  ,   0  ;...
        0  ,  0  ,   0  ;...
        0  ,  0  ,   0   ...
    ];


% Implement the equation of motion for translation
pos_double_dot = zeros(3,1);


% Compute the body rates from the Euler angluar rates
w = zeros(3,1);


% Implement the equation of motion for body frame rotations
w_dot = zeros(3,1);


% Implement the equation of motion for Euler angle rotations
psi_double_dot = zeros(3,1);


%% STUDENTS SHOULD EDIT THE ABOVE TO COMPLETE THE EQUATIONS OF MOTION
%% --------------------------------------------------------------------- %%





% Put together the "full_state_dot" return variable
full_state_dot = [...
        pos_dot ;...
        pos_double_dot ;...
        psi_dot ;...
        psi_double_dot ...
    ];


% Put together the "body_rates" return variable
body_rates = w;


% Finally compute the accelerations in the body frame
% > by "simply" rotating the inertial frame linear accelerations, i.e.,
%   "pos_double_dot", from the inertial frame into the body frame
% > do you need to account for the fact that the body frame accelerations
%   are measured in a rotating frame?
body_accel = transpose(R_B2I) * psi_double_dot;


end