/*=======================================================================

    Functions for vector operations

    Copyright 1993 (C) David Hinds - All Rights Reserved
    
    Source file vectors.c Version 3.3, Updated 3/15/93

    In general, these routines all assume scaler and vector operands
    are of type float_t, and results are calculated with type dbl_t.
    
=======================================================================*/

#include <stdlib.h>
#include <math.h>
#include <stdio.h>

#define VECTORS_C
#include "utypes.h"
#include "errors.h"
#include "umath.h"
#include "xalloc.h"
#include "vectors.h"

static const char ident_c[] = "@(#) vectors.c version 3.3 (3/15/93)";

/*=====================================================================*/

/* Returns sum(i=0..n-1, v[3*i]) */

dbl_t vsumby3(const float_t v[], uint_t n)
  {
  dbl_t sum;
  uint_t i, j;

  sum = *v; n--; v += 3;
# ifdef _UNROLL
  j = n & 7;
  for (i = 0; i != j; i++, v += 3) sum += *v;
  for ( ; i != n; i += 8, v += 24)
    sum += (dbl_t)v[0] + (dbl_t)v[3] +
           (dbl_t)v[6] + (dbl_t)v[9] +
           (dbl_t)v[12] + (dbl_t)v[15] +
           (dbl_t)v[18] + (dbl_t)v[21];
#else
  for (i = 0; i != n; i++, v += 3) sum += *v;
#endif
  return sum;
  }

/*=====================================================================*/

/* Returns sum(i=0..n-1, v[i]^2) */
  
dbl_t vsumsqr(const float_t v[], uint_t n)
  {
  dbl_t sum;
  uint_t i, j;
  
  sum = DSQR(*v); n--; v++;
# ifdef _UNROLL
  j = n & 7;
  for (i = 0; i != j; i++, v++) sum += DSQR(*v);
  for ( ; i != n; i += 8, v += 8)
    sum += DSQR(v[0]) + DSQR(v[1]) + DSQR(v[2]) + DSQR(v[3]) +
           DSQR(v[4]) + DSQR(v[5]) + DSQR(v[6]) + DSQR(v[7]);
# else
  for (i = 0; i != n; i++, v++) sum += DSQR(*v);
# endif
  return sum;
  }

/*=====================================================================*/

/* Returns sum(i=0..n-1, a[i]*b[i]) */

dbl_t dvdot(const dbl_t a[], const dbl_t b[], uint_t n)
  {
  dbl_t sum;
  uint_t i, j;
  
  sum = (*a) * (*b); n--; a++; b++;
# ifdef _UNROLL
  j = n & 7;
  for (i = 0; i != j; i++, a++, b++) sum += (*a) * (dbl_t)(*b);
  for ( ; i != n; i += 8, a += 8, b += 8)
    sum += (a[0] * b[0] + a[1] * b[1] +
            a[2] * b[2] + a[3] * b[3] +
            a[4] * b[4] + a[5] * b[5] +
            a[6] * b[6] + a[7] * b[7]);
# else
  for (i = 0; i != n; i++, a++, b++) sum += (*a) * (*b);
# endif
  return sum;
  }

/*=====================================================================*/

/* Returns sum(i=0..n-1, a[3*i]*b[3*i]) */
  
dbl_t vdotby3(const float_t a[], const float_t b[], uint_t n)
  {
  dbl_t sum;
  uint_t i, j;
  
  sum = (dbl_t)(*a) * (dbl_t)(*b); n--; a += 3; b += 3;
# ifdef _UNROLL
  j = n & 7;
  for (i = 0; i != j; i++, a += 3, b += 3)
    sum += (dbl_t)(*a) * (dbl_t)(*b);
  for ( ; i != n; i += 8, a += 24, b += 24)
    sum += ((dbl_t)a[0] * (dbl_t)b[0] +
            (dbl_t)a[3] * (dbl_t)b[3] +
            (dbl_t)a[6] * (dbl_t)b[6] +
            (dbl_t)a[9] * (dbl_t)b[9] +
            (dbl_t)a[12] * (dbl_t)b[12] +
            (dbl_t)a[15] * (dbl_t)b[15] +
            (dbl_t)a[18] * (dbl_t)b[18] +
            (dbl_t)a[21] * (dbl_t)b[21]);
# else
  for (i = 0; i != n; i++, a += 3, b += 3)
    sum += (dbl_t)(*a) * (dbl_t)(*b);
# endif
  return sum;
  }

/*=====================================================================*/

/* Returns index of minimal element */

uint_t vmin(const float_t v[], uint_t n)
  {
  float_t min;
  uint_t i, j;
  min = *v; i = 0; v++; n--;
  for (j = 0; j != n; j++, v++)
    if (min > *v) { min = *v; i = j; };
  return i;
  } /* vmin */
  
/*=====================================================================*/

uint_t vmax(const float_t v[], uint_t n)
  {
  float_t max;
  uint_t i, j;
  max = *v; i = 0; v++; n--;
  for (j = 0; j != n; j++, v++)
    if (max < *v) { max = *v; i = j; };
  return i;
  } /* vmax */
  
