/*
   __dh
   DoubleHopf(neutral saddle) point-->DoubleHopf(neutral saddle) Curve  
   September 30, 1997
*/

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

#include "stagen.h"

#include "_util.h"
#include "_linalg.h"

#include "__dh.h"
#include "_conti.h"	/* continuer's (i.e. generator's) data */


/* Numbers of visible name classes;
   the order must be the same as in /names section */
#define CL_TIME		1
#define CL_PHASE	2
#define CL_PAR		3
#define CL_EIGV		4
#define CL_TEST		5
#define CL_UTEST	6

#define _eps     staData->eps
#define _dx      staData->dx


Local(StagenDataPtr) stagenData=NULL;	/* global copy of Starter's parameter passed by content */
Local(__dh_DataPtr) staData=NULL;	/* global copy of our own data area */
Local(StdLinAlgDataPtr) staFunc=NULL;	/* global copy of pointer standard linear algebra struct */
Local(UtilitiesDataPtr) staUtil=NULL;	/* global copy of pointer to utility functions struct */
Local(Int2Ptr) active=NULL;        	/* index of active parameter(s) */
Local(Int2) nappend;			/* number of appended user functions */ 
Local(Int2) npar,nphase;		/* dims of par and phase spaces */
Local(Vector) x0=NULL;			/* approximate first point */
Local(Vector) x1=NULL;			/* current point */
Local(Vector) v0=NULL;			/* approximate tangent vector at first point */
Local(FloatHiPtr) X=NULL;		/* current set of phases used by Decoder */
Local(FloatHiPtr) P=NULL;		/* current set of params used by Decoder */
Local(FloatHiPtr) E=NULL;		/* current set of eigenvalues */
Local(VAR) zero=0;

#include "_symdiff.c"

/* Working space for defining and test functions  */

Local(Matrix) A,B,C,C1,C2,AA,BB,BP,BT,BJ,BJT,D,DE,G,GM,JM,TVM,TVMT,HM,GM1,GM2;
Local(Vector) vn,xp,xp1,V,W,F,Vp,L,VV,VB1,VB2,V11,W11,V12,V22,W12,W22,Q1,Q2,PP1,PP2;
Local(Vector) G0,G1,G2,G3,TV,HV;
Local(Vector) Re=NULL,Im=NULL;
Local(VAR) test[3];
Local(Int2) m1,n1,m2,n2; /* indices to adapt */

#define BifVector stagenData->BifDataOutPtr
#define ActiveUTestFuns     staData->ActiveUserTest
#define nUTest      	    stagenData->udFuncNum

/* Initialize function common for Starter and Decoder */
Local(Int2) Init(StagenDataPtr stagenptr) {
Int2 i,m;
  /* Set local pointers to data area and functions array */
  stagenData=stagenptr;
  staData=(__dh_DataPtr)stagenptr->StaPtr;
  /* Use standard linear algebra */
  staFunc=(StdLinAlgDataPtr)stagenptr->StdLinAlg;
  staUtil=(UtilitiesDataPtr)stagenptr->Utilities;
  /* Get dimensions of Phase and Par spaces */
  npar=*stagenptr->ClassDim[CL_PAR];
  nphase=*stagenptr->ClassDim[CL_PHASE];
  /* Call udf SetInitPoint function, if any */
  if (!stagenData->ContDataGenLen && FuncParActive(staData->SetInitPoint)) {	/* there is UDF to set InitPoint */
    Int2 Index_X,Index_P;
    Index_X=stagenptr->ParIndex(staData,X);
    Index_P=stagenptr->ParIndex(staData,P);
    FuncParCall(staData->SetInitPoint)(staData);
    stagenptr->UpdatePar(Index_X);
    stagenptr->UpdatePar(Index_P);
  }
  /* Do all the checks related to active parameters and find them */
  for (i=0,nappend=0; i<nUTest; i++) if (ActiveUTestFuns[i]==UDF_APPEND) nappend++;
  active=MemNew(sizeof(Int2)*(nappend+3));    
  if (staUtil->CheckParameters(staData->Active,npar,active,3+nappend)) {
    active=MemFree(active);
    return 1;
  }
  /* Checks related to analytical/numerical differentiation of RHS */
  if (!stagenData->Der1 && staData->dx <= 0) {
    staUtil->ErrMessage("Analytical Jacoby matrix is undefined.\nRecompile system or set positive 'Increment'");
    active=MemFree(active);
    return 2;
  }
  if (!stagenData->Der2 && staData->dx <= 0) {
    staUtil->ErrMessage("Analytical Hessian matrix is undefined.\nRecompile system or set positive 'Increment'");
    active=MemFree(active);
    return 3;
  }
  /* Create a copy parameters */
  m=nphase*(nphase-1)/2;
  if (!P) {
    P=MemNew(sizeof(FloatHi)*npar);
    MemCopy(P,staData->P,sizeof(FloatHi)*npar);
    /* Allocate memory for copy of phase vars used by Decoder */
    X=MemNew(sizeof(FloatHi)*nphase);
    /* Allocate memory for eigenvalues */
    E=MemNew(sizeof(FloatHi)*2*nphase);
    Re=staFunc->CreateVector(nphase);
    Im=staFunc->CreateVector(nphase);
    /* Allocate memory for working space used in defining and test functions */
    A=staFunc->CreateMatrix(nphase,nphase);
    B=staFunc->CreateMatrix(nphase,nphase);
    BP=staFunc->CreateMatrix(m+2,m+2);
    BT=staFunc->CreateMatrix(m+2,m+2);
    V12=staFunc->CreateVector(m+2);
    V22=staFunc->CreateVector(m+2);
    W12=staFunc->CreateVector(m+2);
    W22=staFunc->CreateVector(m+2);
    VB1=staFunc->CreateVector(m+2);
    VB2=staFunc->CreateVector(m+2);
    VV=staFunc->CreateVector(2*nphase);
    Q1=staFunc->CreateVector(m);
    Q2=staFunc->CreateVector(m);
    PP1=staFunc->CreateVector(m+2);
    PP2=staFunc->CreateVector(m+2);
    G=staFunc->CreateMatrix(2,2);
    G0=staFunc->CreateVector(nphase+3+nappend);
    G1=staFunc->CreateVector(nphase+3+nappend);
    G2=staFunc->CreateVector(nphase+3+nappend);
    G3=staFunc->CreateVector(nphase+3+nappend);
    JM=staFunc->CreateMatrix(nphase+3,nphase+3);
    TV=staFunc->CreateVector(nphase+3);
    TVM=staFunc->CreateMatrix(nphase+3,3);
    TVMT=staFunc->CreateMatrix(3,nphase+3);
    HM=staFunc->CreateMatrix(3,3);
    HV=staFunc->CreateVector(3);
    GM1=staFunc->CreateMatrix(nphase+3+nappend,4);
    GM2=staFunc->CreateMatrix(nphase+3+nappend,4);
    
    AA=staFunc->CreateMatrix(nphase,nphase);
    BB=staFunc->CreateMatrix(2*nphase,2*nphase);
    C=staFunc->CreateMatrix(nphase+nappend,nphase+3+nappend);
    C1=staFunc->CreateMatrix(nphase,nphase+3+nappend);
    C2=staFunc->CreateMatrix(2,nphase+3+nappend);
    x0=staFunc->CreateVector(nphase+3+nappend);
    x1=staFunc->CreateVector(nphase+3+nappend);
    v0=staFunc->CreateVector(nphase+3+nappend);
    vn=staFunc->CreateVector(nphase);
    xp=staFunc->CreateVector(nphase+3+nappend);
    xp1=staFunc->CreateVector(nphase+3+nappend);
    V=staFunc->CreateVector(nphase);
    W=staFunc->CreateVector(nphase);
    F=staFunc->CreateVector(nphase);
    L=staFunc->CreateVector(nphase);
    Vp=staFunc->CreateVector(nphase+3+nappend);
    BifVector=MemNew(sizeof(FloatHi)*(4*nphase+2));
    D=staFunc->CreateMatrix(nphase+2+nappend,nphase+3+nappend);
    DE=staFunc->CreateMatrix(nphase+3+nappend,nphase+3+nappend);
    if (staData->ActiveSing[2]) { /* Hopf-BT detection on */
      BJ=staFunc->CreateMatrix(nphase+1,nphase+1);
      BJT=staFunc->CreateMatrix(nphase+1,nphase+1);
      V11=staFunc->CreateVector(nphase+1);
      W11=staFunc->CreateVector(nphase+1);
    }
  }
  Der2num=ind2_(nphase+npar-1,nphase+npar-1)+1;
  Der3num=ind3_(nphase+npar-1,nphase+npar-1,nphase+npar-1)+1;

  ((ContDataPtr)stagenptr->GenPtr)->ActiveSing=staData->ActiveSing;
  ((ContDataPtr)stagenptr->GenPtr)->ActiveUserTest=staData->ActiveUserTest;
  ((ContDataPtr)stagenptr->GenPtr)->TypeTestFuns=staData->TypeTestFunc;
  ((ContDataPtr)stagenptr->GenPtr)->ZeroTestFuns=staData->ZeroTestFunc;
  
  return 0;
}

