/***********************
 *  FUNCTION:  subvec  *
 ***********************/
void subvec (a, b, c)
double *a, *b, *c;
{
int i;

for (i = 0; i <= m; i++)
    a[i] = b[i] - c[i];
}  /*  end of function subvec  */


/***********************
 *  FUNCTION:  maxvec  *
 ***********************/
double maxvec (a)
double *a;
{
int i;
double t, x = fabs(a[0]);

for (i = 1; i <= m; i++)
    {
    t = fabs(a[i]);
    if (t > x)
        x = t;
    }

return(x);
}  /*  end of function maxvec  */


/*********************************************************
 *  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:  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: 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: 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: 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:  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 < m; i++)
    for (j = 0; j < 2; j++)
        U->el[i].el[j] = k * A->el[i].el[j];
}  /* end of function scmultkv2 */


/******************************************
 *  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 < m; i++)
    add2(&U->el[i], &A->el[i], &B->el[i]);
}  /* end of function addkv2 */


/********************************
 *  FUNCTION: saxpy_triblock22  *
 ********************************/
void saxpy_triblock22 (Z, a, X, Y)
struct triblock22 *Z, *X, *Y;
double a;
{
int i, j, k;

for (k = 0; k < m; k++)
    for (i = 0; i < 2; i++)
        for (j = 0; j < 2; j++)
	    Z->a[k].el[i][j] = a*X->a[k].el[i][j] + Y->a[k].el[i][j];

for (k = 0; k < m-1; k++)
    for (j = 0; j < 2; j++)
        {
	Z->b[k].el[j] = a*X->b[k].el[j] + Y->b[k].el[j];
	Z->c[k].el[j] = a*X->c[k].el[j] + Y->c[k].el[j];
	}
}  /*  end of function saxpy_triblock22 */


/***********************************************************************
 *  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, mm2 = m-2;

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

for (j = 0; j <= mm2; 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 = mm2; 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: mult_tri2xu -- multiplies a triblock22 matrix by a kvec2 vector  *
 *  INPUTS:  M = triblock22 matrix                                             *
 *           u = kvec2 vector                                                  *
 *           r = M*u                                                           *
 *******************************************************************************/
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 <= m-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[m-2], &u->el[m-2]);
mult22_2(&temp2, &M->a[m-1], &u->el[m-1]);
add2(&r->el[m-1], &temp1, &temp2);
}  /* end of function mult_tri2xu */


/********************************************************************
 *  FUNCTION: make_M -- form matrix based on differential operator  *
 *                        A(x,t) d2u/dx2 + B(x,t) du/dx + C(x,t) u  *
 ********************************************************************/
void make_M (A, cc, c, h, t)
struct triblock22 *A;
double *cc, *c, *h, t;
{
int i, im1, ip1, i2, i2p1, i2p2, i2p3;

/************************************************************
 *  define entries of matrix A that are independent of BCs  *
 ************************************************************/
for (i = 1; i <= m-2; i++)
    {
    im1 = i - 1;
    i2 = 2 * i;
    i2p1 = i2 + 1;

    A->a[i].el[0][0] = L_gR(cc[i2],   c[i2],   h[i], t);
    A->a[i].el[1][0] = L_gR(cc[i2p1], c[i2p1], h[i], t);
    A->a[i].el[0][1] = L_fL(cc[i2],   c[i2],   h[i], t);
    A->a[i].el[1][1] = L_fL(cc[i2p1], c[i2p1], h[i], t);
    }

i = 0;
i2 = 2 * i;
i2p1 = i2 + 1;
A->a[i].el[0][1] = L_fL(cc[i2],   c[i2],   h[i], t);
A->a[i].el[1][1] = L_fL(cc[i2p1], c[i2p1], h[i], t);

i = m-1;
i2 = 2 * i;
i2p1 = i2 + 1;
A->a[i].el[0][0] = L_gR(cc[i2],   c[i2],   h[i], t);
A->a[i].el[1][0] = L_gR(cc[i2p1], c[i2p1], h[i], t);

for (i = 0; i <= m-2; i++)
    {
    i2 = 2*i;
    i2p1 = i2 + 1;
    i2p2 = i2 + 2;
    i2p3 = i2 + 3;

    A->b[i].el[0] = L_gL(cc[i2],   c[i2],   h[i], t);
    A->b[i].el[1] = L_gL(cc[i2p1], c[i2p1], h[i], t);
    
    ip1 = i+1;
    A->c[i].el[0] = L_fR(cc[i2p2], c[i2p2], h[ip1], t);
    A->c[i].el[1] = L_fR(cc[i2p3], c[i2p3], h[ip1], t);
    }

/**********************************************************
 *  define entries of matrix A that are dependent on BCs  *
 **********************************************************/
i = 0;
i2 = i*2;
i2p1 = i2 + 1;
if (BCTypeL == 1)  /* Dirichlet on L */
    {
    A->a[i].el[0][0] = L_gR(cc[i2],   c[i2],   h[i], t);
    A->a[i].el[1][0] = L_gR(cc[i2p1], c[i2p1], h[i], t);
    }
else  /* Neumann on L */
    {
    A->a[i].el[0][0] = L_fR(cc[i2],   c[i2],   h[i], t);
    A->a[i].el[1][0] = L_fR(cc[i2p1], c[i2p1], h[i], t);
    }

i = m-1;
i2 = i*2;
i2p1 = i2 + 1;
if (BCTypeR == 1)  /* Dirichlet on R */
    {
    A->a[i].el[0][1] = L_gL(cc[i2],   c[i2],   h[i], t);
    A->a[i].el[1][1] = L_gL(cc[i2p1], c[i2p1], h[i], t);
    }
else  /* Neumann on R */
    {
    A->a[i].el[0][1] = L_fL(cc[i2],   c[i2],   h[i], t);
    A->a[i].el[1][1] = L_fL(cc[i2p1], c[i2p1], h[i], t);
    }
}  /* end of function make_M */