/*=====================================================================*/

/* Returns min(i=0..n-1, v[i*s]) */
  
float_t vminby(const float_t v[], uint_t n, const uint_t s)
  {
  float_t min;
  uint_t i, j;

  min = *v; v += s; n--;
# ifdef _UNROLL
  j = n & 7;
  for (i = 0; i != j; i++, v += s) if (min > *v) min = *v;
  for ( ; i != n; i += 8, v += s) {
    if (min > *v) min = *v;
    v += s; if (min > *v) min = *v;
    v += s; if (min > *v) min = *v;
    v += s; if (min > *v) min = *v;
    v += s; if (min > *v) min = *v;
    v += s; if (min > *v) min = *v;
    v += s; if (min > *v) min = *v;
    v += s; if (min > *v) min = *v;
    }
#else
  for (i = 0; i != n; i++, v += s) if (min > *v) min = *v;
#endif
  return min;
  } /* vminby */
  
/*=====================================================================*/

/* Returns max(i=0..n-1, v[i*s]) */
  
float_t vmaxby(const float_t v[], uint_t n, const uint_t s)
  {
  float_t max;
  uint_t i, j;

  max = *v; v += s; n--;
# ifdef _UNROLL
  j = n & 7;
  for (i = 0; i != j; i++, v += s) if (max < *v) max = *v;
  for ( ; i != n; i += 8, v += s) {
    if (max < *v) max = *v;
    v += s; if (max < *v) max = *v;
    v += s; if (max < *v) max = *v;
    v += s; if (max < *v) max = *v;
    v += s; if (max < *v) max = *v;
    v += s; if (max < *v) max = *v;
    v += s; if (max < *v) max = *v;
    v += s; if (max < *v) max = *v;
    }
#else
  for (i = 0; i != n; i++, v += s) if (max < *v) max = *v;
#endif
  return max;
  } /* vmaxby */
  
/*=====================================================================*/

/* Returns sum(i=0..n-1, v[i*s]) */
  
dbl_t vsumby(const float_t v[], uint_t n, const uint_t s)
  {
  dbl_t sum;
  uint_t i, j;

  sum = *v; v += s; n--;
# ifdef _UNROLL
  j = n & 7;
  for (i = 0; i != j; i++, v += s) sum += *v;
  for ( ; i != n; i += 8, v += s) {
    sum += *v;
    v += s; sum += *v;
    v += s; sum += *v;
    v += s; sum += *v;
    v += s; sum += *v;
    v += s; sum += *v;
    v += s; sum += *v;
    v += s; sum += *v;
    }
#else
  for (i = 0; i != n; i++, v += s) sum += *v;
#endif
  return sum;
  } /* vsumby */
  
/*=====================================================================*/

/* Returns sum(i=0..n-1, v[i*s]^2) */
  
dbl_t vsumsqrby(const float_t v[], uint_t n, const uint_t s)
  {
  dbl_t sum;
  uint_t i, j;

  sum = DSQR(*v); v += s; n--;
# ifdef _UNROLL
  j = n & 7;
  for (i = 0; i != j; i++, v += s) sum += DSQR(*v);
  for ( ; i != n; i += 8, v += s) {
    sum += DSQR(*v);
    v += s; sum += DSQR(*v);
    v += s; sum += DSQR(*v);
    v += s; sum += DSQR(*v);
    v += s; sum += DSQR(*v);
    v += s; sum += DSQR(*v);
    v += s; sum += DSQR(*v);
    v += s; sum += DSQR(*v);
    }
#else
  for (i = 0; i != n; i++, v += s) sum += DSQR(*v);
#endif
  return sum;
  } /* vsumby */
  
/*=====================================================================*/

/* Returns sum(i=0..n-1, a[i*s]*b[i*s]) */
  
dbl_t vdotby(const float_t a[], const float_t b[], uint_t n, const uint_t s)
  {
  dbl_t dot;
  uint_t i, j;

  dot = (dbl_t)(*a) * (dbl_t)(*b); a += s; b += s; n--;
# ifdef _UNROLL
  j = n & 7;
  for (i = 0; i != j; i++, a += s, b += s)
    dot += (dbl_t)(*a) * (dbl_t)(*b);
  for ( ; i != n; i += 8, a += s, b += s) {
    dot += (dbl_t)(*a) * (dbl_t)(*b);
    a += s; b += s; dot += (dbl_t)(*a) * (dbl_t)(*b);
    a += s; b += s; dot += (dbl_t)(*a) * (dbl_t)(*b);
    a += s; b += s; dot += (dbl_t)(*a) * (dbl_t)(*b);
    a += s; b += s; dot += (dbl_t)(*a) * (dbl_t)(*b);
    a += s; b += s; dot += (dbl_t)(*a) * (dbl_t)(*b);
    a += s; b += s; dot += (dbl_t)(*a) * (dbl_t)(*b);
    a += s; b += s; dot += (dbl_t)(*a) * (dbl_t)(*b);
    }
#else
  for (i = 0; i != n; i++, a += s, b += s)
    dot += (dbl_t)(*a) * (dbl_t)(*b);
#endif
  return dot;
  } /* vdotby */
  
