/*
   __lp
   Any Point-->Limit point Curve using
   Keller's standard augmented system 
*/

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

#include "stagen.h"

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

#include "__lp.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

#define PI	4.0*atan(1.0)
#define PID2	2.0*atan(1.0)   

Local(StagenDataPtr) stagenData=NULL;	/* global copy of Starter's parameter passed by content */
Local(__lp_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;	        	/* 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(Boolean) SymbNumer1,SymbNumer2;   /* TRUE for symbolic differentiation  of the correcponding order*/
Local(VAR) zero=0;

#include "_symdiff.c"


/* Working space for defining and test functions  */

Local(Matrix) A,B,C,C1,C2,BP=NULL;
Local(Vector) vn,xp,xp1,V,W,F,Vp;
Local(Vector) Re=NULL,Im=NULL;
Local(VAR) test[3],dx,ddx,dddx;

#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;
  /* Set local pointers to data area and functions array */
  stagenData=stagenptr;
  staData=(__lp_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+2));    
  if (staUtil->CheckParameters(staData->Active,npar,active,2+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 ((staData->ActiveSing[0] || staData->ActiveSing[1] || staData->ActiveSing[2]) && 
      !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 */
  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 the initial point and tangent vector */
    x0=staFunc->CreateVector(2*nphase+2+nappend);
    x1=staFunc->CreateVector(2*nphase+2+nappend);
    v0=staFunc->CreateVector(2*nphase+2+nappend);
    /* Allocate memory for working space used in defining and test functions */
    A=staFunc->CreateMatrix(nphase,nphase);     
    B=staFunc->CreateMatrix(nphase,nphase);    
    C=staFunc->CreateMatrix(nphase+nappend,2*nphase+2+nappend);  
    C1=staFunc->CreateMatrix(nphase,2*nphase+2+nappend);
    C2=staFunc->CreateMatrix(nphase,2*nphase+2+nappend);
    if (nphase >= 2 && (staData->ActiveSing[0]||staData->ActiveSing[1]||staData->ActiveSing[2])) 
       BP=staFunc->CreateMatrix(nphase*(nphase-1)/2,nphase*(nphase-1)/2);
    vn=staFunc->CreateVector(nphase);
    xp=staFunc->CreateVector(nphase+2+nappend);
    xp1=staFunc->CreateVector(nphase+2+nappend);
    V=staFunc->CreateVector(nphase);
    W=staFunc->CreateVector(nphase);
    F=staFunc->CreateVector(nphase);
    Vp=staFunc->CreateVector(nphase+2+nappend);
    BifVector=MemNew(sizeof(FloatHi)*(3*nphase+1)); 
  }
  Der2num=ind2_(nphase+npar-1,nphase+npar-1)+1;
  Der3num=ind3_(nphase+npar-1,nphase+npar-1,nphase+npar-1)+1;
  dx=_dx;
  ddx=pow(dx,3.0/4.0);
  dddx=pow(dx,3.0/6.0);

  ((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(void) Term (Boolean OK) {
  if (OK&&P&&(stagenData->Reason==RF_CLEAN)) {
    stagenData->BifDataOutLen=sizeof(VAR)*nphase;
    MemCopy(BifVector,x1+nphase+2+nappend,sizeof(FloatHi)*nphase);
    stagenData->ProcessPoint(stagenData->ProcFuncNum, x1, "Last point");
  } else {
    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);
    C=staFunc->DestroyMatrix(C);
    C1=staFunc->DestroyMatrix(C1);
    C2=staFunc->DestroyMatrix(C2);
    BP=staFunc->DestroyMatrix(BP);
    vn=staFunc->DestroyVector(vn);
    xp=staFunc->DestroyVector(xp);
    xp1=staFunc->DestroyVector(xp1);
    V=staFunc->DestroyVector(V);
    W=staFunc->DestroyVector(W);
    Vp=staFunc->DestroyVector(Vp);
    F=staFunc->DestroyVector(F);
    BifVector=MemFree(BifVector);
  }
}

Local(Int2) JacRHS (Vector x, Matrix J);
Local(Int2) JacDefEquilib (Vector x, Matrix J);
Entry(Int2) lp_JacDefFold (Vector x, Matrix J);

/**************************************/
/* Any point on LP curve ->lp Starter */

Entry(Int2) lp_Starter(StagenDataPtr stagenptr) {
  Int2 i,Ret;
  Vector v1,v2;
  Matrix D,DE;
  if (!stagenptr) {	/* this is request to terminate */
    Term(TRUE);
    return 0;
  }
  if (Init(stagenptr)) return 1;	/* initialize local variables */
  
  SymbNumer1=stagenData->Der1!=NULL && staData->dx!=0;
  SymbNumer2=stagenData->Der2!=NULL && staData->dx!=0;

  /* Create the first point for generator */

  if (!stagenptr->ContDataGenLen) {   /* "Continuation" */

    v1=staFunc->CreateVector(2*nphase+2+nappend);
    v2=staFunc->CreateVector(2*nphase+2+nappend);
    D=staFunc->CreateMatrix(2*nphase+1+nappend,2*nphase+2+nappend);
    DE=staFunc->CreateMatrix(2*nphase+2+nappend,2*nphase+2+nappend);

    MemCopy(x0,staData->X,sizeof(FloatHi)*nphase);
    for (i=0; i<2+nappend; i++) x0[nphase+i]=staData->P[active[i]];

    if (stagenData->BifDataInOk) {
/* Message(MSG_OK,"V exists"); */
    /* V exists */
      MemCopy(x0+nphase+2+nappend,stagenData->BifDataInPtr,sizeof(FloatHi)*nphase);
    } else {
    /* V has to be computed */
/* Message(MSG_OK,"V has to be computed"); */
      Ret=JacRHS(x0,C1);
      staFunc->CopyMatrixBlock(C1,0,0,B,0,0,nphase,nphase);
      if (staFunc->SngSolve(B,_eps,V) != nphase-1) {
        staUtil->ErrMessage("Unable to find a null-eigenvector"); 
        v1=staFunc->DestroyVector(v1);
        v2=staFunc->DestroyVector(v2);
        D=staFunc->DestroyMatrix(D);
        DE=staFunc->DestroyMatrix(DE);
        Term(FALSE);
        return 1;
      } 
      staFunc->NormalizeVector(V);
      staFunc->CopyVectorElements(V,0,x0,nphase+2+nappend,nphase);
    };
    /* Compute a tangent vector to the limit point curve */
    Ret=lp_JacDefFold (x0,D);
    for (i=0; i<2*nphase+2+nappend; i++) {
      v1[i]=1.0;
      staFunc->AppendMatrixVector(D,v1,DE);
      Ret=staFunc->Decomp(DE);
      if (Ret == 0) {
        v2[2*nphase+1+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 limit point 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<2+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) lp_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) lp_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) lp_DefRHS (Vector x, Vector y) {
  CopyToX(x);
  CopyToP(x);
  stagenData->Rhs(X,P,&zero,y);	
  return 0;
}

/************************************/
/* Defining function for equilibria */
Local(Int2) DefEquilib (Vector x, Vector y) {
  lp_DefRHS(x,y);
  if (nappend) lp_DefUser(x,y+nphase);
  return 0;
}

/********************/
/* Jacobian of RHS  */
Local(Int2) JacRHS (Vector x, Matrix Jac) {
  staFunc->CopyVectorElements(x,0,xp,0,nphase+2+nappend);     
  if (SymbNumer1) {
    SymJac(xp,Jac);
  } else {
    if (staFunc->Der(lp_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(lp_DefUser, nappend, x, dx, Jac+nphase);
  return 0;
}

/***************************************/
/* Defining conditions for null-vector */
Entry(Int2) lp_DefKer (Vector x, Vector y) {
  if (JacRHS(x,C2)) return 1;	    /* A,C1,W - global */
  staFunc->CopyMatrixBlock(C2,0,0,A,0,0,nphase,nphase);
  staFunc->CopyVectorElements(x, nphase+2+nappend, W, 0, nphase);
  staFunc->MultiplyMatrixVector(A,W,y);
  return 0;
}

/***************************************************/
/* Jacobian of defining conditions for null-vector */
Local(Int2) JacDefKer (Vector x, Matrix Jac) {
int i,j,k;
double s;
  staFunc->ClearMatrix(Jac);
  if (SymbNumer2) {
    /* creation of DefKer Jacobian matrix Jac using symbolic second derivatives */
     CopyToX(x);
     CopyToP(x);
     H=stagenData->Der2(X,P,&zero);	
     for (i=0; i<nphase; i++) {
       for (k=0; k<nphase+2+nappend; k++) {
         for (s=0.0,j=0; j<nphase; j++) s+=HessianElem(i,j,k)*x[j+nphase+2+nappend];
         Jac[i][k]=s;
       }
     }      
     if (JacRHS(x,C2)) return 1;	    /* C2 - global */
     staFunc->CopyMatrixBlock(C2,0,0,Jac,0,nphase+2+nappend,nphase,nphase);  
  } else {
    if (staFunc->Der(lp_DefKer, nphase, x, dx, Jac)) return 1;
  }
  return(0);
}

/********************************************/
/* Defining functions for limit point curve */

Entry(Int2) lp_DefFold (Vector x, Vector y) {
  if (DefEquilib(x,y)) return 1;
  if (lp_DefKer(x,vn)) return 2;
  staFunc->CopyVectorElements(vn,0,y,nphase+nappend,nphase);
  staFunc->CopyVectorElements(x,nphase+2+nappend,vn,0,nphase);
  y[2*nphase+nappend]=staFunc->ScalProd(vn,vn)-1.0;
  return 0;
}

/********************************************************/
/* Jacobian of defining functions for limit point curve */  
Entry(Int2) lp_JacDefFold (Vector x, Matrix Jac) {
  int i;
  staFunc->ClearMatrix(Jac);
  if (JacDefEquilib(x,C)) return 1;
  staFunc->CopyMatrixBlock(C,0,0,Jac,0,0,nphase+nappend,2*nphase+2+nappend);
  if (JacDefKer(x,C1)) return 1;
  staFunc->CopyMatrixBlock(C1,0,0,Jac,nphase+nappend,0,nphase,2*nphase+2+nappend);
  for (i=0; i<nphase; i++) Jac[2*nphase+nappend][i+nphase+2+nappend]=2.0*x[i+nphase+2+nappend];
  return 0;
}

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

Entry(Int2) lp_ProcDefault(Int2 Ind, Vector x, Vector v1, CharPtr *msg) {
  if (Ind == -1) {  /* zero of user-defined function */
    stagenData->BifDataOutLen=0;     
    return 0;
  }
  /* Assert: v1==NULL, msg==NULL, Ind==0 */
  MemCopy(x1,x,sizeof(FloatHi)*(2*nphase+2+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[120];

/* quadratic coefficient */
Entry(Int2) lp_TestCusp(Vector x, Vector v, FloatHiPtr res) {

  int i,j,k,Ret=0;
  VAR f0,f1,f2;

/*  compose Jacobian matrix */
    staFunc->CopyVectorElements(x,0,xp,0,nphase+2+nappend);
    staFunc->CopyVectorElements(x,nphase+2+nappend,V,0,nphase);
    staFunc->ClearVector(Vp);
    staFunc->CopyVectorElements(x,nphase+2+nappend,Vp,0,nphase);
    JacRHS(x,C1);
    staFunc->CopyMatrixBlock(C1,0,0,A,0,0,nphase,nphase);

/*  compute adjoint null-vector */
    staFunc->TransposeMatrix(A,B);
    if (staFunc->SngSolve(B,_eps,W) != nphase-1) 
      Ret=1;
    else {
      staFunc->NormalizeVectorRelative(W,V);

/*  compute second-order normal form coefficient  */
      if (SymbNumer2) {
        CopyToX(x);
        CopyToP(x);
        H=stagenData->Der2(X,P,&zero);
        for (f0=0.0,i=0; i<nphase; i++) {
           for (j=0; j<nphase; j++) {
              for (k=0; k<nphase; k++) f0+=W[i]*HessianElem(i,j,k)*V[j]*V[k];
           }
        }
        *res=f0;
      } else {
        DefEquilib(xp,F);
        f0=staFunc->ScalProd(W,F);
        staFunc->AddScaledVector(xp,Vp,ddx,xp1);
        DefEquilib(xp1,F);
        f1=staFunc->ScalProd(W,F);
        staFunc->AddScaledVector(xp,Vp,-ddx,xp1);
        DefEquilib(xp1,F);
        f2=staFunc->ScalProd(W,F);
        *res=(f1-2*f0+f2)/ddx/ddx;
      };
    };
  test[0]=*res;
  return Ret;
}
Entry(Int2) lp_ProcCusp(Int2 Ind, Vector x, Vector v, CharPtr *msg) {
  stagenData->BifDataOutLen=0;

  if (Ind) 
    sprintf(buf,"Cusp (?)");
  else {
/*  provide bifurcation data for cp->* starters */
    staFunc->CopyVectorElements(x,0,xp,0,nphase+2+nappend);
    staFunc->CopyVectorElements(x,nphase+2+nappend,V,0,nphase);
    MemCopy(BifVector,V,sizeof(FloatHi)*nphase);
    stagenData->BifDataOutLen=sizeof(FloatHi)*nphase;

    staFunc->ClearVector(Vp);
    staFunc->CopyVectorElements(x,nphase+2+nappend,Vp,0,nphase);

/*  compose Jacobian matrix */
    JacRHS(x,C1);
    staFunc->CopyMatrixBlock(C1,0,0,A,0,0,nphase,nphase);

/*  compute adjoint null-vector */
    staFunc->TransposeMatrix(A,B);
    if (staFunc->SngSolve(B,_eps,W) != nphase-1) {
      sprintf(buf,"Cusp: adjoint null-vector (?)");
      goto final;
    };
    staFunc->NormalizeVectorRelative(W,V);

/* compute the coefficient in the normal form x'=cx^3 */
/* compute vector a=B(V,V)-<W,B(V,V)>V */
{
Vector a,b,F1,F2;
Matrix AA;
VAR c;
Int2 i,j,k,l;
    a=staFunc->CreateVector(nphase);
    b=staFunc->CreateVector(nphase+1);
    F1=staFunc->CreateVector(nphase);
    F2=staFunc->CreateVector(nphase);
    AA=staFunc->CreateMatrix(nphase+1,nphase+1);
    if (SymbNumer2) {
        CopyToX(x);
        CopyToP(x);
        H=stagenData->Der2(X,P,&zero);	/* The 1st elem must be time! */
        for (i=0; i<nphase; i++) {
           for (F[i]=0.0,j=0; j<nphase; j++) {
              for (k=0; k<nphase; k++) F[i]+=HessianElem(i,j,k)*V[j]*V[k];    /* F=B(V,V) */
           }
        }
	staFunc->AddScaledVector(F,V,-staFunc->ScalProd(W,F),a);   
    } else {
        staFunc->AddScaledVector(xp,Vp,ddx,xp1);
        DefEquilib(xp1,a);
        DefEquilib(xp,F);
	staFunc->AddScaledVector(a,F,-2.0,a);
        staFunc->AddScaledVector(xp,Vp,-ddx,xp1);
        DefEquilib(xp1,F);
	staFunc->AddScaledVector(a,F,1.0,a);
	staFunc->MultiplyVectorScalar(a,1.0/ddx/ddx,a);
	staFunc->AddScaledVector(a,V,-staFunc->ScalProd(W,a),a);   
    };
/* solve the bordered system for b */
    staFunc->CopyMatrixBlock(A,0,0,AA,0,0,nphase,nphase);
    staFunc->CopyVectorElements(a,0,b,0,nphase);
    for (i=0; i<nphase; i++) {
      AA[nphase][i]=W[i];
      AA[i][nphase]=V[i];
    }
    staFunc->Decomp(AA);
    staFunc->Solve(AA,b);
    staFunc->CopyVectorElements(b,0,a,0,nphase);

/* compute F=B(V,a) */
    if (SymbNumer2) {
      for (i=0; i<nphase; i++) {
        for (F[i]=0.0,j=0; j<nphase; j++) {
          for (k=0; k<nphase; k++) F[i]+=HessianElem(i,j,k)*V[j]*a[k];    
        }
      }
    } else {
      staFunc->AddScaledVector(V,a,1.0,F1);
      staFunc->CopyVectorElements(F1,0,Vp,0,nphase);
      staFunc->AddScaledVector(xp,Vp,0.5*ddx,xp1);
      DefEquilib(xp1,F1);
      staFunc->AddScaledVector(xp,Vp,-0.5*ddx,xp1);
      DefEquilib(xp1,F);
      staFunc->AddScaledVector(F1,F,1.0,F2);

      staFunc->AddScaledVector(V,a,-1.0,F1);
      staFunc->CopyVectorElements(F1,0,Vp,0,nphase);
      staFunc->AddScaledVector(xp,Vp,0.5*ddx,xp1);
      DefEquilib(xp1,F1);
      staFunc->AddScaledVector(xp,Vp,-0.5*ddx,xp1);
      DefEquilib(xp1,F);
      staFunc->AddScaledVector(F1,F,1.0,F1);
      staFunc->AddScaledVector(F2,F1,-1.0,F);
      staFunc->MultiplyVectorScalar(F,1.0/ddx/ddx,F);   
    }
/* compute c */
    if (stagenData->Der3) {
      CopyToX(x);
      CopyToP(x);
      D3=stagenData->Der3(X,P,&zero);	/* The 1st elem must be time! */
      for (c=0.0,i=0; i<nphase; i++) {
         for (j=0; j<nphase; j++) {
            for (k=0; k<nphase; k++) {
	       for (l=0; l<nphase; l++) {
	         c+=W[i]*D3Elem(i,j,k,l)*V[j]*V[k]*V[l];
	       }
	    }
         }
      }
    } else {
	staFunc->CopyVectorElements(V,0,Vp,0,nphase);
        staFunc->AddScaledVector(xp,Vp,3*dddx/2,xp1);
        DefEquilib(xp1,F1);
	c=staFunc->ScalProd(W,F1);
        staFunc->AddScaledVector(xp,Vp,dddx/2,xp1);
        DefEquilib(xp1,F1);
	c-=3.0*staFunc->ScalProd(W,F1);
        staFunc->AddScaledVector(xp,Vp,-dddx/2,xp1);
        DefEquilib(xp1,F1);
	c+=3.0*staFunc->ScalProd(W,F1);
        staFunc->AddScaledVector(xp,Vp,-3*dddx/2,xp1);
        DefEquilib(xp1,F1);
	c-=staFunc->ScalProd(W,F1);
        c/=(dddx*dddx*dddx);   
     }
      c-=3.0*staFunc->ScalProd(W,F);
 
    sprintf(buf,"Cusp: c=%g",c/6);

    a=staFunc->DestroyVector(a);
    b=staFunc->DestroyVector(b);
    F1=staFunc->DestroyVector(F1);
    F2=staFunc->DestroyVector(F2);
    AA=staFunc->DestroyMatrix(AA);
}
  }
final:
  *msg=buf;
  return 0;
}

/* product <W,V> */
Entry(Int2) lp_TestBogdanovTakens(Vector x, Vector v, FloatHiPtr res) {

  int Ret=0;
  *res=1;
  if (nphase < 2) goto end;

/*  compose Jacobian matrix */
  staFunc->CopyVectorElements(x,0,xp,0,nphase+2+nappend);
  staFunc->CopyVectorElements(x,nphase+2+nappend,V,0,nphase);
  if (JacRHS(x,C1)) {
    Ret=1;
    goto end;
  }
  staFunc->CopyMatrixBlock(C1,0,0,A,0,0,nphase,nphase);

/*  compute adjoint null-vector */
  staFunc->TransposeMatrix(A,B);
  if (staFunc->SngSolve(B,_eps,W) != nphase-1) {
    Ret=1;
    *res=0;
  } else {
    staFunc->NormalizeVector(W);
    *res=staFunc->ScalProd(V,W);
  }
end:
  test[1]=*res;
  return Ret;
}

Entry(Int2) lp_ProcBogdanovTakens(Int2 Ind, Vector x, Vector v, CharPtr *msg) {

  stagenData->BifDataOutLen=0;
  if (Ind) 
    sprintf(buf,"Bogdatov-Takens (?)");
  else {
Vector v1,W0,W1;
Matrix D;
Int2 i,j,k;
double a20,b20,b11,f0,f1,f2,f3,f4,f5,d,u;
    v1=staFunc->CreateVector(2*nphase);
    W0=staFunc->CreateVector(nphase);
    W1=staFunc->CreateVector(nphase);
    D=staFunc->CreateMatrix(2*nphase,2*nphase);

/*  compute (generalized) null-eigenvectors  */
    if (JacRHS (x,C1)) return 1;
    staFunc->CopyMatrixBlock(C1,0,0,A,0,0,nphase,nphase);
    
    staFunc->CopyMatrixBlock(A,0,0,D,0,0,nphase,nphase);
    staFunc->CopyMatrixBlock(A,0,0,D,nphase,nphase,nphase,nphase);
    for (i=0; i<nphase; i++) D[i+nphase][i]=-1;
    i=staFunc->SngSolve(D,_eps,v1);
    if (i != 2*nphase-2) {
      sprintf(buf,"Bogdanov-Takens: eigenvectors(?)");
      goto final;
    }
    staFunc->CopyVectorElements(v1,0,V,0,nphase);       
    d=sqrt(staFunc->ScalProd(V,V));
    staFunc->NormalizeVector(V);
    staFunc->CopyVectorElements(x,nphase+2+nappend,W,0,nphase);
    u=staFunc->ScalProd(V,W);
    staFunc->MultiplyVectorScalar(V,u,V);	/* null-vector as continued */
    staFunc->CopyVectorElements(v1,nphase,W,0,nphase); 
    staFunc->MultiplyVectorScalar(W,u/d,W);	/* generalized null-vector */
    staFunc->AddScaledVector(W,V,-staFunc->ScalProd(V,W),W); 
/*
staFunc->PrintVector(V,"Proc BT on fold: V=");
staFunc->PrintVector(W,"Proc BT on fold: W=");
*/
/*  provide bifurcation data for bt->* starters */
/*  BifVector[0] to [nphase-1]:          null-vector V (A*V=0, <V,V>=1)*/
/*  BifVector[nphase] to [2*nphase-1]:   generalized null-vector W (A*W=V, <W,V>=0) */
/*  BifVector[2*nphase]:                 -lambda*lambda  */
    MemCopy(BifVector,V,sizeof(FloatHi)*nphase);
    MemCopy(BifVector+nphase,W,sizeof(FloatHi)*nphase);

    BifVector[2*nphase]=0.0;
    stagenData->BifDataOutLen=sizeof(FloatHi)*(2*nphase+1);

/* compute the adjoint eigenvectors */
    staFunc->CopyMatrixBlock(C1,0,0,A,0,0,nphase,nphase);
    staFunc->TransposeMatrix(A,B);
    staFunc->ClearMatrix(D);
    staFunc->CopyMatrixBlock(B,0,0,D,0,0,nphase,nphase);
    staFunc->CopyMatrixBlock(B,0,0,D,nphase,nphase,nphase,nphase);
    for (i=0; i<nphase; i++) D[i+nphase][i]=-1;
    i=staFunc->SngSolve(D,_eps,v1);
    if (i != 2*nphase-2) {
      sprintf(buf,"Bogdatov-Takens: adjoint eigenvectors (?)");
      goto final;
    }
    staFunc->CopyVectorElements(v1,0,W1,0,nphase);       /* adjoint null-vector W1 */
    staFunc->CopyVectorElements(v1,nphase,W0,0,nphase);  /* adjoint generalized null-vector W0 */
    d=staFunc->ScalProd(V,W0);
    staFunc->MultiplyVectorScalar(W1,1.0/d,W1);
    staFunc->AddScaledVector(W0,W1,-staFunc->ScalProd(W0,W),W0);
    staFunc->MultiplyVectorScalar(W0,1.0/d,W0);

/* compute the coefficients of the normal form : x'=y; y'=ax^2+bxy */
/*  a20=<W0,B(V,V)>, b20=<W1,B(V,V)>,  b11=<W1,B(V,W)> */
     if (stagenData->Der2)  {
        CopyToX(x);
        CopyToP(x);
        H=stagenData->Der2(X,P,&zero);	/* The 1st elem must be time! */
        for (a20=0,b20=0,b11=0,i=0; i<nphase; i++) {
           for (j=0; j<nphase; j++) {
              for (k=0; k<nphase; k++) { 
	         d=HessianElem(i,j,k);
                 a20+=W0[i]*d*V[j]*V[k];
                 b20+=W1[i]*d*V[j]*V[k];
                 b11+=W1[i]*d*V[j]*W[k];
	      }
           }
        }
     } else {
	staFunc->ClearVector(v0);
/*  a20=<W0,B(V,V)>, b20=<W1,B(V,V)> */
	staFunc->CopyVectorElements(V,0,v0,0,nphase);
        lp_DefRHS(x,F);
        f0=staFunc->ScalProd(W0,F);
        f3=staFunc->ScalProd(W1,F);
        staFunc->AddScaledVector(x,v0,ddx,x1);
        lp_DefRHS(x1,F);
        f1=staFunc->ScalProd(W0,F);
        f4=staFunc->ScalProd(W1,F);
        staFunc->AddScaledVector(x,v0,-ddx,x1);
        lp_DefRHS(x1,F);
        f2=staFunc->ScalProd(W0,F);
        f5=staFunc->ScalProd(W1,F);
	a20=(f1-2*f0+f2)/ddx/ddx;
	b20=(f4-2*f3+f5)/ddx/ddx;

/*  b11=<W1,B(V,W)>  */
        staFunc->AddScaledVector(V,W,1.0,F);
        staFunc->CopyVectorElements(F,0,v0,0,nphase);
        staFunc->AddScaledVector(x,v0,0.5*ddx,x1);
        lp_DefRHS(x1,F);
	b11=staFunc->ScalProd(W1,F);
        staFunc->AddScaledVector(x,v0,-0.5*ddx,x1);
        lp_DefRHS(x1,F);
	b11+=staFunc->ScalProd(W1,F);
        staFunc->AddScaledVector(V,W,-1.0,F);
        staFunc->CopyVectorElements(F,0,v0,0,nphase);
        staFunc->AddScaledVector(x,v0,0.5*ddx,x1);
        lp_DefRHS(x1,F);
	b11-=staFunc->ScalProd(W1,F);
        staFunc->AddScaledVector(x,v0,-0.5*ddx,x1);
        lp_DefRHS(x1,F);
	b11-=staFunc->ScalProd(W1,F);
	b11/=(ddx*ddx); 
     }
    if (b20>=0)
      sprintf(buf,"Bogdanov-Takens: a=%g, b=%g",b20/2,a20+b11);
    else
      sprintf(buf,"Bogdanov-Takens: a=%g, b=%g",-b20/2,-(a20+b11));
final:
    D=staFunc->DestroyMatrix(D);
    v1=staFunc->DestroyVector(v1);
    W0=staFunc->DestroyVector(W0);
    W1=staFunc->DestroyVector(W1);
  }
  *msg=buf;
  return 0;
}

/* zero-sum test function */
Entry(Int2) lp_TestHopf(Vector x, Vector v, FloatHiPtr res) {
  int i,j,p,q,r,s;

  *res=1.0;
  if (nphase < 2) goto end;

  if (JacRHS(x,C1)) return 1;
  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];  
        }
      }
    }
  }
  if (staFunc->Decomp(BP)) {
    *res=0.0;
  } else {
    staFunc->GetMatrixData(BP,res,NULL);
  }
end:
  test[2]=*res;
  return 0;
}

Entry(Int2) lp_ProcZeroHopf(Int2  Ind, Vector x, Vector v, CharPtr *msg) {
  stagenData->BifDataOutLen=0;

  if (Ind) 
    sprintf(buf,"Zero-Hopf (?)");
  else {
/*  provide bifurcation data for zh->* starters */
/*  BifVector[0] to [nphase-1]:        null-vector  */
/*  BifVector[nphase] to [3*nphase-1]: Hopf vectors */
/*  BifVector[3*nphase]              : Hopf number -lambda*lambda */
    MemCopy(BifVector,x+nphase+2,sizeof(FloatHi)*nphase);
{
int i,j,imin,jmin;
double d,s,r,beta,gamma,phi,lambda,omega;
Matrix AA,BB;
Vector V,Qr,Qi;
   
  AA=staFunc->CreateMatrix(nphase,nphase);
  BB=staFunc->CreateMatrix(2*nphase,2*nphase);
  V=staFunc->CreateVector(2*nphase);
  Qr=staFunc->CreateVector(nphase);
  Qi=staFunc->CreateVector(nphase);

    JacDefEquilib(x,C);
    staFunc->CopyMatrixBlock(C,0,0,A,0,0,nphase,nphase);
    staFunc->CopyMatrixBlock(C,0,0,AA,0,0,nphase,nphase);
    staFunc->EigenValues(AA,Re,Im);
    /* find the pair of eigenvalues with zero sum */
    s=fabs(Re[0]+Re[1])+fabs(Im[0]+Im[1]);
    imin=0;
    jmin=1;
    for (i=0; i<nphase; i++) {
      for (j=i+1; j<nphase; j++) {
        d=fabs(Re[i]+Re[j])+fabs(Im[i]+Im[j]);
        if (d<s) {
          imin=i;
          jmin=j;
          s=d;
        }
      }
    }
    if (fabs(Im[imin])+fabs(Im[jmin])==0) {
/* Zero + Neutral saddle */
      lambda=fabs(Re[imin]);
      /* compute real eigenvectors associated with lambda and -lambda */
      staFunc->CopyMatrixBlock(A,0,0,AA,0,0,nphase,nphase);
      for (i=0; i<nphase; i++) AA[i][i]-=lambda;
      if (staFunc->SngSolve(AA,_eps,Qr) != nphase-1) {
        sprintf(buf,"Zero eigenvalue + Neutral saddle (?)");
        goto final;
      } 
      staFunc->CopyMatrixBlock(A,0,0,AA,0,0,nphase,nphase);
      for (i=0; i<nphase; i++) AA[i][i]+=lambda;
      if (staFunc->SngSolve(AA,_eps,Qi) != nphase-1) {
        sprintf(buf,"Zero eigenvalue + Neutral saddle (?)");
        goto final;
      } 
      staFunc->AddScaledVector(Qr,Qi,1.0,Qr);
              
      staFunc->NormalizeVector(Qr);   
      staFunc->NormalizeVector(Qi);
      r=staFunc->ScalProd(Qr,Qi);
      if (r!=0.0) staFunc->AddScaledVector(Qr,Qi,-1.0/r,Qi);

      /* provide bifurcation data for zh->h starter */
/*  BifVector[0] to [nphase-1]:          null-vector*/
/*  BifVector[nphase] to [2*nphase-1]:   Hopf continuation vector*/
/*  BifVector[2*nphase] to [3*nphase-1]: global vector L */
/*  BifVector[3*nphase]:                 -lambda*lambda  */
      MemCopy(BifVector+nphase,Qr,sizeof(FloatHi)*nphase);
      MemCopy(BifVector+2*nphase,Qi,sizeof(FloatHi)*nphase);
      BifVector[3*nphase]=-lambda*lambda;
      stagenData->BifDataOutLen=sizeof(FloatHi)*(3*nphase+1);
      sprintf(buf,"Zero eigenvalue + Neutral saddle: lambda=%g",lambda);
    } else {
/* Zero-Hopf */
      lambda=Re[imin];
      omega=fabs(Im[imin]);
      /* compute real and imaginary part of the eigenvector */
      staFunc->CopyMatrixBlock(A,0,0,BB,0,0,nphase,nphase);
      staFunc->CopyMatrixBlock(A,0,0,BB,nphase,nphase,nphase,nphase);
      for (i=0; i<nphase; i++) {
        BB[i][i+nphase]=omega;
        BB[i][i]-=lambda;
        BB[i+nphase][i]=-omega;
        BB[i+nphase][i+nphase]-=lambda;
      }
      if (staFunc->SngSolve(BB,_eps,V) != 2*nphase-2) {
        sprintf(buf,"Zero-Hopf (?)");
        goto final;
      }
      staFunc->CopyVectorElements(V,0,Qr,0,nphase);
      staFunc->CopyVectorElements(V,nphase,Qi,0,nphase);

      /* normalize real and imaginary parts */
        d=staFunc->ScalProd(Qr,Qr);
        s=staFunc->ScalProd(Qi,Qi);
        r=staFunc->ScalProd(Qr,Qi);
        if (r!=0) {
          beta=sqrt(fabs(4.0*r*r+(s-d)*(s-d)));
          if (2.0*fabs(2.0*r) > beta) {
            phi=atan(fabs((s-d)/(2.0*r)));
          } else {
            phi=PID2-atan(fabs(2.0*r/(s-d)));
          }
          if (r < 0) phi=PI-phi;
          if ((s-d) < 0) phi=-phi;
          phi/=2.0;
          for (i=0; i<nphase; i++) {
            V[i]=(Qr[i]*cos(phi)-Qi[i]*sin(phi));
            V[i+nphase]=(Qr[i]*sin(phi)+Qi[i]*cos(phi));
          }
        }
        staFunc->CopyVectorElements(V,0,Qr,0,nphase);
        d=staFunc->ScalProd(Qr,Qr);
        gamma=1/sqrt(d);
        staFunc->MultiplyVectorScalar(V,gamma,V);

      /* provide bifurcation data for zh->h starter */
/*  BifVector[0] to [nphase-1]:          null-vector*/
/*  BifVector[nphase] to [2*nphase-1]:   Hopf continuation vector*/
/*  BifVector[2*nphase] to [3*nphase-1]: global vector L */
/*  BifVector[3*nphase]:                 -lambda*lambda  */
      MemCopy(BifVector+nphase,V,sizeof(FloatHi)*2*nphase);
      BifVector[3*nphase]=omega*omega;
      stagenData->BifDataOutLen=sizeof(FloatHi)*(3*nphase+1);
      sprintf(buf,"Zero-Hopf: omega=%g",omega);
    }
final:
  AA=staFunc->DestroyMatrix(AA);
  BB=staFunc->DestroyMatrix(BB);
  V=staFunc->DestroyVector(V);
  Qr=staFunc->DestroyVector(Qr);
  Qi=staFunc->DestroyVector(Qi);

}
  }
  *msg=buf;
  return 0;
}

/**********************/
/* Trivial Adaptation */
Entry(Int2) lp_Adapter(Vector X, Vector V) {
  return (0);
}