function dz = rocketDynamics(z,u)
% dz = rocketDynamics(z,u)
%
% The basic dynamics and drag coefficient data are from the paper:
%
%   "Drag-law Effects in the Goddard Problem"
%   P. Tsiotras, H. Kelley, H.Kelley    1991
%
% INPUTS:
%   z = [3,n] = [h; v; m] = state vector
%   u = [1,n] = [T] = control = thrust
%

h = z(1,:);   %Height
v = z(2,:);   %Velocity
m = z(3,:);   %Mass
T = u;        %Thrust

%%%% Density of air:
% altitude = [0, 1e4, 2e4, 3e4, 4e4];  %(m)  %height above the ground
% density = [1.23, 0.41, 0.089, 0.018, 0.004];   %(kg/m^3)  density of air
density = 1.474085291*(0.9998541833.^h);  %Data fit off of wolfram alpha

%%%% Drag coefficient, calculated from paper:
A1 = 0.0095;
A2 = 25;
A3 = 0.953;
A4 = 0.036;
speedOfSound = 280;  %(m/s)  %At 10 km altitude
mach = v/speedOfSound;
Cd = A1*atan(A2*(mach-A3))+A4;

%%%% Compute the drag:
Area = pi*3.66;  %(m^2) cross-sectional area (SpaceX F9 Falcon)
D = 0.5*Cd.*Area.*density.*v.^2;

%%%% Compute gravity from inverse-square law:
rEarth = 6.3674447e6;  %(m) radius of earth
mEarth = 5.9721986e24;  %(kg) mass of earth
G = 6.67e-11; %(Nm^2/kg^2) gravitational constant
g = G*mEarth./((h+rEarth).^2);

%%%% Complete the calculation:
c = 4500;  %(m/s) rocket exhaust velocity - chemical rocket
dh = v;  %vertical velocity
dv = (T-D)./m - g;   %vertical acceleration
dm = -T/c;   %mass rate

dz = [dh;dv;dm];

end


function [ bic ] = ml_gmm_bic(Data,Priors,Mu,Sigma,cov_type)
%ML_GMM_BIC Bayesian Information criterion
%
%  input ------------------------------------------------------------------
%
%   o Data:     D x N array representing N datapoints of D dimensions.
%
%   o Priors:   1 x K array representing the prior probabilities of the
%               K GMM components.
%   o Mu:       D x K array representing the centers of the K GMM components.
%
%   o Sigma:    D x D x K array representing the covariance matrices of the
%               K GMM components.
%
%   o cov_type: string, covariance type = 'full','diag' or 'iso'
%
%  output -----------------------------------------------------------------
%
%   o bic   : (1 x 1)
%
%

[D,N]       = size(Data);
K           = length(Priors);
num_param   = K-1 + K * D;

if strcmp(cov_type,'full') == true
    num_param = num_param + K * ( D * ( D - 1)/2 );
elseif strcmp(cov_type,'diag') == true
    num_param = num_param + K * D;
elseif strcmp(cov_type,'iso') == true
    num_param = num_param + K * 1;
else
   error(['no such covariance type: ' cov_type '  only full | diag | isot ']);
end

% compute the loglikelihood of the data given the model
% loglik: (N x 1)

loglik = ml_LogLikelihood_gmm(Data,Priors,Mu,Sigma);
bic    = - 2 * loglik + num_param * log(N);

end