/*****************************************************************
 *  FUNCTION:  dvector -- dynamically allocates space in memory  *
 *                        for a vector                           *
 *  INPUT: n = number of elements in vector                      *
 *  OUTPUT: v = vector of size n                                 *
 *****************************************************************/
double *dvector(int n)
{
double *v;
  
v = (double *) calloc(n,sizeof(double));
return(v);
}  /* end of function dvector */


/****************************************************************
 *  FUNCTION:  dmatrix -- dynamically allocate space in memory  *
 *                        for a 2-dimensional matrix            *
 *  INPUT: rows = number of rows                                *
 *          cols = number of columns                            *
 *  OUTPUT: A = rows x columns matrix                           *
 *****************************************************************/
double **dmatrix(int rows, int cols)
{
int i;
double **A;

A = (double **) calloc(rows, sizeof(double *));
for(i = 0.; i < rows; i++)
    A[i] = (double *) calloc(cols, sizeof(double));
return(A);
}  /* end of function dmatrix */


/****************************************************************
 *  FUNCTION:  d3matrix -- dynamically allocate space in memory *
 *                         for a 3-dimensional matrix           *
 *  INPUTS: sheets = number of sheets                           *
 *          rows = number of rows                               *
 *          cols = number of columns                            *
 *  OUTPUT: A = sheets x rows x columns matrix                  *
 *****************************************************************/
double ***d3matrix(int sheets, int rows, int cols)
{
int i, j;
double ***A;

A = (double ***) calloc(sheets, sizeof(double **));
for (i = 0; i < sheets; i++)
    {
    A[i] = (double **) calloc(rows, sizeof(double *));
    for (j = 0; j < rows; j++)
        A[i][j] = (double *) calloc(cols, sizeof(double));
    }
return(A);
}  /* end of function d3matrix */


/********************************************
 *  FUNCTION:  blank -- prints blank lines  *
 *  INPUT: n = number of blank lines        *
 ********************************************/
void blank(int n)
{
int i;

for (i = 0; i < n; i++)
    printf("\n");
}  /* end of function blank */


/*************************************************************
 *  FUNCTION:  print22 -- prints a 2x2 matrix to the screen  *
 *  INPUT:   A = 2x2 matrix to be printed                    *
 *************************************************************/
void print22 (A)
struct m22 *A;
{
int i, j;

for (i = 0; i < 2; i++)
    {
    blank(1);
    for (j = 0; j < 2; j++)
        printf("%e  ", A->el[i][j]);
    }
blank(2);
}  /* end of function print22  */


/********************************************************************************
 *  FUNCTION: compmat -- given two matrices, determine the greatest difference  *
 *                       (in absolute value) among corresponding entries        *
 ********************************************************************************/
double compmat (A, B)
double **A, **B;
{
int i, j;
double t, p=0;

for (i = 0; i <= ym; i++)
    for (j = 0; j <= xm; j++)
        {
        t = fabs(A[i][j] - B[i][j]);
	if (t > p)
	    p = t;
	}
return(p);
}  /* end of function compmat */


/***********************
 *  FUNCTION: copymat  *
 ***********************/
void copymat (A, B)
double **A, **B;
{
int i, j;

for (i = 0; i <= ym; i++)
    for (j = 0; j <= xm; j++)
        A[i][j] = B[i][j];
}  /* end of function copymat */


/************************************************************
 *  FUNCTION:  inv22 -- determines inverse of a 2x2 matrix  *
 *  INPUTS:  A = 2x2 matrix whose inverse we compute        *
 *           B = 2x2 matrix computed s.t. B = inv(A)        *
 ************************************************************/
void inv22 (B, A)
struct m22 *B, *A;
{
double det;

det = A->el[0][0]*A->el[1][1] - A->el[1][0]*A->el[0][1];

if (det == 0.0)
    {
    printf("\n\nSingular 2x2 matrix\n");
    print22(A);
    printf("exiting from function inv22...\n\n");
    exit(0);
    }

B->el[0][0] =  A->el[1][1] / det;
B->el[1][1] =  A->el[0][0] / det;
B->el[0][1] = -A->el[0][1] / det;
B->el[1][0] = -A->el[1][0] / det;
}  /* end of function inv22 */


