/*
   __cp
   Curve of Cusp Points  
   September 30, 1997
*/

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

#include "stagen.h"

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

#include "__cp.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(__cp_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,D,DE,VDER;
Local(Vector) v1,vn,xp,xp1,V,W,F,Vp,L,VV,VB1,VB2,V1,W0,W1,Q,PP1,PP2;
Local(Vector) TV,K1,K2,K3,aa,bb,cc,dd,xx,yy;
Local(Vector) Re=NULL,Im=NULL;
Local(VAR) test[2],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,m;
  /* Set local pointers to data area and functions array */
  stagenData=stagenptr;
  staData=(__cp_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;
  }
  if (!stagenData->Der3 && staData->dx <= 0) {
    staUtil->ErrMessage("Analytical third order derivative matrix undefined.\nRecompile system or set positive 'Increment'");
    active=MemFree(active);
    return 4;
  }
  /* 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(nphase+1,nphase+1);
    BT=staFunc->CreateMatrix(nphase+1,nphase+1);
    V1=staFunc->CreateVector(nphase);
    W0=staFunc->CreateVector(nphase);
    W1=staFunc->CreateVector(nphase);
    VB1=staFunc->CreateVector(nphase+1);
    VB2=staFunc->CreateVector(nphase+1);
    VV=staFunc->CreateVector(2*nphase);
    Q=staFunc->CreateVector(nphase);
    PP1=staFunc->CreateVector(nphase+1);
    PP2=staFunc->CreateVector(nphase+1);
    aa=staFunc->CreateVector(nphase);
    bb=staFunc->CreateVector(nphase);
    cc=staFunc->CreateVector(nphase);
    dd=staFunc->CreateVector(nphase);
    xx=staFunc->CreateVector(nphase+3+nappend);
    yy=staFunc->CreateVector(nphase+3+nappend);
    TV=staFunc->CreateVector(nphase+3);
        
    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);
    v1=staFunc->CreateVector(2*nphase);
    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);
    K1=staFunc->CreateVector(nphase);
    K2=staFunc->CreateVector(nphase);          
    K3=staFunc->CreateVector(nphase);
    VDER=staFunc->CreateMatrix(nphase,nphase+3);
  }
  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(Int2) JacRHS (Vector x, Matrix J);

Local(void) Term (Boolean OK) {
Int2 m;
  m=nphase*(nphase-1)/2;
  stagenData->BifDataOutLen=0;
/* saving global vectors V1,W1 used in defining functions */
    if (OK&&P&&(stagenData->Reason==RF_CLEAN)) {
      stagenData->ContDataStaLen=2*nphase*sizeof(FloatHi);
      stagenData->ContDataStaPtr=MemNew(stagenData->ContDataStaLen);
      MemCopy(stagenData->ContDataStaPtr,V1,(nphase)*sizeof(FloatHi));
      MemCopy(stagenData->ContDataStaPtr+(nphase),W1,(nphase)*sizeof(FloatHi));
      stagenData->BifDataOutLen=sizeof(FloatHi)*nphase;
      MemCopy(BifVector,V1,nphase*sizeof(FloatHi));
      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);
      V1=staFunc->DestroyVector(V1);
      W0=staFunc->DestroyVector(W0);
      W1=staFunc->DestroyVector(W1);
      Q=staFunc->DestroyVector(Q);
      PP1=staFunc->DestroyVector(PP1);
      PP2=staFunc->DestroyVector(PP2);
      aa=staFunc->DestroyVector(aa);
      bb=staFunc->DestroyVector(bb);
      cc=staFunc->DestroyVector(cc);
      dd=staFunc->DestroyVector(dd);
      xx=staFunc->DestroyVector(xx);
      yy=staFunc->DestroyVector(yy);
      TV=staFunc->DestroyVector(TV);
           
      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);
      v1=staFunc->DestroyVector(v1);
      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);
      K1=staFunc->DestroyVector(K1);
      K2=staFunc->DestroyVector(K2);     
      K3=staFunc->DestroyVector(K3);
      VDER=staFunc->DestroyMatrix(VDER);
    }
} 