/*=====================================================================*/

/* Vector in-place divide by constant */

void dvdiv(dbl_t a[], uint_t n, const dbl_t b)
  {
  for ( ; n != 0; n--) {
    *a /= b;
    a++;
    }
  }

/*=====================================================================*/

/* multiply vector by a matrix */

void vxform(const dbl_t a[], uint_t n, dbl_t **m, dbl_t b[])
  {
  uint_t i, j;

  for (i = 0; i < n; i++) {
    b[i] = a[0] * m[i][0];
    for (j = 1; j < n; j++)
      b[i] += a[j] * m[i][j];
    }
  } /* vxform */
  
/*=====================================================================*/

/* Gather operations for vectors and matrices */

void gather(const float_t a[], const uint_t ni, const uint_t i[], float_t b[])
  {
  uint_t j;
  for (j = 0; j < ni; j++) b[j] = a[i[j]];
  } /* gather */
  
void dgather(const dbl_t a[], const uint_t ni, const uint_t i[], dbl_t b[])
  {
  uint_t j;
  for (j = 0; j < ni; j++) b[j] = a[i[j]];
  } /* dgather */
  
void mgather(dbl_t **a, const uint_t ni, const uint_t i[], dbl_t **b)
  {
  uint_t j, k;
  for (j = 0; j < ni; j++) {
    for (k = 0; k < ni; k++) b[j][k] = a[i[j]][i[k]];
    }
  } /* mgather */

/*=====================================================================*/

/* LU decomposition routine from Numerical Recipes, pg. 43 */

#define TINY 1e-20
  
void ludcmp(dbl_t **a, const uint_t n, uint_t index[], dbl_t *d)
  {
  uint_t i, imax, j, k;
  dbl_t big, dum, sum, temp;
  dbl_t *vv;

  vv = c_mnew(n, dbl_t);
  *d = 1.0;
  for (i = 0; i < n; i++) {
    big = 0.0;
    for (j = 0; j < n; j++)
      if ((temp = fabs(a[i][j])) > big) big = temp;
    if (big == 0.0) 
      error("singular matrix in LU decomposition\n");
    vv[i] = 1.0/big;
    }
  for (j = 0; j < n; j++) {
    for (i = 0; i < j; i++) {
      sum = a[i][j];
      for (k = 0; k < i; k++) sum -= a[i][k]*a[k][j];
      a[i][j] = sum;
      }
    big = 0.0;
    for (i = j; i < n; i++) {
      sum = a[i][j];
      for (k = 0; k < j; k++) sum -= a[i][k]*a[k][j];
      a[i][j] = sum;
      if ( (dum=vv[i]*fabs(sum)) >= big) {
        big = dum;
        imax = i;
        }
      }
    if (j != imax) {
      for (k = 0; k < n; k++) {
        dum = a[imax][k];
        a[imax][k] = a[j][k];
        a[j][k] = dum;
        }
      *d = -(*d);
      vv[imax] = vv[j];
      }
    index[j] = imax;
    if (a[j][j] == 0.0) a[j][j] = TINY;
    if (j < n-1) {
      dum = 1/a[j][j];
      for (i = j+1; i < n; i++) a[i][j] *= dum;
      }
    }
  free(vv);
  } /* ludcmp */

/*=====================================================================*/

/* LU back substitution from Numerical Recipes, pg. 44 */

void lubksb(dbl_t **lu, const uint_t n, const uint_t index[], dbl_t b[])
  {
  uint_t ip, j;
  int_t i, ii = (-1);
  dbl_t sum;

  for (i = 0; i < n; i++) {
    ip = index[i];
    sum = b[ip];
    b[ip] = b[i];
    if (ii >= 0)
      for (j = ii; j < i; j++) sum -= lu[i][j]*b[j];
    else
      if (sum) ii = i;
    b[i] = sum;
    }
  for (i = n-1; i >= 0; i--) {
    sum = b[i];
    for (j = i+1; j < n; j++) sum -= lu[i][j]*b[j];
    b[i] = sum/lu[i][i];
    }
    
  } /* lubksb */
  
/*=====================================================================*/

/* Inverse of LU matrix from Numerical Recipes, pg. 45*/
  
void luinvrt(dbl_t **lu, const uint_t n, const uint_t index[],
             dbl_t **y)
  {
  uint_t i, j;
  dbl_t *col;
  
  col = c_mnew(n, dbl_t);
  for (j = 0; j < n; j++) {
    for (i = 0; i < n; i++) col[i] = 0.0;
    col[j] = 1.0;
    lubksb(lu, n, index, col);
    for (i = 0; i < n; i++) y[i][j] = col[i];
    }
  free(col);
  } /* luinvrt */
  
/*=====================================================================*/

/* Determinant of LU matrix from Numerical Recipes, pg. 46 */
  
dbl_t ludet(dbl_t **lu, const uint_t n, dbl_t d)
  {
  uint_t i;

  for (i = 0; i < n; i++) d *= lu[i][i];
  return d;
  }