/************************************************************
 *  FUNCTION: mult22 -- multiply two 2x2 matrices together  *
 *  INPUTS:  A, B                                           *
 *  OUTPUT:  R = AB                                         *
 ************************************************************/
void mult22 (R, A, B)
struct m22 *R, *A, *B;
{
R->el[0][0] = A->el[0][0]*B->el[0][0] + A->el[0][1]*B->el[1][0];
R->el[0][1] = A->el[0][0]*B->el[0][1] + A->el[0][1]*B->el[1][1];
R->el[1][0] = A->el[1][0]*B->el[0][0] + A->el[1][1]*B->el[1][0];
R->el[1][1] = A->el[1][0]*B->el[0][1] + A->el[1][1]*B->el[1][1];
}  /* end of function mult22 */


/***************************************************************
 *  FUNCTION:  opp22 -- produces the opposite of a 2x2 matrix  *
 *  INPUTS:  A = given 2x2 matrix                              *
 *           B = -A                                            *
 ***************************************************************/
void opp22 (B, A)
struct m22 *B, *A;
{
int i, j;

for (i = 0; i < 2; i++)
    for (j = 0; j < 2; j++)
        B->el[i][j] = -A->el[i][j];
}  /* end of function opp22 */


/*********************************************
 *  FUNCTION: add22 -- add two 2x2 matrices  *
 *  INPUTS:  A, B                            *
 *  OUTPUT:  R = A + B                       *
 *********************************************/
void add22 (R, A, B)
struct m22 *R, *A, *B;
{
R->el[0][0] = A->el[0][0] + B->el[0][0];
R->el[0][1] = A->el[0][1] + B->el[0][1];
R->el[1][0] = A->el[1][0] + B->el[1][0];
R->el[1][1] = A->el[1][1] + B->el[1][1];
}  /* end of function add22 */


/**************************************************
 *  FUNCTION: sub22 -- subtract two 2x2 matrices  *
 *  INPUTS:  A, B                                 *
 *  OUTPUT:  R = A - B                            *
 **************************************************/
void sub22 (R, A, B)
struct m22 *R, *A, *B;
{
R->el[0][0] = A->el[0][0] - B->el[0][0];
R->el[0][1] = A->el[0][1] - B->el[0][1];
R->el[1][0] = A->el[1][0] - B->el[1][0];
R->el[1][1] = A->el[1][1] - B->el[1][1];
}  /* end of function sub22 */


/***************************************************************
 *  FUNCTION: mult22_2 -- multiply a 2x2 matrix by a 2-vector  *
 *  INPUTS: A = 2x2 matrix                                     *
 *          x = 2-vector                                       *
 *          r = Ax                                             *
 ***************************************************************/
void mult22_2 (r, A, x)
struct m22 *A;
struct v2 *r, *x;
{
r->el[0] = A->el[0][0]*x->el[0] + A->el[0][1]*x->el[1];
r->el[1] = A->el[1][0]*x->el[0] + A->el[1][1]*x->el[1];
}  /* end of function mult22_2 */


/*****************************************
 *  FUNCTION: add2 -- add two 2-vectors  *
 *  INPUTS:  A, B                        *
 *  OUTPUT:  R = A + B                   *
 *****************************************/
void add2 (R, A, B)
struct v2 *R, *A, *B;
{
R->el[0] = A->el[0] + B->el[0];
R->el[1] = A->el[1] + B->el[1];
}  /* end of function add2 */


/************************************************************
 *  FUNCTION:  cmult2 -- multiply a c-matrix by a 2-vector  *
 *  INPUTS: C = c-matrix                                    *
 *          v = 2-vector                                    *
 *          r = Cv                                          *
 ************************************************************/
void cmult2 (r, C, v)
struct v2 *r, *C, *v;
{
r->el[0] = C->el[0] * v->el[1];
r->el[1] = C->el[1] * v->el[1];
}  /* end of function cmult2 */


/************************************************************
 *  FUNCTION:  bmult2 -- multiply a b-matrix by a 2-vector  *
 *  INPUTS: B = b-matrix                                    *
 *          v = 2-vector                                    *
 *          r = Bv                                          *
 ************************************************************/
void bmult2 (r, B, v)
struct v2 *r, *B, *v;
{
r->el[0] = B->el[0] * v->el[0];
r->el[1] = B->el[1] * v->el[0];
}  /* end of function bmult2 */


