/*
 $Id: sysmatbl.cc,v 1.3 1996/10/11 09:10:08 roitzsch Exp $
 (C)opyright 1996 by Konrad-Zuse-Center, Berlin
 All rights reserved.
 Part of the Kaskade distribution
*/

#include "sysmatbl.h"
#include "utils.h"

#include "numerics.h"
#include "dirichlet.h"
#include "family.h"

//-------------------------------------------------------------------------
//-------------------------------------------------------------------------


MLBlockMatrix:: MLBlockMatrix(symmetryType symmetry0, int spaceDim0, int dim, 
			      int nComp0)

      : MLMatrix(symmetry0), MA28Matrix(spaceDim0),

	nComp(nComp0), dimension(dim), 
	decomposed(False), ILUDecomposed(False), inverseDiag(False),
	D(dim/nComp0), L(dim/nComp0), symU(0), 
	dirichlet(dim), mark(dim/nComp0), smooth(dim/nComp0), 
	rp(1), lp(1), Block(dim), aux(0),

	dataAlloc(nComp0*nComp0*sizeof(Num), 100)
{
    int i;

    noOfBlocks = dimension/nComp;
    aux = new Num[1+nComp];

    FORALL(L,i) L[i] = 0;
    FORALL(D,i)
    { 
	D[i] = new BlockNode;
	D[i]->setDataSpace(dataAlloc,nComp);
    }

    lp.resize(noOfBlocks);
    rp.resize(noOfBlocks);
    setBlockArray();

    FORALL(mark,i)   mark[i]   = False;
    FORALL(smooth,i) smooth[i] = True;
    FORALL(dirichlet,i) dirichlet[i] = False;

    reset();
}
//-------------------------------------------------------------------------