Local(Int2) JacRHS (Vector x, Matrix J);

Local(void) Term (Boolean OK) {
Int2 m;
  m=nphase*(nphase-1)/2;
  stagenData->BifDataOutLen=0;
/* saving global vectors V12,W12,V22 and W22 used in defining functions */
    if (OK&&P&&(stagenData->Reason==RF_CLEAN)) {
      stagenData->ContDataStaLen=4*(m+2)*sizeof(FloatHi);
      stagenData->ContDataStaPtr=MemNew(stagenData->ContDataStaLen);
      MemCopy(stagenData->ContDataStaPtr,V12,(m+2)*sizeof(FloatHi));
      MemCopy(stagenData->ContDataStaPtr+(m+2),W12,(m+2)*sizeof(FloatHi));
      MemCopy(stagenData->ContDataStaPtr+2*(m+2),V22,(m+2)*sizeof(FloatHi));
      MemCopy(stagenData->ContDataStaPtr+3*(m+2),W22,(m+2)*sizeof(FloatHi));
      stagenData->BifDataOutLen=0;
      stagenData->ProcessPoint(stagenData->ProcFuncNum, x1, "Last point");
    } else {
      stagenData->ContDataStaLen=0;
      stagenData->BifDataOutLen=0;
    }
    x0=staFunc->DestroyVector(x0);
    x1=staFunc->DestroyVector(x1);
    v0=staFunc->DestroyVector(v0);
    active=MemFree(active);
    if (P) {
      P=MemFree(P);
      X=MemFree(X);
      E=MemFree(E);
      Re=staFunc->DestroyVector(Re);
      Im=staFunc->DestroyVector(Im);
      A=staFunc->DestroyMatrix(A);
      B=staFunc->DestroyMatrix(B);
      BP=staFunc->DestroyMatrix(BP);
      BT=staFunc->DestroyMatrix(BT);
      V12=staFunc->DestroyVector(V12);
      W12=staFunc->DestroyVector(W12);
      V22=staFunc->DestroyVector(V22);
      W22=staFunc->DestroyVector(W22);
      Q1=staFunc->DestroyVector(Q1);
      Q2=staFunc->DestroyVector(Q2);
      PP1=staFunc->DestroyVector(PP1);
      PP2=staFunc->DestroyVector(PP2);
      G=staFunc->DestroyMatrix(G);
      G0=staFunc->DestroyVector(G0);
      G1=staFunc->DestroyVector(G1);      
      G2=staFunc->DestroyVector(G2);      
      G3=staFunc->DestroyVector(G3);      
      JM=staFunc->DestroyMatrix(JM);
      TV=staFunc->DestroyVector(TV);
      TVM=staFunc->DestroyMatrix(TVM);
      TVMT=staFunc->DestroyMatrix(TVMT);
      HM=staFunc->DestroyMatrix(HM);
      HV=staFunc->DestroyVector(HV);
      GM1=staFunc->DestroyMatrix(GM1);
      GM2=staFunc->DestroyMatrix(GM2);
      
      AA=staFunc->DestroyMatrix(AA);
      BB=staFunc->DestroyMatrix(BB);
      VV=staFunc->DestroyVector(VV);
      VB1=staFunc->DestroyVector(VB1);
      VB2=staFunc->DestroyVector(VB2);
      C=staFunc->DestroyMatrix(C);
      C1=staFunc->DestroyMatrix(C1);
      C2=staFunc->DestroyMatrix(C2);
      vn=staFunc->DestroyVector(vn);
      xp=staFunc->DestroyVector(xp);
      xp1=staFunc->DestroyVector(xp1);
      V=staFunc->DestroyVector(V);
      W=staFunc->DestroyVector(W);
      F=staFunc->DestroyVector(F);
      Vp=staFunc->DestroyVector(Vp);
      L=staFunc->DestroyVector(L);
      BifVector=MemFree(BifVector);
      D=staFunc->DestroyMatrix(D);
      DE=staFunc->DestroyMatrix(DE);
      if (staData->ActiveSing[2]) { /* Hopf-BT detection on */
        BJ=staFunc->DestroyMatrix(BJ);
        BJT=staFunc->DestroyMatrix(BJT);
        V11=staFunc->DestroyVector(V11);
        W11=staFunc->DestroyVector(W11);
      }
    }
} 

Entry(Int2) dh_JacDefDoubleHopf (Vector x, Matrix J);
Entry(Int2) dh_DefDoubleHopf (Vector x, Vector J);


