## Copyright (C) 1996,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{csys} =} d2c (@var{sys}@{,@var{tol}@})
## @deftypefnx {Function File } {@var{csys} =} d2c (@var{sys}, @var{opt})
## Convert discrete (sub)system to a purely continuous system.  Sampling
## time used is @code{sysgettsam(@var{sys})}
## 
## @strong{Inputs}
## @table @var
## @item   sys
##  system data structure with discrete components
## @item   tol
## Scalar value.
##  tolerance for convergence of default @code{"log"} option (see below)
## @item   opt
##  conversion option.  Choose from:
## @table @code
## @item         "log"
##  (default) Conversion is performed via a matrix logarithm.
## Due to some problems with this computation, it is
## followed by a steepest descent algorithm to identify continuous time 
## @var{A}, @var{B}, to get a better fit to the original data.  
## 
## If called as @code{d2c}(@var{sys},@var{tol}), @var{tol=}positive scalar, 
## 	the @code{"log"} option is used.  The default value for @var{tol} is 
## 	@code{1e-8}.
## @item        "bi"
##  Conversion is performed via bilinear transform 
## @math{z = (1 + s T / 2)/(1 - s T / 2)} where @var{T} is the 
## system sampling time (see @code{sysgettsam}).
## 
## FIXME: bilinear option exits with an error if @var{sys} is not purely discrete
## 
## @end table
## @end table
## @strong{Outputs} @var{csys} continuous time system (same dimensions and
## signal names as in @var{sys}).
## @end deftypefn
## 

 
function csys = d2c(sys,opt)

  ## Written by R. Bruce Tenison August 23, 1994
  ## Updated by John Ingram for system data structure  August 1996
  ## SYS_INTERNAL accesses members of system data structure

  if( (nargin != 1) & (nargin != 2) )
    usage("csys = d2c(sys[,tol]), csys = d2c(sys,opt)");
  elseif (!is_struct(sys))
    error("sys must be in system data structure");
  elseif(nargin == 1)
    opt = "log";
    tol = 1e-12;
  elseif(isstr(opt))   # all remaining cases are for nargin == 2
    tol = 1e-12;
    if( !(strcmp(opt,"log") | strcmp(opt,"bi") ) )
      error(["d2c: illegal opt passed=",opt]);
    endif
  elseif(!is_sample(opt))
    error("tol must be a postive scalar")
  elseif(opt > 1e-2)
    warning(["d2c: ridiculous error tolerance passed=",num2str(opt); ...
	", intended c2d call?"])
  else
    tol = opt;
    opt = "log";
  endif
  T = sysgettsam(sys);

  if(strcmp(opt,"bi"))
    ## bilinear transform
    ## convert with bilinear transform
    if (! is_digital(sys) )
       error("d2c requires a discrete time system for input")
    endif
    [a,b,c,d,tsam,n,nz,stname,inname,outname,yd] = sys2ss(sys);

    poles = eig(a);
    if( find(abs(poles-1) < 200*(n+nz)*eps) )
      warning("d2c: some poles very close to one.  May get bad results.");
    endif

    I = eye(size(a));
    tk = 2/sqrt(T);
    A = (2/T)*(a-I)/(a+I);
    iab = (I+a)\b;
    B = tk*iab;
    C = tk*(c/(I+a));
    D = d- (c*iab);
    stnamec = strappend(stname,"_c");
    csys = ss2sys(A,B,C,D,0,rows(A),0,stnamec,inname,outname);
  elseif(strcmp(opt,"log"))
    sys = sysupdate(sys,"ss");
    [n,nz,m,p] = sysdimensions(sys);
  
    if(nz == 0)
      warning("d2c: all states continuous; setting outputs to agree");
      csys = syssetsignals(sys,"yd",zeros(1,1:p));
      return;
    elseif(n != 0)
      warning(["d2c: n=",num2str(n),">0; performing c2d first"]);
      sys = c2d(sys,T);
    endif
    [a,b] = sys2ss(sys);
  
    [ma,na] = size(a);
    [mb,nb] = size(b);
  
    if(isempty(b) )
      warning("d2c: empty b matrix");
      Amat = a;
    else
      Amat = [a, b; zeros(nb,na), eye(nb)];
    endif
  
    poles = eig(a);
    if( find(abs(poles) < 200*(n+nz)*eps) )
      warning("d2c: some poles very close to zero.  logm not performed");
      Mtop = zeros(ma, na+nb);
    elseif( find(abs(poles-1) < 200*(n+nz)*eps) )
      warning("d2c: some poles very close to one.  May get bad results.");
      logmat = real(logm(Amat)/T);
      Mtop = logmat(1:na,:);
    else
      logmat = real(logm(Amat)/T);
      Mtop = logmat(1:na,:);
    endif
  
    ## perform simplistic, stupid optimization approach.
    ## should re-write with a Davidson-Fletcher CG approach
    mxthresh = norm(Mtop);
    if(mxthresh == 0)
      mxthresh = 1;
    endif
    eps1 = mxthresh;	#gradient descent step size
    cnt = max(20,(n*nz)*4);	#max number of iterations
    newgrad=1;	#signal for new gradient
    while( (eps1/mxthresh > tol) & cnt)
      cnt = cnt-1;
      ## calculate the gradient of error with respect to Amat...
      geps = norm(Mtop)*1e-8;
      if(geps == 0)
        geps = 1e-8;
      endif
      DMtop = Mtop;
      if(isempty(b))
        Mall = Mtop;
        DMall = DMtop;
      else
        Mall = [Mtop; zeros(nb,na+nb)];
        DMall = [DMtop; zeros(nb,na+nb) ];
      endif
  
      if(newgrad)
        GrMall = zeros(size(Mall));
        for ii=1:rows(Mtop)
          for jj=1:columns(Mtop)
  	  DMall(ii,jj) = Mall(ii,jj) + geps;
            GrMall(ii,jj) = norm(Amat - expm(DMall*T),'fro') ...
  	    - norm(Amat-expm(Mall*T),'fro');
      	  DMall(ii,jj) = Mall(ii,jj);
          endfor
        endfor
        GrMall = GrMall/norm(GrMall,1);
        newgrad = 0;
      endif
  
      ## got a gradient, now try to use it
      DMall = Mall-eps1*GrMall;
  
      FMall = expm(Mall*T);
      FDMall = expm(DMall*T);
      FmallErr = norm(Amat - FMall);
      FdmallErr = norm(Amat - FDMall);
      if( FdmallErr < FmallErr)
        Mtop = DMall(1:na,:);
        eps1 = min(eps1*2,1e12);
        newgrad = 1;
      else
        eps1 = eps1/2;
      endif
  
      if(FmallErr == 0)
        eps1 = 0;
      endif
      
    endwhile
  
    [aa,bb,cc,dd,tsam,nn,nz,stnam,innam,outnam,yd] = sys2ss(sys);
    aa = Mall(1:na,1:na);
    if(!isempty(b))
      bb = Mall(1:na,(na+1):(na+nb));
    endif
    csys = ss2sys(aa,bb,cc,dd,0,na,0,stnam,innam,outnam);
    
    ## update names
    nn = sysdimensions(sys);
    for ii = (nn+1):na
      strval = sprintf("%s_c",sysgetsignals(csys,"st",ii,1));
      csys = syssetsignals(csys,"st",strval,ii);
    endfor
  endif

endfunction