#define CS	1	/* check subscripts on */

#include "common.h"
#include "corefunc.h"

#include <values.h>

#define Var		FloatHi
#define VarPtr		FloatHiPtr

#if DEB_MEM
#define DEBPAR , char *file, int line
#define DEBARG ,file,line
#else
#define DEBPAR
#define DEBARG
#endif

#ifdef WIN_MSWIN

static FILE *deb=NULL;

static void DebOpen(void) {
  if (!deb) deb=fopen("deb","wt");
}
#endif

#if CS
Local(Boolean) Check(Int2 sub, Int2 lim, Int2 line) {
  if (sub<0 || sub>=lim) {
    if (Message(MSG_OKC,"Index is out of range:\n"
                   " %i not in [0,%i)\n"
		   "_linalg.c, line %i",
		   (int)sub,(int)lim,(int)line) == ANS_CANCEL) {
      abort();
    } else return TRUE;
  }
  return FALSE;
}
#endif

/***********/
/* Vectors */

#define VectPtr		VarPtr

/* Implementation-dependent vectors description.
   A vector is represented by:
   - it's dimension,
   - possible pad field (architecture dependent), and
   - array of components.
*/
typedef struct {
  Int2 n;			/* dimension */
  Var elements[1];		/* elements */
} Vector, PNTR VectorPtr;	/* a Vector */

#define VECTSIZE(n) (sizeof(Vector)+((n)-1)*sizeof(Var))
#define VECTPTR(p)  ((VectorPtr)((CharPtr)p-(sizeof(Vector)-sizeof(Var))))

/******************************************/
/* Functions used for debug purposes only */

Entry(void) PrintVector(VectPtr vp, CharPtr title) {
  VectorPtr v;
  int n,i;
  if (vp==NULL) {
#ifdef WIN_MSWIN
    DebOpen();
    fprintf(deb,"\n%s=NULL\n",title);
#else
    printf("\n%s=NULL\n",title);
#endif
    return;
  }
  v=VECTPTR(vp);
  n=v->n;
#ifdef WIN_MSWIN
  DebOpen();
  fprintf(deb,"\n%s",title);
#else
  printf("\n%s",title);
#endif
  for (i=0; i<n; i++) {
#ifdef WIN_MSWIN
    fprintf(deb,i ? ", " : "(");
    fprintf(deb,"%.20lg",v->elements[i]);
#else
    printf(i ? ", " : "(");
    printf("%.20lg",v->elements[i]);
#endif
  }
#ifdef WIN_MSWIN
  fprintf(deb,")\n");
  fflush(deb);
#else
  printf(")\n");
#endif
}

/***************************************/

/* Creates a Vector of dimension n   */
Entry(VectPtr) CreateVector(int n DEBPAR) {
  VectorPtr vp;
  vp = MemNewDeb(VECTSIZE(n) DEBARG);
  vp->n=(Int2)n;
  return vp->elements;
}

/* Destroys a vector */
Entry(VectPtr) DestroyVector(VectPtr vp) {
  return vp ? MemFree(VECTPTR(vp)) : NULL;
}

/* Returns a dimension of a vector */
Entry(Int2) GetVectorDim(VectPtr vp) {
  return VECTPTR(vp)->n;
}

/* Copies a vector */
Entry(VectPtr) CopyVector(VectPtr from, VectPtr to DEBPAR) {
  VectorPtr vfrom=VECTPTR(from);
  if (!to) to=CreateVector(vfrom->n DEBARG);
  MemCopy(VECTPTR(to),vfrom,VECTSIZE(vfrom->n));
  return to;
}

/* Clears a Vector  */
Entry(void) ClearVector(VectPtr vp) {
  MemFill(vp,0,sizeof(Var)*VECTPTR(vp)->n);
}

/* Computes scalar product */
Entry(Var) ScalProd(VectPtr a, VectPtr b) {
  Var s;
  Int2 i,n=VECTPTR(a)->n;
  for (s=0.0,i=0; i<n; i++) s+=a[i]*b[i];
  return s;
}

/* Compute the norm of a Vector */
Entry(Var) NormOfVector(VectPtr a) {
  Var s;
  Int2 i,n=VECTPTR(a)->n;
  for (s=0.0,i=0; i<n; i++) s+=a[i]*a[i];
  return sqrt(s);
}

/* Normalize a Vector  */
Entry(Int2) NormalizeVector(VectPtr W) {
  Var s;
  Int2 i,n=VECTPTR(W)->n;
  s=ScalProd(W,W);
  if (s>0) {
    for (s=sqrt(s),i=0; i<n; i++) W[i]/=s;
    return 0;
  } else {
    return 1;   /* zero vector */
  };
}

/* Normalize a Vector W with respect to Vector V to have <W,V>=1 */
Entry(Int2) NormalizeVectorRelative(VectPtr W, VectPtr V) {
  Var s;
  Int2 i,n=VECTPTR(W)->n;
  s=ScalProd(W,V);
  if (s == 0) {
    return 1;   /* orthogonal vectors */
  } else {
    for (i=0; i<n; i++) W[i]/=s;
    return 0;
  };
}

