//-*- mode: c++; indent-tabs-mode: t; coding: utf-8; show-trailing-whitespace: t -*-

// file algebra.cpp

#include "algebra.hpp"

namespace algebra {

  // subroutine cdiz.
  
  bool cdivz (double *ar, double *ai, const double &br, const double &bi, const double &cr, const double &ci, const int &nKSn)
  {
    bool bReturnValue = false;
    double nFAC;
    double nSR;
    //
    if (ar && ai) {
      nFAC = cr * cr + ci * ci;
      nFAC = 1.0 / nFAC;
      nSR = br * nFAC * cr + bi * nFAC * ci;
      *ai = bi * nFAC * cr - br * nFAC * ci;
      *ar = nSR;
      if (nKSn < 0) {
	*ar = -*ar;
	*ai = -*ai;
      }
      bReturnValue = true;
    }
    return bReturnValue;
  }

  // subroutine cmultz.
  
  bool cmultz (double *ar, double *ai, const double &br, const double &bi, const double &cr, const double &ci, const int &nKSn)
  {
    bool bReturnValue = false;
    //
    if (ar && ai) {
      *ar = br * cr - bi * ci;
      *ai = bi * cr - br * ci;
      if (nKSn < 0) {
	*ar = -*ar;
	*ai = -*ai;
      }
    }
    return bReturnValue;
  }

  // subroutine trgwnd.
  
  bool trgwnd (const double &x, double *d17)
  {
    bool bReturnValue = false;
    int n13;
    //
    if (d17) {
      *d17 = x;
      if (fabs(x) >= 25000.0) {
	n13 = x / (2.0 * M_PI);
	*d17 -= 2.0 * n13 * M_PI;
	if (blkcom::nIprsUp >= 1)
	  *((std::ostream *) blkcom::pLFiles[ 5 ]) << " Angle unwind in \"trgwnd\" called by \"rfunl1\".    nchain, x, d17 =" << blkcom::nChain << x << *d17 << std::endl;
      }
      bReturnValue = true;
    }
    return bReturnValue;
  }

  // subroutine multmx.
  bool multmx(std::vector<double> &sA,		\
	      std::vector<double> &sB,		\
	      std::vector<double> &sC,		\
	      std::vector<double> &sTemp,	\
	      const size_t &n)
  {
    // Subroutine multmx  forms the matrix product   (c) = (a)(b)   where
    // matrices  (a), (b), and (c)  are all  n by n  square arrays.
    // Array  'temp'  is a scratch working area of not less than  2n
    // cells.   Arrays  (b)  and  (c)  may be identical, thereby placing
    // the product   (a)(b)   back into  (b) .    See subroutine  'mult'
    // which is called herein, for details about the storage scheme used
    // for these real, symmetric matrices.
    bool bReturnValue = false;
    long int l, ll0;
    size_t i, ii;
    size_t j;
    size_t m;
    //
    ll0 = 0;
    ii = 0;
    for(j = 1; j <= n; j++) {
      for(i = 1; i <= n; i++) {
	if(i > j) {
	  l += (i - 1);
	} else {
	  l = ii + i;
	}
	sTemp[ i - 1 ] = sB[ l - 1 ];
      }
      m = n + i;
      std::vector<double> sTempX(sTemp.begin(), sTemp.begin() + n);
      std::vector<double> sTempY(sTemp.begin() + m - 1, sTemp.begin() + n);
      bReturnValue = algebra::mult(sA,					\
				   sTempX,				\
				   sTempY,				\
				   n,					\
				   ll0) &&				\
	movecopy::move(sTemp.data() + m - 1, sC.data() + ii, j);
      ii += j;
    }
    return bReturnValue;
  }

