## Copyright (C) 1998 Auburn University.  All Rights Reserved
## 
## This file is part of Octave.
## 
## Octave is free software; you can redistribute it and/or modify it
## under the terms of the GNU General Public License as published by the
## Free Software Foundation; either version 2, or (at your option) any
## later version.
## 
## Octave is distributed in the hope that it will be useful, but WITHOUT
## ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
## FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
## for more details.
## 
## You should have received a copy of the GNU General Public License
## along with Octave; see the file COPYING.  If not, write to the Free
## Software Foundation, 59 Temple Place, Suite 330, Boston, MA 02111 USA.

## -*- texinfo -*-
## @deftypefn {Function File } {[@var{dsys}, @var{fidx}] =} dmr2d (@var{sys}, @var{idx}, @var{sprefix}, @var{Ts2} @{,@var{cuflg}@})
##  convert a multirate digital system to a single rate digital system
##  states specified by @var{idx}, @var{sprefix} are sampled at @var{Ts2}, all 
##   others are assumed sampled at @var{Ts1} = @code{sysgettsam(@var{sys})}.
## 
## @strong{Inputs}
## @table @var
## @item   sys
## discrete time system;
## @code{dmr2d} exits with an error if @var{sys} is not discrete
## @item   idx
## list of states with sampling time @code{sysgettsam(@var{sys})} (may be empty)
## @item   sprefix
## list of string prefixes of states with sampling time @code{sysgettsam(@var{sys})}
## (may be empty)
## @item   Ts2
## sampling time of states not specified by @var{idx}, @var{sprefix}
## must be an integer multiple of @code{sysgettsam(@var{sys})}
## @item   cuflg
## "constant u flag" if @var{cuflg} is nonzero then the system inputs are 
##         assumed to be constant over the revised sampling interval @var{Ts2}.
##         Otherwise, since the inputs can change during the interval
##         @var{t} in @math{[k Ts2, (k+1) Ts2]}, an additional set of inputs is
##         included in the revised B matrix so that these intersample inputs
##         may be included in the single-rate system.
##         default
##  @var{cuflg} = 1.
## @end table
## 
## @strong{Outputs}
## @table @var
## @item   dsys
##  equivalent discrete time system with sampling time @var{Ts2}.
## 
##          The sampling time of sys is updated to @var{Ts2}.
## 
##          if @var{cuflg}=0 then a set of additional inputs is added to
##          the system with suffixes _d1, ..., _dn to indicate their
##          delay from the starting time k @var{Ts2}, i.e.
##          u = [u_1; u_1_d1; ..., u_1_dn] where u_1_dk is the input
##              k*Ts1 units of time after u_1 is sampled. (Ts1 is
##              the original sampling time of discrete time sys and
##              @var{Ts2} = (n+1)*Ts1)
## 
## @item   fidx
## indices of "formerly fast" states specified by @var{idx} and @var{sprefix};
## these states are updated to the new (slower) sampling interval @var{Ts2}.
## @end table
## 
## @strong{WARNING} Not thoroughly tested yet; especially when @var{cuflg} == 0.
## 
## @end deftypefn