/* Add scaled vector (vpres=vp1+h*vp2) */
Entry(VectPtr) AddScaledVector(VectPtr vp1, VectPtr vp2, FloatHi h, VectPtr vpres DEBPAR) {
  Int2 i,n=VECTPTR(vp2)->n;
  if (!vpres) vpres=CreateVector(n DEBARG);
  if (vp1) {
    if (h==1.0) {
      for (i=0; i<n; i++) vpres[i]=vp1[i]+vp2[i];
      return vpres;
    }
    if (h==-1.0) {
      for (i=0; i<n; i++) vpres[i]=vp1[i]-vp2[i];
      return vpres;
    }
    for (i=0; i<n; i++) vpres[i]=vp1[i]+h*vp2[i];
  } else {
    if (h==1.0) {
      MemCopy(vpres,vp2,sizeof(Var)*n);
      return vpres;
    }
    if (h==-1.0) {
      for (i=0; i<n; i++) vpres[i]=-vp2[i];
      return vpres;
    }
    for (i=0; i<n; i++) vpres[i]=h*vp2[i];
  }
  return vpres;
}

/* Multiply a vector by a scalar */
Entry(VectPtr) MultiplyVectorScalar(VectPtr vp, Var scalar, VectPtr vpres DEBPAR) {
  Int2 i,n=VECTPTR(vp)->n;
  if (!vpres) vpres=CreateVector(n DEBARG);
  for (i=0; i<n; i++) vpres[i]=vp[i]*scalar;
  return vpres;
}

/* Copy elements */
Entry(void) CopyVectorElements(VectPtr from, Int2 frompos, VectPtr to, Int2 topos, Int2 stuk) {
#if CS
  if (Check(frompos,VECTPTR(from)->n,__LINE__)) return;
  if (Check(frompos+stuk-1,VECTPTR(from)->n,__LINE__)) return;
  if (Check(topos,VECTPTR(to)->n,__LINE__)) return;
  if (Check(topos+stuk-1,VECTPTR(to)->n,__LINE__)) return;
#endif
  MemCopy(to+topos,from+frompos,sizeof(Var)*stuk);
}

/************/
/* Matrices */

/* A matrix */
typedef struct {
  Var det,cond;		/* determinant and condition number */
  Int2Ptr PermInd;	/* permutation index  */
  Int2 n,m;		/* dimensions */
  Var elements[1];	/* m[i][j] m*n stuk */
  /* A pointer to the variable itself (MatrixPtr), 1 stuk */
  /* Index of rows, n stuk */
} Matrix, PNTR MatrixPtr;

typedef VarPtr * MatrPtr;

#define MATRSIZE(n,m) (sizeof(Matrix)+((n)+1)*sizeof(VarPtr)+((n)*(m)-1)*sizeof(Var))
#define MATRPTR(p) (*((MatrixPtr *)p-1))
#define MATRIXELEM(mp,i,j) (mp)[i][j]


Entry(void) PrintMatrix(MatrPtr mpp, CharPtr title, Int2 Level) {
  MatrixPtr mp;
  int n,m,i,j;
  if (mpp==NULL) {
#ifdef WIN_MSWIN
    DebOpen();
    fprintf(deb,"\n%s=NULL\n",title);
#else
    printf("\n%s=NULL\n",title);
#endif
    return;
  }
  mp=MATRPTR(mpp);
  n=mp->n;
  m=mp->m;
#ifdef WIN_MSWIN
  DebOpen();
  fprintf(deb,"\n%s\n",title);
#else
  printf("\n%s\n",title);
#endif
  for (i=0; i<n; i++) {
    for (j=0; j<m; j++)
#ifdef WIN_MSWIN 
      fprintf(deb," %.20lg",MATRIXELEM(mpp,i,j));
    fprintf(deb,"\n");
#else
      printf(" %.10lg",MATRIXELEM(mpp,i,j));
    printf("\n");
#endif
  }
  if (mp->PermInd) {
     for (i=0;i<n;i++) {
#ifdef WIN_MSWIN
         if (i==0) fprintf(deb,"\n");
	 fprintf(deb," %i",(int)mp->PermInd[i]); 
     }
         fprintf(deb,"\n");
#else
         if (i==0) printf("\n");
	 printf(" %i",(int)mp->PermInd[i]);
     }
         printf("\n");
#endif
  } else {
#ifdef WIN_MSWIN
     fprintf(deb,"PermInd=NULL\n");
  }
  fflush(deb);
#else
     printf("PermInd=NULL\n");
  }
#endif
}


/* Creates a Matrix of dimensions n,m */
Entry(MatrPtr) CreateMatrix(int n, int m DEBPAR) {
  MatrixPtr mp;
  
  Int2 i;
  mp = MemNewDeb(MATRSIZE(n,m) DEBARG);
  mp->n=(Int2)n;
  mp->m=(Int2)m;
  mp->PermInd=MemNewDeb(n*sizeof(*mp->PermInd) DEBARG);
  *(MatrixPtr*)(mp->elements+n*m)=mp;
  for (i=0; i<n; i++) *((VarPtr *)(mp->elements+n*m)+1+i)=mp->elements+i*m;
  return (VarPtr *)(mp->elements+n*m)+1;
}

/* Destroys a Matrix and free the memory it occupies */
Entry(MatrPtr) DestroyMatrix(MatrPtr mpp) {
  MatrixPtr mp;
  if (mpp) {
    mp=MATRPTR(mpp);
    MemFree(mp->PermInd);
    return MemFree(mp);
  }
  return NULL;
}