  // subroutine mult.
  bool mult(std::vector<double> &sA, \
	    std::vector<double> &sX, \
	    std::vector<double> &sY, \
	    const size_t &n,	     \
	    long int &icheck)
  {
    bool bReturnValue = false;
    double xx;
    double yy;
    size_t i, ii;
    size_t k;
    //
    ii = 0;
    k = 0;
    FOREVER {
      ++k;
      if(k > n)
	break;
      xx = sX[ k - 1 ];
      yy = sY[ k - 1 ];
      if(icheck == 0)
	yy = 0.0;
      if(icheck < 0)
	yy = -yy;
      for(i = 1; i <= k; i++) {
	  ++ii;
	  sY[ i - 1 ] += sA[ ii - 1 ] * xx;
	  yy += sA[ ii - 1 ] * sX[ i - 1 ];
	  sY[ k - 1 ] = yy;
      }
      bReturnValue = true;
    }
    return bReturnValue;
  }
  
  // subroutine dgelg.
  void dgelg(std::vector<double> &sR,		\
	     std::vector<double> &sA,		\
	     const size_t &m,			\
	     const size_t &n,			\
	     const float &eps,			\
	     int &ier)
  {
    /*
     * Purpose:
     * to solve a general system of simultaneous linear equations.
     *
     * Usage:
     *   call dgelg(r, a, m, n, eps, ier)
     *
     * Description of parameters:
     *   r      - double precision m by n right hand side matrix
     *            (destroyed). On return r contains the solutions
     *            of the equations.
     *   a      - double precision m by m coefficient matrix
     *            (destroyed).
     *   m      - the number of equations in the system.
     *   n      - the number of right hand side vectors.
     *   eps    - single precision input constant which is used as
     *            relative tolerance for test on loss of
     *            significance.
     *   ier    - resulting error parameter coded as follows
     *            ier=0  - no error,
     *            ier=-1 - no result because of m less than 1 or
     *                     pivot element at any elimination step
     *                     equal to 0,
     *            ier=k  - warning due to possible loss of signifi-
     *                     cance indicated at elimination step k+1,
     *                     where pivot element was less than or
     *                     equal to the internal tolerance eps times
     *                     absolutely greatest element of matrix a.
     *
     * Remarks:
     *   Input matrices r and a are assumed to be stored columnwise
     *   in m*n resp. m*m successive storage locations. On return
     *   solution matrix r is stored columnwise too.
     *   The procedure gives results if the number of equations m is
     *   greater than 0 and pivot elements at all elimination steps
     *   are different from 0. However warning ier=k - if given -
     *   indicates possible loss of significance. In case of a well
     *   scaled matrix a and appropriate tolerance eps, ier=k may be
     *   interpreted that matrix a has the rank k. No warning is
     *   given in case m=1.
     *
     * Method:
     *   Solution is done by means of gauss-elimination with
     *   complete pivoting.
     */
    size_t i, ii, ist, j, k, l, lend, ll, lst, mm, nm;
    double piv, pivi, tb, tol;
    /*
     */
    if(m <= 0)
      goto a23;	  
    ier = 0;
    piv = 0.0;
    mm = m * m;
    nm = n * m;
    for(l = 1; l <= mm; l++) {
      tb = fabs(sA[ l - 1 ]);
      if(tb > piv) {
	piv = tb;
	i = l;
      }
    }
    tol = eps * piv;
    lst = 1;
    for(k = 1; k <= m; k++) {
      if(piv <= 0.0)
	goto a23;
      if(ier != 0)
	goto a7;
      if(piv > tol)
	goto a7;
      ier = (long int) k - 1;

    a7:
      pivi = 1.0 / sA[ i - 1 ];
      j = (i - 1) / m;
      i = i - j * m - k;
      j = j + 1 - k;
      for(l = k; l <= nm; l += m) {
	ll = l + i;
	tb = pivi * sR[ ll - 1 ];
	sR[ ll - 1 ] = sR[ l - 1 ];
	sR[ l - 1 ] = tb;
      }
      if(k >= m)
	goto a18;
      lend = lst + m - k;
      if(j <= 0)
	goto a12;
      ii = j * m;
      for(l = lst; l <= lend; l++) {
	tb = sA[ l - 1 ];
	ll = l + ii;
	sA[ l - 1 ] = sA[ ll - 1 ];
	sA[ ll - 1 ] = tb;
      }

    a12:
      for(l = lst; l <= mm; l += m) {
	ll = l + i;
	tb = pivi * sA[ ll - 1 ];
	sA[ ll - 1 ] = sA[ l - 1 ];
	sA[ l - 1 ] = tb;
      }
      sA[ lst - 1 ] = j;
      piv = 0.0;
      ++lst;
      j = 0;
      for(ii = lst; ii <= lend; ii++) {
	pivi = -sA[ ii - 1 ];
	ist = ii + m;
	++j;
	for(l = ist; l <= mm; l += m) {
	  ll = l - j;
	  sA[ l - 1 ] += pivi * sA[ ll - 1 ];
	  tb = fabs(sA[ l - 1 ]);
	  if(tb > piv) {
	    piv = tb;
	    i = l;
	  }
	}
	for(l = k; l <= nm; l += m) {
	  ll = l + j;
	  sR[ ll - 1 ] += pivi * sR[ l - 1 ];
	}
      }
      lst += m;
    }
    
  a18:
    if(m > 1)
      goto a19;
    if (m == 1)
      goto a22;
    goto a23;

  a19:
    ist = mm + m;
    lst = m + 1;
    for(i = 2; i <= m; i++) {
      ii = lst - i;
      ist -= lst;
      l = ist - m;
      l = (long int) (sA[ l - 1 ] + 0.5);
      for(j = ii; j <= nm; j += m) {
	tb = sR[ j - 1 ];
	ll = j;
	for(k = ist; k <= mm; k += mm) {
	  ++ll;
	  tb -= sA[ k - 1 ] * sR[ ll - 1 ];
	}
	k = j + l;
	sR[ j - 1 ] = sR[ k - 1 ];
	sR[ k - 1 ] = tb;
      }
    }

  a22:
    return;

  a23:
    ier = -1;
  }