/*******************************************************
 *  FUNCTION: make_T -- form matrix based on Q(x) * u  *
 *******************************************************/
void make_T (A, cc, c, h)
struct triblock22 *A;
double *cc, *c, *h;
{
int i, im1, ip1, i2, i2p1, i2p2, i2p3;

/************************************************************
 *  define entries of matrix A that are independent of BCs  *
 ************************************************************/
for (i = 1; i <= m-2; i++)
    {
    im1 = i - 1;
    i2 = 2 * i;
    i2p1 = i2 + 1;

    A->a[i].el[0][0] = J_gR(cc[i2],   c[i2],   h[i]);
    A->a[i].el[1][0] = J_gR(cc[i2p1], c[i2p1], h[i]);
    A->a[i].el[0][1] = J_fL(cc[i2],   c[i2]);
    A->a[i].el[1][1] = J_fL(cc[i2p1], c[i2p1]);
    }

i = 0;
i2 = 2 * i;
i2p1 = i2 + 1;
A->a[i].el[0][1] = J_fL(cc[i2],   c[i2]);
A->a[i].el[1][1] = J_fL(cc[i2p1], c[i2p1]);

i = m-1;
i2 = 2 * i;
i2p1 = i2 + 1;
A->a[i].el[0][0] = J_gR(cc[i2],   c[i2],   h[i]);
A->a[i].el[1][0] = J_gR(cc[i2p1], c[i2p1], h[i]);

for (i = 0; i <= m-2; i++)
    {
    i2 = 2*i;
    i2p1 = i2 + 1;
    i2p2 = i2 + 2;
    i2p3 = i2 + 3;

    A->b[i].el[0] = J_gL(cc[i2],   c[i2],   h[i]);
    A->b[i].el[1] = J_gL(cc[i2p1], c[i2p1], h[i]);
    
    ip1 = i+1;
    A->c[i].el[0] = J_fR(cc[i2p2], c[i2p2]);
    A->c[i].el[1] = J_fR(cc[i2p3], c[i2p3]);
    }

/**********************************************************
 *  define entries of matrix A that are dependent on BCs  *
 **********************************************************/
i = 0;
i2 = i*2;
i2p1 = i2 + 1;
if (BCTypeL == 1)  /* Dirichlet on L */
    {
    A->a[i].el[0][0] = J_gR(cc[i2],   c[i2],   h[i]);
    A->a[i].el[1][0] = J_gR(cc[i2p1], c[i2p1], h[i]);
    }
else  /* Neumann on L */
    {
    A->a[i].el[0][0] = J_fR(cc[i2],   c[i2]);
    A->a[i].el[1][0] = J_fR(cc[i2p1], c[i2p1]);
    }

i = m-1;
i2 = i*2;
i2p1 = i2 + 1;
if (BCTypeR == 1)  /* Dirichlet on R */
    {
    A->a[i].el[0][1] = J_gL(cc[i2],   c[i2],   h[i]);
    A->a[i].el[1][1] = J_gL(cc[i2p1], c[i2p1], h[i]);
    }
else  /* Neumann on R */
    {
    A->a[i].el[0][1] = J_fL(cc[i2],   c[i2]);
    A->a[i].el[1][1] = J_fL(cc[i2p1], c[i2p1]);
    }
}  /* end of function make_T */
	     

/************************************************************
 *  FUNCTION:  blank -- prints n blank lines to the screen  *
 ************************************************************/
void blank (n)
int n;
{
int i;

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