/*
   __hom
   --> Homoclinic orbit
*/

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

#include "stagen.h"

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

#include "__hom.h"
#include "__o.h"	/* for integrator call */
#include "_conti.h"	/* continuator's (i.e. generator's) data */
#include "_integr1.h"	/* integrator's (i.e. generator's) data */

#define MOIE	1	/* Manually Optimize Inductive Expresions */


/* 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_PA		4
#define CL_TEST		6
#define CL_UTEST	7
#define CL_STDFUN	8

#define TWOPI	8.0*atan(1.0)
#define PI	4.0*atan(1.0)
#define PID2	2.0*atan(1.0)
#define SCALE	45.0/atan(1.0)

Local(StagenDataPtr) stagenData=NULL;	/* global copy of Starter's parameter passed by content */
Local(__hom_DataPtr) staData=NULL;	/* global copy of our own data area */
Local(StdLinAlgDataPtr) staFunc=NULL;	/* global copy of pointer to standard linea algebra functions */
Local(BvpLinAlgDataPtr) bvpFunc=NULL;   /* global copy of pointer to BVP linea algebra functions */
Local(UtilitiesDataPtr) staUtil=NULL;	/* global copy of pointer to utility functions */
Local(Int2Ptr) active;			/* index of active parameter */
Local(Int2) npar,nphase;		/* dims of par and phase spaces */
Local(Int2) nappend;			/* number of user functions */
Local(Int2) nbc,nnint,nfree;	        /* number of boundary and integral conds; and free parameters */
Local(Vector) x1=NULL;			/* ptr to vector - the first point produced by starter */
Local(Vector) x0=NULL;			/* approximate first point */
Local(Vector) v0=NULL;			/* approximate tangent vector at first point */
Local(Vector) StdFun=NULL;		/* L2_NORM and MIN&MAX */
Local(FloatHiPtr) X=NULL;		/* current set of coord used by Decoder */
Local(FloatHiPtr) P=NULL;		/* current set of params used by Decoder */
Local(FloatHiPtr) Rhs=NULL;		/* current set of rhs vals used by Decoder */
Local(FloatHiPtr) J;			/* current set of Jacobian vals */
Local(FloatHiPtr) H;                   	/* current set of Hessian vals */
Local(Boolean) SymbNumer1,SymbNumer2;	/* TRUE if symbolic differentiation of 1st and 2nd order */
Local(Int2) Der2num;			/* number of 2nd order derivatives */
Local(FloatHi) test[4];

/* Working space for defining and test functions  */

Local(Vector) tm,rtm;    	/* (new and restart) mesh points */
Local(Matrix) upoldp; 		/* time derivatives at the preceding cycle */
Local(Vector) xp,frhs;
Local(Vector) wi,uip,uic,ficd;
Local(Matrix) wp=NULL,wploc=NULL,wt=NULL,ups=NULL,vps=NULL;
Local(Vector) Re=NULL,Im=NULL;
Local(Matrix) rups=NULL,rvps=NULL;
Local(Matrix) A=NULL,B=NULL,BS=NULL,BM=NULL,BQ=NULL;
Local(Vector) ubc0, ubc1, fbc;
Local(Vector) Tint,V,W;
Local(Matrix) Uint=NULL,Vint=NULL;

Local(bvpMatrix) D=NULL,D1=NULL;

Local(VAR) Period,Epsilon,Delta;

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

#define _ncol	staData->ncol	/* number of collocation points */
#define _ntst	staData->ntst	/* number of mesh intervals */

Local(void) wint(Vector wi);
Local(void) genwts(Matrix wt, Matrix wp);