/************/
/* Starters */
/* dh -> dh Starter */
Entry(Int2) dh_Starter(StagenDataPtr stagenptr) {
  Int2 i,j,k,kp1,Ret,m,p,q,r,s,l;
  Vector v1,v2,PIVOT;
  Matrix D,DE;
  VAR scal,t;
  if (!stagenptr) {	/* this is request to terminate */
    Term(TRUE);
    return 0;
  }
  if (Init(stagenptr)) return 1;	/* initialize local variables */
  m=nphase*(nphase-1)/2;
  
  /* Create the first point for generator */

  if (stagenptr->ContDataGenLen) {       /* "Extension" */
    MemCopy(V12,stagenptr->ContDataStaPtr,sizeof(FloatHi)*(m+2));
    MemCopy(W12,stagenptr->ContDataStaPtr+(m+2),sizeof(FloatHi)*(m+2));
    MemCopy(V22,stagenptr->ContDataStaPtr+2*(m+2),sizeof(FloatHi)*(m+2));
    MemCopy(W22,stagenptr->ContDataStaPtr+3*(m+2),sizeof(FloatHi)*(m+2));
    MemFree(stagenptr->ContDataStaPtr);
    stagenptr->ContDataStaLen=0;
    MemCopy(x0,staData->X,sizeof(FloatHi)*nphase);
    for (i=0; i<3+nappend; i++) x0[nphase+i]=staData->P[active[i]];
    if (JacRHS(x0,C1)) return 1;	    
    staFunc->CopyMatrixBlock(C1,0,0,A,0,0,nphase,nphase);
    if (staData->ActiveSing[0]) {
/* Fill in the bordered bi-product matrix BP */
      staFunc->ClearMatrix(BP);
      i=-1;
      for (p=1; p<nphase; p++) {
        for (q=0; q<p; q++) {
          i++;
          j=-1;
          for (r=1; r<nphase; r++) {
            for (s=0; s<r; s++) {
              j++;
              if (r == q) BP[i][j]=-A[p][s];       
              if (r != p && s == q) BP[i][j]=A[p][r];   
              if (r == p && s == q) BP[i][j]=A[p][p]+A[q][q]; 
              if (r == p && s != q) BP[i][j]=A[q][s];
              if (s == p) BP[i][j]=-A[q][r];  
            }
          }
        }
      }
      for (i=0; i<m; i++) {
        BP[m][i]=V12[i];
        BP[m+1][i]=V22[i];
      }
      for (i=0; i<m+2; i++) {
       BP[i][m]=W12[i];
       BP[i][m+1]=W22[i];
      }
    }    
  } else {
    v1=staFunc->CreateVector(nphase+3+nappend);
    v2=staFunc->CreateVector(nphase+3+nappend);
    D=staFunc->CreateMatrix(nphase+2+nappend,nphase+3+nappend);
    DE=staFunc->CreateMatrix(nphase+3+nappend,nphase+3+nappend);

    MemCopy(x0,staData->X,sizeof(FloatHi)*nphase);
    for (i=0; i<3+nappend; i++) x0[nphase+i]=staData->P[active[i]];
    if (JacRHS(x0,C1)) return 1;

/* continuation data have to be computed */

/* Fill in the bordered bi-product matrix BP */
    staFunc->CopyMatrixBlock(C1,0,0,A,0,0,nphase,nphase);
    staFunc->ClearMatrix(BP);
    i=-1;
    for (p=1; p<nphase; p++) {
      for (q=0; q<p; q++) {
        i++;
        j=-1;
        for (r=1; r<nphase; r++) {
          for (s=0; s<r; s++) {
            j++;
            if (r == q) BP[i][j]=-A[p][s];       
            if (r != p && s == q) BP[i][j]=A[p][r];   
            if (r == p && s == q) BP[i][j]=A[p][p]+A[q][q]; 
            if (r == p && s != q) BP[i][j]=A[q][s];
            if (s == p) BP[i][j]=-A[q][r];  
          }
        }
      }
    }
   
    GM=staFunc->CreateMatrix(m+1,m+1);
    staFunc->CopyMatrixBlock(BP,0,0,GM,0,0,m,m);
    for (i=0; i<m; i++) {
      GM[i][m]=i;
      GM[m][i]=i;
    } 
 /* Calculation of PIVOTS - complete pivoting */
    for (k=0;k<m-2;k++) {
      t=0.0;
      for (i=k;i<m;i++) {
	for (j=k;j<m;j++) {
    	  if (fabs(GM[i][j]) > t) {
            p = i;
            q = j;
            t = fabs(GM[i][j]);
          }           
	}
      }      
      if (k!=q) { /* switch colom k with colom  q */        
        for (i=0;i<=m;i++) {
          t=GM[i][k];
          GM[i][k]=GM[i][q];
          GM[i][q]=t;
	}
      }
      if (k!=p) { /* switch row k with row p */       
        for (i=0;i<=m;i++) {
          t=GM[k][i];         
          GM[k][i]=GM[p][i];
          GM[p][i]=t;
	}
      }        
      if (fabs(GM[k][k]) == 0.0) {
        staUtil->ErrMessage("Biproduct has rank defect at least three");
        Term(FALSE);
        return 1;
      }
      kp1=k+1;
      for (i=kp1;i<m;i++) { /* perform elimination */
        t=-GM[i][k]/GM[k][k];
        for (j=kp1;j<m;j++) {
          GM[i][j]+=t*GM[k][j];
	}
      }
    } 
    p=(Int2)GM[m-2][m];
    q=(Int2)GM[m-1][m];   
    staFunc->ClearVector(W12);
    staFunc->ClearVector(W22);
    W12[p]=1.0;
    W22[q]=1.0;
    p=(Int2)GM[m][m-2];
    q=(Int2)GM[m][m-1];   
    staFunc->ClearVector(V12);
    staFunc->ClearVector(V22);
    V12[p]=1.0;
    V22[q]=1.0;    
    GM=staFunc->DestroyMatrix(GM);
    for (i=0;i<m;i++) {
     BP[m][i] = V12[i];
     BP[m+1][i] = V22[i];
    }
    for (i=0;i<m;i++) {
     BP[i][m]=W12[i];
     BP[i][m+1]=W22[i];    
    }  
    staFunc->TransposeMatrix(BP,BT);
    Ret=staFunc->Decomp(BT);       
    if (Ret == 0) {
      staFunc->ClearVector(VB1);
      VB1[m]=1.0;
      staFunc->Solve(BT,VB1);
      staFunc->CopyVector(VB1,W12);  
      staFunc->ClearVector(VB2);
      VB2[m+1]=1.0;
      staFunc->Solve(BT,VB2);
      staFunc->CopyVector(VB2,W22);  
      staFunc->CopyMatrix(BP,BT);
      Ret=staFunc->Decomp(BT);       
      if (Ret == 0) {
        staFunc->ClearVector(VB1);
	VB1[m]=1.0;
        staFunc->Solve(BT,VB1);
        staFunc->CopyVector(VB1,V12);  
        staFunc->ClearVector(VB2);
	VB2[m+1]=1.0;
        staFunc->Solve(BT,VB2);
        staFunc->CopyVector(VB2,V22);  
      }
    }
    if (Ret) {
      staUtil->ErrMessage("Unable to find a bordering vector");   
      v1=staFunc->DestroyVector(v1);
      v2=staFunc->DestroyVector(v2);
      D=staFunc->DestroyMatrix(D);
      DE=staFunc->DestroyMatrix(DE);
      Term(FALSE);
      return 1;
    } 
    staFunc->NormalizeVector(V12); 
    staFunc->NormalizeVector(V22);
    staFunc->NormalizeVector(W12); 
    staFunc->NormalizeVector(W22);
  /* bordering the biproduct matrix with V12, V22, W12 and W22 */
    for (i=0;i<m;i++) {
     BP[m][i] = V12[i];
     BP[m+1][i] = V22[i];
    }
    for (i=0;i<m+2;i++) {
     BP[i][m] = W12[i];
     BP[i][m+1] = W22[i];
    }
  /* Calculation of derivatives of components of G */
    for (l=0;l<nphase+3+nappend;l++) {
      staFunc->CopyVector(x0,xp);
      staFunc->CopyVector(x0,xp1);
      xp[l]+=_dx;
      xp1[l]-=_dx;
      JacRHS(xp,C1);
      staFunc->CopyMatrixBlock(C1,0,0,A,0,0,nphase,nphase);
      staFunc->ClearMatrix(BT);
      i=-1;
      for (p=1; p<nphase; p++) {
        for (q=0; q<p; q++) {
          i++;
          j=-1;
          for (r=1; r<nphase; r++) {
            for (s=0; s<r; s++) {
              j++;
              if (r == q) BT[i][j]=-A[p][s];       
              if (r != p && s == q) BT[i][j]=A[p][r];   
              if (r == p && s == q) BT[i][j]=A[p][p]+A[q][q]; 
              if (r == p && s != q) BT[i][j]=A[q][s];
              if (s == p) BT[i][j]=-A[q][r];  
            }
          }
        }
      }
      for (i=0;i<m;i++) {
       BT[m][i] = V12[i];
       BT[m+1][i] = V22[i];
      }
      for (i=0;i<m+2;i++) {
       BT[i][m] = W12[i];
       BT[i][m+1] = W22[i];
      }
      staFunc->Decomp(BT);
      staFunc->ClearVector(VB1);
      VB1[m]=1.0;
      staFunc->Solve(BT,VB1);
      G0[l]=VB1[m];
      G2[l]=VB1[m+1];
      staFunc->ClearVector(VB2);
      VB2[m+1]=1.0;
      staFunc->Solve(BT,VB2);
      G1[l]=VB2[m];
      G3[l]=VB2[m+1];

      JacRHS(xp1,C1);
      staFunc->CopyMatrixBlock(C1,0,0,A,0,0,nphase,nphase);
      staFunc->ClearMatrix(BT);
      i=-1;
      for (p=1; p<nphase; p++) {
        for (q=0; q<p; q++) {
          i++;
          j=-1;
          for (r=1; r<nphase; r++) {
            for (s=0; s<r; s++) {
              j++;
              if (r == q) BT[i][j]=-A[p][s];       
              if (r != p && s == q) BT[i][j]=A[p][r];   
              if (r == p && s == q) BT[i][j]=A[p][p]+A[q][q]; 
              if (r == p && s != q) BT[i][j]=A[q][s];
              if (s == p) BT[i][j]=-A[q][r];  
            }
          }
        }
      }
      for (i=0;i<m;i++) {
       BT[m][i] = V12[i];
       BT[m+1][i] = V22[i];
      }
      for (i=0;i<m+2;i++) {
       BT[i][m] = W12[i];
       BT[i][m+1] = W22[i];
      }
      staFunc->Decomp(BT);
      staFunc->ClearVector(VB1);
      VB1[m]=1.0;
      staFunc->Solve(BT,VB1);
      G0[l]=(G0[l]-VB1[m])/(_dx+_dx);
      G2[l]=(G2[l]-VB1[m+1])/(_dx+_dx);
      staFunc->ClearVector(VB2);
      VB2[m+1]=1.0;
      staFunc->Solve(BT,VB2);
      G1[l]=(G1[l]-VB2[m])/(_dx+_dx);
      G3[l]=(G3[l]-VB2[m+1])/(_dx+_dx);      
    } /* for l ... */

  /* selection of indices */    
    staFunc->ClearMatrix(JM);
    JacRHS(x0,C1);
    staFunc->CopyMatrixBlock(C1,0,0,JM,0,0,nphase,nphase+3);
  /* placement of ones determined by partial pivoting */
    PIVOT=staFunc->CreateVector(nphase+3);
    staFunc->ClearVector(PIVOT);
    staFunc->TransposeMatrix(JM,JM);
    PIVOT[nphase+2]=1.0;
    for (k=0;k<=nphase+1;k++) {
     kp1=k+1;
     p = k;
     for (i=kp1;i<=nphase+2;i++) {
      if (fabs(JM[i][k]) > fabs(JM[p][k]))
        p = i;
     }
     PIVOT[k]=p;
     if (p!=k) PIVOT[nphase+2]=-PIVOT[nphase+2];
     t=JM[p][k];
     JM[p][k]=JM[k][k];
     JM[k][k]=t;
     if (t==0.0) continue;
     for (i=kp1;i<=nphase+2;i++) JM[i][k]=-JM[i][k]/t;
     for (j=kp1;j<=nphase+2;j++) {
       t=JM[p][j];
       JM[p][j]=JM[k][j];
       JM[k][j]=t;
       if (t!=0.0) for (i=kp1;i<=nphase+2;i++) JM[i][j]+=JM[i][k]*t;
     }
    }
    staFunc->ClearVector(TV);
    for (i=0;i<nphase+3;i++) TV[i]=i;
    for (k=0;k<nphase+2;k++) {
      i=(Int2)PIVOT[k];
      t=TV[i];
      TV[i]=TV[k];
      TV[k]=t;
    }
    p=(Int2)TV[nphase];
    q=(Int2)TV[nphase+1];
    r=(Int2)TV[nphase+2];  
    PIVOT=staFunc->DestroyVector(PIVOT);
  /* end of pivot information */
    staFunc->ClearMatrix(JM);
    staFunc->CopyMatrixBlock(C1,0,0,JM,0,0,nphase,nphase+3);
    JM[nphase][p]=1.0;
    JM[nphase+1][q]=1.0;
    JM[nphase+2][r]=1.0;
    staFunc->ClearVector(TV);
    TV[nphase]=1.0;
    staFunc->Decomp(JM);
    staFunc->Solve(JM,TV);
    for (i=0;i<nphase+3;i++) TVM[i][0] = TV[i];
    staFunc->ClearVector(TV);
    TV[nphase+1]=1.0;  
    staFunc->Solve(JM,TV);
    for (i=0;i<nphase+3;i++) TVM[i][1] = TV[i];    
    staFunc->ClearVector(TV);
    TV[nphase+2]=1.0;
    staFunc->Solve(JM,TV);
    for (i=0;i<nphase+3;i++) TVM[i][2] = TV[i]; 
    
    staFunc->TransposeMatrix(TVM,TVMT);
    staFunc->MultiplyMatrixMatrix(TVMT,TVM,HM); 
    staFunc->Decomp(HM);

    staFunc->MultiplyMatrixVector(TVMT,G0,HV);  
    staFunc->Solve(HM,HV);
    staFunc->MultiplyMatrixVector(TVM,HV,G0);  
    staFunc->MultiplyMatrixVector(TVMT,G1,HV);  
    staFunc->Solve(HM,HV);
    staFunc->MultiplyMatrixVector(TVM,HV,G1);      
    staFunc->MultiplyMatrixVector(TVMT,G2,HV);  
    staFunc->Solve(HM,HV);
    staFunc->MultiplyMatrixVector(TVM,HV,G2);
    staFunc->MultiplyMatrixVector(TVMT,G3,HV);  
    staFunc->Solve(HM,HV);
    staFunc->MultiplyMatrixVector(TVM,HV,G3);      

  /* Determine component with maximum norm => gives m1,n1 */
    G[0][0] = staFunc->NormOfVector(G0);
    G[0][1] = staFunc->NormOfVector(G1);
    G[1][0] = staFunc->NormOfVector(G2);
    G[1][1] = staFunc->NormOfVector(G3);    
    m1=0;
    n1=0;      
    if (G[0][1] > G[m1][n1]) n1=1;  
    if (G[1][0] > G[m1][n1]) { m1=1; n1=0; }
    if (G[1][1] > G[m1][n1]) { m1=1; n1=1; }
    for (i=0;i<nphase+3;i++) {
      GM1[i][0] = G0[i]; 
      GM1[i][1] = G1[i];
      GM1[i][2] = G2[i];
      GM1[i][3] = G3[i];
    }
  /* G0 has biggest norm ; necessary for projection */ 
    for (i=0;i<nphase+3;i++) {
      G0[i] = GM1[i][2*m1+n1];
    }      
    staFunc->NormalizeVector(G0);
  /* G1,G2,G3 are projected on G0 */ 
    for (j=0;j<4;j++) {
      if (j != (2*m1+n1)) {
       for (i=0;i<nphase+3;i++) { /* neem een vector, niet de grootste */
          G1[i] = GM1[i][j];
       }
       scal = staFunc->ScalProd(G1,G0);
       staFunc->AddScaledVector(G1,G0,-scal,G1);
       for (i=0;i<nphase+3;i++) { /* zet nieuwe vector terug */
         GM1[i][j] = G1[i];
       }
      }
    }      
  /* compute biggest norm between G1,G2 and G3  */
    for (i=0;i<nphase+3;i++) {
      G0[i] = GM1[i][0];
      G1[i] = GM1[i][1];
      G2[i] = GM1[i][2];
      G3[i] = GM1[i][3];
    }
    G[0][0] = staFunc->NormOfVector(G0);
    G[0][1] = staFunc->NormOfVector(G1);
    G[1][0] = staFunc->NormOfVector(G2);
    G[1][1] = staFunc->NormOfVector(G3);    
    G[m1][n1]=-1.0;
    m2=0;
    n2=0;
    if (G[0][1] > G[m2][n2]) n2=1;  
    if (G[1][0] > G[m2][n2]) { m2=1; n2=0; }
    if (G[1][1] > G[m2][n2]) { m2=1; n2=1; }    
    
  /* Compute a tangent vector to the Double Hopf curve */
    dh_DefDoubleHopf(x0,v0);
    Ret=dh_JacDefDoubleHopf(x0,D);  
    for (i=0; i<nphase+3+nappend; i++) {
      v1[i]=1.0;
      staFunc->AppendMatrixVector(D,v1,DE);
      Ret=staFunc->Decomp(DE);
      if (Ret == 0) {
        v2[nphase+2+nappend]=stagenData->Forward ? 1 : -1;
        staFunc->Solve(DE,v2);
        staFunc->CopyVector(v2,v0);
        break;
      };
      v1[i]=0.0;
    };
    if (Ret) {
       staUtil->ErrMessage
         ("Unable to find a tangent vector to the Double Hopf curve");   
       v1=staFunc->DestroyVector(v1);
       v2=staFunc->DestroyVector(v2);
       D=staFunc->DestroyMatrix(D);
       DE=staFunc->DestroyMatrix(DE);
       Term(FALSE);
       return 1;
    }  
    staFunc->NormalizeVector(v0);

    v1=staFunc->DestroyVector(v1);
    v2=staFunc->DestroyVector(v2);
    D=staFunc->DestroyMatrix(D);
    DE=staFunc->DestroyMatrix(DE);
  }
  /* Initialize continuation */
  ((ContDataPtr)stagenptr->GenPtr)->X0=x0;
  ((ContDataPtr)stagenptr->GenPtr)->V0=v0;
  ((ContDataPtr)stagenptr->GenPtr)->ActiveSing=staData->ActiveSing;
  ((ContDataPtr)stagenptr->GenPtr)->ActiveUserTest=staData->ActiveUserTest;
  ((ContDataPtr)stagenptr->GenPtr)->TypeTestFuns=staData->TypeTestFunc;
  ((ContDataPtr)stagenptr->GenPtr)->ZeroTestFuns=staData->ZeroTestFunc;
  /* That's all */
  return 0;
}