Entry(Int2) cp_JacDefCusp (Vector x, Matrix J);
Entry(Int2) cp_DefCusp (Vector x, Vector J);


/************/
/* Starters */
/* cp -> cp Starter */
Entry(Int2) cp_Starter(StagenDataPtr stagenptr) {
  Int2 i,j,k,kp1,Ret,m,p,q;
  Vector v1,v2;
  Matrix D,DE;
  VAR  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) {       /* "Extention" */
    MemCopy(V1,stagenptr->ContDataStaPtr,sizeof(FloatHi)*(nphase));
    MemCopy(W1,stagenptr->ContDataStaPtr+(nphase),sizeof(FloatHi)*(nphase));
    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]];    
  } 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]];
    JacRHS(x0,C1);
    staFunc->CopyMatrixBlock(C1,0,0,A,0,0,nphase,nphase);
 
    staFunc->ClearMatrix(BP);
    staFunc->CopyMatrixBlock(A,0,0,BP,0,0,nphase,nphase);
    for (i=0; i<nphase; i++) {
      BP[i][nphase]=i;
      BP[nphase][i]=i;
    }
 /* Calculation of PIVOTS - complete pivoting */
    for (k=0;k<=nphase-2;k++) {
      t=0.0;
      for (i=k;i<nphase;i++) {
	for (j=k;j<nphase;j++) {
    	  if (fabs(BP[i][j]) > t) {
            p = i;
            q = j;
            t = fabs(BP[i][j]);
          }           
	}
      }
      if (k!=q) { /* switch colom k with colom  q */
        for (i=0;i<=nphase;i++) {
          t=BP[i][k];
          BP[i][k]=BP[i][q];
          BP[i][q]=t;
	}
      }
      if (k!=p) { /* switch row k with row p */
        for (i=0;i<=nphase;i++) {
          t=BP[k][i];
          BP[k][i]=BP[p][i];
          BP[p][i]=t;
	}
      }    
      if (fabs(BP[k][k]) == 0.0) {
        staUtil->ErrMessage("Matrix has rank defect at least two");
        Term(FALSE);
        break;
      }
      kp1=k+1;
      for (i=kp1;i<nphase;i++) { /* perform elimination */
        t=-BP[i][k]/BP[k][k];
        for (j=kp1;j<nphase;j++) {
          BP[i][j]+=t*BP[k][j];
	}
      }
    } 
    p=(Int2)BP[nphase][nphase-1];
    q=(Int2)BP[nphase-1][nphase];
/* use bifurcation data if available; otherwise use pivot information */
/* BifVector[0] to [nphase-1]:       null-vector V (A*V=0, <V,V>=1) */
    if (stagenData->BifDataInOk) {
      MemCopy(V1,stagenData->BifDataInPtr,sizeof(FloatHi)*nphase);
      staFunc->NormalizeVector(V1); 
    } else {
        staFunc->ClearVector(V1); 
        V1[p]=1.0;
      }         
    staFunc->ClearVector(W1); 
    W1[q]=1.0;

/* perform one adaptation step for V1 and W1 */
    staFunc->ClearMatrix(BP);
    staFunc->CopyMatrixBlock(C1,0,0,BP,0,0,nphase,nphase);
    for (i=0; i<nphase; i++) {
      BP[nphase][i]=V1[i];
      BP[i][nphase]=W1[i];
    }
    staFunc->CopyMatrix(BP,BT);
    Ret=staFunc->Decomp(BT);
    if (Ret != 0) { 
       goto final;
    }    
    staFunc->ClearVector(VB1); 
    VB1[nphase]=1.0; 
    staFunc->Solve(BT,VB1);
    staFunc->CopyVectorElements(VB1,0,V1,0,nphase);   
    staFunc->NormalizeVector(V1);    
    staFunc->TransposeMatrix(BP,BT);
    Ret=staFunc->Decomp(BT);
    if (Ret != 0) { 
      goto final;
    }
    staFunc->ClearVector(PP1);
    PP1[nphase]=1.0;
    staFunc->Solve(BT,PP1);
    staFunc->CopyVectorElements(PP1,0,W1,0,nphase);   
    staFunc->NormalizeVector(W1);
