/******************************************************************
 *                                                                *
 * File          : iterativ.c                                     *
 * Programmers   : Scott D. Cohen and Alan C. Hindmarsh @ LLNL    *
 * Last Modified : 1 September 1994                               *
 *----------------------------------------------------------------*
 * This is the implementation file for the iterativ.h header      *
 * file. It contains the implementation of functions that may be  *
 * useful for many different iterative solvers of A x = b.        *
 *                                                                *
 ******************************************************************/


#include "iterativ.h"
#include "llnltyps.h"
#include "vector.h"
#include "llnlmath.h"


#define FACTOR RCONST(1000.0)
#define ZERO   RCONST(0.0)
#define ONE    RCONST(1.0)


/************************* ModifiedGS ***********************************
 This implementation of ModifiedGS is a slight modification of a previous
 modified Gram-Schmidt routine (called mgs) written by Milo Dorr.
*************************************************************************/
 
int ModifiedGS(N_Vector *v, real **h, int k, int p, real *new_vk_norm)
{
  int  i, k_minus_1, i0;
  real new_norm_2, new_product, vk_norm, temp;
  
  vk_norm = RSqrt(N_VDotProd(v[k],v[k]));
  k_minus_1 = k - 1;
  i0 = MAX(k-p, 0);
  
  /* Perform modified Gram-Schmidt */
  
  for (i=i0; i < k; i++) {
    h[i][k_minus_1] = N_VDotProd(v[i], v[k]);
    N_VLinearSum(ONE, v[k], -h[i][k_minus_1], v[i], v[k]);
  }

  /* Compute the norm of the new vector at v[k].  */

  *new_vk_norm = RSqrt(N_VDotProd(v[k], v[k]));

  /* If the norm of the new vector at v[k] is less than
     FACTOR (== 1000) times unit roundoff times the norm of the
     input vector v[k], then the vector will be reorthogonalized
     in order to ensure that nonorthogonality is not being masked
     by a very small vector length.           */

  temp = FACTOR * vk_norm;
  if ((temp + (*new_vk_norm)) != temp) return(0);
  
  new_norm_2 = ZERO;

  for (i=i0; i < k; i++) {
    new_product = N_VDotProd(v[i], v[k]);
    temp = FACTOR * h[i][k_minus_1];
    if ((temp + new_product) == temp) continue;
    h[i][k_minus_1] += new_product;
    N_VLinearSum(ONE, v[k],-new_product, v[i], v[k]);
    new_norm_2 += SQR(new_product);
  }

  if (new_norm_2 != ZERO) {
    new_product = SQR(*new_vk_norm) - new_norm_2;
    *new_vk_norm = (new_product > ZERO) ? RSqrt(new_product) : ZERO;
  }

  return(0);
}

/************************ ClassicalGS ********************************
 This implementation of ClassicalGS was contributed to by Homer Walker
 and Peter Brown.
**********************************************************************/

int ClassicalGS(N_Vector *v, real **h, int k, int p, real *new_vk_norm,
		N_Vector temp, real *s)
{
  int  i, k_minus_1, i0;
  real vk_norm;

  k_minus_1 = k - 1;
  
  /* Perform Classical Gram-Schmidt */

  vk_norm = RSqrt(N_VDotProd(v[k], v[k]));

  i0 = MAX(k-p, 0);
  for (i=i0; i < k; i++) {
    h[i][k_minus_1] = N_VDotProd(v[i], v[k]);
  }

  for (i=i0; i < k; i++) {
    N_VLinearSum(ONE, v[k], -h[i][k_minus_1], v[i], v[k]);
  }

  /* Compute the norm of the new vector at v[k].  */

  *new_vk_norm = RSqrt(N_VDotProd(v[k], v[k]));

  /* Reorthogonalize if necessary */

  if ((FACTOR * (*new_vk_norm)) < vk_norm) {

    for (i=i0; i < k; i++) {
      s[i] = N_VDotProd(v[i], v[k]);
    }

    if (i0 < k) {
      N_VScale(s[i0], v[i0], temp);
      h[i0][k_minus_1] += s[i0];
    }
    for (i=i0+1; i < k; i++) {
      N_VLinearSum(s[i], v[i], ONE, temp, temp);
      h[i][k_minus_1] += s[i];
    }
    N_VLinearSum(ONE, v[k], -ONE, temp, v[k]);

    *new_vk_norm = RSqrt(N_VDotProd(v[k],v[k]));
  }

  return(0);
}