MLBlockMatrix:: ~MLBlockMatrix() 
{
    int i;
    removeLU();
    FORALL(D,i) if (D[i]) { D[i]->removeDataSpace(dataAlloc);  delete D[i]; }
    delete aux;
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: extend(int noOfNodes)
{
    int i;

    if (noOfNodes != dimension)		// matrix will be extended
    {
	MA28RemoveLUFactorization();
	decomposed    = False;
	ILUDecomposed = False;
	inverseDiag = False;
    
	dimension  = noOfNodes;
	noOfBlocks = noOfNodes/nComp;
	
	const int prevDim = D.high();
	D.extendAndCopy(noOfBlocks);

	for (i=prevDim+1; i<=noOfBlocks; ++i) 
	  { D[i] = new BlockNode;  D[i]->setDataSpace(dataAlloc,nComp); }
    }

    removeLU();

    L.resize(noOfBlocks);	
    FORALL(L,i) L[i] = 0;

    mark.resize  (noOfBlocks);
    smooth.resize(noOfBlocks);
    dirichlet.resize(noOfNodes);
    FORALL(mark,i)   mark[i]   = False;
    FORALL(smooth,i) smooth[i] = True;
    FORALL(dirichlet,i) dirichlet[i] = False;

    lp.resize(noOfBlocks);
    rp.resize(noOfBlocks);
    setBlockArray();

    reset();
}
//-------------------------------------------------------------------------

void MLBlockMatrix:: reset()
{
    int i;
    FORALL(L,i) resetRow(i);
    FORALL(D,i) D[i]->reset(nComp);
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: setBlockArray()
{
    int i, blockNo, n;

    Block.resize(dimension);
    i = 1;
    for (blockNo=1; blockNo<=noOfBlocks; ++blockNo)
      FORNCOMP(n) Block[i++] = blockNo;
}
//-------------------------------------------------------------------------

void MLBlockMatrix:: resetRow(int block)
{
    NeighbourBlock* np;
    for (np=L[block]; np; np=np->next)  np->reset(nComp);
}
//-------------------------------------------------------------------------

void MLBlockMatrix:: removeRow(int block)
{
    NeighbourBlock* np, *toRem;

    if (L[block] == 0) return;

    np = L[block];  
    L[block] = 0;

    while (np)
    {
	toRem = np;
	np = np->next;
	toRem->removeDataSpace(dataAlloc);
	delete toRem;
    }
}
//-------------------------------------------------------------------------

void MLBlockMatrix:: removeLU()
{
    int i;
    removeSymmetricExpansion();
    FORALL(L,i) removeRow(i);
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: store(const Matrix<Num>& AElem, 
			   const Vector<int>& globalNodes)
{
    int i, n, k, kMax, row, col;
    const int dim = globalNodes.high();

    Vector<Num*> Ap(nComp);

    for (i=1; i<=dim; i+=nComp)
    {
	n = i;
	FORALL(Ap,k) Ap[k] = AElem(n++); 	// set pointers to rows

	if ((row=globalNodes[i]))
	{
	    kMax = (symmetry==sym)? i : dim;

	    for (k=1; k<=kMax; k+=nComp) 
	    {
		if ((col=globalNodes[k])) addBlock(row, col, Ap);

		FORNCOMP(n) Ap[n] += nComp;
	    }
	}
    }
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: addBlock(int row, int col, Vector<Num*>& Ap)
{
    int i, k, bRow, bCol, bR, bC;
    NeighbourBlock* bp;

    bRow = Block[row];
    bCol = Block[col];

    if (bRow == bCol) 
    {
	if (symmetry==sym) 		// set transposed U
	{
	    FORNCOMP(i) for (k=1; k<i; ++k)  Ap[k][i] = Ap[i][k];
	}
	D[bRow]->Add(Ap);
	return;
    } 

    if (bRow > bCol) { bR = bRow; bC = bCol; }
    else	     { bR = bCol; bC = bRow; }


    for (bp=L[bR]; bp; bp=bp->next)
    {
	if (bp->block == bC) 
	{
	    if (bRow > bCol) bp->LAdd(Ap);
	    else	     bp->UAdd(Ap);
	    return;
	}
    }

    
    // -- 	neighbour entry not found: insert a new entry

    NeighbourBlock* newNeigh;
    if (symmetry == sym) newNeigh = new NeighbourBlock;
    else		 newNeigh = new AsymNeighbourBlock;

    newNeigh->setDataSpace(dataAlloc,nComp);
    
    newNeigh->block = bC;
    newNeigh->next  = L[bR];
    L[bR] = newNeigh;
    
    if (bRow > bCol) newNeigh->LAdd(Ap);
    else	     newNeigh->UAdd(Ap);
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: addBlock(int row, int col, Vector<Num>& data)
{
    int i;
    Vector<Num*> Ap(nComp);

    Ap[1] = data.v;
    for (i=2; i<=nComp; ++i) Ap[i] = Ap[i-1] + nComp;

    addBlock(row, col, Ap);
}
//-------------------------------------------------------------------------


Num& MLBlockMatrix:: operator() (int row, int col)
{
    int bRow, bCol, bC, bR, comp1, comp2;
    NeighbourBlock* bp;
    const int transpose = 1;

    bRow = Block[row];
    bCol = Block[col];

    comp1 = MCNode.comp(row, nComp);

    if (row==col) comp2 = comp1;
    else	  comp2 = MCNode.comp(col, nComp);


    if (bRow == bCol) return (*D[row])(comp1,comp2,nComp);    // diagonal block

    if (bRow > bCol) { bR = bRow; bC = bCol; }
    else	     { bR = bCol; bC = bRow; }

    for (bp=L[bR]; bp; bp=bp->next)
    {
	if (bp->block == bC) 
	{
	    if (bRow > bCol) return (*bp)(comp1,comp2,nComp);
	    else	     return (*bp)(comp1,comp2,nComp,transpose);
	}
    }

    cout << "\n*** MLBlockMatrix:: operator(...) : entry (" 
         << row << "," << col << ") not found\n";

    cout.flush(); abort();
    return (*D[1])(1,1,1);	// dummy return
}
//-------------------------------------------------------------------------

Num& MLBlockMatrix:: Diag(int node) 
{
    const int block = Block[node];
    const int comp  = MCNode.comp(node, nComp);

    return (*D[block])(comp,comp,nComp);
}
//-------------------------------------------------------------------------

Num MLBlockMatrix:: Diag(int node) const
{
    const int block = Block[node];
    const int comp  = MCNode.comp(node, nComp);

    return (*D[block])(comp,comp,nComp);
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: invertDiagonal()
{
    if (inverseDiag) return;

    Matrix<Num> AM(nComp,nComp), AMInv(nComp,nComp); 
    int i;
    FORALL(D,i) if (D[i]) D[i]->Invert(AM, AMInv);

    inverseDiag = True;
}
//-------------------------------------------------------------------------
//-------------------------------------------------------------------------


void MLBlockMatrix:: setDirichletBCs(DirichletBCs& dirichletBCs, Vector<Num>& b,
				     Vector<Num>& bSave, Num& Fct0, Num& E0)
{
    int i;

    if (symmetry == sym)  setSymDirichletBCs(dirichletBCs, b, bSave, Fct0, E0);
    else		 setASymDirichletBCs(dirichletBCs, b, bSave, Fct0, E0);

    dirichlet.resize(Dim());
    FORALL(dirichlet,i)
    {
	if (dirichletBCs.isSet(i)) dirichlet[i] = True;
	else 			   dirichlet[i] = False;
    }
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: setSymDirichletBCs(DirichletBCs& dirichletBCs, 
					Vector<Num>& b, Vector<Num>& /*bS*/, 
					Num& Fct0, Num& /*E0*/)
{
    NeighbourBlock* np;
    int i, row, col, comp1, comp2;
    Num value;
    
    Fct0 = 0.0;
    row = 0;

    FORALL(D,i)					// i is the block index
    {
	FORNCOMP(comp1)
	{
	    ++row;

	    if (dirichletBCs.isSet(row))		// row with Dir. BC
	    { 
		value = dirichletBCs.value(row);
		
		for (np=L[i]; np; np=np->next)
		{
		    col = nComp*(np->block - 1);
		    
		    FORNCOMP(comp2)
		    {
			++col;
			Num& Aij = (*np)(comp1,comp2,nComp);

			if (dirichletBCs.isSet(col)) 
			        Fct0 += Aij * value * dirichletBCs.value(col);
			else  b[col] -= Aij * value;

			Aij = 0.0;  // (*np)(comp1,comp2,nComp) = 0.0;
		    }
		}

		//  --   the off-diagonals of the diagonal block:

		col = nComp*(i-1);

		for (comp2=1; comp2<comp1; ++comp2)
		{
		    ++col;
		    Num& Aij = (*D[i])(comp1,comp2,nComp);
		    
		    if (dirichletBCs.isSet(col)) 
		    	    Fct0 += Aij * value * dirichletBCs.value(col);
		    else  b[col] -= Aij * value;

		    Aij = 0.0;  
		}
		
		Num& diag = (*D[i])(comp1,comp1,nComp);
		Fct0 -= b[row] * value;			// - u0*b0
		Fct0 += diag * value * value;  
		b[row] = diag * value;
	    }
	    else
	    {
		for (np=L[i]; np; np=np->next)
		{
		    col = nComp*(np->block - 1);
		    
		    FORNCOMP(comp2)
		    {
			++col;
			if (dirichletBCs.isSet(col))  
			{
			    Num& Aij = (*np)(comp1,comp2,nComp);
			    b[row] -= Aij * dirichletBCs.value(col);  // ADF*u0
			    
			    Aij = 0.0; 
			}
		    }    
		}
		
		//  --   the off-diagonals of the diagonal block:
		
		col = nComp*(i-1);
		
		for (comp2=1; comp2<comp1; ++comp2)
		{
		    ++col;
		    if (dirichletBCs.isSet(col)) 
		    {
			Num& Aij = (*D[i])(comp1,comp2,nComp);
			b[row] -= Aij * dirichletBCs.value(col);  // ADF*u0

			Aij = 0.0; 
		    }
		}
	    }
	}
    }    
    
    removeDirichletEntries(dirichletBCs);
    FORALL(D,i) D[i]->setTranspose(nComp);
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: setASymDirichletBCs(DirichletBCs& dirichletBCs, 
					 Vector<Num>& b, Vector<Num>& bSave, 
					 Num& Fct0, Num& E0)
{
    NeighbourBlock* np;
    int i, row, col, comp1, comp2;
    Num value;
    
    FORALL(bSave,i) bSave[i] = 0.0;  
    Fct0 = 0.0;
    E0   = 0.0;
  
    row = 0;

    FORALL(D,i)					// i is the block index
    {
	FORNCOMP(comp1)
	{
	    ++row;

	    if (dirichletBCs.isSet(row))		// row with Dir. BC
	    { 
		value = dirichletBCs.value(row);
		
		for (np=L[i]; np; np=np->next)
		{
		    col = nComp*(np->block - 1);
		    
		    FORNCOMP(comp2)
		    {
			++col;
			Num& Aij = (*np)(comp1,comp2,nComp);

			if (dirichletBCs.isSet(col)) 
			           Fct0 += Aij * value * dirichletBCs.value(col);
			else bSave[col] += Aij * value;

			Aij = 0.0;
		    }
		}

		//  --   the off-diagonals of the diagonal block:

		col = nComp*(i-1);

		FORNCOMP(comp2)
		{
		    ++col;
		    if (comp1 == comp2) continue;

		    Num& Aij = (*D[i])(comp1,comp2,nComp);
		    
		    if (dirichletBCs.isSet(col)) 
		    	       Fct0 += Aij * value * dirichletBCs.value(col);
		    else bSave[col] += Aij * value;

		    Aij = 0.0;
		}
		
		Num& diag = (*D[i])(comp1,comp1,nComp);
		Fct0 -= b[row] * value;			// - u0*b0
		Fct0 += diag * value * value;  
		b[row] = diag * value;

		E0 += b[row] * value;	
		E0 -= diag * value * value;
	    }


	    for (np=L[i]; np; np=np->next)		// the columns
	    {
		col = nComp*(np->block - 1);
		
		FORNCOMP(comp2)
		{
		    ++col;
		    if (dirichletBCs.isSet(col))  
		    {
			Num& Aji = (*np)(comp1,comp2,nComp,1);

			if (dirichletBCs.isSet(row))  
			{	
			    Fct0 += 0.5 * Aji * dirichletBCs.value(row) *
						    dirichletBCs.value(col);
			}
			else bSave[row] += 0.5 * Aji * dirichletBCs.value(col);
			
			Aji = 0.0;
		    }
		}    
	    }
	    
	    //  --   the off-diagonals of the diagonal block:
		
	    for (np=L[i]; np; np=np->next)		// the columns
	    {
		col = nComp*(i-1);
		
		FORNCOMP(comp2)
		{
		    ++col;
		    
		    if (comp1 == comp2) continue;
		    
		    Num& Aji = (*np)(comp1,comp2,nComp,1);
		    
		    if (dirichletBCs.isSet(col))  
		    {
			if (dirichletBCs.isSet(row))  
			{	
			    Fct0 += 0.5 * Aji * dirichletBCs.value(row) *
						    dirichletBCs.value(col);
			}
			else bSave[row] += 0.5 * Aji * dirichletBCs.value(col);
			
			Aji = 0.0;
		    }
		}
	    }
	}
    }
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: removeDirichletEntries(DirichletBCs& dirichletBCs)
{
    NeighbourBlock* np, *prev=0, *toRem;
    int i, row, col, n, count;

    row = 0;

    FORALL(D,i) 
    {
	count = 0;
	FORNCOMP(n) if (dirichletBCs.isSet(++row)) ++count;

	if (count == nComp) removeRow(i);	// -> dirichlet 'block row'
	else
	{ 
	    np = L[i];
	    while (np)
	    {
		col = nComp*(np->block - 1);
		    
		count = 0;
		FORNCOMP(n) if (dirichletBCs.isSet(++col)) ++count;
		    
		if (count == nComp)		// delete entry
		{
		    if (np == L[i]) L[i] = np->next;
		    else  	    prev->next = np->next;

		    toRem = np;
		    np = np->next;
		    toRem->removeDataSpace(dataAlloc);
		    delete toRem;
		}
		else
		{
		    prev = np;
		    np = np->next;
		}
	    }
	}
    }
}
//-------------------------------------------------------------------------
//-------------------------------------------------------------------------

void MLBlockMatrix:: markNodes()   { int i; FORALL(mark,i) mark[i] = True; }
void MLBlockMatrix:: unMarkNodes() { int i; FORALL(mark,i) mark[i] = False; }

//-------------------------------------------------------------------------

void MLBlockMatrix:: resetDirichletEntries(Vector<Num>& r) const
{
    int i;
    FORALL(dirichlet,i) { if (dirichlet[i])  r[i] = 0.0; }
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: GalerkinRestriction(MLMatrix& Ah0, Generation& generation,
					 const Vector<SBool>* /*critical*/)
{
    NeighbourBlock* np;
    Son* son, *son2;
    int  i, k, m, n, nFathers, nFathers2, father, father2;
    Real weight, weight2, fact;
    Vector<Num> b(nComp*nComp);

    if (!generation.castToMCGeneration()) 
    {
	cout << "\n*** GalerkinRestriction : generation is no MCGeneration\n";
	cout.flush(); abort();
    }

    MLBlockMatrix* Ah = Ah0.castToMLBlockMatrix();
    removeLU();
    reset();

    FORALL(Ah->D,i) 
    {
	// if (critical) if ((*critical)[i]) continue;

	son = generation.getSon(i);
	nFathers = son->NoOfFathers();


	// --		restrict diagonal term:

	if (nFathers == 1) 
	{
	    father = son->getFather();

	    if (isMarked(father)) 
	    {
		Ah->D[i]->getD(b,nComp);
		addBlock(father,father,b);
	    }
	}
	else
	{
	    for (k=1; k<=nFathers; ++k)
	    for (m=1; m<=k; ++m)
	    {
		father  = son->getFather(k);
		father2 = son->getFather(m);
		
		if (dirichlet[father] || dirichlet[father2]) continue;

		if (isMarked(father) || isMarked(father2))
		{
		    weight  = son->getWeight(k);
		    weight2 = son->getWeight(m);
		    fact = weight * weight2;

		    Ah->D[i]->getD(b,nComp);
		    FORALL(b,n) b[n] *= fact;
		    addBlock(father,father2,b);

		    if (symmetry==asym && father!=father2) 
		      addBlock(father2,father,b);
		}
	    }
	}
    
    
	// --		off-diagonal terms:

	for (np=Ah->L[i]; np; np=np->next) 
	{
	    // if (critical) if ((*critical)[np->node]) continue;

	    son2 = generation.getSon(np->block);
	    nFathers2 = son2->NoOfFathers();
	    
	    for (k=1; k<=nFathers;  ++k)
	    for (m=1; m<=nFathers2; ++m)
	    {	
		father  = son ->getFather(k);
		father2 = son2->getFather(m);

		if (dirichlet[father] || dirichlet[father2]) continue;

		if (isMarked(father) || isMarked(father2))
		{	       
		    weight  = son ->getWeight(k);
		    weight2 = son2->getWeight(m);
		    fact = weight*weight2;

		    if (symmetry == sym)
		    {
			if (father == father2) fact *= 2.0;
			np->getL(b,nComp);
			FORALL(b,n) b[n] *= fact;
			addBlock(father,father2,b);
		    }
		    else
		    {
			np->getL(b,nComp);
			FORALL(b,n) b[n] *= fact;
			addBlock(father,father2,b);

			np->getU(b,nComp);
			FORALL(b,n) b[n] *= fact;
			addBlock(father2,father,b);
		    }
		}
	    }
	}
    }
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: print() const
{
    NeighbourBlock* np;
    int i, row, col, comp1, comp2;

    if (decomposed || ILUDecomposed)
    {
	cout << "\n* Matrix decomposed: print() not available\n";
	return;
    }

    cout << "\n--- MLBlockMatrix diagonal: \n\n";  

    for (i=1; i<=dimension; ++i)  cout << Diag(i) << " ";
    

    cout << "\nLower Triangle:";
    row = 0;
    FORALL(D,i) 
    {
	FORNCOMP(comp1)
	{
	    ++row;
	    cout << "\nrow " << row << " : ";

	    for (np=L[i]; np; np=np->next) 
	    {
		col = nComp*(np->block-1);
		FORNCOMP(comp2)
		{
		    ++col;
		    cout << " "<< (*np)(comp1,comp2,nComp) << "[" << col << "]";
		}
	    }
	}
    }

    if (symmetry == asym)
    {
	cout << "\nUpper Triangle:";
	row = 0;
	FORALL(D,i) 
	{
	    FORNCOMP(comp1)
	    {
		++row;
		cout << "\ncol " << row << " : ";

		for (np=L[i]; np; np=np->next) 
		{
		    col = nComp*(np->block-1);
		    FORNCOMP(comp2)
		    {
			++col;
			cout << " " << (*np)(comp1,comp2,nComp,1) 
			    				<< "[" << col << "]";
		    }
		}
	    }
	}
    }

    if (symU)
    {
	cout << "\nSymmetric Expansion: \n";
	row = 0;
	FORALL(D,i) 
	{
	    FORNCOMP(comp1)
	    {
		++row;
		cout << "\nrow " << row << " : ";

		for (np=(*symU)[i]; np; np=np->next) 
		{
		    col = nComp*(np->block - 1);
		    FORNCOMP(comp2)
		    {
			++col;
			cout <<" "<< (*np)(comp1,comp2,nComp) << "[" <<col<< "]";
		    }
		}
	    }
	}
    }
    cout << "\n";
}
//-------------------------------------------------------------------------


int MLBlockMatrix:: memSpace(Bool print) const
{
    int space = dataAlloc.MemSpace();

    space += L.memSpace();
    space += D.memSpace();
    space += mark.memSpace();
    space += smooth.memSpace();
    space += dirichlet.memSpace();
    if (symU) space += symU->memSpace();

    space += MA28MemSpace();

    if (print)
      cout << "\n    Memory allocated for MLBlockMatrix without LU: " 
           << Form("%1.6f", float(space)/1e6) << " MB\n";

    return space;
}
//-------------------------------------------------------------------------

int MLBlockMatrix:: memSpaceLU(Bool print) const
{
    int i, space = 0;

    FORALL(L,i) if (L[i]) { space += L[i]->alloc.MemSpace(); break; }

    if (print)
      cout << "\n    Memory allocated for LU in MLSparseMatrices: " 
           << Form("%1.6f", float(space)/1e6) << " MB\n";

    return space;
}
//-------------------------------------------------------------------------
//-------------------------------------------------------------------------


void MLBlockMatrix:: countEntries(int* dim, int* nEntries)
{
    int i;
    NeighbourBlock* np;

    const int inBlock = nComp*nComp;
    int n = noOfBlocks*inBlock;		// diagonal contribution

    FORALL(L,i) { for (np=L[i]; np; np=np->next)  n += 2*inBlock; }

    *nEntries = n;
    *dim = dimension;
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: Decompose(Bool removeOriginal)
{
    int i;

    if (decomposed) return;

    MA28Decompose();

    if (removeOriginal) 
    { 
	removeLU(); 

	FORALL(D,i) { D[i]->removeDataSpace(dataAlloc);  delete D[i];  }
	D.resize(1);
	FORALL(D,i) D[i] = 0;
    }
    decomposed = True;
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: ILUDecompose(Real dropTol)
{
    if (ILUDecomposed || decomposed) return;

    MA28Decompose(dropTol);

    ILUDecomposed = True;
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: FBSubst(Vector<Num>& lhs, Vector<Num>& rhs)
{
    if (!decomposed)
    {
	cout << "\n*** MLBlockMatrix::FBSubst(...) : Matrix not decomposed\n";
	cout.flush(); abort();
    }

    int i;
    FORALL(lhs,i) lhs[i] = rhs[i];
    Num* b = lhs.v + lhs.low();

    MA28FBSubst(b);
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: ILUFBSubst(Vector<Num>& lhs, Vector<Num>& rhs)
{
    int i;

    if (!ILUDecomposed) FBSubst(lhs, rhs);
    else
    {
	FORALL(lhs,i) lhs[i] = rhs[i];
	Num* b = lhs.v + lhs.low();
	
	MA28FBSubst(b);
    }
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: fillMA28Vectors()
{
    int i, n, row, col, comp1, comp2;
    NeighbourBlock* np;
    const int transpose = 1;

    n = -1;			// start with 0 in ma28-arrays
    row = 0;

    FORALL(D,i)  		// array A must start with diagonal 
    {
	FORNCOMP(comp1)
	{
	    ++n;
	    ++row;
	    A  [n] = (*D[i])(comp1,comp1,nComp);
	    IRN[n] = row;
	    ICN[n] = row;
	} 
    }

    row = 0;

    FORALL(D,i)  				
    {
	FORNCOMP(comp1)	
	{
	    ++row;
	    col = nComp*(i-1);

	    FORNCOMP(comp2)     // rest of diagonal blocks 
	    {
		++col;
		if (comp2 != comp1)
		{
		    ++n;
		    A  [n] = (*D[i])(comp1,comp2,nComp);
		    IRN[n] = row;
		    ICN[n] = col;
		} 
	    }
	
	    for (np=L[i]; np; np=np->next)  	     // off-diagonals
	    {
		col = nComp*(np->block - 1);

		FORNCOMP(comp2)
		{
		    ++col;
		    ++n;
		    A  [n] = (*np)(comp1,comp2,nComp);
		    IRN[n] = row;
		    ICN[n] = col;

		    ++n;
		    A  [n] = (*np)(comp1,comp2,nComp,transpose);
		    IRN[n] = col;
		    ICN[n] = row;
		} 
	    }
	}
    }
}
//-------------------------------------------------------------------------
//-------------------------------------------------------------------------


void MLBlockMatrix:: setBlockPointers(Vector<Num>& lhs, Vector<Num>& rhs)
{
    int i;

    lp[1] = lhs.v;
    rp[1] = rhs.v;
    for (i=2; i<=noOfBlocks; ++i)
    {
	lp[i] = lp[i-1] + nComp;
	rp[i] = rp[i-1] + nComp;
    }
}
//-------------------------------------------------------------------------

void MLBlockMatrix:: Mult(Vector<Num>& lhs, Vector<Num>& rhs)
{
    NeighbourBlock* np;
    int i;

    if (lhs.v == rhs.v) callError("Mult");

    setBlockPointers(lhs,rhs);
    FORALL(lhs,i) lhs[i] = 0.0;

    FORALL(D,i) D[i]->Mult(lp[i], rp[i], nComp);
    FORALL(D,i) 
    {
	for (np=L[i]; np; np=np->next)
	{
	    np->LMult(lp[i], rp[np->block], nComp);
	    np->UMult(lp[np->block], rp[i], nComp);
	}
    }
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: ATMult(Vector<Num>& lhs, Vector<Num>& rhs)
{
    NeighbourBlock* np;
    int i;

    if (lhs.v == rhs.v) callError("ATMult");

    setBlockPointers(lhs,rhs);
    FORALL(lhs,i) lhs[i] = 0.0;

    FORALL(D,i) D[i]->TMult(lp[i], rp[i], nComp);
    FORALL(D,i) 
    {
	for (np=L[i]; np; np=np->next)
	{
	    np->UTMult(lp[i], rp[np->block], nComp);
	    np->LTMult(lp[np->block], rp[i], nComp);
	}
    }
}
//-------------------------------------------------------------------------

void MLBlockMatrix:: DiagDiv(Vector<Num>& lhs, Vector<Num>& rhs, Real omega)
{
    int i,n;

    if (!inverseDiag) invertDiagonal();
    setBlockPointers(lhs,rhs);

    FORALL(D,i) 
    {
	FORNCOMP(n) aux[n] = 0.0;
	D[i]->Div(aux, rp[i], nComp);
	FORNCOMP(n) lp[i][n] = aux[n];
    }
    if (!equal(omega,1.0)) FORALL(lhs,i) lhs[i] *= omega;
}
//-------------------------------------------------------------------------

void MLBlockMatrix:: DiagMult(Vector<Num>& lhs, Vector<Num>& rhs, Real omega)
{
    int i,n;

    setBlockPointers(lhs,rhs);

    FORALL(D,i) 
    {
	FORNCOMP(n) aux[n] = 0.0;
	D[i]->Mult(aux, rp[i], nComp);
	FORNCOMP(n) lp[i][n] = aux[n];
    }
    if (!equal(omega,1.0)) FORALL(lhs,i) lhs[i] *= omega;
}
//-------------------------------------------------------------------------

void MLBlockMatrix:: LMult(Vector<Num>& lhs, Vector<Num>& rhs)
{
    NeighbourBlock* np;
    int i;

    if (lhs.v == rhs.v) callError("LMult");

    setBlockPointers(lhs,rhs);
    FORALL(lhs,i) lhs[i] = 0.0;

    FORALL(D,i) 
      for (np=L[i]; np; np=np->next)  np->LMult(lp[i], rp[np->block], nComp);
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: UMult(Vector<Num>& lhs, Vector<Num>& rhs)
{
    NeighbourBlock* np;
    int i;

    if (lhs.v == rhs.v) callError("LMult");

    setBlockPointers(lhs,rhs);
    FORALL(lhs,i) lhs[i] = 0.0;

    FORALL(D,i) 
      for (np=L[i]; np; np=np->next)  np->UMult(lp[np->block],rp[i],nComp);
}
//-------------------------------------------------------------------------
//-------------------------------------------------------------------------

	//  F = D + omega*L  of Matrix A;  lhs = F*rhs;  


void MLBlockMatrix:: F(Vector<Num>& lhs, Vector<Num>& rhs, Real omega)
{
    NeighbourBlock* np;
    int i, n;

    if (lhs.v == rhs.v) callError("F"); 

    setBlockPointers(lhs,rhs);
    FORALL(lhs,i) lhs[i] = 0.0;

    FORALL(D,i) D[i]->Mult(lp[i], rp[i], nComp);

    if (equal(omega,1.0))
    {
	FORALL(D,i) 
	  for (np=L[i]; np; np=np->next) np->LMult(lp[i], rp[np->block], nComp);
    }
    else
    {
	FORALL(D,i) 
	{
	    for (np=L[i]; np; np=np->next) 
	    {
		FORNCOMP(n) aux[n] = 0.0;
		np->LMult(aux, rp[np->block], nComp); 
		FORNCOMP(n) lp[i][n] += omega * aux[n];
	    }
	}
    }
}
//-------------------------------------------------------------------------

	//  FT = D + omega*U  of Matrix A;  lhs = FT*rhs 


void MLBlockMatrix:: FT(Vector<Num>& lhs, Vector<Num>& rhs, Real omega)
{
    NeighbourBlock* np;
    int i;
    if (lhs.v == rhs.v) callError("FT"); 

    setBlockPointers(lhs,rhs);
    FORALL(lhs,i) lhs[i] = 0.0;

    FORALL(D,i) 
      for (np=L[i]; np; np=np->next) np->UMult(lp[np->block], rp[i], nComp);

    if (!equal(omega,1.0))  FORALL(lhs,i) lhs[i] *= omega;

    FORALL(D,i) D[i]->Mult(lp[i], rp[i], nComp);
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: Fm1(Vector<Num>& lhs, Vector<Num>& rhs, Real omega)
{
    NeighbourBlock* np;
    int i, n;
    Bool Omega = equal(omega,1.0) ?  False : True; 

    if (!inverseDiag) invertDiagonal();
    setBlockPointers(lhs,rhs);

    if (lhs.v != rhs.v)  FORALL(lhs,i) lhs[i] = rhs[i];

    FORALL(D,i) 
    {
	FORNCOMP(n) aux[n] = 0.0;
	for (np=L[i]; np; np=np->next) np->LMult(aux, lp[np->block], nComp);
	
	if (Omega) FORNCOMP(n) aux[n] = lp[i][n] - omega*aux[n];
	else	   FORNCOMP(n) aux[n] = lp[i][n] - aux[n];
	
	FORNCOMP(n) lp[i][n] = 0.0;
	D[i]->Div(lp[i], aux, nComp);
    }
}
//-------------------------------------------------------------------------


void MLBlockMatrix:: FmT(Vector<Num>& lhs, Vector<Num>& rhs, Real omega)
{
    NeighbourBlock* np;
    int i, n;
    Num* s = new Num[1+nComp];
    Bool Omega = equal(omega,1.0) ?  False : True; 

    if (!inverseDiag) invertDiagonal();
    setBlockPointers(lhs,rhs);

    if (lhs.v != rhs.v)  FORALL(lhs,i) lhs[i] = rhs[i];

    FORALL_DOWNWARD(D,i) 
    {
	FORNCOMP(n) aux[n] = 0.0;
	D[i]->Div(aux, lp[i], nComp);
	FORNCOMP(n) lp[i][n] = aux[n];
    
	if (Omega) FORNCOMP(n) s[n] = omega * lp[i][n];
	else       FORNCOMP(n) s[n] = lp[i][n];
	
	for (np=L[i]; np; np=np->next) 
	{
	    FORNCOMP(n) aux[n] = 0.0;
	    np->UMult(aux, s, nComp);
	    FORNCOMP(n) lp[np->block][n] -= aux[n];
	}
    }
    delete s;
}
//-------------------------------------------------------------------------
//-------------------------------------------------------------------------


void LocalMLBlockMatrix:: removeNonSmoothedEntries()
{
    NeighbourBlock* np, *prev=0, *toRem;
    int i;

    FORALL(D,i) 
    {
	if (!smooth[i])	
	{
	    np = L[i];
	    while (np)
	    {
		if (!smooth[np->block])
		{
		    if (np == L[i]) L[i] = np->next;
		    else  	    prev->next = np->next;

		    toRem = np;
		    np = np->next;
		    toRem->removeDataSpace(dataAlloc);
		    delete toRem;
		}
		else
		{
		    prev = np;
		    np = np->next;
		}
	    }
	}
    }
}
//-------------------------------------------------------------------------


void LocalMLBlockMatrix:: invertDiagonal()
{
    if (inverseDiag) return;

    Matrix<Num> AM(nComp,nComp), AMInv(nComp,nComp); 
    int i;
    FORALL(D,i) if (D[i] && smooth[i]) D[i]->Invert(AM, AMInv);

    inverseDiag = True;
}
//-------------------------------------------------------------------------


void LocalMLBlockMatrix:: DiagDiv(Vector<Num>& lhs, Vector<Num>& rhs, Real omega)
{
    int i,n;

    if (!inverseDiag) invertDiagonal();
    setBlockPointers(lhs,rhs);

    if (equal(omega,1.0)) 
    {
	FORALL(D,i) 
	{
	    FORNCOMP(n) aux[n] = 0.0;
	    if (smooth[i]) D[i]->Div(aux, rp[i], nComp);
	    FORNCOMP(n) lp[i][n] = aux[n];
	}
    }
    else
    {
	FORALL(D,i) 
	{
	    FORNCOMP(n) aux[n] = 0.0;
	    if (smooth[i]) 
	    {
		D[i]->Div(aux, rp[i], nComp);
		FORNCOMP(n) aux[n] *= omega;
	    }
	    FORNCOMP(n) lp[i][n] = aux[n];
	}
    }
}
//-------------------------------------------------------------------------


void LocalMLBlockMatrix:: DiagMult(Vector<Num>& lhs, Vector<Num>& rhs, 
				   Real omega)
{
    int i,n;
    setBlockPointers(lhs,rhs);

    if (equal(omega,1.0)) 
    {
	FORALL(D,i) 
	  if (smooth[i]) 
	  {
	      FORNCOMP(n) aux[n] = 0.0;
	      D[i]->Mult(aux, rp[i], nComp);
	      FORNCOMP(n) lp[i][n] = aux[n];
	  }
    }
    else
    {
	FORALL(D,i) 
	  if (smooth[i]) 
	  {
	      FORNCOMP(n) aux[n] = 0.0;
	      D[i]->Mult(aux, rp[i], nComp);
	      FORNCOMP(n) lp[i][n] = omega*aux[n];
	  }
    }
}
//-------------------------------------------------------------------------

void LocalMLBlockMatrix:: F(Vector<Num>& /*lhs*/, Vector<Num>& /*rhs*/, Real /*omega*/)
{
     notImplemented("LocalMLBlockMatrix:: F");
}
//-------------------------------------------------------------------------

void LocalMLBlockMatrix:: FT(Vector<Num>& /*lhs*/, Vector<Num>& /*rhs*/, Real /*omega*/)
{
     notImplemented("LocalMLBlockMatrix:: FT");
}
//-------------------------------------------------------------------------


void LocalMLBlockMatrix:: Fm1(Vector<Num>& lhs, Vector<Num>& rhs, Real omega)
{
    NeighbourBlock* np;
    int i, n;
    Bool Omega = equal(omega,1.0) ?  False : True; 

    if (!inverseDiag) invertDiagonal();
    setBlockPointers(lhs,rhs);

    FORALL(D,i) { if (smooth[i]) FORNCOMP(n) lp[i][n] = rp[i][n]; }

    FORALL(D,i) 
      if (smooth[i])
      {
	  FORNCOMP(n) aux[n] = 0.0;
	  for (np=L[i]; np; np=np->next) np->LMult(aux, lp[np->block], nComp);
	  
	  if (Omega) FORNCOMP(n) aux[n] = lp[i][n] - omega*aux[n];
	  else	     FORNCOMP(n) aux[n] = lp[i][n] - aux[n];

	  FORNCOMP(n) lp[i][n] = 0.0;
	  D[i]->Div(lp[i], aux, nComp);
      }
}
//-------------------------------------------------------------------------


void LocalMLBlockMatrix:: FmT(Vector<Num>& lhs, Vector<Num>& rhs, Real omega)
{
    NeighbourBlock* np;
    int i, n;
    Num* s = new Num[1+nComp];
    Bool Omega = equal(omega,1.0) ?  False : True; 

    if (!inverseDiag) invertDiagonal();
    setBlockPointers(lhs,rhs);

    FORALL(D,i) { if (smooth[i]) FORNCOMP(n) lp[i][n] = rp[i][n]; }

    FORALL_DOWNWARD(D,i) 
    {
	if (smooth[i]) 
	{
	    FORNCOMP(n) aux[n] = 0.0;
	    D[i]->Div(aux, lp[i], nComp);
	    FORNCOMP(n) lp[i][n] = aux[n];
	}
	
	if (Omega) FORNCOMP(n) s[n] = omega * lp[i][n];
	else       FORNCOMP(n) s[n] = lp[i][n];
	
	for (np=L[i]; np; np=np->next) 
	{
	    if (smooth[np->block]) 
	    {
		FORNCOMP(n) aux[n] = 0.0;
		np->UMult(aux, s, nComp);
		FORNCOMP(n) lp[np->block][n] -= aux[n];
	    }
	}
    }
    delete s;
}