## Copyright (C) 1998, 2000, 2002, 2004, 2005, 2006, 2007
##               Auburn University.  All rights reserved.
##
##
## This program 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 3 of the License, or (at
## your option) any later version.
##
## This program 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 this program; see the file COPYING.  If not, see
## <http://www.gnu.org/licenses/>.

## -*- 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
## indices or names of states with sampling time 
## @code{sysgettsam(@var{sys})} (may be empty); see @code{cellidx}
## @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, @dots{}, _dn to indicate their
## delay from the starting time k @var{ts2}, i.e.
## u = [u_1; u_1_d1; @dots{}, u_1_dn] where u_1_dk is the input
## k*ts1 units of time after u_1 is sampled. (@var{ts1} is
## the original sampling time of the discrete time system 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

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

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

  ## parse input arguments
  if (nargin != 4)
    print_usage ();
  elseif (! isstruct (sys))
    error ("sys must be in system data structure form");
  elseif (! is_digital (sys))
    error ("sys must be discrete-time; continuous time passed");
  endif

  if (is_signal_list (idx) || ischar (idx))
    idx = sysidx (sys, "st", idx);
  elseif (! (isvector (idx) || isempty (idx)))
    error ("idx(%dx%d) must be a vector", rows (idx), columns (idx));
  elseif (any (idx <= 0))
    idv = find (idx <= 0);
    ii = idv(1);
    error ("idx(%d)=%g; entries of idx must be positive", ii, idx(ii));
  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=%g; invalid sampling time", Ts2);
  endif

  ## optional argument: cuflg
  if (nargin <= 4)
    cuflg = 1;          # default: constant inputs over Ts2 sampling interv.
  elseif (! isscalar (cuflg))
    error ("cuflg must be a scalar")
  elseif (cuflg != 0 || cuflg != 1)
    error ("cuflg = %g, should be 0 or 1", cuflg);
  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=%g > Ts2=%g", Ts1, Ts2);
  endif
  nstp = floor (Ts2/Ts1+0.5);
  if (abs ((Ts2 - Ts1*nstp)/Ts1) > 1e-12)
    warning ("dmr2d: Ts1=%g, Ts2=%g, selecting nsteps=%d; mismatch",
	     Ts1, Ts2, nstp);
  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 = sprefix{kk};  # get next prefix and length
      spl = length (spk);

      ## check each state name
      for ii = 1:nz
        sti = 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 = %d < 0; this shouldn't be!", nstp);
  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
    ## FIXME: check tolerance relative to ||b1||
    nzdx = find (max (abs (b1)) != 0);
    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 = sprintf ("%s_d%d", innamenz, kk-1);
      inname = __sysconcat__ (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 = ss (da, db, dc, dd, Ts2, 0, nz, stname, inname, outname,
	     find (yd == 1));

endfunction