/*************** QRfact **********************************************
 This implementation of QRfact is a slight modification of a previous
 routine (called qrfact) written by Milo Dorr.
**********************************************************************/

int QRfact(int n, real **h, real *q, int job)
{
  real c, s, temp1, temp2, temp3;
  int i, j, k, q_ptr, n_minus_1, code=0;

  switch (job) {
  case 0:
    /* Compute a new factorization of H. */
    code = 0;
    for (k=0; k < n; k++) {
      
      /* Multiply column k by the previous k-1 Givens rotations. */
      for (j=0; j < k-1; j++) {
	i = 2*j;
	temp1 = h[j][k];
	temp2 = h[j+1][k];
	c = q[i];
	s = q[i+1];
	h[j][k] = c*temp1 - s*temp2;
	h[j+1][k] = s*temp1 + c*temp2;
      }
      
      /* Compute the Givens rotation components c and s */
      q_ptr = 2*k;
      temp1 = h[k][k];
      temp2 = h[k+1][k];
      if( temp2 == ZERO) {
	c = ONE;
	s = ZERO;
      } else if (ABS(temp2) >= ABS(temp1)) {
	temp3 = temp1/temp2;
	s = -ONE/RSqrt(ONE+SQR(temp3));
	c = -s*temp3;
      } else {
	temp3 = temp2/temp1;
	c = ONE/RSqrt(ONE+SQR(temp3));
	s = -c*temp3;
      }
      q[q_ptr] = c;
      q[q_ptr+1] = s;
      if( (h[k][k] = c*temp1 - s*temp2) == ZERO) code = k+1;
    }
    break;

  default:
    /* Update the factored H to which a new column has been added. */
    n_minus_1 = n - 1;
    code = 0;
    
    /* Multiply the new column by the previous n-1 Givens rotations. */
    for (k=0; k < n_minus_1; k++) {
      i = 2*k;
      temp1 = h[k][n_minus_1];
      temp2 = h[k+1][n_minus_1];
      c = q[i];
      s = q[i+1];
      h[k][n_minus_1] = c*temp1 - s*temp2;
      h[k+1][n_minus_1] = s*temp1 + c*temp2;
    }
    
    /* Compute new Givens rotation and multiply it times the last two
       entries in the new column of H.  Note that the second entry of 
       this product will be 0, so it is not necessary to compute it. */
    temp1 = h[n_minus_1][n_minus_1];
    temp2 = h[n][n_minus_1];
    if (temp2 == ZERO) {
      c = ONE;
      s = ZERO;
    } else if (ABS(temp2) >= ABS(temp1)) {
      temp3 = temp1/temp2;
      s = -ONE/RSqrt(ONE+SQR(temp3));
      c = -s*temp3;
    } else {
      temp3 = temp2/temp1;
      c = ONE/RSqrt(ONE+SQR(temp3));
      s = -c*temp3;
    }
    q_ptr = 2*n_minus_1;
    q[q_ptr] = c;
    q[q_ptr+1] = s;
    if ((h[n_minus_1][n_minus_1] = c*temp1 - s*temp2) == ZERO)
      code = n;
  }
  
  return (code);
}

/*************** QRsol ************************************************
 This implementation of QRsol is a slight modification of a previous
 routine (called qrsol) written by Milo Dorr.
**********************************************************************/

int QRsol(int n, real **h, real *q, real *b)
{
  real c, s, temp1, temp2;
  int i, k, q_ptr, code=0;

  /* Compute Q*b. */
  
  for (k=0; k < n; k++) {
    q_ptr = 2*k;
    c = q[q_ptr];
    s = q[q_ptr+1];
    temp1 = b[k];
    temp2 = b[k+1];
    b[k] = c*temp1 - s*temp2;
    b[k+1] = s*temp1 + c*temp2;
  }

  /* Solve  R*x = Q*b. */

  for (k=n-1; k >= 0; k--) {
    if (h[k][k] == ZERO) {
      code = k + 1;
      break;
    }
    b[k] /= h[k][k];
    for (i=0; i < k; i++) b[i] -= b[k]*h[i][k];
  }
  
  return (code);
}