/* Copies a Matrix */
Entry(MatrPtr) CopyMatrix(MatrPtr fromp, MatrPtr top DEBPAR) {
  MatrixPtr from=MATRPTR(fromp),to;
  if (!top) top=CreateMatrix(from->n,from->m DEBARG);
  to=MATRPTR(top);
  MemCopy(top[0],fromp[0],sizeof(Var)*from->n*from->m);
  MemCopy(to->PermInd,from->PermInd,sizeof(*from->PermInd)*from->n);
  to->det=from->det;
  to->cond=from->cond;
  return top;
}

/* Returns number of rows */
Entry(Int2) GetRowNum(MatrPtr mp) {
  return MATRPTR(mp)->n;
}

/* Returns number of columns */
Entry(Int2) GetColNum(MatrPtr mp) {
  return MATRPTR(mp)->m;
}

/* Returns det and cond numbers of a DECOMPOSED matrix */
Entry(void) GetMatrixData(MatrPtr mp, FloatHiPtr det, FloatHiPtr cond) {
   if (det) *det=MATRPTR(mp)->det;
   if (cond) *cond=MATRPTR(mp)->cond;
}

/* Transpose a matrix  */
Entry(MatrPtr) TransposeMatrix(MatrPtr from, MatrPtr to DEBPAR) {
  #define A(j,i) MATRIXELEM(from,j,i)
  #define B(i,j) MATRIXELEM(to,i,j)
  int i,j,n=MATRPTR(from)->n,m=MATRPTR(from)->m;
  if (!to) to=CreateMatrix(m,n DEBARG);
  for (i=0; i<m; i++) {
    for (j=0; j<n; j++) {
      B(i,j)=A(j,i);
    };
  };
  return to;
  #undef A 
  #undef B
}
/* Clears a Matrix */
Entry(void) ClearMatrix(MatrPtr mp) {
  MemFill(mp[0],0,sizeof(Var)*MATRPTR(mp)->n*MATRPTR(mp)->m);
}

/* Multilpy Matrix by vector */
Entry(VectPtr) MultiplyMatrixVector(MatrPtr mp, VectPtr vp, VectPtr vpres DEBPAR) {
  Var s;
  VarPtr a,v,vres;
  Int2 i,j,n=MATRPTR(mp)->n,m=MATRPTR(mp)->m;
  if (!vpres) vpres=CreateVector(n DEBARG);
  m=MIN(m,VECTPTR(vp)->n);
  for (a=mp[0],vres=vpres,i=0; i<n; i++) {
    for (s=0,v=vp,j=0; j<m; j++) s+=*a++**v++;
    *vres++=s;
  }
  return vpres;
}

/* Multilpy Matrix by array */
Entry(VectPtr) MultiplyMatrixArray(MatrPtr mp, VarPtr vp, VectPtr vpres DEBPAR) {
  Var s;
  VarPtr a,v,vres;
  Int2 i,j,n=MATRPTR(mp)->n,m=MATRPTR(mp)->m;
  if (!vpres) vpres=CreateVector(n DEBARG);
  for (a=mp[0],vres=vpres,i=0; i<n; i++) {
    for (s=0,v=vp,j=0; j<m; j++) s+=*a++**v++;
    *vres++=s;
  }
  return vpres;
}

/* Appends a vector to a Matrix as the last row */
Entry(MatrPtr) AppendMatrixVector(MatrPtr mp, VectPtr vp, MatrPtr mpres DEBPAR) {
  Int2 n=MATRPTR(mp)->n, m=MATRPTR(mp)->m;
  if (!mpres) mpres=CreateMatrix(n+1,m DEBARG);
  CopyMatrix(mp,mpres DEBARG);	/* ! It's a trick ! */
  MemCopy(mpres[n],vp,sizeof(Var)*m);
  return mpres;
}

/* Copy block */
Entry(void) CopyMatrixBlock(MatrPtr from, Int2 frow, Int2 fcol,
                            MatrPtr to, Int2 trow, Int2 tcol,
                            Int2 nrow, Int2 ncol) {
  Int2 i;
#if CS
  if (Check(frow,MATRPTR(from)->n,__LINE__)) return;
  if (Check(frow+nrow-1,MATRPTR(from)->n,__LINE__)) return;
  if (Check(fcol,MATRPTR(from)->m,__LINE__)) return;
  if (Check(fcol+ncol-1,MATRPTR(from)->m,__LINE__)) return;
  if (Check(trow,MATRPTR(to)->n,__LINE__)) return;
  if (Check(trow+nrow-1,MATRPTR(to)->n,__LINE__)) return;
  if (Check(tcol,MATRPTR(to)->m,__LINE__)) return;
  if (Check(tcol+ncol-1,MATRPTR(to)->m,__LINE__)) return;
#endif
  for (i=0; i<nrow; i++)
    MemCopy(to[trow+i]+tcol,from[frow+i]+fcol,sizeof(Var)*ncol);
}

/* Copy vector to matrix as a row */
Entry(void) CopyVectorMatrixRow(VectPtr from, MatrPtr to, Int2 trow) {
#if CS
  if (Check(trow,MATRPTR(to)->n,__LINE__)) return;
#endif
  MemCopy(to[trow],from,sizeof(Var)*MATRPTR(to)->m);
}