final:
      if (Ret) {
        v1=staFunc->DestroyVector(v1);
        v2=staFunc->DestroyVector(v2);
        D=staFunc->DestroyMatrix(D);
        DE=staFunc->DestroyMatrix(DE);
        Term(FALSE);
        return 1;
      }
  
  /* Compute a tangent vector to the BT curve */
    cp_DefCusp(x0,v0);
    Ret=cp_JacDefCusp(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 cusp 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) cp_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) cp_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) cp_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) {
  cp_DefRHS(x,y);
  if (nappend) cp_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(cp_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(cp_DefUser, nappend, x, dx, Jac+nphase);
  return 0;
}
/***********************************/
/* cusp defining conditions */
Entry(Int2) cp_DefBord (Vector x, Vector y) {
Int2 i,j,k,Ret;
VAR S;

  if (JacRHS(x,C1)) return 1;	    /* A,C1,BP,V1,W1,Q - global */

/* Fill in the bordered matrix BP */
  staFunc->ClearMatrix(BP);
  staFunc->CopyMatrixBlock(C1,0,0,BP,0,0,nphase,nphase);

/* global V1 and W1 are assumed to be correct for bordering */  
  
  for (i=0; i<nphase; i++) {
    BP[nphase][i]=V1[i];
  }
  for (i=0; i<nphase; i++) {
     BP[i][nphase]=W1[i];
    }

/* compute cusp defining functions */
  staFunc->CopyMatrix(BP,BT);
  Ret=staFunc->Decomp(BT);
  if (Ret != 0) {
    return 1;
  }
  staFunc->ClearVector(VB1);
  VB1[nphase]=1.0;
  staFunc->Solve(BT,VB1);
  staFunc->CopyVectorElements(VB1,0,Q,0,nphase);   /* make global null-vector  AQ=0 */
  y[0]=VB1[nphase];
  staFunc->ClearVector(VB2);
  if (stagenData->Der2) {
    CopyToX(x);
    CopyToP(x);
    H=stagenData->Der2(X,P,&zero);
    for (i=0; i<nphase; i++) {
      for (j=0, S=0.0; j<nphase; j++) {
        for (k=0; k<nphase; k++) {
          S+=Q[j]*Q[k]*HessianElem(i,j,k);
        }
      }
      VB2[i]=-S;
    }
    staFunc->Solve(BT,VB2);
    y[1]=VB2[nphase];
  } else {
      staFunc->ClearVector(xx);
      staFunc->CopyVectorElements(Q,0,xx,0,nphase);
      cp_DefRHS(x,aa);
      staFunc->AddScaledVector(x,xx,ddx,xp1);
      cp_DefRHS(xp1,bb);
      staFunc->AddScaledVector(x,xx,-ddx,xp1);
      cp_DefRHS(xp1,cc);
      staFunc->AddScaledVector(bb,aa,-2.0,aa);
      staFunc->AddScaledVector(aa,cc,1.0,aa);
      staFunc->MultiplyVectorScalar(aa,-1.0/ddx/ddx,aa);
      staFunc->CopyVectorElements(aa,0,VB2,0,nphase);
      staFunc->Solve(BT,VB2);
      y[1]=VB2[nphase];
    } 
  return 0;
}

/***************************************/
/* Jacobian of cusp defining condition */
Local(Int2) JacDefBord (Vector x, Matrix Jac) {
int i,j,k,l;
VAR S,T,U;

  staFunc->ClearMatrix(Jac);
  if (stagenData->Der2 && stagenData->Der3) {
/* creation of DefBord Jacobian matrix Jac using symbolic second derivatives */
    CopyToX(x);
    CopyToP(x);
    H=stagenData->Der2(X,P,&zero);
    D3=stagenData->Der3(X,P,&zero);

    for (l=0; l<nphase+3; l++) {
      staFunc->ClearVector(VB1);
      for (i=0; i<nphase; i++) {
   	for (k=0, S=0.0; k<nphase; k++) {
	  S+=Q[k]*HessianElem(i,k,l);
        }
        VB1[i]=-S;
      }
      staFunc->Solve(BT,VB1);
      for (i=0; i<nphase; i++) {
       VDER[i][l]=VB1[i];
      }
      Jac[0][l]=VB1[nphase];
    }
    
/* compute PP1 using the transposed bordered bi-product */
    staFunc->TransposeMatrix(BP,BT);
    staFunc->Decomp(BT);

    staFunc->ClearVector(PP1);
    PP1[nphase]=1.0;
    staFunc->Solve(BT,PP1);
    
    for (j=0; j<nphase+3; j++) {  
      staFunc->ClearVector(K1);
      for (i=0; i<nphase; i++) {
	for (k=0, S=0.0; k<nphase; k++) {
	  S+=VB2[k]*HessianElem(i,k,j);
	}
    	K1[i]=S;
      }      
      staFunc->ClearVector(K2);
      for (i=0; i<nphase; i++) {
	for (k=0, S=0.0; k<nphase; k++) {
	  for (l=0; l<nphase; l++) {
	    S+=Q[k]*Q[l]*D3Elem(i,k,l,j);
	  }
	}
        K2[i]=S;
      }	
      staFunc->ClearVector(K3);
      for (i=0; i<nphase; i++) {
        for (k=0, S=0.0; k<nphase; k++) {
	  for (l=0; l<nphase; l++) {
	    S+=Q[k]*VDER[l][j]*HessianElem(i,k,l);
	  }
	}
        K3[i]=S;
      }
      for (i=0, S=0.0, T=0.0, U=0.0; i<nphase; i++) {
        S+=PP1[i]*K1[i];
        T+=PP1[i]*K2[i];
        U+=2.0*PP1[i]*K3[i];
      }
      Jac[1][j]=-S-T-U;
    } /* for j loop */
  } else {
     if (staFunc->Der(cp_DefBord, 2, x, dx, Jac)) return 1;
  }
  return(0);
}

/*************************************/
/* Defining functions for cusp curve */

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

/*************************************************/
/* Jacobian of defining functions for cusp curve */  
Entry(Int2) cp_JacDefCusp (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 (JacDefBord(x,C2)) return 2;
  staFunc->CopyMatrixBlock(C2,0,0,Jac,nphase+nappend,0,2,nphase+3+nappend);
  return 0;
}

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

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

  cp_DefBord (x, xp);					/* update of global Q and VB1 */
  /* 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->CopyMatrixBlock(C1,0,0,A,0,0,nphase,nphase);
  staFunc->EigenValues(A,Re,Im);
  return 0;
}

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

Entry(Int2) cp_TestST(Vector x, Vector v, FloatHiPtr res) {
int i,j,k,l;
VAR S;

  if (stagenData->Der2 && stagenData->Der3) {
    CopyToX(x);
    CopyToP(x);
    H=stagenData->Der2(X,P,&zero);
    D3=stagenData->Der3(X,P,&zero);
    
    staFunc->ClearVector(K1);
    for (i=0; i<nphase; i++) {
      for (j=0,S=0.0; j<nphase; j++) {
	for (k=0; k<nphase; k++) {
	  for(l=0; l<nphase; l++) {
	    S+=Q[j]*Q[k]*Q[l]*D3Elem(i,j,k,l);
	  }
	}
      }
      K1[i]=S;	
    }
    staFunc->ClearVector(K2);
    for (i=0; i<nphase; i++) {
      for (k=0,S=0.0; k<nphase; k++) {
	for (l=0; l<nphase; l++) {
	  S+=Q[k]*VB2[l]*HessianElem(i,k,l);
	}
      }
      K2[i]=S;	
    }
    staFunc->ClearVector(PP2);
    for (i=0, S=0.0; i<nphase; i++) {
      PP2[i]=-K1[i]-3.0*K2[i];
      S+=PP1[i]*PP2[i];
    }    
    *res=S; 
  } else {
   /* compute PP1 using the transposed bordered bi-product */
      staFunc->TransposeMatrix(BP,BT);
      staFunc->Decomp(BT);
      staFunc->ClearVector(PP1);
      PP1[nphase]=1.0;
      staFunc->Solve(BT,PP1);

      staFunc->ClearVector(xx);
      staFunc->CopyVectorElements(Q,0,xx,0,nphase);
      staFunc->AddScaledVector(x,xx,3.0*dddx,xp1);
      cp_DefRHS(xp1,aa);
      staFunc->AddScaledVector(x,xx,dddx,xp1);
      cp_DefRHS(xp1,bb);
      staFunc->AddScaledVector(x,xx,-dddx,xp1);
      cp_DefRHS(xp1,cc);
      staFunc->AddScaledVector(x,xx,-3.0*dddx,xp1);
      cp_DefRHS(xp1,dd); 
      staFunc->AddScaledVector(aa,bb,-3.0,aa);
      staFunc->AddScaledVector(aa,cc,3.0,aa);
      staFunc->AddScaledVector(aa,dd,-1.0,aa);
      staFunc->MultiplyVectorScalar(aa,-1.0/(8.0*dddx*dddx*dddx),aa);
      staFunc->CopyVectorElements(aa,0,PP2,0,nphase);
      staFunc->ClearVector(yy);
      staFunc->CopyVectorElements(VB2,0,yy,0,nphase); 
      staFunc->AddScaledVector(x,xx,ddx,xp1);
      staFunc->AddScaledVector(xp1,yy,ddx,xp1);
      cp_DefRHS(xp1,aa);
      staFunc->AddScaledVector(x,xx,ddx,xp1);
      staFunc->AddScaledVector(xp1,yy,-ddx,xp1);
      cp_DefRHS(xp1,bb);
      staFunc->AddScaledVector(x,xx,-ddx,xp1);
      staFunc->AddScaledVector(xp1,yy,ddx,xp1);
      cp_DefRHS(xp1,cc);
      staFunc->AddScaledVector(x,xx,-ddx,xp1);
      staFunc->AddScaledVector(xp1,yy,-ddx,xp1);
      cp_DefRHS(xp1,dd);
      staFunc->AddScaledVector(aa,bb,-1.0,aa);
      staFunc->AddScaledVector(aa,cc,-1.0,aa);
      staFunc->AddScaledVector(aa,dd,1.0,aa);
      staFunc->MultiplyVectorScalar(aa,-3.0/(4.0*ddx*ddx),aa);
      for (i=0, S=0.0; i<nphase; i++) {
        PP2[i]+=aa[i];
        S+=PP1[i]*PP2[i];
      }
      *res=S;
     }
  test[0]=*res;
  return 0;
}