#ifdef WIN32
static int _USERENTRY hom_SortCmp(const void *f, const void *s) {
#else
Entry(int) hom_SortCmp(const void *f, const void *s) {
#endif
  if (*(FloatHiPtr)f==*(FloatHiPtr)s) return 0;
  return (*(FloatHiPtr)f<*(FloatHiPtr)s) ? -1 : 1;
}

/* 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=(__hom_DataPtr)stagenptr->StaPtr;
  /* Use standard and BVP linea algebras */
  staFunc=(StdLinAlgDataPtr)stagenptr->StdLinAlg;
  bvpFunc=(BvpLinAlgDataPtr)stagenptr->StaFunc;
  staUtil=(UtilitiesDataPtr)stagenptr->Utilities;
  /* Get dimensions of Phase and Par spaces */
  npar=*stagenptr->ClassDim[CL_PAR];
  nphase=*stagenptr->ClassDim[CL_PHASE];
  for (i=0,nappend=0; i<nUTest; i++) if (ActiveUTestFuns[i]==UDF_APPEND) nappend++;
  /* append activation of the period */
  active=MemNew(sizeof(Int2)*(2+nappend));

  nbc=nphase+1; 			/* 1D HOM SPECIFIC */
  nnint=0;				/* 1D HOM SPECIFIC */
  nfree=nbc+nnint-nphase+1+nappend;    	

  /* Do all the checks related to active parameter and find it */
  if (staUtil->CheckParameters(staData->Active,npar+3,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 Jacobian matrix is undefined.\nSet 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.\nSet positive 'Increment'");
    active=MemFree(active);
    return 3;
  }
  /* Create a copy parameters */
  if (!P) {
    X=MemNew(sizeof(FloatHi)*nphase);
    MemCopy(X,staData->X,sizeof(FloatHi)*nphase);
    P=MemNew(sizeof(FloatHi)*(npar+3));  /* including the parameters of the algorithm */
    MemCopy(P,staData->P,sizeof(FloatHi)*(npar+3));
    /* Allocate memory for copy of phase vars used by Decoder */
    /* Allocate memory for values of rhs used by Decoder/Define/Tests */
    Rhs=MemNew(sizeof(FloatHi)*nphase);
    /* Allocate memory for multipliers */
    StdFun=staFunc->CreateVector(1+2*nphase);
 
 /* Initialize BVP algebra package */
    bvpFunc->Init(staFunc,nphase,_ntst,_ncol,nbc,nnint,nfree);
    wi=staFunc->CreateVector(_ncol+1);
    wt=staFunc->CreateMatrix(_ncol+1,_ncol);
    wp=staFunc->CreateMatrix(_ncol+1,_ncol);
    wint(wi);                   /* weights for integration */
    genwts(wt,wp);   	        /* weights */

    /* Allocate memory used in defining, test functions and in the default processor */

    x1=staFunc->CreateVector((_ntst)*nphase*_ncol+nphase+nfree);
    v0=staFunc->CreateVector((_ntst)*nphase*_ncol+nphase+nfree);
    A=staFunc->CreateMatrix(nphase,nphase+2+nappend);
    B=staFunc->CreateMatrix(nphase,nphase);
    BM=staFunc->CreateMatrix(nphase,nphase);
    BS=staFunc->CreateMatrix(nbc+nphase+nfree,nbc+nphase+nfree);
    
    tm=staFunc->CreateVector(_ntst+1);   		/* (new) mesh points  */
    xp=staFunc->CreateVector(nphase+2+nappend);
    frhs=staFunc->CreateVector(nphase);
    wploc=staFunc->CreateMatrix(_ncol+1,_ncol);

    ups=staFunc->CreateMatrix(_ntst+1,nphase*_ncol);
    vps=staFunc->CreateMatrix(_ntst+1,nphase*_ncol);
    upoldp=staFunc->CreateMatrix(_ntst+1,nphase*_ncol); /* time derivatives at the preceeding cycle */
    ubc0=staFunc->CreateVector(nphase);
    ubc1=staFunc->CreateVector(nphase);
    fbc=staFunc->CreateVector(nbc);
    uic=staFunc->CreateVector(nphase);
    uip=staFunc->CreateVector(nphase);
    ficd=staFunc->CreateVector(nnint);

    Re=staFunc->CreateVector(nphase);
    Im=staFunc->CreateVector(nphase);
    V=staFunc->CreateVector(nphase);
    W=staFunc->CreateVector(nphase);

    BifVector=MemNew(sizeof(FloatHi)*2+2*sizeof(FloatHi)*((_ntst)*nphase*_ncol+nphase+nfree));

    Tint=staFunc->CreateVector(_ntst+1);
    Uint=staFunc->CreateMatrix(_ntst+1,nphase*_ncol);
    Vint=staFunc->CreateMatrix(_ntst+1,nphase*_ncol);
    if (nappend) BQ=staFunc->CreateMatrix(nappend,(_ntst)*nphase*_ncol+nphase+nfree);
  
  }
  Der2num=ind2_(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(void) Term (Boolean OK) {
Int2 i;
    stagenData->BifDataOutLen=0;
    if (OK&&P&&(stagenData->Reason==RF_CLEAN)) {
      /* Extend data */
      stagenData->ContDataStaLen=(_ntst+1)*(1+nphase*_ncol)*sizeof(VAR);
      stagenData->ContDataStaPtr=MemNew(stagenData->ContDataStaLen);
      MemCopy(stagenData->ContDataStaPtr,tm,(_ntst+1)*sizeof(VAR));
      for (i=0; i<_ntst+1; i++) MemCopy(stagenData->ContDataStaPtr+(_ntst+1)+i*nphase*(_ncol),
        upoldp[i],nphase*(_ncol)*sizeof(VAR));
      /* Compute data */
      stagenData->BifDataOutLen=2*sizeof(FloatHi);
      BifVector[0]=(VAR)_ntst;
      BifVector[1]=(VAR)_ncol;

      stagenData->ProcessPoint(stagenData->ProcFuncNum, x1, "Last point");
    }
    if (P) {
      x1=staFunc->DestroyVector(x1);
      x0=staFunc->DestroyVector(x0);
      v0=staFunc->DestroyVector(v0);
      X=MemFree(X);
      P=MemFree(P);
      StdFun=staFunc->DestroyVector(StdFun);
      Rhs=MemFree(Rhs);
/* destroy global arrays */
      A=staFunc->DestroyMatrix(A);
      B=staFunc->DestroyMatrix(B);
      BM=staFunc->DestroyMatrix(BM);
      BS=staFunc->DestroyMatrix(BS);
      tm=staFunc->DestroyVector(tm);
      xp=staFunc->DestroyVector(xp);
      frhs=staFunc->DestroyVector(frhs);
      wi=staFunc->DestroyVector(wi);
      wp=staFunc->DestroyMatrix(wp);
      wploc=staFunc->DestroyMatrix(wploc);
      wt=staFunc->DestroyMatrix(wt);
      ubc0=staFunc->DestroyVector(ubc0);
      ubc1=staFunc->DestroyVector(ubc1);
      fbc=staFunc->DestroyVector(fbc);
      ups=staFunc->DestroyMatrix(ups);
      vps=staFunc->DestroyMatrix(vps);
      upoldp=staFunc->DestroyMatrix(upoldp);
      uic=staFunc->DestroyVector(uic);
      uip=staFunc->DestroyVector(uip);
      ficd=staFunc->DestroyVector(ficd);
      Re=staFunc->DestroyVector(Re);
      Im=staFunc->DestroyVector(Im);
      V=staFunc->DestroyVector(V);
      W=staFunc->DestroyVector(W);
      BifVector=MemFree(BifVector);
      Tint=staFunc->DestroyVector(Tint);
      Uint=staFunc->DestroyMatrix(Uint);
      Vint=staFunc->DestroyMatrix(Vint);
      D=bvpFunc->DestroyMatrix(D);
      D1=bvpFunc->DestroyMatrix(D1);
      if (nappend) BQ=staFunc->DestroyMatrix(BQ);
    }
  active=MemFree(active);
}

/**********************************************/
/* Starters to continue the cycle curve */

Local(void) newmsh (Matrix ups, Vector TMOLD, Vector TMNEW);
Local(void) interp (Int2 NC, Vector tm, Matrix ups, Int2 NC1, Vector TM1, Matrix UPS1);

Entry(Int2) hom_DefHom (Vector x, Vector y);
Entry(Int2) hom_JacDefHom (Vector x, bvpMatrix mp);
Entry(Int2) hom_ProcDefault(Int2 Ind, Vector x, Vector v1, CharPtr *msg);
Local(Int2) SystemJac (Vector X, Matrix A);

/* Extract Period from x depending on Active */
Local(VAR) GetPeriod(Vector x) {
Int2 i;
  if (staData->Active[npar]) {
    for (i=0; i<2+nappend; i++) 
      if (active[i]==npar) return bvpFunc->GetVectorElementFC(x,nphase+i);    
  } else {
    return staData->P[npar];
  }
  return -1;
}
/* Extract Delta from x depending on Active */
Local(VAR) GetDelta(Vector x) {
Int2 i;
  if (staData->Active[npar+1]) {
    for (i=0; i<2+nappend; i++) 
      if (active[i]==npar+1) return bvpFunc->GetVectorElementFC(x,nphase+i);     
  } else {
    return staData->P[npar+1];
  }
  return -1;
}
/* Extract Epsilon from x depending on Active */
Local(VAR) GetEpsilon(Vector x) {
Int2 i;
  if (staData->Active[npar+2]) {
    for (i=0; i<2+nappend; i++) 
      if (active[i]==npar+2) return bvpFunc->GetVectorElementFC(x,nphase+i);     
  } else {
    return staData->P[npar+2];
  }
  return -1;
}

/* hom->hom Starter */
Entry(Int2) hom_Starter(StagenDataPtr stagenptr) {
  Vector v1=NULL;
  Int2 rntst,rncol;
  Int2 i,j,Ret;
  if (!stagenptr) {	/* this is request to terminate */
    Term(TRUE);  
    return 0;
  }
  /* Initialize local variables */
  if (Init(stagenptr)) return 1;

  if (stagenptr->ContDataGenLen) {   	/* Extend */    
    x0=staFunc->CreateVector((_ntst)*nphase*_ncol+nphase+nfree);
    MemCopy(tm,stagenData->ContDataStaPtr,(_ntst+1)*sizeof(VAR));
    for (i=0; i<_ntst+1; i++) MemCopy(upoldp[i],
      stagenData->ContDataStaPtr+(_ntst+1)+i*nphase*(_ncol),nphase*(_ncol)*sizeof(VAR));
    stagenptr->ContDataStaPtr=MemFree(stagenptr->ContDataStaPtr);
    stagenptr->ContDataStaLen=0;
    D=bvpFunc->CreateMatrix((_ntst)*nphase*_ncol+nphase+nfree-1,
			   (_ntst)*nphase*_ncol+nphase+nfree);
    D1=bvpFunc->CreateMatrix((_ntst)*nphase*_ncol+nphase+nfree,
			   (_ntst)*nphase*_ncol+nphase+nfree);
  } else {   				/* Compute */
    if (FuncParActive(staData->SetInitPoint)) {
      /* there is a user-defined function to set InitHom */
      VAR PNTR work=NULL;
      VAR tt,slope;
      Int2 Index_P,Index_Xc;
      Int2 i,j,k,npoint=0;
      Boolean error=FALSE;
      Index_P=stagenptr->ParIndex(staData,P);
      Index_Xc=stagenptr->ParIndex(staData,Xc);
      staData->time0=MemGet(sizeof(VAR)*1,FALSE);
      staData->phase0=MemGet(sizeof(VAR)*nphase,FALSE);
      /* Call udf SetInitHom function, if any */
      if(FuncParCall(staData->SetInitPoint)(-1,staData)) {	/* init (e.g. fopen) */
	error=TRUE;
	staUtil->ErrMessage("Nonzero return code from function SetInitHom (when ip=-1)");        
      } else {
        while (FuncParCall(staData->SetInitPoint)(npoint,staData)==0) { /* while there is a point */
	    npoint++;
	    if (work) work=MemMore(work,sizeof(VAR)*(1+nphase)*npoint);
	    else work=MemGet(sizeof(VAR)*(1+nphase),FALSE);
	    MemCopy(work+(1+nphase)*(npoint-1),staData->time0,sizeof(VAR)*1);
	    MemCopy(work+(1+nphase)*(npoint-1)+1,staData->phase0,sizeof(VAR)*nphase);
        }
        FuncParCall(staData->SetInitPoint)(-2,staData);	/* term (e.g. fclose) */
      }
      staData->time0=MemFree(staData->time0);
      staData->phase0=MemFree(staData->phase0);
      qsort(work,npoint,sizeof(VAR)*(1+nphase),hom_SortCmp);
      if (!error && npoint<2*2) {
	error=TRUE;
	staUtil->ErrMessage("The number of points must be more than 3");
      }
      if (!error && !(work[0]<work[(npoint-1)*(1+nphase)])) {
	error=TRUE;
	staUtil->ErrMessage("The time interval must have positive length");
      }
      if (!error && staData->ncol<2 || staData->ncol>7) {
	error=TRUE;
	staUtil->ErrMessage("The value of ncol must be in [2,7]");
      }
      if (!error && staData->ntst<2) {
	error=TRUE;
	staUtil->ErrMessage("The value of ntst must be more than 1");
      }
      for (i=1; i<npoint; i++)
	if (!error && work[(i-1)*(1+nphase)]==work[i*(1+nphase)]) {
	error=TRUE;
	staUtil->ErrMessage("Two identical times found");
	break;
      }
      if (error) {
	staData->lenT=0;
	staData->lenX=0;
	Term(FALSE);
	return 10;
      }
      staData->P[npar]=work[(npoint-1)*(1+nphase)]-work[0];     /*  period */
      /* interpolate uniformely to the given ntst and ncol and place to the new staData->T and staData->X*/
      /* LINEAR INTERPOLATION IS IMPLEMENTED */
      rntst=_ntst;   
      rncol=_ncol;   
      stagenptr->UpdatePar(Index_P);
      stagenptr->UpdatePar(Index_Xc);

      staData->T=MemFree(staData->T);
      staData->X=MemFree(staData->X);
      staData->lenT=sizeof(VAR)*(rntst*rncol+1);
      staData->T=MemGet(staData->lenT,FALSE);		/* new times */
      staData->lenX=sizeof(VAR)*nphase*(rntst*rncol+1);
      staData->X=MemGet(staData->lenX,FALSE);		/* new components */

      staData->T[0]=work[0];
      MemCopy(staData->X,work+1,sizeof(VAR)*nphase);
      for (i=1; i<rntst*rncol; i++) {
        staData->T[i]=staData->P[npar]*i/(rntst*rncol);
	for (j=0; j<npoint && staData->T[i]>=work[j*(1+nphase)]; j++) ; /* the body is empty here! */
        /* ASSERT staData->T[i] < work[j*(1+nphase)]*/
        j-=1;
        tt=staData->T[i]-work[j*(1+nphase)];
	for (k=0; k<nphase; k++) {
	  slope=(work[(j+1)*(1+nphase)+1+k]-work[j*(1+nphase)+1+k])/
	        (work[(j+1)*(1+nphase)]-work[j*(1+nphase)]);
	  staData->X[i*nphase+k]=work[j*(1+nphase)+1+k]+tt*slope;
	}
      }
      staData->T[rntst*rncol]=work[(npoint-1)*(1+nphase)];
      MemCopy(staData->X+rntst*rncol*nphase,work+(npoint-1)*(1+nphase)+1,sizeof(VAR)*nphase);
      
      MemFree(work);
      for (i=0; i<rntst*rncol+1; i++) staData->T[i]/=staData->P[npar];
    } else {	/* no SetInitHom function */
      if (!staData->lenX) {
	  staUtil->ErrMessage("No initial cycle.\n"
            "Specify SetInitHom or pick up a computed homoclinic orbit and do not change system parameters");
          Term(FALSE);
	  return 1;
      }
      if (stagenData->BifDataInOk) {
        rntst=stagenData->BifDataInPtr[0];
        rncol=stagenData->BifDataInPtr[1];
      } else { 
	  staUtil->ErrMessage("No data for cycle computation.\n"
            "Specify SetInitHom or pick up a computed homoclinic orbit and do not change system parameters");
          Term(FALSE);
	  return 1;
      }
    }
    /* Create the first point and tangent vector for generator */
    /* Initialize BVP algebra package with the restart rntst & rncol */
    bvpFunc->Init(staFunc,nphase,rntst,rncol,nbc,nnint,nfree);

    x0=staFunc->CreateVector((rntst)*nphase*rncol+nphase+nfree);
    MemCopy(x0,staData->X,sizeof(VAR)*(rntst*rncol+1)*nphase);
    for (i=0; i<2+nappend; i++) bvpFunc->SetVectorElementFC(x0,nphase+i,staData->P[active[i]]);

    if (rntst!=_ntst || rncol!=_ncol) { 
      /* Interpolation */
      rups=staFunc->CreateMatrix(rntst+1,nphase*rncol);
      rtm=staFunc->CreateVector(rntst+1); 	/* (restart) mesh points */

      for (i=0; i<rntst; i++) {
        for (j=0; j<nphase*rncol; j++) {
          rups[i][j]=bvpFunc->GetVectorElementFA(x0,i,j);   
        }
      }
      for (j=0; j<nphase; j++) rups[rntst][j]=bvpFunc->GetVectorElementFC(x0,j);
      for (i=0; i<rntst+1; i++) rtm[i]=staData->T[i*rncol];
      x0=staFunc->DestroyVector(x0);

      /* interpolate to the new mesh */  
      newmsh (rups, rtm, tm); 
      interp (rncol, rtm, rups, _ncol, tm, ups);

      /* Initialize BVP algebra package with the new _ntst & _ncol*/
      bvpFunc->Init(staFunc,nphase,_ntst,_ncol,nbc,nnint,nfree);
      x0=staFunc->CreateVector((_ntst)*nphase*_ncol+nphase+nfree);  
      for (i=0; i<_ntst; i++) {
        for (j=0; j<nphase*_ncol; j++) {
          bvpFunc->SetVectorElementFA(x0,i,j,ups[i][j]);
        }
      }
      for (j=0; j<nphase; j++) bvpFunc->SetVectorElementFC(x0,j,ups[_ntst][j]);
      for (i=0; i<2+nappend; i++) bvpFunc->SetVectorElementFC(x0,nphase+i,staData->P[active[i]]);
    
      rtm=staFunc->DestroyVector(rtm);
      rups=staFunc->DestroyMatrix(rups);
    } else { /* no interpolation, i.e. extend or compute with the same ntst & ncol */
      if (!stagenData->ContDataGenLen) for (i=0; i<_ntst+1; i++) tm[i]=staData->T[i*_ncol];
    }
    /* Solve for the tangent vector */
    v1=staFunc->CreateVector((_ntst)*nphase*_ncol+nphase+nfree);
    D=bvpFunc->CreateMatrix((_ntst)*nphase*_ncol+nphase+nfree-1,
			   (_ntst)*nphase*_ncol+nphase+nfree);
    D1=bvpFunc->CreateMatrix((_ntst)*nphase*_ncol+nphase+nfree,
			   (_ntst)*nphase*_ncol+nphase+nfree);
    staFunc->ClearVector(v0);
    for (j=(_ntst)*nphase*_ncol+nphase+nfree-1; j>=0; j--) {
/*      i=j-1;
      if (j==0) i=(_ntst)*nphase*_ncol+nphase+nfree-1; 
*/
      v1[j]=1.0;
      Ret=hom_ProcDefault(0, x0, v1, NULL); /* Setup UPOLDP */
      if (Ret == 0) {
        bvpFunc->SetVectorElementFC(v0,nphase+1,stagenData->Forward ? 1 : -1);
        bvpFunc->Solve(D1,v0);
       break;
      };
      v1[j]=0.0;
    };
    if (Ret) {
       staUtil->ErrMessage(
       "Unable to find a tangent vector to the cycle curve at the initial point"); 
       staFunc->DestroyVector(v1);
       D=bvpFunc->DestroyMatrix(D);
       D1=bvpFunc->DestroyMatrix(D1);
       Term(FALSE);
       return 1;
    };
    staFunc->NormalizeVector(v0);
    v1=staFunc->DestroyVector(v1);
  }
  /* Initialize continuation */
  ((ContDataPtr)stagenptr->GenPtr)->X0=x0;
  ((ContDataPtr)stagenptr->GenPtr)->V0=v0;
  /* That's all */
  return 0;
}


/* ep->hom Starter */
Entry(Int2) ep_hom_Starter(StagenDataPtr stagenptr) {
Int2 i,j,k,l,imin,jmin,I,J,K,K1,K2,imax;
VAR T,S,DT,RN;
VAR eps=0.01;
printf("ep->hom Starter eneterd\n");
  if (!stagenptr) {	/* this is request to terminate */
    Term(TRUE); 
    return 0;
  }
  /* Initialize local variables */
  if (Init(stagenptr)) return 1;
printf("_ntst=%i, _ncol=%i, nfree=%i, nphase=%i\n",_ntst, _ncol, nfree, nphase); 
  x0=staFunc->CreateVector((_ntst)*nphase*_ncol+nphase+nfree);  
  D=bvpFunc->CreateMatrix((_ntst)*nphase*_ncol+nphase+nfree-1,
			   (_ntst)*nphase*_ncol+nphase+nfree);
  D1=bvpFunc->CreateMatrix((_ntst)*nphase*_ncol+nphase+nfree,
			   (_ntst)*nphase*_ncol+nphase+nfree);
    
  if (stagenptr->ContDataGenLen) {   	/* Extend */
    MemCopy(tm,stagenData->ContDataStaPtr,(_ntst+1)*sizeof(VAR));
    for (i=0; i<_ntst+1; i++) MemCopy(upoldp[i],
      stagenData->ContDataStaPtr+(_ntst+1)+i*nphase*(_ncol),nphase*(_ncol)*sizeof(VAR));
    stagenptr->ContDataStaPtr=MemFree(stagenptr->ContDataStaPtr);
    stagenptr->ContDataStaLen=0;
  } else {   				/* Compute */
printf("Compute eigenvalues\n");
    /* Copy the equilibrium and parameters to xp */
    staFunc->ClearVector(xp);
staFunc->PrintVector(xp,"  xp=");
    SystemJac(xp, A);   	  /* xp, A - global */
staFunc->PrintMatrix(A,"  A=",0);
    /* compute eigenvalies */
    staFunc->CopyMatrixBlock(A,0,0,B,0,0,nphase,nphase);
    staFunc->EigenValues(B,Re,Im);
staFunc->PrintVector(Re,"    Re=");
staFunc->PrintVector(Im,"    Im=");
    imax=0;
    for (I=0; I<nphase; I++) if(Re[I]>0) {imax=I; break;};  /* here assumed only one unstable eigenvalue */
printf("imax=%i\n",imax);
    /* compute eigenvectors */
    staFunc->CopyMatrixBlock(A,0,0,B,0,0,nphase,nphase);
    for (I=0; I<nphase; I++) B[I][I]-=Re[imax];
    staFunc->SngSolve(B,eps,V);    /* V - unstable eigenvector (global)  */ 
    staFunc->NormalizeVector(V);   
staFunc->PrintVector(V,"  V=");

    staFunc->CopyMatrixBlock(A,0,0,B,0,0,nphase,nphase);
    staFunc->TransposeMatrix(B,BM);
    for (I=0; I<nphase; I++) BM[I][I]-=Re[imax];
    staFunc->SngSolve(BM,eps,W);   /* W - unstable adjoint eigenvector (global)  */
    staFunc->NormalizeVectorRelative(W,V);   
staFunc->PrintVector(W,"  W=");
  
    Period=staData->P[npar];    
    Epsilon=staData->P[npar+2];       
    Delta=Epsilon*exp(Period*Re[imax]);
printf("Period=%g, Epsilon=%g, Delta=%g\n",Period,Epsilon,Delta);

    /* Initialize BVP algebra package with _ntst & _ncol*/
    bvpFunc->Init(staFunc,nphase,_ntst,_ncol,nbc,nnint,nfree);
    
    /* Setup equidistant mesh */
    for (i=0; i<_ntst+1; i++) tm[i]=i*1.0/(_ntst);

    /* Create the first point and tangent vector for generator */
    DT=1.0/(_ntst*_ncol);
    for (k=0; k<_ntst; k++) {
      for (j=0; j<_ncol; j++) {
        T=tm[k]+j*DT;
        for (i=0; i<nphase; i++) {
	  S=V[i]*exp(Period*Re[imax]*T);
	  bvpFunc->SetVectorElementFA(x0,k,nphase*j+i,xp[i]+S*Epsilon);
	  bvpFunc->SetVectorElementFA(v0,k,nphase*j+i,S);
	}
      }
    }
    T=1.0;
    for (i=0; i<nphase; i++) {
      S=V[i]*exp(Period*Re[imax]*T);
      bvpFunc->SetVectorElementFC(x0,i,xp[i]+S*Epsilon);
      bvpFunc->SetVectorElementFC(v0,i,S);
    }

    bvpFunc->SetVectorElementFC(x0,nphase,Period); /* Period */
    bvpFunc->SetVectorElementFC(x0,nphase+1,Delta);   /* Delta */
    bvpFunc->SetVectorElementFC(v0,nphase,0);   
    bvpFunc->SetVectorElementFC(v0,nphase+1,0); 
    staFunc->NormalizeVector(v0);

/*  Setup UPOLDP */
    for (J=0; J<_ntst; J++) {
      RN=1.0/_ncol;
      for (I=0; I<_ncol; I++) {
        T=tm[J]+I*RN*(tm[J+1]-tm[J]);
        K1=I*nphase;
        K2=(I+1)*nphase;
        for (K=K1; K<K2; K++) upoldp[J][K]=V[K-K1]*Epsilon*Period*Re[imax]*exp(Period*Re[imax]*T);
     }
    }
    T=1.0;
    for (K=0; K<nphase; K++) upoldp[_ntst][K]=V[K]*Epsilon*Period*Re[imax]*exp(Period*Re[imax]*T);

  }
  /* Initialize continuation */
  ((ContDataPtr)stagenptr->GenPtr)->X0=x0;
  ((ContDataPtr)stagenptr->GenPtr)->V0=v0;
  /* That's all */
  return 0;
}

/* p->hom Starter */
static VAR PNTR work;
static Int2 npoint;

/* MUST be Global, not Entry */
Global(Int2) hom_OutIntegr(Pointtype type, VoidPtr point, CharPtr msg) {
  /* from __o.c CL_TIME ~ 1, CL_PAHSE ~ 2 */
#define integrCL_TIME 1
#define integrCL_PHASE 2
  if (type) {	/* the last point; ignore it because it coinsides with the prev. */
    if (stagenData->Slave->ContDataGenLen) {
      stagenData->Slave->ContDataGenPtr=MemFree(stagenData->Slave->ContDataGenPtr);
      stagenData->Slave->ContDataGenLen=0;
    }
    return 0;
  }
  stagenData->Slave->pDecoder(point,stagenData->Slave->IndirectValues,1);
  npoint++;
  if (work) 
    work=MemMore(work,sizeof(VAR)*(1+nphase)*npoint);
  else 
    work=MemGet(sizeof(VAR)*(1+nphase),FALSE);
  MemCopy(work+(1+nphase)*(npoint-1),stagenData->Slave->IndirectValues[integrCL_TIME],sizeof(VAR)*1);
  MemCopy(work+(1+nphase)*(npoint-1)+1,stagenData->Slave->IndirectValues[integrCL_PHASE],sizeof(VAR)*nphase);
  return 0;   /* continue integration */
#undef integrCL_TIME
#undef integrCL_PHASE
}
Entry(Int2) p_hom_Starter(StagenDataPtr stagenptr) {
  Vector v1=NULL;
  Int2 rntst,rncol;
  Int2 i,j,k,Ret;
  if (!stagenptr) {	/* this is request to terminate */
    Term(TRUE);  
    return 0;
  }
  /* Initialize local variables */
  if (Init(stagenptr)) return 1;

  if (stagenptr->ContDataGenLen) {   	/* Extend */    
    x0=staFunc->CreateVector((_ntst)*nphase*_ncol+nphase+nfree);
    MemCopy(tm,stagenData->ContDataStaPtr,(_ntst+1)*sizeof(VAR));
    for (i=0; i<_ntst+1; i++) MemCopy(upoldp[i],
      stagenData->ContDataStaPtr+(_ntst+1)+i*nphase*(_ncol),nphase*(_ncol)*sizeof(VAR));
    stagenptr->ContDataStaPtr=MemFree(stagenptr->ContDataStaPtr);
    stagenptr->ContDataStaLen=0;
    D=bvpFunc->CreateMatrix((_ntst)*nphase*_ncol+nphase+nfree-1,
			   (_ntst)*nphase*_ncol+nphase+nfree);
    D1=bvpFunc->CreateMatrix((_ntst)*nphase*_ncol+nphase+nfree,
			   (_ntst)*nphase*_ncol+nphase+nfree);
  } else {   				/* Compute */
    VAR tt,slope;
    Boolean error=FALSE;
    if (FuncParActive(staData->SetInitPoint)) {
      /* there is a user-defined function to set InitHom */
      Int2 Index_P,Index_Xc;
      FuncParCall(staData->SetInitPoint)(npoint,staData);
      Index_P=stagenptr->ParIndex(staData,P);
      Index_Xc=stagenptr->ParIndex(staData,Xc);
      stagenptr->UpdatePar(Index_P);
      stagenptr->UpdatePar(Index_Xc);
    }
    work=NULL;
    npoint=0;
    /* assign initial data for integration __o.h */
    MemCopy(((__o_DataPtr)(stagenData->Slave->StaPtr))->X,staData->Xc,nphase*sizeof(VAR));
    MemCopy(((__o_DataPtr)(stagenData->Slave->StaPtr))->P,staData->P,npar*sizeof(VAR));
    if (((__o_DataPtr)(stagenData->Slave->StaPtr))->T)
      ((__o_DataPtr)(stagenData->Slave->StaPtr))->T[0]=0;
    if (((__o_DataPtr)(stagenData->Slave->StaPtr))->Tdef)
      ((__o_DataPtr)(stagenData->Slave->StaPtr))->Tdef[0]=0;
    ((__o_DataPtr)(stagenData->Slave->StaPtr))->SetInitPoint._Selected=0; /* deactivate SetInitPoint */
    
    /* Set ProcessPoint hook for integrator */
    stagenData->Slave->ProcessPoint=(CallbackPtr)hom_OutIntegr;

    /* call o_Starter */
    
    stagenData->Slave->Reason=RF_DO;
    if (!stagenData->Slave->pStarter(stagenData->Slave)) {
      /* assign integration parameters for __integr*.h */
      ((IntegrDataPtr)(stagenData->Slave->GenPtr))->Tint=staData->P[npar];
      stagenData->Slave->Forward=TRUE;

      /* call integrator */
      if (!stagenData->Slave->pGenerator(stagenData->Slave)) {
        if (!error && npoint<2*2) {
	  error=TRUE;
	  staUtil->ErrMessage("The number of points must be more than 3");
        }
        if (!error && staData->ncol<2 || staData->ncol>7) {
	  error=TRUE;
	  staUtil->ErrMessage("The value of ncol must be in [2,7]");
        }
        if (!error && staData->ntst<2) {
	  error=TRUE;
	  staUtil->ErrMessage("The value of ntst must be more than 1");
        }
        if (error) {
	  Term(FALSE);
	  return 10;
        }

        /* interpolate uniformely to the given ntst and ncol and place to the new staData->T and staData->X*/
        /* LINEAR INTERPOLATION IS IMPLEMENTED */
        rntst=_ntst;   
        rncol=_ncol;   

        MemFree(staData->T);
        MemFree(staData->X);

        staData->lenT=sizeof(VAR)*(rntst*rncol+1);
        staData->T=MemGet(staData->lenT,FALSE);		/* new times */
        staData->lenX=sizeof(VAR)*nphase*(rntst*rncol+1);
        staData->X=MemGet(staData->lenX,FALSE);		/* new components */

        staData->T[0]=work[0];
        MemCopy(staData->X,work+1,sizeof(VAR)*nphase);
        for (i=1; i<rntst*rncol; i++) {
          staData->T[i]=staData->P[npar]*i/(rntst*rncol);
	  for (j=0; j<npoint && staData->T[i]>=work[j*(1+nphase)]; j++) ; /* the body is empty here! */
          /* ASSERT staData->T[i] < work[j*(1+nphase)]*/
          j-=1;
          tt=staData->T[i]-work[j*(1+nphase)];
	  for (k=0; k<nphase; k++) {
	    slope=(work[(j+1)*(1+nphase)+1+k]-work[j*(1+nphase)+1+k])/
	        (work[(j+1)*(1+nphase)]-work[j*(1+nphase)]);
	    staData->X[i*nphase+k]=work[j*(1+nphase)+1+k]+tt*slope;
	  }
        }
        staData->T[rntst*rncol]=work[(npoint-1)*(1+nphase)];
        MemCopy(staData->X+rntst*rncol*nphase,work+(npoint-1)*(1+nphase)+1,sizeof(VAR)*nphase);
      
        MemFree(work);
        for (i=0; i<rntst*rncol+1; i++) staData->T[i]/=staData->P[npar];
      } else {
	stagenData->Slave->Reason=RF_CLEAN;
        stagenData->Slave->pStarter(NULL);	/* terminate integrator's starter */
	MemFree(work);
	Term(FALSE);
	return 25;
      }
      stagenData->Slave->Reason=RF_CLEAN;
      stagenData->Slave->pStarter(NULL);	/* terminate integrator's starter */
    } else {
      return 30;
    }
 
    /* Create the first point and tangent vector for generator */
    /* Initialize BVP algebra package with the restart rntst & rncol */
    bvpFunc->Init(staFunc,nphase,rntst,rncol,nbc,nnint,nfree);

    x0=staFunc->CreateVector((rntst)*nphase*rncol+nphase+nfree);
    MemCopy(x0,staData->X,sizeof(VAR)*(rntst*rncol+1)*nphase);
    for (i=0; i<2+nappend; i++) bvpFunc->SetVectorElementFC(x0,nphase+i,staData->P[active[i]]);
    if (!stagenData->ContDataGenLen) for (i=0; i<_ntst+1; i++) tm[i]=staData->T[i*_ncol];
    /* Solve for the tangent vector */
    v1=staFunc->CreateVector((_ntst)*nphase*_ncol+nphase+nfree);
    D=bvpFunc->CreateMatrix((_ntst)*nphase*_ncol+nphase+nfree-1,
			   (_ntst)*nphase*_ncol+nphase+nfree);
    D1=bvpFunc->CreateMatrix((_ntst)*nphase*_ncol+nphase+nfree,
			   (_ntst)*nphase*_ncol+nphase+nfree);
    staFunc->ClearVector(v0);
    for (j=(_ntst)*nphase*_ncol+nphase+nfree-1; j>=0; j--) {
/*      i=j-1;
      if (j==0) i=(_ntst)*nphase*_ncol+nphase+nfree-1; 
*/
      v1[j]=1.0;
      Ret=hom_ProcDefault(0, x0, v1, NULL); /* Setup UPOLDP */
      if (Ret == 0) {
        bvpFunc->SetVectorElementFC(v0,nphase+1,stagenData->Forward ? 1 : -1);
        bvpFunc->Solve(D1,v0);
       break;
      };
      v1[j]=0.0;
    };
    if (Ret) {
       staUtil->ErrMessage(
       "Unable to find a tangent vector to the cycle curve at the initial point"); 
       staFunc->DestroyVector(v1);
       D=bvpFunc->DestroyMatrix(D);
       D1=bvpFunc->DestroyMatrix(D1);
       Term(FALSE);
       return 1;
    };
    staFunc->NormalizeVector(v0);
    v1=staFunc->DestroyVector(v1);
  }
  /* Initialize continuation */
  ((ContDataPtr)stagenptr->GenPtr)->X0=x0;
  ((ContDataPtr)stagenptr->GenPtr)->V0=v0;
  /* That's all */

  return 0;
}

/* bt->hom Starter */

#define _branch   staData->branch     /* switch data  */

Entry(Int2) bt_hom_Starter(StagenDataPtr stagenptr) {
  Vector v1=NULL,DDP=NULL;
  VAR DDT;
  Int2 rntst,rncol;
  Int2 i,j,Ret;
	  staUtil->ErrMessage("NOT IMPLEMENTED");
	  return 1;
  if (!stagenptr) {	/* this is request to terminate */
    Term(TRUE);  
    return 0;
  }
  /* Initialize local variables */
  if (Init(stagenptr)) return 1;

  if (stagenptr->ContDataGenLen) {   	/* Extend */    
    x0=staFunc->CreateVector((_ntst)*nphase*_ncol+nphase+nfree);
    MemCopy(tm,stagenData->ContDataStaPtr,(_ntst+1)*sizeof(VAR));
    for (i=0; i<_ntst+1; i++) MemCopy(upoldp[i],
      stagenData->ContDataStaPtr+(_ntst+1)+i*nphase*(_ncol),nphase*(_ncol)*sizeof(VAR));
    stagenptr->ContDataStaPtr=MemFree(stagenptr->ContDataStaPtr);
    stagenptr->ContDataStaLen=0;
    D=bvpFunc->CreateMatrix((_ntst)*nphase*_ncol+nphase+nfree-1,
			   (_ntst)*nphase*_ncol+nphase+nfree);
    D1=bvpFunc->CreateMatrix((_ntst)*nphase*_ncol+nphase+nfree,
			   (_ntst)*nphase*_ncol+nphase+nfree);
  } else {   				/* Compute */
    if (FuncParActive(staData->SetInitPoint)) {
      /* there is a user-defined function SetInitHom */
      staUtil->ErrMessage("No way to start from a user-supplied branching cycle");
      return 1;
    } else {	/* no SetInitHom function */
      if (!stagenData->BifDataInOk) {
	  staUtil->ErrMessage("No branching cycle.\n"
            "Pick up a computed BP-cycle and do not change system parameters");
	  return 1;
      }
      rntst=stagenData->BifDataInPtr[0];
      rncol=stagenData->BifDataInPtr[1];
    }
    /* Create the first point and tangent vector for generator */
    /* Initialize BVP algebra package with the restart rntst & rncol */
    bvpFunc->Init(staFunc,nphase,rntst,rncol,nbc,nnint,nfree);

    x0=staFunc->CreateVector((rntst)*nphase*rncol+nphase+nfree);
    MemCopy(x0,staData->X,sizeof(VAR)*(rntst*rncol+1)*nphase);
    bvpFunc->SetVectorElementFC(x0,nphase,staData->P[active[0]]);
    bvpFunc->SetVectorElementFC(x0,nphase+1,staData->P[active[1]]);

    v1=staFunc->CreateVector((rntst)*nphase*rncol+nphase+nfree);
    if (_branch==0) {
      /* primary branch */
      MemCopy(v1,stagenData->BifDataInPtr+2,sizeof(VAR)*(rntst*rncol+1)*nphase+nfree);
    } else {
      /* secondary branch */
      MemCopy(v1,stagenData->BifDataInPtr+2+(rntst*rncol+1)*nphase+nfree,
                                            sizeof(VAR)*(rntst*rncol+1)*nphase+nfree);
    }
    DDT=bvpFunc->GetVectorElementFC(v1,nphase);
    if (rntst!=_ntst || rncol!=_ncol) { 
      /* Interpolation */
      DDP=staFunc->CreateVector(nappend+1);
      for (i=0; i<=nappend; i++) DDP[i]=bvpFunc->GetVectorElementFC(v1,nphase+1+i);
      rups=staFunc->CreateMatrix(rntst+1,nphase*rncol);
      rvps=staFunc->CreateMatrix(rntst+1,nphase*rncol);
      rtm=staFunc->CreateVector(rntst+1); 	/* (restart) mesh points */

      for (i=0; i<rntst; i++) {
        for (j=0; j<nphase*rncol; j++) {
          rups[i][j]=bvpFunc->GetVectorElementFA(x0,i,j);   
          rvps[i][j]=bvpFunc->GetVectorElementFA(v1,i,j);   
        }
      }
      for (j=0; j<nphase; j++) {
        rups[rntst][j]=bvpFunc->GetVectorElementFC(x0,j);
        rvps[rntst][j]=bvpFunc->GetVectorElementFC(v1,j);
      }
      for (i=0; i<rntst+1; i++) rtm[i]=staData->T[i*rncol];
      x0=staFunc->DestroyVector(x0);

      /* interpolate to the new mesh */  
      newmsh (rups, rtm, tm); 
      interp (rncol, rtm, rups, _ncol, tm, ups);
      interp (rncol, rtm, rvps, _ncol, tm, vps);

      /* Initialize BVP algebra package with the new _ntst & _ncol*/
      bvpFunc->Init(staFunc,nphase,_ntst,_ncol,nbc,nnint,nfree);
      x0=staFunc->CreateVector((_ntst)*nphase*_ncol+nphase+nfree);  
      for (i=0; i<_ntst; i++) {
        for (j=0; j<nphase*_ncol; j++) {
          bvpFunc->SetVectorElementFA(x0,i,j,ups[i][j]);
          bvpFunc->SetVectorElementFA(v0,i,j,vps[i][j]);
        }
      }
      for (j=0; j<nphase; j++) {
        bvpFunc->SetVectorElementFC(x0,j,ups[_ntst][j]);
        bvpFunc->SetVectorElementFC(v0,j,vps[_ntst][j]);
      }
      for (i=0; i<2+nappend; i++) bvpFunc->SetVectorElementFC(x0,nphase+i,staData->P[active[i]]);
      bvpFunc->SetVectorElementFC(v0,nphase,DDT);
      for (i=0; i<= nappend; i++) bvpFunc->SetVectorElementFC(v0,nphase+1+i,DDP[i]);
      DDP=staFunc->DestroyVector(DDP);
    
      rtm=staFunc->DestroyVector(rtm);
      rups=staFunc->DestroyMatrix(rups);
      rvps=staFunc->DestroyMatrix(rvps);
    } else { /* no interpolation, i.e. extend or compute with the same ntst & ncol */
      staFunc->CopyVector(v1,v0);
      if (!stagenData->ContDataGenLen) for (i=0; i<_ntst+1; i++) tm[i]=staData->T[i*_ncol];
    }
    /* Specify the direction vector */
    D=bvpFunc->CreateMatrix((_ntst)*nphase*_ncol+nphase+nfree-1,
			   (_ntst)*nphase*_ncol+nphase+nfree);
    D1=bvpFunc->CreateMatrix((_ntst)*nphase*_ncol+nphase+nfree,
			   (_ntst)*nphase*_ncol+nphase+nfree);
    staFunc->NormalizeVector(v0);
    staFunc->MultiplyVectorScalar(v0,stagenData->Forward ? 1 : -1,v0);
    staFunc->AddScaledVector(x0,v0,Delta,x0);
    hom_ProcDefault(0, x0, v0, NULL); /* Setup UPOLDP */
    v1=staFunc->DestroyVector(v1);
  }
  /* Initialize continuation */
  ((ContDataPtr)stagenptr->GenPtr)->X0=x0;
  ((ContDataPtr)stagenptr->GenPtr)->V0=v0;
  /* That's all */
  return 0;
}


#define _dx       staData->dx         /* increment */


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


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

Local(void) CopyToPDecoder(Vector x) {
Int2 i;
  for (i=0; i<2+nappend; i++) P[active[i]]=bvpFunc->GetVectorElementFC((Vector)x,nphase+i);
}

/***************************/
/* User-defined functions  */
Entry(Int2) hom_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;
}

Local(VAR) ZERO=0.0;
/***************************/
/* System Right Hand Sides */
Entry(Int2) hom_SystemRHS(Vector xp, Vector y) {
  CopyToP(xp); 
  stagenData->Rhs(xp,P,&ZERO,y);	
  return 0;
}

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

Local(VAR) Time,peralg[3];

Entry(Int2) hom_Decoder(VoidPtr vpoint, FloatHiPtr PNTR indirect, Int2 oper) {
VAR DT;
Int2 mesh;
  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 _ntst*_ncol+1;
    default:		/* extract next M-point from G-point pointed by vpoint */
      /* vpoint is a Vector */
      oper--;
      mesh=oper/_ncol;
      if (mesh>=_ntst) DT=0; else DT=(tm[mesh+1]-tm[mesh])/_ncol;
      Time=tm[mesh]+(oper%_ncol)*DT;
      indirect[CL_TIME]=&Time;
      indirect[CL_PHASE]=(Vector)vpoint+oper*nphase;
      indirect[CL_PAR]=P;
      CopyToPDecoder(vpoint);
      indirect[CL_PA]=peralg;
      peralg[0]=GetPeriod((Vector)vpoint);
      peralg[1]=GetDelta((Vector)vpoint);
      peralg[2]=GetEpsilon((Vector)vpoint);
      indirect[CL_STDFUN]=StdFun;
      indirect[CL_TEST]=test;
  }
  return 0;
}

Local(VAR) zero=0;

#include "_symdiff.c"
/***************************************************/
/* Jacobian of defining conditions for equilibria  */
Local(Int2) SystemJac (Vector X, Matrix A)
{
  if (stagenData->Der1) {
    SymJac(X,A);
  } else {
    if (staFunc->Der(hom_SystemRHS, nphase, X, _dx, A)) return(1);
  }
  return(0);
}

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

#define  NCOL   _ncol
#define  NCP1   (_ncol+1)

#define NDIM	nphase
#define NTST	_ntst
#define NBC	nbc
#define NINT	nnint
#define NRA     nphase*_ncol

#define WI(i)	   wi[i]
#define WP(i,j)	   wp[i][j]
#define WPLOC(i,j) wploc[i][j]
#define WT(i,j)	   wt[i][j]
#define UPS(j,k)   ups[j][k]   /* j - blocks; k - columns */
#define VPS(j,k)   vps[j][k]   /* j - blocks; k - columns */
#define UPOLDP(j,k) upoldp[j][k]
#define UBC0(i)    ubc0[i]
#define UBC1(i)    ubc1[i]
#define FBC(i)	   fbc[i]
#define TM(i)      tm[i]
#define F(i)       frhs[i]
#define UIC(i)     uic[i]
#define UIP(i)     uip[i]
#define FICD(i)	   ficd[i]

/*    Boundary conditions for homoclinic orbit  */
Local(void) bcps(Vector x, Vector ubc0, Vector ubc1, Vector fbc) {
Int2 I,imax;
VAR eps=0.01;

/*
  staFunc->ClearVector(xp);
  for (I=0; I<2+nappend; I++) xp[NDIM+I]=bvpFunc->GetVectorElementFC(x,nphase+I);    
  SystemJac(xp, A);   	  

  staFunc->CopyMatrixBlock(A,0,0,B,0,0,nphase,nphase);
  staFunc->EigenValues(B,Re,Im);
  imax=0;
  for (I=0; I<nphase; I++) if(Re[I]>0) {imax=I; break;};

  staFunc->CopyMatrixBlock(A,0,0,B,0,0,nphase,nphase);
  for (I=0; I<nphase; I++) if(Re[I]>0) B[I][I]-=Re[imax];
  staFunc->SngSolve(B,eps,V);    
  staFunc->NormalizeVector(V);   

  staFunc->CopyMatrixBlock(A,0,0,B,0,0,nphase,nphase);
  staFunc->TransposeMatrix(B,BM);
  for (I=0; I<nphase; I++) if(Re[I]>0) BM[I][I]-=Re[imax];
  staFunc->SngSolve(BM,eps,W);  
  staFunc->NormalizeVector(W);   
  
  Epsilon=GetEpsilon(x);
  Delta=GetDelta(x);
*/
  for (I=0; I<NDIM; I++) FBC(I)=UBC0(I)-Epsilon*V[I];
  FBC(NDIM)=staFunc->ScalProd(ubc1,W)-Delta;
  return;
}

/*    Integral phase conditions  */
Local(void) icps(Vector uip, Vector uic, Vector ficd) { 
Int2 I;
  for (I=0,FICD(0)=0; I<NDIM; I++) FICD(0)+=UIP(I)*UIC(I);
  return;
}

/* Defining functions for homoclinic orbits (translation of SETRHS from AUTO with opposite sign) */
Entry(Int2) hom_DefHom (Vector x, Vector y) {

Int2 I,I1,J,J1,K,K1,JJ,JP1,IB,IC,IC1,L,L1,M;
VAR  RLCUR,DT,DDT,fa,u;  

/* Initialize */
  for (I=0; I<NTST; I++) {
    for (J=0; J<NDIM*NCOL; J++) {
      UPS(I,J)=bvpFunc->GetVectorElementFA(x,I,J);   
    }
  }
  
  for (J=0; J<NDIM; J++) UPS(NTST,J)=bvpFunc->GetVectorElementFC(x,J);

  RLCUR=GetPeriod(x);     /* period  */
  for (K=0; K<2+nappend; K++) xp[NDIM+K]=bvpFunc->GetVectorElementFC(x,nphase+K);  /* active parameters */    

  staFunc->ClearVector(y);
						      						      
/* Generate FA :   */
  for (JJ=0; JJ<NTST; JJ++) {
    J=JJ;
    JP1=J+1;
    DT=TM(J+1)-TM(J);	/* global TM */
    DDT=1.0/DT;
    for (IC=0; IC<NCOL; IC++) {
      for (IB=0; IB<NCP1; IB++) WPLOC(IB,IC)=DDT*WP(IB,IC);
    }
    for (IC=0; IC<NCOL; IC++) {
      for (K=0; K<NDIM; K++) {
        u=WT(NCOL,IC)*UPS(JP1,K);
        for (L=0; L<NCOL; L++) {
          L1=L*NDIM+K;
          u+=WT(L,IC)*UPS(J,L1);
        }
	xp[K]=u;
      }
      hom_SystemRHS(xp, frhs);      /* xp, frhs - global */

      IC1=IC*NDIM;
      for (I=0; I<NDIM; I++) {
        /* multiplication by RLCUR=T */ 
        /* - -> + */
	fa=-RLCUR*F(I)+WPLOC(NCOL,IC)*UPS(JP1,I);
        for (K=0; K<NCOL; K++) {  
          K1=K*NDIM+I;
	  fa+=WPLOC(K,IC)*UPS(J,K1);	/* - -> + */ 
        }
	bvpFunc->SetVectorElementFA(y,JJ,IC1+I,fa);
      }
    }
  }
   
/*    Generate FC :   */    
/*    Boundary conditions :  */        
  if (NBC > 0) {
    for (I=0; I<NDIM; I++) {
      UBC0(I)=UPS(0,I);
      UBC1(I)=UPS(NTST,I);
    }
    bcps(x,ubc0,ubc1,fbc);
    for (I=0; I<NBC; I++) bvpFunc->SetVectorElementFC(y,I,FBC(I)); /* - -> + */
  }     
/*     Integral constraints :  */     
  if (NINT > 0) {
    for (JJ=0; JJ<NTST; JJ++) {
      J=JJ;
      JP1=J+1;
      for (K=0; K<NCP1; K++) {
        for (I=0; I<NDIM; I++) {
          I1=K*NDIM+I;
          J1=J;
          if (K == NCOL) {
            I1=I;
            J1=JP1;
          }
          UIC(I)=UPS(J1,I1);
          UIP(I)=UPOLDP(J1,I1); /* global UPOLDP is recomputed at each found point (see Default Proc)*/
        }
        icps(uic,uip,ficd);
        for (M=0; M<NINT; M++) {
	  bvpFunc->SetVectorElementFC(y,NBC+M,
	     bvpFunc->GetVectorElementFC(y,NBC+M)+(TM(J+1)-TM(J))*WI(K)*FICD(M));
	}   /* - -> + */
      }
    }
  }
  if (nappend) hom_DefUser(x,y+NDIM*NCOL*NTST+NDIM+1);

bvpFunc->PrintVector(y,"y:");

  return 0;
}


Local(void) wint(Vector wi) { 
/*
  Generates the weights for the integration formula based on polynomial
  interpolation at N equally spaced points in [0,1].
*/
VAR C;
  switch (NCOL) {
    case 2:
      C=1.0/6.0;
      WI(0)=C;
      WI(1)=4.0*C;
      WI(2)=C;
      break; 
    case 3:
      C=1.0/8.0;
      WI(0)=C;
      WI(1)=3.0*C;
      WI(2)=WI(1);
      WI(3)=C;
      break;
    case 4:
      C=1.0/90.0;
      WI(0)=7.0*C;
      WI(1)=32.0*C;
      WI(2)=12.0*C;
      WI(3)=WI(1);
      WI(4)=WI(0);
      break;
    case 5:
      WI(0)=19.0/288.0;
      WI(1)=25.0/96.0;
      WI(2)=25.0/144.0;
      WI(3)=WI(2);
      WI(4)=WI(1);
      WI(5)=WI(0);
      break;
    case 6:
      WI(0)=41.0/840.0;
      WI(1)=9.0/35.0;
      WI(2)=9.0/280.0;
      WI(3)=34.0/105.0;
      WI(4)=WI(2);
      WI(5)=WI(1);
      WI(6)=WI(0);
      break;
    case 7:
      WI(0)=751.0/17280.0;
      WI(1)=3577.0/17280.0;
      WI(2)=49.0/640.0;
      WI(3)=2989.0/17280.0;
      WI(4)=WI(3);
      WI(5)=WI(2);
      WI(6)=WI(1);
      WI(7)=WI(0);
  }
  return;
}
#define XM(i)	xm[i]
#define ZM(i)	zm[i]

Local(void) cpnts(Vector zm) {
/* Generates the collocation points with respect to [0,1] */
VAR C,C1,C2,C3,R;
  switch (NCOL) {
    case 2:
       C=0.5/sqrt(3.0);
       ZM(0)=.5-C;
       ZM(1)=.5+C;
       break;
    case 3:
       C=.5*sqrt(0.6);
       ZM(0)=.5-C;
       ZM(1)=.5;
       ZM(2)=.5+C;
       break;
    case 4:
       R=6.0/7.0;
       C=.5*sqrt(R*R-12.0/35.0);
       C1=.5*sqrt(3.0/7.0+C);
       C2=.5*sqrt(3.0/7.0-C);
       ZM(0)=.5-C1;
       ZM(1)=.5-C2;
       ZM(2)=.5+C2;
       ZM(3)=.5+C1;
       break;
    case 5:
       C1=.5*0.90617984593866399280;
       C2=.5*0.53846931010568309104;
       ZM(0)=.5-C1;
       ZM(1)=.5-C2;
       ZM(2)=.5;
       ZM(3)=.5+C2;
       ZM(4)=.5+C1;
       break;
    case 6:
       C1=.5*0.93246951420315202781;
       C2=.5*0.66120938646626451366;
       C3=.5*0.23861918608319690863;
       ZM(0)=.5-C1;
       ZM(1)=.5-C2;
       ZM(2)=.5-C3;
       ZM(3)=.5+C3;
       ZM(4)=.5+C2;
       ZM(5)=.5+C1;
       break;
    case 7:
       C1=.5*0.949107991234275852452;
       C2=.5*0.74153118559939443986;
       C3=.5*0.40584515137739716690;
       ZM(0)=.5-C1;
       ZM(1)=.5-C2;
       ZM(2)=.5-C3;
       ZM(3)=.5;
       ZM(4)=.5+C3;
       ZM(5)=.5+C2;
       ZM(6)=.5+C1;
  }
  return;
}

Local(void) cntdif(Vector D) {
/*
  Generates the coefficients of the central difference formula for
  Nth derivative at uniformly spaced points
               0 = x  < x  < ... < x  = 1.
                    0    1          N
*/
Int2 I,K,K1,N;
VAR SC;

  N=staFunc->GetVectorDim(D)-1;   /* number of the intervals */
  D[0]=1.0;
  if (N==0) return;

  for (I=0; I<N; I++) {
    D[I+1]=0;
    for (K=0; K<=I; K++) {
      K1=I+1-K;
      D[K1]=D[K1-1]-D[K1];
    }
    D[0]=-D[0];
  }
/* Scale to [0,1]  */

  SC=pow(N,N);
  for (I=0; I<N+1; I++) D[I]=SC*D[I];
}

Local(void) genwts(Matrix wt, Matrix wp) {
/* Generates weights of the collocation method. The user selected
   number of collocation points (ncol) must be one of {2,...,7}.

   The following weights are generated :
 
          WT : for the function value,
          WP : for the first derivative,
*/
Int2 I,IB,IC,K,L;
Vector zm,xm;
VAR DENOM,P,SUM,D;

  zm=staFunc->CreateVector(NCOL);
  xm=staFunc->CreateVector(NCP1);

/* Generate the collocation points : */
  cpnts(zm);

  D=1.0/NCOL;
  for (I=0; I<NCP1; I++) XM(I)=I*D;

/* Generate weights :  */
  for (IB=0; IB<NCP1; IB++) {
    for (K=0,DENOM=1.0; K<NCP1; K++) {
      if (K==IB) continue;
      DENOM*=(XM(IB)-XM(K));
    }
    for (IC=0; IC<NCOL; IC++) {
/* Weights for the function values : */
      for (K=0,P=1.0; K<NCP1; K++) {
        if (K==IB) continue;
        P*=(ZM(IC)-XM(K));
      }
      WT(IB,IC)=P/DENOM;
/* Weights for derivatives : */
      for (L=0,SUM=0.0; L<NCP1; L++) {
        if (L == IB) continue;
        for (K=0,P=1.0; K<NCP1; K++) {
          if (K==IB || K==L) continue;
          P*=(ZM(IC)-XM(K));
        }
        SUM+=P;
      }
      WP(IB,IC)=SUM/DENOM;
    }
  }
  staFunc->DestroyVector(zm);
  staFunc->DestroyVector(xm);
  return;
}
#undef XM
#undef ZM

/***************************************************/
/* Jacobian of defining functions for homoclinic orbits */

Entry(Int2) hom_JacDefHom (Vector x, bvpMatrix mp) {
/* translation of SETUBV */
Int2 I,J,K,K1,JJ,JP1,IB,IB1,IC,IC1,L,L1;
VAR  RLCUR,DT,DDT,u;  

#if MOIE
VARPTR pA,pAic,pAicib,pAicibi,pAe1,pAe;	/* pAe points to element mp->A(IB1+K,IC1+I,JJ) */
VARPTR PNTR aA;	/* A+I */
VARPTR aAi;	/* A[I] */
VAR factor;
int Ajn;
Int2 Aj;
#endif

/* Initialize */

  for (I=0; I<NTST; I++) {
    for (J=0; J<nphase*NCOL; J++) {
      UPS(I,J)=bvpFunc->GetVectorElementFA(x,I,J);   
    }
  }
  for (J=0; J<nphase; J++) UPS(NTST,J)=bvpFunc->GetVectorElementFC(x,J);
  
  RLCUR=GetPeriod(x);     /* period  */
printf("RLCUR=%g\n",RLCUR);
  for (K=0; K<2+nappend; K++) xp[nphase+K]=bvpFunc->GetVectorElementFC(x,nphase+K);  /* active parameters */    

  bvpFunc->ClearMatrix(mp);
  
/* Generate AA , BB : */

       for (JJ=0; JJ<NTST; JJ++) {
#if MOIE
	  pA=bvpFunc->GetMatrixA(mp,JJ,&Aj);
	  Ajn=Aj*NDIM;
#endif
          J=JJ;
          JP1=J+1;
          DT=TM(J+1)-TM(J);
          DDT=1.0/DT;
          for (IC=0; IC<NCOL; IC++) {
             for (IB=0; IB<NCP1; IB++) WPLOC(IB,IC)=DDT*WP(IB,IC);
          }
#if MOIE
	  pAic=pA;	/* pA+Aj*IC1 */
#endif
          for (IC=0; IC<NCOL; IC++) {
             for (K=0; K<NDIM; K++) {
                for (L=0,u=WT(NCOL,IC)*UPS(JP1,K); L<NCOL; L++) {
                   L1=L*NDIM+K;
                   u+=WT(L,IC)*UPS(J,L1);
                }
		xp[K]=u;
             }

             hom_SystemRHS(xp, frhs);      /* xp, frhs - global */
	     SystemJac(xp, A);   	  /* xp, A - global */

             IC1=IC*NDIM;
#if MOIE
	     pAicib=pAic;	/* pA+Aj*IC1+IB1 */
#endif
             for (IB=0; IB<NCP1; IB++) {
                IB1=IB*NDIM;
#if MOIE
		pAicibi=pAicib;	/* pA+Aj*IC1+IB1+Aj*I */
		factor=WT(IB,IC)*RLCUR;
		pAe1=pAicib;	/* pA+Aj*IC1+IB1+Aj*I+I */
		aA=A;
#endif
                for (I=0; I<NDIM; I++) {
#if MOIE
		   *pAe1=WPLOC(IB,IC);
		   pAe1+=Aj+1;
#else
                   bvpFunc->SetMatrixAElement(mp,IB1+I,IC1+I,JJ,WPLOC(IB,IC));
#endif
#if MOIE
		   pAe=pAicibi;	/* pA+Aj*IC1+IB1+I+K */
		   aAi=*aA;
#endif
                   for (K=0; K<NDIM; K++) {
#if MOIE
		      *pAe-=factor**aAi;
		      pAe++;
		      aAi++;
#else
		      u=bvpFunc->GetMatrixAElement(mp,IB1+K,IC1+I,JJ)-WT(IB,IC)*RLCUR*A[I][K];
                      bvpFunc->SetMatrixAElement(mp,IB1+K,IC1+I,JJ,u);
#endif
		      /* scaled by RLCUR=T */
                   }
#if MOIE
		  pAicibi+=Aj;
		  aA++;
#endif
                }
#if MOIE
		pAicib+=NDIM;	/* IB1 increment is NDIM */
#endif
             }

             for (I=0; I<NDIM; I++) {
	       bvpFunc->SetMatrixBElement(mp,0,IC1+I,JJ,-F(I));
/*  for (K=0; K<=nappend; K++) bvpFunc->SetMatrixBElement(mp,1+K,IC1+I,JJ,-RLCUR*A[I][NDIM+K]); */
             for (K=0; K<=nappend; K++) bvpFunc->SetMatrixBElement(mp,1+K,IC1+I,JJ,0);   /* Delta-spec */
	     }
#if MOIE
	  pAic+=Ajn;	/* IC1 increment is NDIM */
#endif
          }
       }
    
/*     Generate CC, DD :  */
/*     Boundary conditions : */

  if (NBC > 0) {
     for (I=0; I<NBC-1; I++) {
       bvpFunc->SetMatrixCElement(mp,I,I,0,1.0);                     /* 1D HOM SPECIFIC */
       bvpFunc->SetMatrixCElement(mp,NRA+I,nphase,NTST-1,W[I]);      /* 1D HOM SPECIFIC */
     }
  }     
    
/*     Integral constraints (phase condition): */

  if (NINT > 0) {
    for (JJ=0; JJ<NTST; JJ++) {
      J=JJ;
      for (K=0; K<NCP1; K++) {
	for (I=0; I<NDIM; I++) {
          K1=K*NDIM+I;
          bvpFunc->SetMatrixCElement(mp,K1,NBC,JJ,(TM(J+1)-TM(J))*WI(K)*UPOLDP(J,K1));
        }
      }
    }
  }
  /* DD=0 so not specified */
  bvpFunc->SetMatrixDElement(mp,1,nphase,-1.0);    /* 1D HOM SPECIFIC */
  if (nappend) {
    staFunc->Der(hom_DefUser, nappend, x, _dx, BQ);  /* BQ is global standard matrix */
    for (J=0; J<nappend; J++) {
      for (JJ=0; JJ<nphase*_ncol*_ntst+nphase+nfree; JJ++) x1[JJ]=BQ[J][JJ];
      for (I=0; I<_ncol*nphase; I++) 
        bvpFunc->SetMatrixCElement(mp,I,J+NBC+1,0,bvpFunc->GetVectorElementFA(x1,0,I));
      for (K=1; K<_ntst-1; K++) {
        for (I=0; I<nphase; I++) {
          bvpFunc->SetMatrixCElement(mp,I,J+NBC+1,K,0.5*bvpFunc->GetVectorElementFA(x1,K,I));
	  bvpFunc->SetMatrixCElement(mp,nphase*_ncol+I,J+NBC+1,K-1,0.5*bvpFunc->GetVectorElementFA(x1,K,I));
	}
        for (I=nphase; I<_ncol*nphase; I++)
          bvpFunc->SetMatrixCElement(mp,I,J+NBC+1,K,bvpFunc->GetVectorElementFA(x1,K,I));
        for (I=_ncol*nphase; I<(_ncol+1)*nphase; I++) {
          bvpFunc->SetMatrixCElement(mp,I,J+NBC+1,K,0.5*bvpFunc->GetVectorElementFA(x1,K+1,I-nphase*_ncol));
          bvpFunc->SetMatrixCElement(mp,I-nphase*_ncol,J+NBC+1,K+1,0.5*bvpFunc->GetVectorElementFA(x1,K+1,I-nphase*_ncol));
	}
    }
    for (I=0; I<nphase; I++) {
      bvpFunc->SetMatrixCElement(mp,I,J+NBC+1,_ntst-1,0.5*bvpFunc->GetVectorElementFA(x1,_ntst-1,I));
      bvpFunc->SetMatrixCElement(mp,nphase*_ncol+I,J+NBC+1,_ntst-2,0.5*bvpFunc->GetVectorElementFA(x1,_ntst-1,I));
    }
    for (I=nphase; I<_ncol*nphase; I++)
      bvpFunc->SetMatrixCElement(mp,I,J+NBC+1,_ntst-1,bvpFunc->GetVectorElementFA(x1,_ntst-1,I));
    for (I=_ncol*nphase; I<(_ncol+1)*nphase; I++)
      bvpFunc->SetMatrixCElement(mp,I,J+NBC+1,_ntst-1,bvpFunc->GetVectorElementFC(x1,I-nphase*_ncol));
    for (I=0; I<nfree; I++)
      bvpFunc->SetMatrixDElement(mp,I,J+NBC+1,bvpFunc->GetVectorElementFC(x1,I+nphase));
    }
  } 

bvpFunc->PrintMatrix(mp,"mp:",0);

  return 0;
}

#ifdef WIN32
static int _USERENTRY hom_SortMult(const void *f, const void *s) {
#else
Entry(int) hom_SortMult(const void *f, const void *s) {
#endif
VAR Mod1,Mod2;
  Mod1=((FloatHiPtr)f)[0]*((FloatHiPtr)f)[0]+((FloatHiPtr)f)[1]*((FloatHiPtr)f)[1];
  Mod2=((FloatHiPtr)s)[0]*((FloatHiPtr)s)[0]+((FloatHiPtr)s)[1]*((FloatHiPtr)s)[1];
  if (Mod1==Mod2) return 0;
  return (Mod1<Mod2) ? 1 : -1;
}

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

Entry(Int2) hom_ProcDefault(Int2 Ind, Vector x, Vector v1, CharPtr *msg) {
/* compute multipliers (not implemented) and UPOLDP (see STUPS in autlib1.f) */
Int2 I,J,NC1,N1,K,K1,K2;
Int2 IC,JP1;
Int2 Ret;
VAR RLCUR,T,RN,S,SJ,RMNUPS,RMXUPS;
/* Assert: msg==NULL */
  if (Ind == -1) {   /* zero of a user-defined function */
    stagenData->BifDataOutLen=2*sizeof(FloatHi);
    BifVector[0]=(VAR)_ntst;
    BifVector[1]=(VAR)_ncol;
    return 0;
  }

/* Compute UPS */
  for (I=0; I<NTST; I++) {
    for (J=0; J<NDIM*NCOL; J++) {
      UPS(I,J)=bvpFunc->GetVectorElementFA(x,I,J);   
    }
  }
  for (J=0; J<NDIM; J++) UPS(NTST,J)=bvpFunc->GetVectorElementFC(x,J);

/* print UPS  
printf("\n T, UPS(T):\n");
  for (J=0; J<NTST; J++) {
    RN=1.0/NCOL;
    for (I=0; I<NCOL; I++) {
             T=TM(J)+I*RN*(TM(J+1)-TM(J));
             K1=I*NDIM;
             K2=(I+1)*NDIM;
	     printf("\n %.20lg",T);
	     for (K=K1; K<K2; K++) printf(" %.20lg",UPS(J,K));
    }
  }
printf("\n %.20lg",TM(NTST));
for (K=0; K<NDIM; K++) printf(" %.20lg",UPS(NTST,K));
*/
 
if (Ind==0 ) {
/* Compute UPOLDP */

  RLCUR=GetPeriod(x);   	/* period  */
  for (I=0; I<2+nappend; I++) xp[nphase+I]=bvpFunc->GetVectorElementFC(x,nphase+I); /* active parameters */

  for (J=0; J<NTST; J++) {
    RN=1.0/NCOL;
    for (I=0; I<NCOL; I++) {
       T=TM(J)+I*RN*(TM(J+1)-TM(J));
       K1=I*NDIM;
       K2=(I+1)*NDIM;
       for (K=K1; K<K2; K++) xp[K-K1]=UPS(J,K);
       hom_SystemRHS(xp, frhs);      
       for (K=K1; K<K2; K++) UPOLDP(J,K)=RLCUR*F(K-K1);
    }
  }
  for (K=0; K<NDIM; K++) xp[K]=UPS(NTST,K);
  hom_SystemRHS(xp, frhs); 
  for (K=0; K<NDIM; K++) UPOLDP(NTST,K)=RLCUR*F(K);
  
/* print UPOLDP:
printf("\n Default processor:\n T, UPOLDP(T):\n");
  for (J=0; J<NTST; J++) {
    RN=1.0/NCOL;
    for (I=0; I<NCOL; I++) {
             T=TM(J)+I*RN*(TM(J+1)-TM(J));
             K1=I*NDIM;
             K2=(I+1)*NDIM;
	     printf("\n %.20lg",T);
	     for (K=K1; K<K2; K++) printf(" %.20lg",UPOLDP(J,K));
    }
  }
  printf("\n %.20lg",TM(NTST));
  for (K=0; K<NDIM; K++) printf(" %.20lg",UPOLDP(NTST,K));
  printf("\n");
*/
}
  /* L2_NORM */
  if (staData->L2NormComp) {
    for (S=0.0,IC=0; IC<NDIM; IC++) {
      for (J=0; J<NTST; J++) {
        JP1=J+1;
	for (SJ=0.0,K=0; K<NCOL; K++) {
          K1=K*NDIM+IC;
          SJ+=WI(K)*UPS(J,K1)*UPS(J,K1);
        }
        SJ+=WI(NCOL)*UPS(JP1,IC)*UPS(JP1,IC);
        S+=(TM(J+1)-TM(J))*SJ;
      }
    }
    StdFun[0]=sqrt(S);
  }
  /* MIN&MAX */
  if (staData->MinMaxComp) {
    for (I=0; I<NDIM; I++) {
      RMNUPS=UPS(0,I);
      RMXUPS=UPS(0,I);
      for (J=0; J<NTST; J++) {
        for (K=0; K<NCOL; K++) {
           K1=K*NDIM+I;
           if (UPS(J,K1) < RMNUPS) RMNUPS=UPS(J,K1);
           if (UPS(J,K1) > RMXUPS) RMXUPS=UPS(J,K1);
        }
      }
      if (UPS(NTST,I) < RMNUPS) RMNUPS=UPS(NTST,I);
      if (UPS(NTST,I) > RMXUPS) RMXUPS=UPS(NTST,I);
      StdFun[1+I]=RMNUPS;
      StdFun[1+NDIM+I]=RMXUPS;
    }
  }
  /* Compute and decompose Jacobian (D,D1 are global) */
  bvpFunc->ClearMatrix(D);
  hom_JacDefHom(x,D);
  bvpFunc->AppendMatrixVector(D,v1,D1);
  Ret=bvpFunc->Decomp(D1); 
  
  MemCopy(x1,x,sizeof(VAR)*(_ntst*nphase*_ncol+nphase+nfree));     /* for the last point */

  return Ret;
}

/*********************************/
/* Test and processing functions */

Local(Char) buf[80];

Entry(Int2) hom_TestDummy(Vector x, Vector v,  FloatHiPtr res) {
  *res=1.0;
  test[0]=*res;
  return 0;
}

Entry(Int2) hom_ProcDummy(Int2 Ind, Vector x, Vector v1, CharPtr *msg) {
  stagenData->BifDataOutLen=0;
  *msg=buf;
  return 0;
}


/***********************/
/* Adaptation routines */

Local(void) ordr (Vector tm, Vector TM1, Vector ITM1) {
/*
  tm and TM1 are two ascending arrays with values in [0,1]. On exit the
  value of ITM1(i) specifies the index of the TM-interval in which
  TM1(i) lies.
*/
Int2 K0,K1,J,J1,N,N1;
  N=staFunc->GetVectorDim(tm);
  N1=staFunc->GetVectorDim(TM1);
  K0=1;
  for (J1=0; J1<N1; J1++) {
    for (J=K0; J<N; J++) {
      K1=J;
      if (TM1[J1]<TM(J)) break;
    }
    ITM1[J1]=K1-1;
    K0=K1;
  }
}
Local(void) intwts (VAR Z, Vector X, Vector WTS) {
/* 
  Generates weights for Lagrange interpolation 
*/
Int2 N,K,IB;
VAR P,DENOM;
  N=staFunc->GetVectorDim(X);
  for (IB=0; IB<N; IB++) {
    P=1.0;
    DENOM=1.0;
    for (K=0; K<N; K++) {
      if (K!=IB) {
        P*=(Z-X[K]);
        DENOM*=(X[IB]-X[K]);
      }
    }
    WTS[IB]=P/DENOM;
  }
}

#undef NCP1

Local(void) interp (Int2 NC, Vector tm, Matrix ups, Int2 NC1, Vector TM1, Matrix UPS1) {
/*
  Finds interpolant (TM(.) , UPS(.) ) on new mesh TM1.
*/
Vector X,W,TM2,ITM1;
Int2 I,J,J1,K,K1,L,L1,NCP1,N,N1,N1M1;
VAR RI,D,Z;

  N=staFunc->GetVectorDim(tm);
  N1=staFunc->GetVectorDim(TM1);

  NCP1=NC+1;
  N1M1=N1-1;
  
  TM2=staFunc->CreateVector(N1M1);
  ITM1=staFunc->CreateVector(N1M1);
  X=staFunc->CreateVector(NCP1);
  W=staFunc->CreateVector(NCP1);
  
  for (I=0; I<NC1; I++) {
    RI=I;
    D=RI/NC1;
    for (J1=0; J1<N1M1; J1++) TM2[J1]=TM1[J1]+D*(TM1[J1+1]-TM1[J1]);
    ordr (tm,TM2,ITM1);
    for (J1=0; J1<N1M1; J1++) {
       J=ITM1[J1];
       Z=TM2[J1];
       D=(TM(J+1)-TM(J))/NC;
       for (L=0; L<NCP1; L++) X[L]=TM(J)+L*D;
       intwts (Z,X,W);
       for (K=0; K<NDIM; K++) {
         K1=I*NDIM+K;
         UPS1[J1][K1]=W[NC]*UPS(J+1,K);
	 for (L=0; L<NC; L++) {
           L1=K+L*NDIM;
           UPS1[J1][K1]=UPS1[J1][K1]+W[L]*UPS(J,L1);
        }
      }
    }
  }
  for (I=0; I<NDIM; I++) UPS1[N1-1][I]=UPS(N-1,I);

  staFunc->DestroyVector(TM2);
  staFunc->DestroyVector(ITM1);
  staFunc->DestroyVector(X);
  staFunc->DestroyVector(W);
}

#define HMACH 	1.0e-7
#define RSMALL 	1.0e-30
#define RLARGE	1.0e+30


Local(void) eqdf (Vector tm, Matrix ups, Vector EQF) {
Int2 I,J,K,K1,JP1;
Int2 SMALL;
Int2 lntst,lncol;
VAR SC,E,PWR,DTAV;
Vector WH;
Matrix HD;

  lntst=staFunc->GetVectorDim(tm)-1;
  lncol=staFunc->GetColNum(ups)/NDIM;
  WH=staFunc->CreateVector(lncol+1);
  HD=staFunc->CreateMatrix(lntst+1,NDIM);

/* Compute approximation to NCOL-th derivative : */
  cntdif(WH);

  SMALL=1;
  for (J=0; J<lntst; J++) {
    JP1=J+1;
    SC=1.0/pow((TM(J+1)-TM(J)),NCOL);
    for (I=0; I<NDIM; I++) {
      HD[J][I]=WH[lncol]*UPS(JP1,I);
      for (K=0; K<lncol; K++) {
             K1=I+K*NDIM;
             HD[J][I]=HD[J][I]+WH[K]*UPS(J,K1);
      }
      HD[J][I]=SC*HD[J][I];
      if (fabs(HD[J][I])>=HMACH) SMALL=0;
    }
  }
/* Take care of "small derivative" case */
  if (SMALL) {
    for (I=0; I<lntst+1; I++) EQF[I]=I;
    goto end;
  }

/* Extend by extrapolation */    
  for (I=0; I<NDIM; I++) HD[lntst][I]=2*HD[lntst-1][I]-HD[lntst-2][I];

/* Compute approximation to (NCOL+1)-st derivative */
  for (J=0; J<lntst; J++) {
    JP1=J+1;
    if (J<lntst-1) {
      DTAV=0.5*(TM(J+2)-TM(J));
    } else {
      DTAV=TM(lntst)-TM(lntst-1);
    }
    SC=1.0/DTAV;
    for (I=0; I<NDIM; I++) {
      HD[J][I]=SC*(HD[JP1][I]-HD[J][I]);
    }
  }

/* Define the equidistribution function */
   PWR=1.0/(lncol+1);
   EQF[0]=0.0;
   for (J=0; J<lntst; J++) {
     for (I=0, E=0.0; I<NDIM; I++) E+=pow(fabs(HD[J][I]),PWR);
     EQF[J+1]=EQF[J]+(TM(J+1)-TM(J))*E;
  }
end:
  staFunc->DestroyVector(WH);
  staFunc->DestroyMatrix(HD);
}

Local(void) newmsh (Matrix ups, Vector TMOLD, Vector TMNEW) {
/*
  Redistributes the mesh according to the function EQDF.
*/
Vector EQF,UNEQ,IAL;
Int2 NOLD,NNEW,J,J1;
VAR DAL,X;
  
  NOLD=staFunc->GetVectorDim(TMOLD);   /* NOLD=NTST+1 */
  NNEW=staFunc->GetVectorDim(TMNEW);

  UNEQ=staFunc->CreateVector(NNEW);
  IAL=staFunc->CreateVector(NNEW);
  EQF=staFunc->CreateVector(NOLD);

/* Put the values of the monotonely increasing function EQDF in EQF */
  eqdf (TMOLD, ups, EQF);

/* Uniformly divide the range of EQDF */
  DAL=EQF[NOLD-1]/(NNEW-1);
  for (J=0; J<NNEW; J++) UNEQ[J]=J*DAL ;

  ordr (EQF, UNEQ, IAL);

/* Generate the new mesh in TMNEW */
  for (J1=0; J1<NNEW; J1++) {
    J=IAL[J1];
    X=(UNEQ[J1]-EQF[J])/(EQF[J+1]-EQF[J]);
    TMNEW[J1]=(1.0-X)*TMOLD[J]+X*TMOLD[J+1];
  }
  staFunc->DestroyVector(UNEQ);
  staFunc->DestroyVector(IAL);
  staFunc->DestroyVector(EQF);
}

Local(void) adapt (Int2 NCOLD,  Int2 NCNEW, Vector tm, Matrix ups, Matrix vps) {
/*
  Adapts the distribution of the mesh points so that the increase of the
  monotone function EQDF becomes approximately equidistributed over the
  intervals. The functions UPS and VPS are interpolated on new mesh.
*/
Int2 NM;
Int2 i;
  /* Tint, Uint, Vint - global */

  NM=staFunc->GetVectorDim(tm);

  staFunc->ClearMatrix(Uint);
  staFunc->ClearMatrix(Vint);
  
  newmsh (ups, tm, Tint);
  for (i=0; i<NM; i++) Tint[i]=(tm[i]+Tint[i])/2;   /* demping */

  
  interp (NCOLD, tm, ups, NCNEW, Tint, Uint);
  interp (NCOLD, tm, vps, NCNEW, Tint, Vint); 
}
/**************/
/* Adaptation */
Entry(Int2) hom_Adapter(Vector x, Vector v) {
Int2 I,J,K,K1,K2;
VAR T,RN;

/* Generate UPS, VPS */
  for (I=0; I<NTST; I++) {
    for (J=0; J<NDIM*NCOL; J++) {
      UPS(I,J)=bvpFunc->GetVectorElementFA(x,I,J);   
    }
  }
  for (J=0; J<NDIM; J++) UPS(NTST,J)=bvpFunc->GetVectorElementFC(x,J);

/* print UPS 
printf("\n T, UPS(T) before Adapt:\n");
  for (J=0; J<NTST; J++) {
    RN=1.0/NCOL;
    for (I=0; I<NCOL; I++) {
             T=TM(J)+I*RN*(TM(J+1)-TM(J));
             K1=I*NDIM;
             K2=(I+1)*NDIM;
	     printf("\n %.10lg",T);
	     for (K=K1; K<K2; K++) printf(" %.10lg",UPS(J,K));
    }
  }
printf("\n %.10lg",TM(NTST));
for (K=0; K<NDIM; K++) printf(" %.10lg",UPS(NTST,K));
*/ 

  for (I=0; I<NTST; I++) {
    for (J=0; J<NDIM*NCOL; J++) {
      VPS(I,J)=bvpFunc->GetVectorElementFA(v,I,J);   
    }
  }
  for (J=0; J<NDIM; J++) VPS(NTST,J)=bvpFunc->GetVectorElementFC(v,J);

/* print VPS 
printf("\n T, VPS(T):\n");
  for (J=0; J<NTST; J++) {
    RN=1.0/NCOL;
    for (I=0; I<NCOL; I++) {
             T=TM(J)+I*RN*(TM(J+1)-TM(J));
             K1=I*NDIM;
             K2=(I+1)*NDIM;
	     printf("\n %g",T);
	     for (K=K1; K<K2; K++) printf(" %g",VPS(J,K));
    }
  }
printf("\n %g",TM(NTST));
for (K=0; K<NDIM; K++) printf(" %g",VPS(NTST,K));
*/

/* Adaptation */
  adapt (NCOL, NCOL, tm, ups, vps);   

/* Replace mesh and values */
  staFunc->CopyVector(Tint,tm);           /* Tint, Uint, Vint - global */
  staFunc->CopyMatrix(Uint,ups);
  staFunc->CopyMatrix(Vint,vps);

/* print UPS  
printf("\n T, UPS(T) after Adapt:\n");
  for (J=0; J<NTST; J++) {
    RN=1.0/NCOL;
    for (I=0; I<NCOL; I++) {
             T=TM(J)+I*RN*(TM(J+1)-TM(J));
             K1=I*NDIM;
             K2=(I+1)*NDIM;
	     printf("\n %.10lg",T);
	     for (K=K1; K<K2; K++) printf(" %.10lg",UPS(J,K));
    }
  }
printf("\n %.10lg",TM(NTST));
for (K=0; K<NDIM; K++) printf(" %.10lg",UPS(NTST,K));
*/

/* Replace x and v by the new ones */
  for (I=0; I<NTST; I++) {
    for (J=0; J<NDIM*NCOL; J++) {
      bvpFunc->SetVectorElementFA(x,I,J,UPS(I,J));
    }
  }
  for (J=0; J<NDIM; J++) bvpFunc->SetVectorElementFC(x,J,UPS(NTST,J));
  for (I=0; I<NTST; I++) {
    for (J=0; J<NDIM*NCOL; J++) {
      bvpFunc->SetVectorElementFA(v,I,J,VPS(I,J));
    }
  }
  for (J=0; J<NDIM; J++) bvpFunc->SetVectorElementFC(v,J,VPS(NTST,J));
  
  return (0);
}