/* Copy matrix row to a vector */
Entry(void) CopyMatrixRowVector(MatrPtr from, Int2 frow, VectPtr to) {
#if CS
  if (Check(frow,MATRPTR(from)->n,__LINE__)) return;
#endif
  MemCopy(to,from[frow],sizeof(Var)*VECTPTR(to)->n);
}

/* Multilpy Matrix by Matrix */
Entry(MatrPtr) MultiplyMatrixMatrix (MatrPtr mpa, MatrPtr mpb, MatrPtr mpc DEBPAR) {
#define A(j,i) MATRIXELEM(mpa,j,i)
#define B(i,j) MATRIXELEM(mpb,i,j)
#define C(i,j) MATRIXELEM(mpc,i,j)
  Int2 i,j,k;
  Int2 n=MATRPTR(mpa)->n,m=MATRPTR(mpa)->m,l=MATRPTR(mpb)->m;
  Var s;
  if (!mpc) mpc=CreateMatrix(n,l DEBARG);

  for (i=0; i<n; i++) {
    for (j=0; j<l; j++) {
       for (k=0,s=0.0; k<m; k++) s+=A(i,k)*B(k,j);
       C(i,j)=s;
    }
  }
  return mpc;
#undef A 
#undef B
#undef C
}

/* Add scaled Matrix to Matrix C=A+h*B */
/*                             C=h*B, if A==NULL */
Entry(MatrPtr) AddScaledMatrix (MatrPtr mpa, MatrPtr mpb, FloatHi h, MatrPtr mpc DEBPAR) {
#define A(j,i) MATRIXELEM(mpa,j,i)
#define B(i,j) MATRIXELEM(mpb,i,j)
#define C(i,j) MATRIXELEM(mpc,i,j)
  Int2 i,j;
  Int2 n=MATRPTR(mpb)->n,m=MATRPTR(mpb)->m;

  if (!mpc) mpc=CreateMatrix(n,m DEBARG);

  if (mpa) {
    if (h==1.0) {
      for (i=0; i<n; i++)
	for (j=0; j<m; j++) C(i,j)=A(i,j)+B(i,j);
      return mpc;
    }
    if (h==-1.0) {
      for (i=0; i<n; i++)
	for (j=0; j<m; j++) C(i,j)=A(i,j)-B(i,j);
      return mpc;
    }
    for (i=0; i<n; i++)
      for (j=0; j<m; j++) C(i,j)=A(i,j)+h*B(i,j);
  } else
    if (h==-1.0)
      for (i=0; i<n; i++)
	for (j=0; j<m; j++) C(i,j)=-B(i,j);
    else
      for (i=0; i<n; i++)
	for (j=0; j<m; j++) C(i,j)=h*B(i,j);
  return mpc;
#undef A 
#undef B
#undef C
}

#undef MATRSIZE


/*****************************************************/
/* Numerical differentiation by finite differencies  */
typedef EntryPtr(Int2,DiffFun,(VectPtr x, VectPtr y));   /* any numerically differentiable function */

Entry(Int2) der (DiffFun fun,
         int n,              /* number of function components */
	 VectPtr x,
	 Var d,
	 MatrPtr A DEBPAR)
{
#define a(i,j) MATRIXELEM(A,i,j)	/* system matrix */
   int i, j, m;
   Var h;
   VectPtr y1, y2;    /* work space  */

   m = GetVectorDim(x); 
   y1 = CreateVector(m DEBARG);
   y2 = CreateVector(m DEBARG);

   h = d + d;

   for (j=0; j<m; j++) {
     x[j]=x[j]-d;
     if (fun (x, y1)) return (1);
     x[j]=x[j]+h; 
     if (fun (x, y2)) return (1);
     x[j]=x[j]-d;
     for (i=0; i<n; i++) a(i,j)=(y2[i]-y1[i])/h;
   }
   DestroyVector(y1);
   DestroyVector(y2);
   return 0;
#undef a
}

/* Similar to der except x is an array not Vector */
Entry(Int2) dera (DiffFun fun,
         int n,              /* number of function components */
	 VarPtr x,
	 Var d,
	 MatrPtr A DEBPAR)
{
#define a(i,j) MATRIXELEM(A,i,j)	/* system matrix */
   int i, j, m;
   Var h;
   VectPtr y1, y2;    /* work space  */
   m = GetColNum(A);	/* the only difference with der */
   y1 = CreateVector(n DEBARG);
   y2 = CreateVector(n DEBARG);

   h = d + d;
   for (j=0; j<m; j++) {
     x[j]=x[j]-d;
     if (fun (x, y1)) return (1);
     x[j]=x[j]+h; 
     if (fun (x, y2)) return (1);
     x[j]=x[j]-d;
     for (i=0; i<n; i++) a(i,j)=(y2[i]-y1[i])/h;
   }
   DestroyVector(y1);
   DestroyVector(y2);
   return 0;
#undef a
}

/***********************************/
/* Standard solver and decomposer  */

