% This function defines the system dynamics for "sfbindir_ex4"

function [y] = sfbindir_ex4sys(t, x, flag, SysData, fcnflag)
global tnext;

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

% Define the plant dynamics
sig_d = 5e3;
T_f = 40;   % Dahl friction
Tc = 10;
phi = pi/2;
N = 60;
J = 20;

% 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
f = 2*pi*0.5;
m = 10*pi/180;
r = m*cos(f*t);
dr = -m*f*sin(f*t);
ddr = -m*f^2*cos(f*t);


% Define the error system
e1 = xi(1) - r;
de1 = xi(2) - dr;
e2 = de1 + kappa*e1;

% Define the static controller
nu_s = ddr - kappa*(e2 - kappa*e1) - kappa*e2 - e1;

% This is for Delta
V_s = 0.5*(e1*e1 + e2*e2);
dVb = e2;
z = -eta*dVb;
[fs, mu] = fuzzy_sys(V_s, [0; SysData.Vmax], hat_theta(4:p));
dFdth = zeros(1, p);
dFdth(1) = sin(N*xi(1));
dFdth(2) = cos(N*xi(1));
dFdth(4:p) = z*mu;
F_delta = hat_theta(1)*sin(xi(1)) +  hat_theta(2)*cos(xi(1)) + z*fs;

% this is the adaptive control law
nu_a = (F_delta + nu_s)/hat_theta(3);


% This is for Pi
Af = zeros(1, p);
Af(3) = nu_a;

switch (fcnflag)
case 'deriv'

  dxi(1) = xi(2);
  dxi(2) = (Tc*sin(N*xi(1) + phi) - xi(3) + nu_a)/J;
  dxi(3) = sig_d*xi(2)*(1 - xi(3)*sign(xi(2))/T_f);
  
  if (SysData.adapt)
    % Define the adaptive controller dynamics
    dhat_theta = -Gamma*((dVb*(dFdth - Af))' + sigma*hat_theta);

    % define a projection algorithm
    min_th = 1/100;
    if ((hat_theta(3) <= min_th) & (dhat_theta(3) < 0))
      dhat_theta(3) = 0.;
    end

    % display the sim time
    if (t >= tnext)
      fprintf('t = %f\n', tnext)
      tnext = tnext + 0.1;
    end
  end

  y = [dxi; dhat_theta];

case 'output'
  
  y = [xi; r; nu_a];

case 'friction'
  dxi(2) = 20*(dr - xi(2));
  dxi(3) = sig_d*xi(2)*(1 - xi(3)*sign(xi(2))/T_f);
  y = [dxi; dhat_theta];

end