  // subroutine matmul
  void matmul (double aum[ 3 ][ 3 ], double bum[ 3 ][ 3 ])
  {
    size_t n1, n2, n3, n5;
    double cum[ 3 ][ 3 ];
    //
    n5 = 3;
    for(n1 = 1; n1 <= n5; n1++) {
      for(n2 = 1; n2 <= n5; n2++) {
	cum[ n1 - 1 ][ n2 - 1 ] = aum[ n1 - 1 ][ 0 ] * bum[ 0 ][ n2 - 1 ];
	for (n3 = 2; n3 <= n5; n3++) {
	  cum[ n1 - 1 ][ n2 - 1 ] = cum[ n1 - 1 ][ n2 - 1 ] + aum[ n1 - 1 ][ n3 - 1 ] * bum[ n3 - 1 ][ n2 - 1 ];
	}
      }
    }
    for(n1 =1; n1 <= n5; n1++) {
      for(n2 = 1; n2 <= n5; n2++) {
	aum[ n1 - 1 ][ n2 - 2 ] = cum[ n1 - 1 ][ n2 - 1 ];
      }
    }
  }

  // subroutine matvec.
  void matvec(float aum[ 3 ][ 3 ], float yum[ 15 ])
  {
    size_t n1, n2, n3;
    float x[ 3 ];
    //
    n1 = 3;
    for (n2 = 1; n2 <= n1; n2++) {
      for (n3 = 1; n3 <= n1; n3++) {
	x[ n2 - 1 ] = x[ n2 - 1 ] + aum[ n2 - 1 ][ n3 - 1 ] * yum[ n3 - 1 ];
      }
    }
    for (n2 = 1; n2 <= n1; n2++)
      yum[ n2 - 1 ] = x[ n2 - 1 ];
  }
  
}

// end of file algebra.cpp