Entry(void) Solve(MatrPtr AA, VectPtr BB) {
#define A(i,j) MATRIXELEM(AA,i,j)	/* system matrix */
#define B BB				/* rhs vector -> solution vector */
	 
  int NM1, I, K, M, KB, KM1, KP1, N1;
  int N=MATRPTR(AA)->n;			/* dimension  */
  Int2Ptr IPVT=MATRPTR(AA)->PermInd;	/* permutation index  */
  Var T;

      N1 = N - 1;
      if (N == 1) {
	 B[0] /= A(0,0);
	 return;
      }

      NM1 = N1 - 1;
      for (K=0; K<=NM1; K++) {
	  KP1 = K + 1;
	  M   = IPVT[K];
	  T   = B[M];
	  B[M]= B[K];
	  B[K]= T;
	  for (I=KP1; I<=N1; I++) B[I] += A(I,K)*T;
      }
      for (KB=0; KB<=NM1; KB++) {
	  KM1 = NM1 - KB;
	  K   = KM1 + 1;
	  B[K] = B[K]/A(K,K);
	  T   = -B[K];
	  for (I=0; I<=KM1; I++) B[I] += A(I,K)*T;
      }
      B[0] /= A(0,0);
      return;
#undef A
#undef B
}

Entry(MatrPtr) SolveMatrix(MatrPtr AA, MatrPtr BB, MatrPtr XX, VectPtr work DEBPAR) {
/*           AA*XX=BB              AA -decomposed matrix */
#define B(i,j) MATRIXELEM(BB,i,j)	/* rhs matrix (not changed) */
#define X(i,j) MATRIXELEM(XX,i,j)       /* solution matrix */

Int2 i,j;
Int2 n=MATRPTR(AA)->n,m=MATRPTR(BB)->m;
Boolean Delete=FALSE;

  if (!XX) XX=CreateMatrix(n,m DEBARG);
  if (!work){
    work=CreateVector(n DEBARG);
    Delete=TRUE;
  }
  for (j=0; j<m; j++) {
    for (i=0; i<n; i++) work[i]=B(i,j);
    Solve(AA, work);
    for (i=0; i<n; i++) X(i,j)=work[i];   
  }
  if (Delete) work=DestroyVector(work);
  return XX;

#undef B
#undef X
}

Entry(Int2) Decomp(MatrPtr AA DEBPAR) {
#define A(i,j) MATRIXELEM(AA,i,j)	/* matrix elements */
#define N      MATRPTR(AA)->n		/* dimension */
#define COND   MATRPTR(AA)->cond	/* condition number */
#define DET    MATRPTR(AA)->det		/* SCALED determinant */
#define WORK WorkVector			/* work space  */

   int I, J, M, K, KB, KP1, KM1, NM1, N1;
   Int2Ptr IPVT=MATRPTR(AA)->PermInd;		/* permutation index */
   Var ANORM, T, EK, YNORM, ZNORM;
   VectPtr WorkVector;

      N1 = N - 1;
      IPVT[N1] = 1;
      if (N == 1) {
	 COND = 1.0;
	 if (A(0,0) != 0.0) { 
            DET = A(0,0);
            return(0);
	 } else {
	    COND = MAXDOUBLE;	/***1.0e+64;***/
	    return (1);
	 }
      }
      NM1 = N1 - 1;
      ANORM = 0.0;
      for (J=0; J<=N1; J++) {
	  T = 0.0;
	  for (I=0; I<=N1; I++) T += fabs (A(I,J));
	  if (T > ANORM) ANORM = T;
      }
      for (K=0; K<=NM1; K++) {
	  KP1 = K + 1;
	 M = K;
	 for (I=KP1; I<=N1; I++) {
	     if (fabs(A(I,K)) > fabs(A(M,K))) M = I;
	 }
	 IPVT[K] = M;
	 if (M != K) IPVT[N1] = -IPVT[N1];
	 T = A(M,K);
	 A(M,K) = A(K,K);
	 A(K,K) = T;
	 if (T == 0.0) continue;
	 for (I=KP1; I<=N1; I++) A(I,K) = -A(I,K)/T;
	 for (J=KP1; J<=N1; J++) {
	     T = A(M,J);
	     A(M,J) = A(K,J);
	     A(K,J) = T;
	     if (T != 0.0) for (I=KP1; I<=N1; I++) A(I,J) += A(I,K)*T;
	 }
      }
      WorkVector=CreateVector(N DEBARG);

      for (K=0; K<=N1; K++) {
	 T = 0.0;
	 if (K != 0) {
	    KM1 = K - 1;
	    for (I=0; I<=KM1; I++) T += A(I,K)*WORK[I];
	 }
	 EK = 1.0;
	 if (T < 0.0) EK = -1.0;
	 if (A(K,K) == 0.0) {
	    COND = MAXDOUBLE;   /***1.0e+64;***/
	    DestroyVector (WorkVector);
	    return (1);
	 }
	 WORK[K] = -(EK + T)/A(K,K);
      }
      for (KB=0; KB<=NM1; KB++) {
	  K = NM1 - KB;
	  T = 0.0;
	  KP1 = K + 1;
	  for (I=KP1; I<=N1; I++) T += A(I,K)*WORK[K];
	  WORK[K] = T;
	  M = IPVT[K];
	  if (M == K) continue;
	  T = WORK[M];
	  WORK[M] = WORK[K];
	  WORK[K] = T;
      }

      YNORM = 0.0;
      for (I=0; I<=N1; I++) YNORM += fabs(WORK[I]);
      Solve (AA, WorkVector);

      ZNORM = 0.0;
      K = IPVT[N1];
      for (I=0, T=0.0; I<=N1; I++) {
          ZNORM += fabs(WORK[I]);
	  if (A(I,I) < 0.0) K=-K;
	  T += log(fabs(A(I,I)));
      }
      if (T <= 0) {
        DET=K*exp(T);
      } else {
        DET=K*(1.0+T);
      }
/*
        det(A)=IPVT(N)*A(1,1)*A(2,2)*...*A(N,N).
	DET=det if |det|<=1 
	DET=sign(det)(1+log(abs(det))) if |det|>1
*/
      COND = ANORM*ZNORM/YNORM;
      DestroyVector (WorkVector);
      if (COND+1==COND) return(1);
      if (COND < 1.0) COND = 1.0;
      
      return (0);
#undef A
#undef N
#undef COND
#undef DET
#undef WORK
}