/****************************************************************
 *  FUNCTION:  mult44_4 -- multiply a 4x4 matrix by a 4-vector  *
 *  INPUTS: M = 4x4 matrix                                      *
 *          v = 4-vector                                        *
 *          u = Mv                                              *
 ****************************************************************/
void mult44_4 (u, M, v)
struct m44 *M;
struct v4 *u, *v;
{
struct v2 temp1, temp2;

mult22_2(&temp1, &M->A, &v->el[0]);
mult22_2(&temp2, &M->B, &v->el[1]);
add2(&u->el[0], &temp1, &temp2);

mult22_2(&temp1, &M->C, &v->el[0]);
mult22_2(&temp2, &M->D, &v->el[1]);
add2(&u->el[1], &temp1, &temp2);
}  /* end of function mult44_4 */


/******************************************************************
 *  FUNCTION:  bmult2_4 -- multiply a 4x4 b-matrix by a 4-vector  *
 *  INPUTS: B = 4x4 c matrix                                      *
 *          v = 4-vector                                          *
 *          u = Cv                                                *
 ******************************************************************/
void bmult2_4 (u, B, v)
struct v4 *u, *v;
struct m42 *B;
{
mult22_2(&u->el[0], &B->el[0], &v->el[0]);
mult22_2(&u->el[1], &B->el[1], &v->el[0]);
}  /* end of function bmult2_4 */


/*****************************************
 *  FUNCTION: add4 -- add two 4-vectors  *
 *  INPUTS:  A, B                        *
 *  OUTPUT:  R = A + B                   *
 *****************************************/
void add4 (R, A, B)
struct v4 *R, *A, *B;
{
add2(&R->el[0], &A->el[0], &B->el[0]);
add2(&R->el[1], &A->el[1], &B->el[1]);
}  /* end of function add4 */


/******************************************************************
 *  FUNCTION:  cmult2_4 -- multiply a 4x4 c-matrix by a 4-vector  *
 *  INPUTS: C = 4x4 c matrix                                      *
 *          v = 4-vector                                          *
 *          u = Cv                                                *
 ******************************************************************/
void cmult2_4 (u, C, v)
struct v4 *u, *v;
struct m42 *C;
{
mult22_2(&u->el[0], &C->el[0], &v->el[1]);
mult22_2(&u->el[1], &C->el[1], &v->el[1]);
}  /* end of function cmult2_4 */


/**********************************************
 *  FUNCTION: sub2 -- subtract two 2-vectors  *
 *  INPUTS:  r, a, b   (r = a-b)              *
 **********************************************/
void sub2 (r, a, b)
struct v2 *r, *a, *b;
{
r->el[0] = a->el[0] - b->el[0];
r->el[1] = a->el[1] - b->el[1];
}  /* end of function sub2 */


/*************************************************
 *  FUNCTION:  sub2_4 -- subtract two 4-vectors  *
 *  INPUTS: k, t, u = k-t                        *
 *************************************************/
void sub2_4 (u, k, t)
struct v4 *u, *k, *t;
{
sub2(&u->el[0], &k->el[0], &t->el[0]);
sub2(&u->el[1], &k->el[1], &t->el[1]);
}  /* end of function sub2_4 */


/*********************************************************
 *  FUNCTION: solve22 -- solves the 2x2 matrix equation  *
 *                            Au = k                     *
 *  INPUTS: A, u, k                                      *
 *********************************************************/
void solve22 (u, A, k)
struct m22 *A;
struct v2 *u, *k;
{
double a,b,c,d,e,f, det;

a = A->el[0][0];
b = A->el[0][1];
c = A->el[1][0];
d = A->el[1][1];
e = k->el[0];
f = k->el[1];

det = a*d - b*c;

u->el[0] = (d*e - b*f) / det;
u->el[1] = (a*f - c*e) / det;
}  /* end of function solve22 */


/***********************************************************
 *  FUNCTION: cmultb -- multiply a c-matrix by a b-matrix  *
 *  INPUTS: C = c-matrix                                   *
 *          B = b-matrix                                   *
 *          R = c*b                                        *
 ***********************************************************/
void cmultb (R, C, B)
struct v2 *R, *C, *B;
{
R->el[0] = C->el[0] * B->el[1];
R->el[1] = C->el[1] * B->el[1];
}  /* end of function cmultb */


