% This function defines the system dynamics for "sfbdir_ex3"

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;

% 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
r = SysData.step_mag*mod(floor(t/SysData.step_t) + 1, 2);
dr = 0;
ddr = 0;


% Define the control input
e1 = xi(1) - r;
e2 = xi(2) + kappa*e1 - dr;
%u = ddr - e1 - kappa*(e2 - kappa*e1) - 0.5*kappa*e2;
u = ddr - e1 - kappa*(e2 - kappa*e1) - 1.5*kappa*e2;

dFdth = [kappa;
xi(1)^2;
xi(1)*xi(2);
xi(2)*xi(2)]';
u = u + dFdth*hat_theta;

switch (fcnflag)
case 'deriv'

  % Define the plant dynamics
  rho1 = 1;
  rho2 = -1;
  rho3 = 2;
  rho4 = 1;

  Delta1 = rho1;
  Delta2 = xi'*[rho2 rho3; rho3 rho4]*xi;

  dxi(1) = Delta1 + xi(2);
  dxi(2) = Delta2 + u;

  % Define the adaptive controller dynamics
  dhat_theta = -Gamma*(e2*dFdth' + sigma*hat_theta);

  y = [dxi; dhat_theta];

case 'output'
  
  y = [xi(1); xi(2); r; e1; u];
end