Entry(Int2) cp_ProcST(Int2 Ind, Vector x, Vector v, CharPtr *msg) {
  if (Ind==0) {
    sprintf(buf,"Swallow Tail");
  } else {
    sprintf(buf,"Swallow Tail (?)");
  }
  *msg=buf;
  return 0;
}

Entry(Int2) cp_TestZA(Vector x, Vector v, FloatHiPtr res) {
int Ret;

  staFunc->CopyMatrix(BP,BT);
  Ret=staFunc->Decomp(BT);
  staFunc->ClearVector(PP2);
  staFunc->CopyVectorElements(Q,0,PP2,0,nphase); 
  staFunc->Solve(BT,PP2); 
  *res=PP2[nphase];
  test[1]=*res;
  return 0;
}

Entry(Int2) cp_ProcZA(Int2 Ind, Vector x, Vector v, CharPtr *msg) {
  if (Ind==0) {
    sprintf(buf,"Degenerate BT: triple equilibrium");
  } else {
    sprintf(buf,"Degenerate BT: triple equilibrium (?)");
  }
  *msg=buf;
  return 0;
}

/**************************************************/
/*  Adaptation of the bordering vectors V1 and W1 */
Entry(Int2) cp_Adapter(Vector x, Vector v) {

/* global Q is assumed to be correct */
  staFunc->CopyVector(Q,V1);
  staFunc->NormalizeVector(V1);

/* global BP is assumed to be correct */
  staFunc->TransposeMatrix(BP,BT);
  staFunc->Decomp(BT);

  staFunc->ClearVector(VB2);
  VB2[nphase]=1.0;
  staFunc->Solve(BT,VB2);
  staFunc->CopyVectorElements(VB2,0,W1,0,nphase);
  staFunc->NormalizeVector(W1);
  return 0;
}