/****************************************************************
 *  FUNCTION:  subb -- subtract a b-matrix from a dense matrix  *
 *  INPUTS:  A = 2x2 matrix                                     *
 *           B = 2-vector (matrix)                              *
 *  OUTPUT:  R = 2x2 matrix A-B                                 *
 ****************************************************************/
void subb (R, A, B)
struct m22 *R, *A;
struct v2 *B;
{
R->el[0][0] = A->el[0][0] - B->el[0];
R->el[0][1] = A->el[0][1];
R->el[1][0] = A->el[1][0] - B->el[1];
R->el[1][1] = A->el[1][1];
}  /* end of function subb */


/**********************************************
 *  FUNCTION:  inv44 -- inverts a 4x4 matrix  *
 *  INPUTS:  M = given matrix to be inverted  *
 *           V = inv(M) (i.e., we compute V)  *
 **********************************************/
void inv44 (V, M)
struct m44 *V, *M;
{
struct m22 Ainv, Binv, Cinv, Dinv, temp1, temp2,
           Astar, Bstar, Cstar, Dstar,
           Astarinv, Bstarinv, Cstarinv, Dstarinv;

/* get Ainv and Dinv */
inv22(&Ainv, &M->A);
inv22(&Dinv, &M->D);

/* get Schur complements */
mult22(&temp1, &M->B, &Dinv);
mult22(&temp2, &temp1, &M->C);
sub22(&temp1, &M->A, &temp2);
inv22(&V->A, &temp1);

mult22(&temp1, &M->C, &Ainv);
mult22(&temp2, &temp1, &M->B);
sub22(&temp1, &M->D, &temp2);
inv22(&V->D, &temp1);

/* get rest of V */
mult22(&temp1, &Dinv, &M->C);
opp22(&temp2, &temp1);
mult22(&V->C, &temp2, &V->A);

mult22(&temp1, &Ainv, &M->B);
opp22(&temp2, &temp1);
mult22(&V->B, &temp2, &V->D);
}  /* end of function inv44 */


/**************************************************************
 *  FUNCTION:  multb4 -- multiply a 4x4 matrix by a b matrix  *
 *  INPUTS:  M = 4x4 dense matrix                             *
 *           b = 4x4 b matrix                                 *
 *           u = Mb                                           *
 **************************************************************/
void multb4 (u, M, b)
struct m44 *M;
struct m42 *u, *b;
{
struct m22 temp1, temp2;

mult22(&temp1, &M->A, &b->el[0]);
mult22(&temp2, &M->B, &b->el[1]);
add22(&u->el[0], &temp1, &temp2);

mult22(&temp1, &M->C, &b->el[0]);
mult22(&temp2, &M->D, &b->el[1]);
add22(&u->el[1], &temp1, &temp2);
}  /* end of function multb4 */


/*********************************************************************
 *  FUNCTION:  cmultb4 -- multiply a 4x4 c matrix by a 4x4 b matrix  *
 *  INPUTS:  c, b, u = cb                                            *
 *********************************************************************/
void cmultb4 (u, c, b)
struct m42 *u, *c, *b;
{
mult22(&u->el[0], &c->el[0], &b->el[1]);
mult22(&u->el[1], &c->el[1], &b->el[1]);
}  /* end of function cmultb4 */


/*************************************************************************
 *  FUNCTION:  subb4 -- subtract a 4x4 b matrix from a 4x4 dense matrix  *
 *  INPUTS:  M = 4x4 dense matrix                                        *
 *           b = 4x4 b matrix                                            *
 *           U = M-b                                                     *
 *************************************************************************/
void subb4 (U, M, b)
struct m42 *b;
struct m44 *M, *U;
{
sub22(&U->A, &M->A, &b->el[0]);
sub22(&U->C, &M->C, &b->el[1]);
U->B = M->B;
U->D = M->D;
}  /* end of function subb4 */


/**********************************************************
 *  FUNCTION:  scmultkv2 -- multiply a kvec2 by a scalar  *
 *  INPUTS: U(=kA), k, A                                  *
 **********************************************************/
void scmultkv2 (U, k, A)
struct kvec2 *U, *A;
double k;
{
int i, j;

for (i = 0; i < xm; i++)
    for (j = 0; j < 2; j++)
        U->el[i].el[j] = k * A->el[i].el[j];
}  /* end of function scmultkv2 */


