% DOSIMULATE simulates a discrete ssm with given inputs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% DESCRIPTION: DOSIMULATE simulates a discrete ssm with given inputs.
%
% CALL:  [x, y, lastX] = doSimulate(SSini, Nsamples, A, Baos, Coutputs, Cstates, Daos, Bnoise, Dnoise, Bcst, Dcst, aos_vect, doTerminate, terminationCond, displayTime, timestep)

%
% INPUTS:
%
% OUTPUTS:
%
%
% VERSION: $Id: doSimulate.m,v 1.8 2010/09/08 11:39:20 adrien Exp $
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% TO DO: Check input aos for the timestep, tsdata, and ssm.timestep
% options to be defined (NL case)
% add check if one input mach no ssm input variable
% allow use of other LTPDA functions to generate white noise


function [x, y, lastX] = doSimulate(SSini, Nsamples, A, Baos, Coutputs, Cstates, Daos, Bnoise, Dnoise, Bcst, Dcst, aos_vect, doTerminate, terminationCond, displayTime, timestep, forceComplete)
  
  % We do a simple simulate if all these are satisfied:
  % 1) Bnoise is empty or all zeros
  % 2) Cstates is empty
  % 3) Dnoise is empty or all zeros
  % 4) Bcst is empty or all zeros
  % 5) Dcst is empty or all zeros
  % 6) doTerminate is false
  
  if  (isempty(Bnoise) || all(all(Bnoise==0))) && ...
      isempty(Cstates) && ...
      (isempty(Dnoise) || all(all(Dnoise==0))) && ...
      (isempty(Bcst) || all(all(Bcst==0))) && ...
      (isempty(Dcst) || all(all(Dcst==0))) && ...
      ~doTerminate && ...
      ~forceComplete
    % do a fast simulation
    Nstates = numel(SSini);
    
    if Nstates >= 100
      % except if Matlab is faster
      [x,y,lastX] = doSimulateSimple(SSini, Nsamples, A, Baos, Coutputs, Daos, aos_vect);
    else
      try
        % call to the mex file
        x = [];
        [y,lastX] = ltpda_ssmsim(SSini, A.', Coutputs.', Cstates.', Baos.', Daos.', aos_vect);
      catch
        % backup if the mex-file is broken
        warning('Failed to run mex file ltpda_ssmsim');
        [x,y,lastX] = doSimulateSimple(SSini, Nsamples, A, Baos, Coutputs, Daos, aos_vect);
      end
    end
  else
    % the standard old script, more complete (DC and noise inputs)
    [x,y,lastX] = doSimulateComplete(SSini, Nsamples, A, Baos, Coutputs, Cstates, Daos, Bnoise, Dnoise, Bcst, Dcst, aos_vect, doTerminate, terminationCond, displayTime, timestep);
  end
  
end


function [x,y,lastX] = doSimulateSimple(lastX, Nsamples, A, Baos, Coutputs, Daos, aos_vect)
  
  disp('Running simulate simple...');
  % initializing fields
  x = [];
  y = zeros(size(Coutputs,1), Nsamples);
  lastX  = A*lastX + Baos*aos_vect(:,1);
  % time for displaying remaining time
  
  % simulation loop
  for k = 1:Nsamples
    % computing and storing outputs
    y(:,k) = Coutputs*lastX + Daos*aos_vect(:,k);
    % computing and storing states
    lastX  = A*lastX + Baos*aos_vect(:,k);
  end
  
end

function [x,y,lastX] = doSimulateComplete(lastX, Nsamples, A, Baos, Coutputs, Cstates, Daos, Bnoise, Dnoise, Bcst, Dcst, aos_vect, doTerminate, terminationCond, displayTime, timestep)
  
  disp('Running simulate complete...');
  
  %% converting to sparse matrices
  if numel(A)>0; if(sum(sum(A==0))/numel(A))>0.5; A = sparse(A); end, end
  if numel(Baos)>0; if(sum(sum(Baos==0))/numel(Baos))>0.5; Baos = sparse(Baos); end, end
  if numel(Coutputs)>0; if(sum(sum(Coutputs==0))/numel(Coutputs))>0.5; Coutputs = sparse(Coutputs); end, end
  if numel(Cstates)>0; if(sum(sum(Cstates==0))/numel(Cstates))>0.5; Cstates = sparse(Cstates); end, end
  if numel(Daos)>0; if(sum(sum(Daos==0))/numel(Daos))>0.5; Daos = sparse(Daos); end, end
  if numel(Bnoise)>0; if(sum(sum(Bnoise==0))/numel(Bnoise))>0.5; Bnoise = sparse(Bnoise); end, end
  if numel(Dnoise)>0; if(sum(sum(Dnoise==0))/numel(Dnoise))>0.5; Dnoise = sparse(Dnoise); end, end
  if numel(Bcst)>0; if(sum(sum(Bcst==0))/numel(Bcst))>0.5; Bcst = sparse(Bcst); end, end
  if numel(Dcst)>0; if(sum(sum(Dcst==0))/numel(Dcst))>0.5; Dcst = sparse(Dcst); end, end
  
  %% initializing fields
  x = zeros(size(Cstates,1), Nsamples);
  y = zeros(size(Coutputs,1), Nsamples);
  Nnoise = size(Bnoise,2);
  BLOCK_SIZE = min( [ floor(1e6/(size(Baos,2) + size(Baos,1) + size(Bnoise,2) + 1)) , Nsamples]);
  noise_array = randn(Nnoise, BLOCK_SIZE);
  lastX  = A*lastX +  Bcst + Bnoise*noise_array(:,1) + Baos*aos_vect(:,1);
  % time for displaying remaining time
  time1=time;
  
  %% simulation loop
  for k = 1:Nsamples
    %% writing white noise and displaying time
    if rem(k,BLOCK_SIZE)==0
      noise_array = randn(Nnoise, BLOCK_SIZE);
      if displayTime
        display( ['         simulation time : ',num2str(k*timestep) ]);
        time2 = time;
        tloop = floor(time2.utc_epoch_milli-time1.utc_epoch_milli)/1000;
        display( ['remaining computing time : ',num2str(tloop*(Nsamples-k+1)/k), 's' ]);
      end
    end
    
    %% evaluating differences
    knoise = rem(k,BLOCK_SIZE-1)+1;
    
    %% computing and storing outputs
    y(:,k) = Coutputs*lastX +  Dcst + Dnoise*noise_array(:,knoise) + Daos*aos_vect(:,k);
    
    %% computing and storing states
    x(:,k) = Cstates*lastX;
    lastX  = A*lastX +  Bcst + Bnoise*noise_array(:,knoise) + Baos*aos_vect(:,k);
    
    %% checking possible termination condition
    if doTerminate
      if eval(terminationCond)
        x = x(:,1:k);
        y = y(:,1:k);
        break;
      end
    end
    
  end
  
end