typedef EntryPtr(Int2,TestFuncPtr,(Vector xx, Vector vv, FloatHiPtr TV));
typedef EntryPtr(Int2,ProcFuncPtr,(Int2 Ind, Vector xx, Vector vv, CharPtr *msg));


/************************/
/* Some common routines */
Local(void) CopyToX(Vector vp) {
  MemCopy(X,vp,sizeof(FloatHi)*nphase);  
}

Local(void) CopyToP(Vector vp) {
Int2 i;
  for (i=0; i<3+nappend; i++) P[active[i]]=vp[nphase+i];
}

Local(void) CopyToE(Vector Re, Vector Im) {
  MemCopy(E,Re,sizeof(FloatHi)*nphase);
  MemCopy(E+nphase,Im,sizeof(FloatHi)*nphase);
}


/***********/
/* Decoder */

Entry(Int2) dh_Decoder(VoidPtr vpoint, FloatHiPtr PNTR indirect, Int2 oper) {
  switch (oper) {
    case DECODER_INIT:	/* point is a pointer to StagenData structure */
    case DECODER_TERM:
     break;
    case DECODER_DIM:	/* returns number of M-points in given G-point */
      return 1;
    default:		/* extract next M-point from G-point pointed by vpoint */
      /* vpoint is a Vector */
      indirect[CL_TIME]=&zero;
      indirect[CL_PHASE]=X;
      CopyToX(vpoint);
      indirect[CL_PAR]=P;
      CopyToP(vpoint);
      indirect[CL_EIGV]=E;
      CopyToE(Re,Im);
      indirect[CL_TEST]=test;
  }
  return 0;
}
/***************************/
/* User-defined functions  */
Entry(Int2) dh_DefUser (Vector x, Vector y) {
Int2 i,j;
  for (i=j=0; i<nUTest; i++) 
    if (ActiveUTestFuns[i]==UDF_APPEND) {
      (stagenData->udFunc)(x,i,&y[j]);
      j++;
    }
  return 0;
}

