tt_rsmpc_init.m

% Input args:
%   1: Frequency
%   2: Horizon
function tt_rsmpc_init(args)
  ttInitKernel('prioFP');

  SysC = sysmodel();
  Qc  = diag([args(7); args(8); args(9)]);
  Rc = args(10);

  data.mpcL = init_mpc(1.0/args(1), SysC, Qc, Rc);
  data.mpcR = init_mpc(1.0/args(3), SysC, Qc, Rc);

  data.NL = args(2);
  data.NR = args(4);
  data.u = 0;
  data.Lu = 0;
  data.pend_Lu = 0;
  data.pend_j = -1;
  data.Ru = zeros(1,3);
  data.LQu = 0;
  data.which = 0;
  data.localExec = 0;


  % Here are parameters from the paper algorithm
  % NOTE: At this point j does not correspond. j in the paper is j+2 here.
  % NOTE: At this point i does not correspond. i in the paper is i+1 here.
  data.Delta = zeros(1,4)
  data.sigma = 2;
  data.applyi = -100;
  data.applyj = -100;
  data.r = -1

  data.u_i = zeros(1,3); % Data from remote
  data.flag_i = -2; % Flag from remote
  data.flag_j = -2; % Flag from local

  data.k = -1; % The HF time step
  data.i = -100; % Remote data timestamp, (time when opt was requested)
  data.j = -100; % Local data timestamp, (time when opt was requested)
  data.local_run_at = -1; % A timestamp (k) when local mode shall be run

  data.zetaVariant = args(5); % 0 = LQ, 1 = u(0), 2 = OpenLoop, 3 = u(1)

  data.mode = args(6) % 1=A, 2=B, 3=C

  if data.mode == 0 || data.mode == 4
    data.zetaVariant = 0; % Always use LQ backup for these to not make things confusing
  end

  ttCreatePeriodicTask('rsmpc', 0, data.mpcR.h, 'tt_rsmpc_main', data);
  ttSetPriority(1, 'rsmpc');
end