function [dsys,fidx] = dmr2d (sys, idx, sprefix, Ts2,cuflg)

  ## Adapted from c2d by a.s.hodel@eng.auburn.edu

  save_val = implicit_str_to_num_ok;	# save for later
  implicit_str_to_num_ok = 1;

  ## parse input arguments
  if(nargin != 4 | nargout > 2)
    usage("[dsys,fidx] = dmr2d (sys, idx, sprefix, Ts2 {,cuflg})");

  elseif (!is_struct(sys))
    error("sys must be in system data structure form");

  elseif(!is_digital(sys))
    error("sys must be discrete-time; continuous time passed");

  elseif (!(is_vector(idx) | isempty(idx)))
    error(["idx(",num2str(rows(idx)),"x",num2str(columns(idx)), ...
      ") must be a vector"]);

  elseif (any(idx <= 0))
    idv = find(idx <= 0);
    ii = idv(1);
    error(["idx(",num2str(ii),")=",num2str(idx(ii)), ...
      "; entries of idx must be positive"]);

  elseif(!(is_signal_list(sprefix) | isempty(sprefix)))
    error("sprefix must be a signal list (see is_signal_list) or empty");

  elseif(!is_sample(Ts2))
    error(["Ts2=",num2str(Ts2),"; invalid sampling time"]);

  endif

  ## optional argument: cuflg
  if(nargin <= 4)
    cuflg = 1;		# default: constant inputs over Ts2 sampling interv.
  elseif( !is_scalar(cuflg) )
    error("cuflg must be a scalar")
  elseif( cuflg != 0 | cuflg != 1)
    error(["cuflg = ",num2str(cuflg),", should be 0 or 1"]);
  endif

  ## extract  state space information
  [da,db,dc,dd,Ts1,nc,nz,stname,inname,outname,yd] = sys2ss(sys);

  ## compute number of steps
  if(Ts1 > Ts2)
    error(["Current sampling time=",num2str(Ts1),"< Ts2=",num2str(Ts2)]);
  endif
  nstp = floor(Ts2/Ts1+0.5);
  if(abs((Ts2 - Ts1*nstp)/Ts1) > 1e-12)
    warning(["dmr2d: Ts1=",num2str(Ts1),", Ts2=",num2str(Ts2), ...
      ", selecting nsteps=",num2str(nstp),"; mismatch"]);
  endif

  if(isempty(sprefix) & isempty(idx))
    warning("both sprefix and idx are empty; returning dsys=sys");
    fidx = [];
    dsys = sys;
    return
  elseif(isempty(sprefix))
    fidx = idx;
  else
    fidx = reshape(idx,1,length(idx));
    ## find states whose name begins with any strings in sprefix.
    ns = length(sprefix);
    for kk=1:ns
      spk = nth(sprefix,kk);  # get next prefix and length
      spl = length(spk);

      ## check each state name
      for ii=1:nz
        sti = nth(stname,ii);  # compare spk with this state name
        if(length(sti) >= spl)
          ## if the prefix matches and ii isn't already in the list, add ii
          if(strcmp(sti(1:spl),spk) & !any(fidx == ii) ) 
            fidx = sort([fidx,ii]);
          endif
        endif
      endfor
    endfor
  endif

  if(nstp == 0)
    warning("dmr2d: nstp = 0; setting tsam and returning");
    dsys = syschtsam(sys,Ts2);
    return
  elseif(nstp < 0)
    error(["nstp = ", num2str(nstp)," < 0; this shouldn't be!"]);
  endif

  ## permute system matrices
  pv = sysreorder(nz,fidx);
  pv = pv(nz:-1:1);          # reverse order to put fast modes in leading block

  ## construct inverse permutation
  Inz = eye(nz);
  pvi = (Inz(pv,:)'*[1:nz]')';

  ## permute A, B (stname permuted for debugging only)
  da = da(pv,pv);
  db = db(pv,:);
  stname = stname(pv,:);

  ## partition A, B:
  lfidx = length(fidx);
  bki = 1:lfidx;
  a11 = da(bki,bki);
  b1 = db(bki,:);

  if(lfidx < nz)
    lfidx1 = lfidx+1;
    bki2 = (lfidx1):nz;
    a12 = da(bki,bki2);
    b2 = db(bki2,:);
  else
    warning("dmr2d: converting entire A,B matrices to new sampling rate");
    lfidx1 = -1;
    bki2 = [];
  endif

  ## begin system conversion: nstp steps

  ## compute abar_{n-1}*a12 and appropriate b matrix stuff
  a12b = a12;      # running  total of abar_{n-1}*a12
  a12w = a12;      # current a11^n*a12  (start with n = 0)
  if(cuflg)
    b1b = b1;
    b1w = b1;
  else
    ## cuflg == 0, need to keep track of intersample inputs too
    nzdx = find(max(abs(b1)) != 0);  # FIXME: check tolerance relative to ||b1||
    b1w = b1(nzdx);
    innamenz = inname(nzdx);
    b1b = b1;                        # initial b1 must match columns in b2
  endif

  ## compute a11h = a11^nstp by squaring
  a11h = eye(size(a11));
  p2 = 1;
  a11p2 = a11;        #a11^p2

  nstpw = nstp;       # workspace for computing a11^nstp
  while(nstpw > 0.5)
    oddv = rem(nstpw,2);
    if(oddv)
      a11h = a11h*a11p2;
    endif
    nstpw = (nstpw-oddv)/2;
    if(nstpw > 0.5)
      a11p2 = a11p2*a11p2;    # a11^(next power of 2)
    endif
  endwhile
  
  ## FIXME: this part should probably also use squaring, but
  ## that would require exponentially growing memory.  What do do?
  for kk=2:nstp
    ## update a12 block to sum(a12 + ... + a11^(kk-1)*a12)
    a12w = a11*a12w;
    a12b = a12b + a12w;

    ## similar for b1 block (checking for cuflg first!)
    b1w = a11*b1w;
    if(cuflg)
      b1b = b1b + b1w;        # update b1 block just like we did a12
    else
      b1b = [b1b, b1w];       # append new inputs
      newin = strappend(innamenz,["_d",num2str(kk-1)]);
      inname = append(inname,newin);
    endif
  endfor

  ## reconstruct system and return
  da(bki,bki) = a11h;
  db(bki,1:columns(b1b)) = b1b;
  if(!isempty(bki2))
    da(bki,bki2) = a12b;
  endif

  da = da(pvi,pvi);
  db = db(pvi,:);
  stname = stname(pvi,:);

  ## construct new system and return
  dsys = ss2sys(da,db,dc,dd,Ts2,0,nz,stname,inname,outname,find(yd == 1));

  implicit_str_to_num_ok = save_val;	# restore value

endfunction