/*******/
/* RHS */
Entry(Int2) dh_DefRHS (Vector x, Vector y) {
  CopyToX(x);
  CopyToP(x);
  stagenData->Rhs(X,P,&zero,y);	
  return 0;
}

/*************************************/
/* Defining function for equilibrium */
Local(Int2) DefEquilib (Vector x, Vector y) {
  dh_DefRHS(x,y);
  if (nappend) dh_DefUser(x,y+nphase);
  return 0;
}

/********************/
/* Jacobian of RHS  */
Local(Int2) JacRHS (Vector x, Matrix Jac) {
  staFunc->CopyVectorElements(x,0,xp,0,nphase+3+nappend);     
  if (stagenData->Der1) {
    SymJac(xp,Jac);
  } else {
    if (staFunc->Der(dh_DefRHS, nphase, xp, _dx, Jac)) return 1;
  }
  return 0;
}
/***************************************************/
/* Jacobian of defining conditions for equilibria  */
Local(Int2) JacDefEquilib (Vector x, Matrix Jac) {
  staFunc->ClearMatrix(Jac);
  JacRHS(x, Jac);
  if (nappend) staFunc->Der(dh_DefUser, nappend, x, _dx, Jac+nphase);
  return 0;
}
/***********************************/
/* Double Hopf defining conditions */
Entry(Int2) dh_DefBip (Vector x, Vector y) {
Int2 i,j,p,q,r,s,m;
Int2 Ret;
  m=nphase*(nphase-1)/2;

  if (JacRHS(x,C1)) return 1;	    /* A,C1,BP,V12,V22,V22,W22,Q1,Q2 - global */
  staFunc->CopyMatrixBlock(C1,0,0,A,0,0,nphase,nphase);
  
/* Fill in the bordered bi-product matrix BP */
  staFunc->ClearMatrix(BP);
  i=-1;
  for (p=1; p<nphase; p++) {
    for (q=0; q<p; q++) {
      i++;
      j=-1;
      for (r=1; r<nphase; r++) {
        for (s=0; s<r; s++) {
          j++;
          if (r == q) BP[i][j]=-A[p][s];       
          if (r != p && s == q) BP[i][j]=A[p][r];   
          if (r == p && s == q) BP[i][j]=A[p][p]+A[q][q]; 
          if (r == p && s != q) BP[i][j]=A[q][s];
          if (s == p) BP[i][j]=-A[q][r];  
        }
      }
    }
  }
  for (i=0; i<m; i++) {
    BP[m][i]=V12[i];
    BP[m+1][i]=V22[i];
  }
  for (i=0; i<m+2; i++) {
   BP[i][m]=W12[i];
   BP[i][m+1]=W22[i];
  }

/* compute Double Hopf defining functions */
  staFunc->CopyMatrix(BP,BT);
  Ret=staFunc->Decomp(BT);
  if (Ret != 0) {
    return 1;
  }
  staFunc->ClearVector(VB1);
  VB1[m]=1.0;
  staFunc->Solve(BT,VB1);
  staFunc->CopyVectorElements(VB1,0,Q1,0,m);
  G[0][0]=VB1[m];
  G[1][0]=VB1[m+1];

  staFunc->ClearVector(VB2);
  VB2[m+1]=1.0;
  staFunc->Solve(BT,VB2);
  staFunc->CopyVectorElements(VB2,0,Q2,0,m);
  G[0][1]=VB2[m];
  G[1][1]=VB2[m+1];
  
  y[0]=G[m1][n1];          /* the indices are calculated in adapter */
  y[1]=G[m2][n2]; 
  return 0;
}

