% This function defines the system dynamics for magnetic control example

function [y] = ofbappl_mlsys(t, xi, flag, SysData, fcnflag)
global tnext;

n = SysData.n;
p = SysData.p;

% Get the system parameters
m = SysData.m;
G = SysData.G;
ka = SysData.ka;
kb = SysData.kb;
Ra = SysData.Ra;
Rb = SysData.Rb;
La = SysData.La;
Lb = SysData.Lb;

% Get the controller parameters
kappa1 = SysData.kappa1;
kappa2 = SysData.kappa2;
rho = SysData.rho;
epsilon = SysData.epsilon;
b = SysData.b;
Gamma = SysData.Gamma;
sigma = SysData.sigma;
t_step = SysData.t_step;
step = SysData.step;
method = SysData.method;

% Get the state estimator parameters
eps_eta = SysData.eps_eta;
L = SysData.L;

% Strip out the plant states
dxi = zeros(n, 1);     % pre-allocate
dhat_theta = zeros(p, 1);  % pre-allocate
hat_theta = xi((n+1):(n+p)); hat_theta = hat_theta(:);
hat_x = xi((n+p+1):(n+p+2));
x = xi(1);
v = xi(2);
ia = xi(3);
ib = xi(4);

% The gaps
ga = G - x;
gb = G + x;

% Run the state estimator for velocity
dv = ka*ia*ia/(2*m*ga*ga) - kb*ib*ib/(2*m*gb*gb);  % modeled portion of accel
switch method
  case 'aofb'
  epsinv = diag([1/eps_eta, 1/eps_eta^2]);
  dhat_x = [hat_x(2); dv] + epsinv*L*(x - hat_x(1));
  v = hat_x(2);

  % use the saturated estimate in the controller
  sat = 0.1;  % max of 10mm/sec
  if (v > sat) v = sat; end;
  if (v < -sat) v = -sat; end;
  
  otherwise
  dhat_x = zeros(2,1);
end

% The estimated gap velocity
dga = -v;
dgb = v;

% Define the reference trajectory
r = step*(2*(sin(2*pi*t/t_step) > 0) - 1);

% Define the control input
e1 = x - r;
de1 = v;
dde1 = dv;
e2 = v + kappa1*e1;
de2 = dv + kappa1*de1;

switch method
  case 'sfb'
  z = -kappa1*de1 - e1 - kappa1*e2;
  dz = -kappa1*dde1 - de1 - kappa1*de2;

  case {'asfb', 'aofb'}
  range = [-G, G];
  sig = (max(range) - min(range))/p;
  [nn, dF] = rbnn(x, range, sig, hat_theta);
  z = -kappa1*de1 - e1 - (kappa1 + rho)*e2 + nn/m;
  dz = -kappa1*dde1 - de1 - (kappa1 + rho)*de2;
end

etaa = (z + sqrt(z^2 + epsilon))/2 + b;
etab = (-z + sqrt(z^2 + epsilon))/2 + b;
detaa = (dz - ((z^2 + epsilon)^(-3/2))*z*dz)/2;
detab = (-dz - ((z^2 + epsilon)^(-3/2))*z*dz)/2;
nu_a = sqrt(2*m*etaa/ka)*ga;
nu_b = sqrt(2*m*etab/kb)*gb;

e3 = ia - nu_a;
e4 = ib - nu_b;

qa = -((2*m*etaa/ka)^(-3/2))*m*detaa*ga/ka + sqrt(2*m*etaa/ka)*dga;
qb = -((2*m*etab/kb)^(-3/2))*m*detab*gb/kb + sqrt(2*m*etab/kb)*dgb;
switch method
  case {'asfb', 'aofb'}
  dnuadz = (-(2*m*etaa/ka)^(-3/2)*m*ga/ka)*(1 + 2*sqrt(z*z + epsilon)*z)/2;
  dnuadth = dnuadz*dF/m;
  dnuadv = -dnuadz*(2*kappa1 + rho);
  dnubdz = (-(2*m*etab/kb)^(-3/2)*m*gb/kb)*(1 + 2*sqrt(z*z + epsilon)*z)/2;
  dnubdth = dnubdz*dF/m;
  dnubdv = -dnubdz*(2*kappa1 + rho);
  dhat_theta = -Gamma*(dF*(e2 - e3*dnuadv/m - e4*dnubdv/m) + sigma*hat_theta);
  qa = qa + dnuadth'*dhat_theta;
  qb = qb + dnubdth'*dhat_theta;

  case 'sfb'
  dhat_theta = zeros(p, 1);
end

ua = Ra*ia + La*(-e2*(ka*(ia + nu_a)/(2*m*ga*ga)) + qa - kappa2*e3);
ub = Rb*ib + Lb*(e2*(kb*(ib + nu_b)/(2*m*gb*gb)) + qb - kappa2*e4);

e = [e1; e2; e3; e4];
V = 0.5*e'*e;

switch (fcnflag)
case 'deriv'

  % Define the uncertainty
  Delta = 0.5;

  % Define the plant dynamics
  v = xi(2);  % use the true rather than estimated value here
  dxi(1) = v;
  dxi(2) = ka*ia*ia/(2*m*ga*ga) - kb*ib*ib/(2*m*gb*gb) + Delta/m;
  dxi(3) = (-Ra*ia + ua)/La;
  dxi(4) = (-Rb*ib + ub)/Lb;

  y = [dxi; dhat_theta; dhat_x];

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

case 'output'
  y = [x; v; ia; ib; r; e1; e2; V];
end