Entry(Int2) SngSolve (MatrPtr AA, FloatHi EPS, VectPtr XX) {

#define A(i,j) MATRIXELEM(AA,i,j)	/* matrix elements (destroyed) */
#define X      XX                       /* solution vector of A*X=0 */

                                        /* return = rank A  with tolerance EPS */

   Var AM,AM1,AMAX,R,S;
   int I,J,J1,K,L1,M,IKL,IK1;
   int L=0,IRANG=0;
   int N=MATRPTR(AA)->n;		/* dimension */
   Int2Ptr IK=MATRPTR(AA)->PermInd;	/* permutation index is used as a work array */

   for (I=0; I<N; I++) IK[I]=I;

   while (L < N) {
      AM=0.0;
      AMAX=0.0;
      for (I=L; I<N; I++) {
         for (J=L; J<N; J++) {
            AM1=A(I,J);
            if (AM >= fabs(AM1)) continue;
            AM=fabs(AM1);
            AMAX=AM1;
            K=I;
            IKL=J;
         };
      };
      if (AM <= EPS) break;
      if (L != K) {
        for (J=L; J<N; J++) {
           R=A(K,J);
           A(K,J)=A(L,J);
           A(L,J)=R;
         };
      };
      if (IKL != L) {
        IK1=IK[L];
        IK[L]=IK[IKL];
        IK[IKL]=IK1;
        for (J=0; J<N; J++) {
           R=A(J,L);
           A(J,L)=A(J,IKL);
           A(J,IKL)=R;
        };
      };
      L1=L+1;
      for (J=L1; J<N; J++) {
         R=A(J,L)/AMAX;
         for (I=L1; I<N; I++) A(J,I)-=A(L,I)*R;
      };
      L=L1;
      IRANG++;
   }; 
/*   for (I=IRANG; I<N; I++) X[IK[I]]=1.0; */
   for (I=IRANG; I<N; I++) X[IK[I]]=0.0;
   X[IK[IRANG]]=1.0;
   
   for (I=0; I<IRANG; I++) {
      M=IRANG-1-I;
      S=0.0;
      for (J1=M+1; J1<N; J1++) {  
         S+=A(M,J1)*X[IK[J1]];
      };
      X[IK[M]]=-S/A(M,M);    
   };   
   return IRANG;
#undef A
#undef X   
}

Entry(Int2) CharPolyCoeffs(MatrPtr AA, VectPtr PP DEBPAR) {

#define A(i,j) MATRIXELEM(AA,i,j)	/* matrix elements  */
#define P      PP	                /* coefficients of the characteristic polynomial */
#define B(i,j) MATRIXELEM(BB,i,j)       /* working matrix */
#define Y      YY		        /* work vector */

  int I, J, K, L;
  int N=MATRPTR(AA)->n;
  Var S, S1;
  MatrPtr BB;
  VectPtr YY;

  BB=CreateMatrix(N,N DEBARG);
  YY=CreateVector(N DEBARG);

  for(S=0,I=0; I<N; I++) S+=A(I,I);
  P[0]=-S;
  if (N == 1) return 0;

  for (I=0; I<N; I++) {
    for (J=0; J<N; J++) {
      for (S1=0,K=0; K<N; K++) {
        if (K != J) {
           S1+=A(I,K)*A(K,J);
        } else {
           S1+=A(I,K)*(A(K,K)-S);
        }
      }
      B(I,J)=S1;
    }
  }
  for (S=0,I=0; I<N; I++) S+=B(I,I);
  P[1]=-S/2.0;
  if (N ==2) return 0;
  
  for (L=2; L<N; L++) {
    for (J=0; J<N; J++) {
      for (I=0; I<N; I++) {
        for (S=0,K=0; K<N; K++) {
          if (K != J) {
            S+=A(I,K)*B(K,J);
          } else {
            S+=A(I,K)*(B(J,J)+P[L-1]);
          }
        }
        Y[I]=S;
      }
      for (I=0; I<N; I++) B(I,J)=Y[I];
    }
    for (S=0,I=0; I<N; I++) S+=B(I,I);
    P[L]=-S/(L+1);
  }

  DestroyVector(YY);
  DestroyMatrix(BB);
  return 0;
/*
      R=(fabs(P(N)))**(1.D0/N)
      R=1.D0/(R+1.D0)
      R1=1.D0
      DO 35 I=1,N
        R1=R1*R
        P(I)=P(I)*R1
 35   CONTINUE
      RETURN
      END
*/

#undef A
#undef P   
#undef B
#undef Y   
}