/***********************************************/
/* Jacobian of Double Hopf defining conditions */
Local(Int2) JacDefBip (Vector x, Matrix Jac) {
int i,j,l,m,p,r,s,q;
Matrix S;
  m=nphase*(nphase-1)/2;
  staFunc->ClearMatrix(Jac);
  if (stagenData->Der2) {
/* creation of DefBip Jacobian matrix Jac using symbolic second derivatives */
    CopyToX(x);
    CopyToP(x);
    H=stagenData->Der2(X,P,&zero);
/* take global vector VB1 and VB2 generated at the computation of DefBip */
/* compute PP1 and PP2 using the transposed bordered bi-product */
    staFunc->TransposeMatrix(BP,BT);
    staFunc->Decomp(BT);

    staFunc->ClearVector(PP1);
    PP1[m]=1.0;
    staFunc->Solve(BT,PP1);
    staFunc->ClearVector(PP2);
    PP2[m+1]=1.0;
    staFunc->Solve(BT,PP2);
   
    S=staFunc->CreateMatrix(2,2);
/* compute gradient of DefBip */
    for (l=0; l<nphase+3+nappend; l++) {
      staFunc->ClearMatrix(BT);
      i=-1;
      for (p=1; p<nphase; p++) {
        for (q=0; q<p; q++) {
          i++;
          j=-1;
          for (r=1; r<nphase; r++) {
            for (s=0; s<r; s++) {
              j++;
              if (r == q) BT[i][j]=-HessianElem(p,s,l);       
              if (r != p && s == q) BT[i][j]=HessianElem(p,r,l);   
              if (r == p && s == q) BT[i][j]=HessianElem(p,p,l)+HessianElem(q,q,l); 
              if (r == p && s != q) BT[i][j]=HessianElem(q,s,l);
              if (s == p) BT[i][j]=-HessianElem(q,r,l);  
            }
          }
        }
      }
      for (i=0,S[0][0]=0.0,S[0][1]=0.0,S[1][0]=0.0,S[1][1]=0.0; i<m; i++) {
        for (j=0; j<m; j++) {
	  S[0][0]-=PP1[i]*VB1[j]*BT[i][j];
	  S[0][1]-=PP1[i]*VB2[j]*BT[i][j];
          S[1][0]-=PP2[i]*VB1[j]*BT[i][j];
          S[1][1]-=PP2[i]*VB2[j]*BT[i][j];
	}
      }
      Jac[0][l]=S[m1][n1];
      Jac[1][l]=S[m2][n2];

    }
    S=staFunc->DestroyMatrix(S); /* not needed any more */
  } else {
    if (staFunc->Der(dh_DefBip, 2, x, _dx, Jac)) return 1;
  }
  return(0);
}

/********************************************/
/* Defining functions for Double Hopf curve */

Entry(Int2) dh_DefDoubleHopf (Vector x, Vector y) {
  if (DefEquilib(x,y)) return 1;
  if (dh_DefBip(x,y+nphase+nappend)) return 2;
  return 0;
}

/*************************************************/
/* Jacobian of defining functions for Hopf curve */  
Entry(Int2) dh_JacDefDoubleHopf (Vector x, Matrix Jac) {
  staFunc->ClearMatrix(Jac);
  if (JacDefEquilib(x,C)) return 1;
  staFunc->CopyMatrixBlock(C,0,0,Jac,0,0,nphase+nappend,nphase+3+nappend);
  if (JacDefBip(x,C2)) return 2;
  staFunc->CopyMatrixBlock(C2,0,0,Jac,nphase+nappend,0,2,nphase+3+nappend);
  return 0;
}

/*********************/
/* Default processor */

Entry(Int2) dh_ProcDefault(Int2 Ind, Vector x, Vector v1, CharPtr *msg) {

  /* Assert: v1==NULL, msg==NULL, Ind==0 */
  MemCopy(x1,x,sizeof(FloatHi)*(nphase+3+nappend));     /* for the last point */
  if (!staData->eigcomp) return 0;
  if (JacRHS(x,C1)) return (1); 

  staFunc->EigenValues(C1,Re,Im);
  return 0;
}

/*********************************/
/* Test and processing functions */
Local(Char) buf[80];

Entry(Int2) dh_TestResonance(Vector x, Vector v, FloatHiPtr res) {
int m,Ret;
  m=nphase*(nphase-1)/2;

  staFunc->CopyMatrix(BP,BT);
  Ret=staFunc->Decomp(BT);
  if (Ret) {   
    return 1;
  }
  staFunc->ClearVector(PP1);
  staFunc->CopyVectorElements(VB1,0,PP1,0,m);
  staFunc->Solve(BT,PP1);
  staFunc->ClearVector(PP2);
  staFunc->CopyVectorElements(VB2,0,PP2,0,m);
  staFunc->Solve(BT,PP2);  
  *res=PP1[m]*PP2[m+1]-PP1[m+1]*PP2[m];
  test[0]=*res;
  return 0;
}

Entry(Int2) dh_ProcResonance(Int2 Ind, Vector x, Vector v, CharPtr *msg) {
  if (Ind==0) {
    sprintf(buf,"Resonant Double Hopf / Resonant Double Neutral Saddle");
  } else {
     sprintf(buf,"Resonant Double Hopf / Resonant Double Neutral Saddle (?)");
    }   
  *msg=buf;
  return 0;
}

Entry(Int2) dh_TestZeroDH(Vector x, Vector v, FloatHiPtr res) {
 *res=1.0;
 if (JacRHS(x,C1)) return 1;
 staFunc->CopyMatrixBlock(C1,0,0,A,0,0,nphase,nphase);
 if (staFunc->Decomp(A)) {
   *res=0.0;
 } else {
    staFunc->GetMatrixData(A,res,NULL);
   }
 test[1]=*res;
 return 0;
}

Entry(Int2) dh_ProcZeroDH(Int2 Ind, Vector x, Vector v, CharPtr *msg) {
  if (Ind==0) {
    sprintf(buf,"Zero eigenvalue + Double Hopf");
  } else {
     sprintf(buf,"Zero eigenvalue + Double Hopf (?)");
    }   
  *msg=buf;
  return 0;
}