/**********************************************************
 *  FUNCTION:  scmultkv4 -- multiply a kvec4 by a scalar  *
 *  INPUTS: U(=kA), c, A                                  *
 **********************************************************/
void scmultkv4 (U, c, A)
struct kvec4 *U, *A;
double c;
{
int i, j, k;

for (i = 0; i < xm; i++)
    for (j = 0; j < 2; j++)
        for (k = 0; k < 2; k++)
	    U->el[i].el[j].el[k] = c * A->el[i].el[j].el[k];
}  /* end of function scmultkv4 */


/****************************************************************
 *  FUNCTION: mult_M24 -- multiplies a 24-matrix by a u-vector  *
 *  INPUTS:  M = 24-matrix                                      *
 *           u = u-vector                                       *
 *           r = Mu                                             *
 ****************************************************************/
void mult_M24 (r, M, u)
struct triblock24 *M;
struct kvec2 *r;
struct kvec4 *u;
{
struct v2 temp1, temp2, temp3;
int i, im1;

mult22_2(&temp1, &M->a[0].el[0], &u->el[0].el[0]);
mult22_2(&temp2, &M->a[0].el[1], &u->el[0].el[1]);
add2(&temp1, &temp1, &temp2);
mult22_2(&temp2, &M->b[0], &u->el[1].el[0]);
add2(&r->el[0], &temp1, &temp2);

for (i = 1; i <= xm-2; i++)
    {
    im1 = i-1;
    mult22_2(&temp1, &M->c[im1], &u->el[im1].el[1]);
    mult22_2(&temp2, &M->a[i].el[0], &u->el[i].el[0]);
    add2(&temp3, &temp1, &temp2);
    mult22_2(&temp1, &M->a[i].el[1], &u->el[i].el[1]);
    mult22_2(&temp2, &M->b[i], &u->el[i+1].el[0]);
    add2(&temp1, &temp1, &temp2);
    add2(&r->el[i], &temp1, &temp3);
    }

mult22_2(&temp1, &M->c[xm-2], &u->el[xm-2].el[1]);
mult22_2(&temp2, &M->a[xm-1].el[0], &u->el[xm-1].el[0]);
add2(&temp1, &temp1, &temp2);
mult22_2(&temp2, &M->a[xm-1].el[1], &u->el[xm-1].el[1]);
add2(&r->el[xm-1], &temp1, &temp2);
}  /* end of function mult_M24 */


/*********************************************************************************
 *  FUNCTION: mult_tri2xu -- multiplies a triblock22x matrix by a kvec2x vector  *
 *  INPUTS:  M = triblock22x matrix                                              *
 *           u = kvec2x vector                                                   *
 *           r = Mu                                                              *
 *********************************************************************************/
void mult_tri2xu (r, M, u)
struct triblock22 *M;
struct kvec2 *r, *u;
{
struct v2 temp1, temp2, temp3;
int i, im1;

mult22_2(&temp1, &M->a[0], &u->el[0]);
bmult2(&temp2, &M->b[0], &u->el[1]);
add2(&r->el[0], &temp1, &temp2);

for (i = 1; i <= xm-2; i++)
    {
    im1 = i-1;
    cmult2(&temp1, &M->c[im1], &u->el[im1]);
    mult22_2(&temp2, &M->a[i], &u->el[i]);
    add2(&temp3, &temp1, &temp2);
    bmult2(&temp2, &M->b[i], &u->el[i+1]);
    add2(&r->el[i], &temp3, &temp2);
    }

cmult2(&temp1, &M->c[xm-2], &u->el[xm-2]);
mult22_2(&temp2, &M->a[xm-1], &u->el[xm-1]);
add2(&r->el[xm-1], &temp1, &temp2);
}  /* end of function mult_tri2xu */


/******************************************
 *  FUNCTION:  addkv2 -- add two kvec2's  *
 *  INPUTS: U(=A+B), A, B                 *
 ******************************************/
void addkv2 (U, A, B)
struct kvec2 *U, *A, *B;
{
int i;

for (i = 0; i < xm; i++)
    add2(&U->el[i], &A->el[i], &B->el[i]);
}  /* end of function addkv2 */