Entry(Int2) dbalan(MatrPtr AA, Int2 *LOW, Int2 *IGH) {

#define A(i,j) MATRIXELEM(AA,i-1,j-1)	/* matrix elements  */

int I,J,JJ,K,L,M,IEXC;
int NOCONV;
int N=MATRPTR(AA)->n;			/* dimension */
double RADIX,B2,F,C,R,G,S;

  RADIX=16.0;
  B2=RADIX*RADIX;
  
  K=1;
  L=N;
  goto L100;

L20:  /** Internal permutation of rows and columns **/
  if (J==M) goto L50;

  for (I=1; I<=L; I++) {
    F=A(I,J);
    A(I,J)=A(I,M);
    A(I,M)=F;
  }
  for (I=K; I<=N; I++) {
    F=A(J,I);
    A(J,I)=A(M,I);
    A(M,I)=F;
  }

L50:
  if (IEXC==1) goto L80;
  if (IEXC==2) goto L130;

L80:
  if (L==1) goto final;
  L=L-1;

L100:
  for (JJ=1; JJ<=L; JJ++) {
    J=L+1-JJ;
    for (I=1; I<=L; I++) {
      if (I==J) continue;
      if (A(J,I)!=0.0) goto L120;
    } 
    M=L;
    IEXC=1;
    goto L20;
L120:
    ;
  }
  goto L140;

L130:
  K=K+1;

L140:
  for (J=K; J<=L; J++) {
    for (I=K; I<=L; I++) {
      if (I==J) continue;
      if (A(I,J)!=0.0) goto L170;
    };
    M=K;
    IEXC=2;
    goto L20;
L170: ;
  } 

iterate:
  NOCONV=0;
  for (I=K; I<=L; I++) {
    for (C=R=0.0, J=K; J<=L; J++) {
      if (J==I) continue;
      C+=fabs(A(J,I));
      R+=fabs(A(I,J));
    }
    if (C==0.0 || R==0.0) continue;
    G=R/RADIX;
    F=1.0;
    S=C+R;

    while (C<G) {
      F*=RADIX;
      C*=B2;
    }                                                                   
    G=R*RADIX;
    while (C>=G) {
      F/=RADIX;
      C/=B2;
    }

    if ((C+R)/F>=0.950*S) continue;
    G=1.0/F;
    NOCONV=1;
    for (J=K; J<=N; J++) A(I,J)*=G;
    for (J=1; J<=L; J++) A(J,I)*=F;
  }
  if (NOCONV) goto iterate;

final:
  *LOW=K;
  *IGH=L;
  return 0;
#undef A
}


Entry(Int2) delmhe(MatrPtr AA, Int2 LOW, Int2 IGH) {

#define A(i,j) MATRIXELEM(AA,i-1,j-1)	/* matrix elements  */

  int LA,KP1,M,MM1,MP1,I,J;
  int N=MATRPTR(AA)->n;			/* dimension */
  double X,Y;

  LA=IGH-1;
  KP1=LOW+1;
  if (LA < KP1) goto final;

  for (M=KP1; M<=LA; M++) {
    MM1=M-1;
    I=M;
    for (X=0.0,J=M; J<=IGH; J++) {
      if (fabs(A(J,MM1)) <= fabs(X)) continue;
      X=A(J,MM1);
      I=J;
    }

    MATRPTR(AA)->PermInd[M-1]=I;
    if (I==M) goto L130;

/** permutation of rows and columns of A **/
    for (J=MM1; J<=N; J++) {
      Y=A(I,J);
      A(I,J)=A(M,J);
      A(M,J)=Y;
    }
    for (J=1; J<=IGH; J++) {     
      Y=A(J,I);
      A(J,I)=A(J,M);
      A(J,M)=Y;
    }
/*********** end of permutation ***********/

L130:
    if (X==0.0) continue;
    MP1=M+1;

    for (I=MP1; I<=IGH; I++) {
      Y=A(I,MM1);
      if (Y==0.0) continue;
      Y/=X;
      A(I,MM1)=Y;
      for (J=M; J<=N; J++) A(I,J)-=Y*A(M,J);
      for (J=1; J<=IGH; J++) A(J,M)+=Y*A(J,I);
    }
  }
final:
  return 0;
#undef A
}