Entry(Int2) dh_TestHBT(Vector x, Vector v, FloatHiPtr res) {
int i,j,k,kp1,p,q,Ret;
VAR t;

   staFunc->ClearMatrix(BJ);
   if (JacRHS(x,C1)) return 1;
   staFunc->CopyMatrixBlock(C1,0,0,BJ,0,0,nphase,nphase);
   for (i=0; i<nphase; i++) {
     BJ[i][nphase]=i;
     BJ[nphase][i]=i;
   } 
 /* Calculation of PIVOTS - complete pivoting */
   for (k=0;k<nphase-1;k++) {
     t=0.0;
     for (i=k;i<nphase;i++) {
       for (j=k;j<nphase;j++) {
    	 if (fabs(BJ[i][j]) > t) {
           p = i;
           q = j;
           t = fabs(BJ[i][j]);
         }           
       }
     }      
     if (k!=q) { /* switch colom k with colom  q */        
       for (i=0;i<=nphase;i++) {
         t=BJ[i][k];
         BJ[i][k]=BJ[i][q];
         BJ[i][q]=t;
       }
     }
     if (k!=p) { /* switch row k with row p */       
       for (i=0;i<=nphase;i++) {
         t=BJ[k][i];         
         BJ[k][i]=BJ[p][i];
         BJ[p][i]=t;
       }
     }        
     if (fabs(BJ[k][k]) == 0.0) {
       staUtil->ErrMessage("Jacobian has rank defect at least two");
       Term(FALSE);
       return 1;
     }
     kp1=k+1;
     for (i=kp1;i<nphase;i++) { /* perform elimination */
       t=-BJ[i][k]/BJ[k][k];
       for (j=kp1;j<nphase;j++) {
         BJ[i][j]+=t*BJ[k][j];
       }
     }
   } 
   p=(Int2)BJ[nphase][nphase-1];
   q=(Int2)BJ[nphase-1][nphase]; 
   staFunc->ClearMatrix(BJ);
   staFunc->CopyMatrixBlock(C1,0,0,BJ,0,0,nphase,nphase);
   BJ[nphase][p]=1.0;
   BJ[q][nphase]=1.0; 
   staFunc->CopyMatrix(BJ,BJT);  
   Ret=staFunc->Decomp(BJT);
   if (Ret) {
     return 1;
   } 
   staFunc->ClearVector(V11);
   V11[nphase]=1.0;
   staFunc->Solve(BJT,V11);
 
   staFunc->TransposeMatrix(BJ,BJT);
   Ret=staFunc->Decomp(BJT);
   if (Ret) {
     return 1;
   } 
   staFunc->ClearVector(W11);
   W11[nphase]=1.0;
   staFunc->Solve(BJT,W11);
   for (i=0,*res=0.0; i<nphase; i++) {
     *res+=V11[i]*W11[i];
   }
   test[2]=*res;
   return 0;
}

Entry(Int2) dh_ProcHBT(Int2 Ind, Vector x, Vector v, CharPtr *msg) {
  if (Ind==0) {
    sprintf(buf,"Hopf + Bogdanov-Takens");
  } else {
     sprintf(buf,"Hopf + Bogdanov-Takens (?)");
    }   
  *msg=buf;
  return 0;
}

