% This function defines the system dynamics for "sfbindir_ex1"

function [y] = sfbindir_ex1sys(t, x, flag, SysData, fcnflag)

n = SysData.n;
p = SysData.p;
kappa = SysData.kappa;
eta = SysData.eta;
Gamma = SysData.Gamma;
sigma = SysData.sigma;
J = SysData.J;
adapt = SysData.adapt;


% Strip out the plant states
xi = x(1:n); xi = xi(:);
dxi = zeros(n, 1);     % pre-allocate
hat_theta = x((n+1):(n+p)); hat_theta = hat_theta(:);
dhat_theta = zeros(p, 1);  % pre-allocate

% define a few variables
phi = xi(1);   % roll
theta = xi(2); % pitch
psi = xi(3);   % yaw
wx = xi(4);
wy = xi(5);
wz = xi(6);


% Define the reference trajectory
r_phi = 0;   dr_phi = 0;   ddr_phi = 0;
r_theta = 0; dr_theta = 0; ddr_theta = 0;
r_psi = 0;   dr_psi = 0;   ddr_psi = 0;


% The body rates
dphi = wx + (wy*sin(phi) + wz*cos(phi))*tan(theta);
dtheta = wy*cos(phi) - wz*sin(phi);
dpsi = (wy*sin(phi) + wz*cos(phi))/cos(theta);


% Define the error system
e = zeros(6,1);
e(1) = phi - r_phi;
e(2) = theta - r_theta;
e(3) = psi - r_psi;

e(4) = dphi - dr_phi + kappa*e(1);
e(5) = dtheta - dr_theta + kappa*e(2);
e(6) = dpsi - dr_psi + kappa*e(3);


% Define the static controller
de = -kappa*e(1:3) + e(4:6);
M = [1 sin(phi)*tan(theta) cos(phi)*tan(theta);
   0 cos(phi) -sin(phi);
   0 sin(phi)/cos(theta) cos(phi)/cos(theta)];
cor = [0 wz -wy;-wz 0 wx;wy -wx 0]*J*[wx; wy; wz];
alpha = zeros(3,1);
alpha(1) = (wy*cos(phi) - wz*sin(phi))*tan(theta)*dphi;
alpha(1) = alpha(1) + (wy*sin(phi) + wz*cos(phi))*dtheta/(cos(theta)^2);
alpha(1) = alpha(1) - ddr_phi + kappa*de(1);
alpha(2) = -(wy*sin(phi) + wz*cos(phi))*dphi;
alpha(2) = alpha(2) - ddr_phi + kappa*de(2);
alpha(3) = (wy*cos(phi) - wz*sin(phi))*dphi/cos(theta);
alpha(3) = alpha(3) + (wy*sin(phi) + wx*cos(phi))*sin(theta)*dtheta/(cos(theta)^2);
alpha(3) = alpha(3) - ddr_psi + kappa*de(3);

alpha = alpha + J\cor;
nu_s = J*inv(M)*(-alpha - e(1:3) - kappa*e(4:6));  % static control law


% the nonlinear damping term
dVb = [e(4) e(5) e(6)]*M*inv(J);


% calculate the output of the neural network
nn = 0;
dF = zeros(1, length(hat_theta));
if (adapt)
  range = [-2.5; 2.5];
  sig = (max(range) - min(range))/p;
  [nn, dF] = rbnn(dphi, range, sig, hat_theta);
  dF = dF(:)';   % make sure its a row vector
end
F_d = [nn; 0; 0];  % the adaptive compensator
dFdth = [dF; zeros(2, length(dF))];


% this is the adaptive control law
u = F_d - eta*dVb' + nu_s;
ux = u(1);
uy = u(2);
uz = u(3);


% Define the system uncertainty
Dx = 0;
Dy = 0;
Dz = 0;
if (t >= 2)
   Dx = 10 + 450*dphi/(1 + 2*abs(dphi));
end

switch (fcnflag)
case 'deriv'

  % Define the plant dynamics
  dxi(1) = wx + (wy*sin(phi) + wz*cos(phi))*tan(theta);
  dxi(2) = wy*cos(phi) - wz*sin(phi);
  dxi(3) = (wy*sin(phi) + wz*cos(phi))/cos(theta);
  dxi(4:6) = J\(cor + [Dx+ux; Dy+uy; Dz+uz]);
  
  % Define the adaptive controller dynamics
  dhat_theta = -Gamma*((dVb*dFdth)' + sigma*hat_theta);

  y = [dxi; dhat_theta];

case 'output'
  
  y = [xi(1:3); dphi; dtheta; dpsi; u];


case 'nn'
  y = dF;
end