/****************************************************************
 *  FUNCTION: mult_M42 -- multiplies a 42-matrix by a u-vector  *
 *  INPUTS:  M = 42-matrix                                      *
 *           u = u-vector                                       *
 *           r = Mu                                             *
 ****************************************************************/
void mult_M42 (r, M, u)
struct triblock22 *M;
struct kvec2 *r, *u;
{
struct v2 temp1, temp2, temp3;
int i, im1;

mult22_2(&temp1, &M->a[0], &u->el[0]);
bmult2(&temp2, &M->b[0], &u->el[1]);
add2(&r->el[0].el[0], &temp1, &temp2);

for (i = 1; i <= xm-2; i++)
    {
    im1 = i-1;
    cmult2(&temp1, &M->c[im1], &u->el[im1]);
    mult22_2(&temp2, &M->a[i], &u->el[i]);
    add2(&temp3, &temp1, &temp2);
    bmult2(&temp2, &M->b[i], &u->el[i+1]);
    add2(&r->el[i].el[0], &temp3, &temp2);
    }

cmult2(&temp1, &M->c[xm-2], &u->el[xm-2]);
mult22_2(&temp2, &M->a[xm-1], &u->el[xm-1]);
add2(&r->el[xm-1].el[0], &temp1, &temp2);
}  /* end of function mult_M42 */


/****************************************************
 *  FUNCTION:  seam -- "adds" together two vectors  *
 *  INPUTS: C = A+B                                 *
 ****************************************************/
void seam (C, A, B)
struct kvec4 *C;
struct kvec2 *A, *B;
{
int i, j;

for (i = 0; i < xm; i++)
    for (j = 0; j < 2; j++)
        {
	C->el[i].el[j].el[0] = A->el[i].el[j];
	C->el[i].el[j].el[1] = B->el[i].el[j];
        }
}  /* end of function seam  */


/*********************************************************************
 *  FUNCTION: mult_M44 -- multiplies a triblock44 matrix by a kvec4  *
 *  INPUTS:  M = triblock44 matrix                                   *
 *           u = kvec4 vector                                        *
 *           r = Mu                                                  *
 *********************************************************************/
void mult_M44 (r, M, u)
struct triblock44 *M;
struct kvec4 *r, *u;
{
struct v4 temp1, temp2, temp3;
int i, im1;

mult44_4(&temp1, &M->a[0], &u->el[0]);
bmult2_4(&temp2, &M->b[0], &u->el[1]);
add4(&r->el[0].el[0], &temp1, &temp2);

for (i = 1; i <= xm-2; i++)
    {
    im1 = i-1;
    cmult2_4(&temp1, &M->c[im1], &u->el[im1]);
    mult44_4(&temp2, &M->a[i], &u->el[i]);
    add4(&temp3, &temp1, &temp2);
    bmult2_4(&temp2, &M->b[i], &u->el[i+1]);
    add4(&r->el[i].el[0], &temp3, &temp2);
    }

cmult2_4(&temp1, &M->c[xm-2], &u->el[xm-2]);
mult44_4(&temp2, &M->a[xm-1], &u->el[xm-1]);
add4(&r->el[xm-1].el[0], &temp1, &temp2);
}  /* end of function mult_M44 */


/******************************************
 *  FUNCTION:  addkv4 -- add two kvec4's  *
 *  INPUTS: U(=A+B), A, B                 *
 ******************************************/
void addkv4 (U, A, B)
struct kvec4 *U, *A, *B;
{
int i, j;

for (i = 0; i < xm; i++)
    for (j = 0; j < 2; j++)
        add2(&U->el[i].el[j], &A->el[i].el[j], &B->el[i].el[j]);
}  /* end of function addkv4 */


/***********************************************
 *  FUNCTION:  subkv2 -- subtract two kvec2's  *
 *  INPUTS: U(=A-B), A, B                      *
 ***********************************************/
void subkv2 (U, A, B)
struct kvec2 *U, *A, *B;
{
int i;

for (i = 0; i < xm; i++)
    sub2(&U->el[i], &A->el[i], &B->el[i]);
}  /* end of function subkv2 */


/***********************************************
 *  FUNCTION:  subkv4 -- subtract two kvec4's  *
 *  INPUTS: U(=A-B), A, B                      *
 ***********************************************/