/********************************************************/
/*  Adaptation of the bordering vectors V12,V22,W12 and W22 */
Entry(Int2) dh_Adapter(Vector x, Vector v) {
Int2 i,j,m,p,q,r,s,l,Ret;
VAR scal;
Matrix S;

  m=nphase*(nphase-1)/2;
  
  staFunc->TransposeMatrix(BP,BT);
  staFunc->Decomp(BT);

  staFunc->ClearVector(PP1);
  PP1[m]=1.0;
  staFunc->Solve(BT,PP1);
  staFunc->CopyVector(PP1,W12);
  staFunc->ClearVector(PP2);
  PP2[m+1]=1.0;
  staFunc->Solve(BT,PP2);
  staFunc->CopyVector(PP2,W22);
  staFunc->CopyVector(Q1,V12);
  staFunc->CopyVector(Q2,V22); 

  staFunc->NormalizeVector(V12);
  staFunc->NormalizeVector(W12);
  staFunc->NormalizeVector(V22);
  staFunc->NormalizeVector(W22);
/* update indices */
/* Calculation of derivatives of components of G */
  if (stagenData->Der2) {    
    for (i=0;i<m;i++) {
     BP[m][i] = V12[i];
     BP[m+1][i] = V22[i];
    }
    for (i=0;i<m+2;i++) {
     BP[i][m] = W12[i];
     BP[i][m+1] = W22[i];
    }
    CopyToX(x);
    CopyToP(x);
    H=stagenData->Der2(X,P,&zero);
/* compute VB1 and VB2 using the bordered bi-product */
    staFunc->CopyMatrix(BP,BT);
    Ret=staFunc->Decomp(BT);
    if (Ret != 0) {
      return 1;
    }
    staFunc->ClearVector(VB1);
    VB1[m]=1.0;
    staFunc->Solve(BT,VB1);
    staFunc->ClearVector(VB2);
    VB2[m+1]=1.0;
    staFunc->Solve(BT,VB2);
 
/* compute PP1 and PP2 using the transposed bordered bi-product */
    staFunc->TransposeMatrix(BP,BT);
    staFunc->Decomp(BT);

    staFunc->ClearVector(PP1);
    PP1[m]=1.0;
    staFunc->Solve(BT,PP1);
    staFunc->ClearVector(PP2);
    PP2[m+1]=1.0;
    staFunc->Solve(BT,PP2);
   
    S=staFunc->CreateMatrix(2,2);
/* compute gradient of DefBip */
    for (l=0; l<nphase+3+nappend; l++) {
      staFunc->ClearMatrix(BT);
      i=-1;
      for (p=1; p<nphase; p++) {
        for (q=0; q<p; q++) {
          i++;
          j=-1;
          for (r=1; r<nphase; r++) {
            for (s=0; s<r; s++) {
              j++;
              if (r == q) BT[i][j]=-HessianElem(p,s,l);       
              if (r != p && s == q) BT[i][j]=HessianElem(p,r,l);   
              if (r == p && s == q) BT[i][j]=HessianElem(p,p,l)+HessianElem(q,q,l); 
              if (r == p && s != q) BT[i][j]=HessianElem(q,s,l);
              if (s == p) BT[i][j]=-HessianElem(q,r,l);  
            }
          }
        }
      }
      for (i=0,S[0][0]=0.0,S[0][1]=0.0,S[1][0]=0.0,S[1][1]=0.0; i<m; i++) {
        for (j=0; j<m; j++) {
	  S[0][0]-=PP1[i]*VB1[j]*BT[i][j];
	  S[0][1]-=PP1[i]*VB2[j]*BT[i][j];
          S[1][0]-=PP2[i]*VB1[j]*BT[i][j];
          S[1][1]-=PP2[i]*VB2[j]*BT[i][j];
	}
      }
      G0[l]=S[0][0];
      G1[l]=S[0][1];
      G2[l]=S[1][0];
      G3[l]=S[1][1];
    }
    S=staFunc->DestroyMatrix(S); /* not needed any more */    
  } else {
   for (l=0;l<nphase+3+nappend;l++) {
    staFunc->CopyVector(x,xp);
    staFunc->CopyVector(x,xp1);
    xp[l]+=_dx;
    xp1[l]-=_dx;
    JacRHS(xp,C1);
    staFunc->CopyMatrixBlock(C1,0,0,A,0,0,nphase,nphase);
    staFunc->ClearMatrix(BT);
    i=-1;
    for (p=1; p<nphase; p++) {
      for (q=0; q<p; q++) {
        i++;
        j=-1;
        for (r=1; r<nphase; r++) {
          for (s=0; s<r; s++) {
            j++;
            if (r == q) BT[i][j]=-A[p][s];       
            if (r != p && s == q) BT[i][j]=A[p][r];   
            if (r == p && s == q) BT[i][j]=A[p][p]+A[q][q]; 
            if (r == p && s != q) BT[i][j]=A[q][s];
            if (s == p) BT[i][j]=-A[q][r];  
          }
        }
      }
    }
    for (i=0;i<m;i++) {
     BT[m][i] = V12[i];
     BT[m+1][i] = V22[i];
    }
    for (i=0;i<m+2;i++) {
     BT[i][m] = W12[i];
     BT[i][m+1] = W22[i];
    }
    staFunc->Decomp(BT);
    staFunc->ClearVector(VB1);
    VB1[m]=1.0;
    staFunc->Solve(BT,VB1);
    G0[l]=VB1[m];
    G2[l]=VB1[m+1];
    staFunc->ClearVector(VB2);
    VB2[m+1]=1.0;
    staFunc->Solve(BT,VB2);
    G1[l]=VB2[m];
    G3[l]=VB2[m+1];

    JacRHS(xp1,C1);
    staFunc->CopyMatrixBlock(C1,0,0,A,0,0,nphase,nphase);
    staFunc->ClearMatrix(BT);
    i=-1;
    for (p=1; p<nphase; p++) {
      for (q=0; q<p; q++) {
        i++;
        j=-1;
        for (r=1; r<nphase; r++) {
          for (s=0; s<r; s++) {
            j++;
            if (r == q) BT[i][j]=-A[p][s];       
            if (r != p && s == q) BT[i][j]=A[p][r];   
            if (r == p && s == q) BT[i][j]=A[p][p]+A[q][q]; 
            if (r == p && s != q) BT[i][j]=A[q][s];
            if (s == p) BT[i][j]=-A[q][r];  
          }
        }
      }
    }
    for (i=0;i<m;i++) {
     BT[m][i] = V12[i];
     BT[m+1][i] = V22[i];
    }
    for (i=0;i<m+2;i++) {
     BT[i][m] = W12[i];
     BT[i][m+1] = W22[i];
    }
    staFunc->Decomp(BT);
    staFunc->ClearVector(VB1);
    VB1[m]=1.0;
    staFunc->Solve(BT,VB1);
    G0[l]=(G0[l]-VB1[m])/(_dx+_dx);
    G2[l]=(G2[l]-VB1[m+1])/(_dx+_dx);
    staFunc->ClearVector(VB2);
    VB2[m+1]=1.0;
    staFunc->Solve(BT,VB2);
    G1[l]=(G1[l]-VB2[m])/(_dx+_dx);
    G3[l]=(G3[l]-VB2[m+1])/(_dx+_dx);      
   } /* for l ... */
  } /* else */
/* save derivatives for further use */ 
  for (i=0;i<nphase+3;i++) {
    GM2[i][0] = G0[i];
    GM2[i][1] = G1[i];
    GM2[i][2] = G2[i];
    GM2[i][3] = G3[i];
  }
/* selection of indices */    
  staFunc->ClearMatrix(JM);
  JacRHS(x,C1);
  staFunc->CopyMatrixBlock(C1,0,0,JM,0,0,nphase,nphase+3);
  for (i=0;i<nphase+3+nappend;i++) {
    JM[nphase][i] = GM2[i][2*m1+n1];
    JM[nphase+1][i] = GM2[i][2*m2+n2];
    JM[nphase+2][i] = v[i]; /* last row = tangent vector to curve */
  }
  staFunc->ClearVector(TV);
  TV[nphase]=1.0;
  staFunc->Decomp(JM);
  staFunc->Solve(JM,TV);
  for (i=0;i<nphase+3;i++) TVM[i][0] = TV[i];
  staFunc->ClearVector(TV);
  TV[nphase+1]=1.0;  
  staFunc->Solve(JM,TV);
  for (i=0;i<nphase+3;i++) TVM[i][1] = TV[i];    
  staFunc->ClearVector(TV);
  TV[nphase+2]=1.0;
  staFunc->Solve(JM,TV);
  for (i=0;i<nphase+3;i++) TVM[i][2] = TV[i]; 
  
  staFunc->TransposeMatrix(TVM,TVMT);
  staFunc->MultiplyMatrixMatrix(TVMT,TVM,HM); 
  staFunc->Decomp(HM);
  
  staFunc->MultiplyMatrixVector(TVMT,G0,HV);  
  staFunc->Solve(HM,HV);
  staFunc->MultiplyMatrixVector(TVM,HV,G0);  
  staFunc->MultiplyMatrixVector(TVMT,G1,HV);  
  staFunc->Solve(HM,HV);
  staFunc->MultiplyMatrixVector(TVM,HV,G1);      
  staFunc->MultiplyMatrixVector(TVMT,G2,HV);  
  staFunc->Solve(HM,HV);
  staFunc->MultiplyMatrixVector(TVM,HV,G2);
  staFunc->MultiplyMatrixVector(TVMT,G3,HV);  
  staFunc->Solve(HM,HV);
  staFunc->MultiplyMatrixVector(TVM,HV,G3);      

/* Determine component with maximum norm => gives m1,n1 */
  G[0][0] = staFunc->NormOfVector(G0);
  G[0][1] = staFunc->NormOfVector(G1);
  G[1][0] = staFunc->NormOfVector(G2);
  G[1][1] = staFunc->NormOfVector(G3);    
  m1=0;
  n1=0;      
  if (G[0][1] > G[m1][n1]) n1=1;  
  if (G[1][0] > G[m1][n1]) { m1=1; n1=0; }
  if (G[1][1] > G[m1][n1]) { m1=1; n1=1; }
  for (i=0;i<nphase+3;i++) {
    GM1[i][0] = G0[i]; 
    GM1[i][1] = G1[i];
    GM1[i][2] = G2[i];
    GM1[i][3] = G3[i];
  }
/* G0 has biggest norm ; needed for projection */ 
  for (i=0;i<nphase+3;i++) {
    G0[i] = GM1[i][2*m1+n1];
  }      
  staFunc->NormalizeVector(G0);
/* G1,G2,G3 are projected on G0 */ 
  for (j=0;j<4;j++) {
    if (j != (2*m1+n1)) {
     for (i=0;i<nphase+3;i++) { /* neem een vector, niet de grootste */
        G1[i] = GM1[i][j];
     }
     scal = staFunc->ScalProd(G1,G0);
     staFunc->AddScaledVector(G1,G0,-scal,G1);
     for (i=0;i<nphase+3;i++) { /* zet nieuwe vector terug */
       GM1[i][j] = G1[i];
     }
    }
  }      
/* compute biggest norm between G1,G2 and G3  */
  for (i=0;i<nphase+3;i++) {
    G0[i] = GM1[i][0];
    G1[i] = GM1[i][1];
    G2[i] = GM1[i][2];
    G3[i] = GM1[i][3];
  }
  G[0][0] = staFunc->NormOfVector(G0);
  G[0][1] = staFunc->NormOfVector(G1);
  G[1][0] = staFunc->NormOfVector(G2);
  G[1][1] = staFunc->NormOfVector(G3);    
  G[m1][n1]=-1.0;
  m2=0;
  n2=0;
  if (G[0][1] > G[m2][n2]) n2=1;  
  if (G[1][0] > G[m2][n2]) { m2=1; n2=0; }
  if (G[1][1] > G[m2][n2]) { m2=1; n2=1; }      
/* Message(MSG_OK,"(m1,n1) = (%i,%i) \n (m2,n2) = (%i,%i)",m1,n1,m2,n2); */
  return 0;
}