// f-expm.cc                                           -*- C++ -*-
/*

Copyright (C) 1993, 1994, 1995 John W. Eaton

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-1307, USA.

*/

// Written by A. S. Hodel <scotte@eng.auburn.edu>

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <cmath>

#include "CmplxAEPBAL.h"
#include "dbleAEPBAL.h"
#include "f77-uscore.h"

#include "defun-dld.h"
#include "error.h"
#include "gripes.h"
#include "help.h"
#include "oct-obj.h"
#include "user-prefs.h"
#include "utils.h"

extern "C"
{
  double F77_FCN (dlange, DLANGE) (const char*, const int&,
				   const int&, const double*,
				   const int&, double*); 

  double F77_FCN (zlange, ZLANGE) (const char*, const int&,
				   const int&, const Complex*,
				   const int&, double*); 
}

DEFUN_DLD_BUILTIN ("expm", Fexpm, Sexpm, FSexpm, 10,
  "expm (X): matrix exponential, e^A")
{
  Octave_object retval;

  int nargin = args.length ();

  if (nargin != 1)
    {
      print_usage ("expm");
      return retval;
    }

  tree_constant arg = args(0);

  // Constants for matrix exponential calculation.

  static double padec [] =
    {
      5.0000000000000000e-1,
      1.1666666666666667e-1,
      1.6666666666666667e-2,
      1.6025641025641026e-3,
      1.0683760683760684e-4,
      4.8562548562548563e-6,
      1.3875013875013875e-7,
      1.9270852604185938e-9,
    };

  int nr = arg.rows ();
  int nc = arg.columns ();

  int arg_is_empty = empty_arg ("expm", nr, nc);

  if (arg_is_empty < 0)
    return retval;
  if (arg_is_empty > 0)
    return Matrix ();

  if (nr != nc)
    {
      gripe_square_matrix_required ("expm");
      return retval;
    }

  char* balance_job = "B";	// variables for balancing

  if (arg.is_real_type ())
    {
      // Compute the exponential.

      Matrix m = arg.matrix_value ();

      if (error_state)
	return retval;

      double trshift = 0;		// trace shift value

      // Preconditioning step 1: trace normalization.

      for (int i = 0; i < nc; i++)
	trshift += m.elem (i, i);

      trshift /= nc;

      for (int i = 0; i < nc; i++)
	m.elem (i, i) -= trshift;

      // Preconditioning step 2: balancing.

      AEPBALANCE mbal (m, balance_job);
      m = mbal.balanced_matrix ();
      Matrix d = mbal.balancing_matrix ();

      // Preconditioning step 3: scaling.

      ColumnVector work(nc);
      double inf_norm
	= F77_FCN (dlange, DLANGE) ("I", nc, nc, m.fortran_vec (),nc,
				    work.fortran_vec ());

      int sqpow = (int) (inf_norm > 0.0
			 ? (1.0 + log (inf_norm) / log (2.0))
			 : 0.0);

      // Check whether we need to square at all.

      if (sqpow < 0)
	sqpow = 0;

      if (sqpow > 0)
	{
	  double scale_factor = 1.0;
	  for (int i = 0; i < sqpow; i++)
	    scale_factor *= 2.0;

	  m = m / scale_factor;
	}

      // npp, dpp: pade' approx polynomial matrices.

      Matrix npp (nc, nc, 0.0);
      Matrix dpp = npp;

      // Now powers a^8 ... a^1.

      int minus_one_j = -1;
      for (int j = 7; j >= 0; j--)
	{
	  npp = m * npp + m * padec[j];
	  dpp = m * dpp + m * (minus_one_j * padec[j]);
	  minus_one_j *= -1;
	}

      // Zero power.

      dpp = -dpp;
      for(int j = 0; j < nc; j++)
	{
	  npp.elem (j, j) += 1.0;
	  dpp.elem (j, j) += 1.0;
	}

      // Compute pade approximation = inverse (dpp) * npp.

      Matrix result = dpp.solve (npp);

      // Reverse preconditioning step 3: repeated squaring.

      while (sqpow)
	{
	  result = result * result;
	  sqpow--;
	}

      // Reverse preconditioning step 2: inverse balancing.

      result = result.transpose();
      d = d.transpose ();
      result = result * d;
      result = d.solve (result);
      result = result.transpose ();

      // Reverse preconditioning step 1: fix trace normalization.

      result = result * exp (trshift);

      retval = result;
    }
  else if (arg.is_complex_type ())
    {
      ComplexMatrix m = arg.complex_matrix_value ();

      if (error_state)
	return retval;

      Complex trshift = 0.0;		// trace shift value

      // Preconditioning step 1: trace normalization.

      for (int i = 0; i < nc; i++)
	trshift += m.elem (i, i);

      trshift /= nc;

      for (int i = 0; i < nc; i++)
	m.elem (i, i) -= trshift;

      // Preconditioning step 2: eigenvalue balancing.

      ComplexAEPBALANCE mbal (m, balance_job);
      m = mbal.balanced_matrix ();
      ComplexMatrix d = mbal.balancing_matrix ();

      // Preconditioning step 3: scaling.

      ColumnVector work (nc);
      double inf_norm
	= F77_FCN (zlange, ZLANGE) ("I", nc, nc, m.fortran_vec (), nc,
				    work.fortran_vec ());

      int sqpow = (int) (inf_norm > 0.0
			 ? (1.0 + log (inf_norm) / log (2.0))
			 : 0.0);

      // Check whether we need to square at all.

      if (sqpow < 0)
	sqpow = 0;

      if (sqpow > 0)
	{
	  double scale_factor = 1.0;
	  for (int i = 0; i < sqpow; i++)
	    scale_factor *= 2.0;

	  m = m / scale_factor;
	}

      // npp, dpp: pade' approx polynomial matrices.

      ComplexMatrix npp (nc, nc, 0.0);
      ComplexMatrix dpp = npp;

      // Now powers a^8 ... a^1.

      int minus_one_j = -1;
      for (int j = 7; j >= 0; j--)
	{
	  npp = m * npp + m * padec[j];
	  dpp = m * dpp + m * (minus_one_j * padec[j]);
	  minus_one_j *= -1;
	}

      // Zero power.

      dpp = -dpp;
      for (int j = 0; j < nc; j++)
	{
	  npp.elem (j, j) += 1.0;
	  dpp.elem (j, j) += 1.0;
	}

      // Compute pade approximation = inverse (dpp) * npp.

      ComplexMatrix result = dpp.solve (npp);
	
      // Reverse preconditioning step 3: repeated squaring.

      while (sqpow)
	{
	  result = result * result;
	  sqpow--;
	}

      // Reverse preconditioning step 2: inverse balancing.
      // XXX FIXME XXX -- should probably do this with Lapack calls
      // instead of a complete matrix inversion.

      result = result.transpose ();
      d = d.transpose ();
      result = result * d;
      result = d.solve (result);
      result = result.transpose ();

      // Reverse preconditioning step 1: fix trace normalization.

      result = result * exp (trshift);

      retval = result;
    }
  else
    {
      gripe_wrong_type_arg ("expm", arg);
    }

  return retval;
}

/*
;;; Local Variables: ***
;;; mode: C++ ***
;;; page-delimiter: "^/\\*" ***
;;; End: ***
*/