Entry(Int2) dhqr(MatrPtr HH, Int2 LOW, Int2 IGH, VectPtr WWR, VectPtr WWI) {

#define H(i,j) MATRIXELEM(HH,i-1,j-1)	/* matrix elements  */
#define WR(i)  WWR[i-1]			/* real parts */
#define WI(i)  WWI[i-1]			/* imaginary parts */

double MACHEP=.11102230246251565e-15;
double ANORM,T,X,S,Y,W,ZZ,P,Q,R;
int I,J,K,L,M,EN,LL,MM,NA,ITS,MP2,ENM2,IERR;
int NOTLAS;
int N=MATRPTR(HH)->n;			/* dimension */

  IERR=0;
  ANORM = 0.0;
  K = 1;
  for (I=1; I<=N; I++) {
    for (J=K; J<=N; J++) ANORM+=fabs(H(I,J));
    K = I;
    if (I>=LOW && I<=IGH) continue;
    WR(I)=H(I,I);
    WI(I)=0.0;
  } 
  EN=IGH;
  T=0.0;

L60: /* find next eigenvalue */
  if (EN<LOW) goto L1001;
  ITS=0;
  NA=EN-1;
  ENM2=NA-1;

L70:
  for (LL=LOW; LL<=EN; LL++) { 
    L=EN+LOW-LL;
    if (L==LOW) break; 
    S=fabs(H(L-1,L-1))+fabs(H(L,L));
    if (S==0.0) S=ANORM;
    if (fabs(H(L,L-1))<=MACHEP*S) break;
  } 
  /* shift */
  X=H(EN,EN);
  if (L==EN) goto L270;
  Y=H(NA,NA);
  W=H(EN,NA)*H(NA,EN);

  if (L==NA) goto L280;

  if (ITS==30) goto L1000;
  if (ITS==10 || ITS==20) {  /* exceptional shift */
    T+=X;
    for (I=LOW; I<=EN; I++) H(I,I)-=X;
    S=fabs(H(EN,NA))+fabs(H(NA,ENM2));
    X=0.75*S;
    Y=X;
    W=-0.4375*S*S;
  }
  ITS++;
  for (MM=L; MM<=ENM2; MM++) {
    M=ENM2+L-MM;
    ZZ=H(M,M);
    R=X-ZZ;
    S=Y-ZZ;
    P=(R*S-W)/H(M+1,M) + H(M,M+1);
    Q=H(M+1,M+1)-ZZ-R-S;
    R=H(M+2,M+1);
    S=fabs(P)+fabs(Q)+fabs(R);
    P/=S;
    Q/=S;
    R/=S;
    if (M==L) break;
    if (fabs(H(M,M-1))*(fabs(Q)+fabs(R)) <= MACHEP*fabs(P)
        *(fabs(H(M-1,M-1))+fabs(ZZ)+fabs(H(M+1,M+1)))) break;
  } 
  MP2=M+2;
  for (I=MP2; I<=EN; I++) {  
    H(I,I-2)=0.0;
    if (I==MP2) continue;
    H(I,I-3)=0.0;
  }
  for (K=M; K<=NA; K++) {
    NOTLAS=(K!=NA);
    if (K==M) goto L170;
    P=H(K,K-1);
    Q=H(K+1,K-1);
    R=0.0;
    if (NOTLAS) R=H(K+2,K-1);
    X=fabs(P)+fabs(Q)+fabs(R);
    if (X==0.0) continue;
    P/=X;
    Q/=X;
    R/=X;
L170:
    S=sqrt(fabs(P*P+Q*Q+R*R));
    if (P<0) S=-S;
    if (K!=M) {
      H(K,K-1)=-S*X;
    } else {
      if (L!=M) H(K,K-1)=-H(K,K-1);
    }
    P+=S;
    X=P/S;
    Y=Q/S;
    ZZ=R/S;
    Q/=P;
    R/=P;
    /* row modification */
    for (J=K; J<=EN; J++) { 
      P=H(K,J)+Q*H(K+1,J);
      if (NOTLAS) {
        P+=R*H(K+2,J);
        H(K+2,J)-=P*ZZ;
      }
      H(K+1,J)-=P*Y;
      H(K,J)-=P*X;
    }
    J=MIN(EN,K+3);
    /* column modification */
    for (I=L; I<=J; I++) {
      P=X*H(I,K)+Y*H(I,K+1);
      if (NOTLAS) {
        P+=ZZ*H(I,K+2);
        H(I,K+2)-=P*R;
      }
      H(I,K+1)-=P*Q;
      H(I,K)-=P;
    }
  } 
  goto L70;

L270:  /* one root found */
  WR(EN)=X+T;
  WI(EN)=0.0;
  EN=NA;
  goto L60;

L280: /* two roots found */
  P=(Y-X)/2.0;
  Q=P*P+W;
  ZZ=sqrt(fabs(Q));
  X+=T;
  if (Q<0.0) goto L320;
/* real pair  */
  if (P>0) {
    ZZ=P+ZZ;
  } else {
    ZZ=P-ZZ;
  }
  WR(NA)=X+ZZ;
  WR(EN)=WR(NA);
  if (ZZ!=0.0) WR(EN)=X-W/ZZ;
  WI(NA)=0.0;
  WI(EN)=0.0;
  goto L330;
/* complex pair */ 
L320:
  WR(NA)=X+P;
  WR(EN)=X+P;
  WI(NA)=ZZ;
  WI(EN)=-ZZ;
L330:
  EN=ENM2;
  goto L60;
/* no convergence */
L1000:
  IERR=EN;
L1001:
  return IERR;

#undef H
#undef WR
#undef WI
}

Entry(Int2) EigenValues(MatrPtr A, VectPtr Re, VectPtr Im) {

Int2 low,high;

  dbalan(A, &low, &high);
  delmhe(A, low, high);
  return dhqr(A, low, high, Re, Im); 
}

#undef MATRSIZE
#undef MATRIXELEM