void subkv4 (U, A, B)
struct kvec4 *U, *A, *B;
{
int i;

for (i = 0; i < xm; i++)
    sub2_4(&U->el[i], &A->el[i], &B->el[i]);
}  /* end of function subkv4 */


/*********************************************************
 *  FUNCTION:  add_collvec -- add two collvecs together  *
 *  INPUTS: C = A+B                                      *
 *********************************************************/
void add_collvec (C, A, B)
struct collvec *A, *B, *C;
{
int i;

addkv2(&C->F, &A->F, &B->F);
addkv2(&C->L, &A->L, &B->L);
for (i = 0; i <= ym-2; i++)
    addkv4(&C->M[i], &A->M[i], &B->M[i]);
}  /* end of function add_collvec */


/***********************************************************************
 *  FUNCTION:  tridiag2 -- solves the 1-d collocation matrix equation  *
 *                            Au = k                                   *
 *  INPUTS:  A = block tridiagonal matrix; each block is a 2x2 matrix  *
 *           k = known RHS vector                                      *
 *           u = solution vector                                       *
 ***********************************************************************/
void tridiag2 (u, A, k)
struct triblock22 *A;
struct kvec2 *u, *k;
{
struct v2 t2;
struct m22 bet, inv;
struct gvec2 g;

int j, jp1;

/*************************
 *  forward elimination  *
 *************************/
bet = A->a[0];
solve22(&u->el[0], &bet, &k->el[0]);

for (j = 0; j <= xm-2; j++)
    {
    jp1 = j+1;

    solve22(&g.el[j], &bet, &A->b[j]);
    
    cmultb(&t2, &A->c[j], &g.el[j]);
    subb(&bet, &A->a[jp1], &t2);
    
    cmult2(&t2, &A->c[j], &u->el[j]);
    sub2(&t2, &k->el[jp1], &t2);
    solve22(&u->el[jp1], &bet, &t2);
    }

/***********************
 *  back substitution  *
 ***********************/
for (j = xm-2; j >= 0; j--)
    {
    bmult2(&t2, &g.el[j], &u->el[j+1]);
    sub2(&u->el[j], &u->el[j], &t2);
    }
}  /* end of function tridiag2 */


/***********************************************************************
 *  FUNCTION:  tridiag4 -- solves the matrix equation                  *
 *                            Au = k                                   *
 *  INPUTS:  A = block tridiagonal matrix; each block is a 4x4 matrix  *
 *           k = known RHS vector                                      *
 *           u = solution vector                                       *
 ***********************************************************************/
void tridiag4 (u, A, k)
struct triblock44 *A;
struct kvec4 *u, *k;
{
struct m44 bet, inv;
struct gvec4 g;
struct m42 temp42;
struct v4 temp4;

int j, jp1;

/*************************
 *  forward elimination  *
 *************************/
inv44(&inv, &A->a[0]);
mult44_4(&u->el[0], &inv, &k->el[0]);

for (j = 0; j <= xm-2; j++)
    {
    jp1 = j+1;
    
    multb4(&g.el[j], &inv, &A->b[j]);
    
    cmultb4(&temp42, &A->c[j], &g.el[j]);
    subb4(&bet, &A->a[jp1], &temp42);
    
    inv44(&inv, &bet);
    cmult2_4(&temp4, &A->c[j], &u->el[j]);
    sub2_4(&temp4, &k->el[jp1], &temp4);
    mult44_4(&u->el[jp1], &inv, &temp4);
    }

/***********************
 *  back substitution  *
 ***********************/
for (j = xm-2; j >= 0; j--)
    {
    bmult2_4(&temp4, &g.el[j], &u->el[j+1]);
    sub2_4(&u->el[j], &u->el[j], &temp4);
    }
}  /* end of function tridiag4 */


/*****************************************************************
 *  FUNCTION:  scmult_collvec -- multiply a collvec by a scalar  *
 *  INPUTS:  A,B = collvecs (B = cA)                             *
 *           c = scalar                                          *
 *****************************************************************/
void scmult_collvec (B, A, c)
struct collvec *B, *A;
double c;
{
int i;

scmultkv2(&B->F, c, &A->F);
scmultkv2(&B->L, c, &A->L);
for (i = 0; i <= ym-2; i++)
    scmultkv4(&B->M[i], c, &A->M[i]);
}  /* end of function scmult_collvec */