% This function defines the system dynamics for "sfbdir_ex4"

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

n = SysData.n;
p = SysData.p;
kappa = SysData.kappa;
eta = SysData.eta;
sigma = SysData.sigma;
Gamma = SysData.Gamma;
fs_flag = SysData.fs_flag;

% 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 the reference trajectory
tmp = sin(2*pi*t/40);
r = 20 + 10*tmp*tmp;
r = 5*floor(t/30) + 10;

dr = 0;

% what are the current values for the fuzzy system?
[fs, mu] = fuzzy_sys(xi(1), [SysData.x1; SysData.x2], hat_theta);

% Vehicle constants
rho = SysData.rho;
mass = SysData.mass;

% Define the control input
e1 = xi(1) - r;
u = mass*(dr - kappa*e1) - eta*e1/mass;

% do we want the adaptive portion turned on here?
if (fs_flag)
  u = u + mu'*hat_theta;
end


switch (fcnflag)
case 'deriv'

  % Define the plant dynamics
  dxi(1) = -rho*xi(1)*xi(1)*sign(xi(1))/mass + u/mass;

  % Define the adaptive controller dynamics
  dhat_theta = -Gamma*(e1*mu/(sum(mu)*mass) + sigma*hat_theta);

  y = [dxi; dhat_theta];

case 'output'
  
  y = [xi(1); r; xi(1)-r; u; mu];
end