/*
   __pd
   --> Limit cycle period doubling
*/

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

#include "stagen.h"

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

#include "__pd.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 */


/* 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_PERIOD	4
#define CL_MULT		5
#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(__pd_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 */
#define active1	active[0]
#define active2	active[1]
Local(Int2) npar,nphase;		/* dims of par and phase spaces */
Local(Int2) nappend;			/* number of user functions */
Local(Int2) nbc,nint,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) M=NULL;		/* current set of multipliers */
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(Matrix) rups=NULL,rvps=NULL;
Local(Matrix) A=NULL,B=NULL,BS=NULL,BM=NULL,BP=NULL,BQ=NULL;
Local(Vector) Re=NULL,Im=NULL,Mod=NULL,Arg=NULL;
Local(Vector) ubc0, ubc1, fbc;
Local(Vector) Tint;
Local(Matrix) Uint=NULL,Vint=NULL;
Local(Matrix) MM;

Local(bvpMatrix) D=NULL,D1=NULL;
Local(bvpMatrix) mp1=NULL,mp2=NULL,mp3=NULL;
Local(Vector) V1,W1;
Local(Vector) vv1,v2,w2;

#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 */
#define _delta  staData->delta  /* initial amplitude of the cycle */
#define _branch staData->branch     /* switch data  */


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

#define NDIM	nphase
#define NTST	_ntst
#define NBC	nbc
#define NINT	nint
#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]

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

#ifdef WIN32
static int _USERENTRY pd_SortCmp(const void *f, const void *s) {
#else
Entry(int) pd_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=(__pd_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 */
  staData->Active[npar]=staData->ActPeriod[0];
  active=MemNew(sizeof(Int2)*(3+nappend));  /* PD CYCLE SPECIFIC */

  nbc=nphase; 				/* CYCLE SPECIFIC */
  nint=1;				/* CYCLE SPECIFIC */
  nfree=nbc+nint-nphase+2+nappend;    	/* PD CYCLE SPECIFIC */

  /* Do all the checks related to active parameter and find it */
  if (staUtil->CheckParameters(staData->Active,npar+1,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 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);
    MemCopy(P,staData->P,sizeof(FloatHi)*npar);
    /* 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 */
    M=MemNew(sizeof(FloatHi)*4*nphase);
    Mod=staFunc->CreateVector(nphase);
    Arg=staFunc->CreateVector(nphase);
    Re=staFunc->CreateVector(nphase);
    Im=staFunc->CreateVector(nphase);
    StdFun=staFunc->CreateVector(1+2*nphase);
 
 /* Initialize BVP algebra package */
    bvpFunc->Init(staFunc,nphase,_ntst,_ncol,nbc,nint,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);
    BP=staFunc->CreateMatrix((nphase*(nphase-1))/2,(nphase*(nphase-1))/2);
    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(nint);

    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);

    V1=staFunc->CreateVector((_ntst)*nphase*(_ncol)+nphase);  /* null-function */
    W1=staFunc->CreateVector((_ntst)*nphase*(_ncol)+nphase);  /* adjoint null-function */
    vv1=staFunc->CreateVector((_ntst)*nphase*_ncol+nphase);
    v2=staFunc->CreateVector((_ntst)*nphase*_ncol+nphase+1);
    w2=staFunc->CreateVector((_ntst)*nphase*_ncol+nphase+1);
    MM=staFunc->CreateMatrix(1,_ntst*_ncol*nphase+nphase+nfree);
    bvpFunc->Init(staFunc,nphase,_ntst,_ncol,nbc,0,1);
    mp2=bvpFunc->CreateMatrix((_ntst)*nphase*_ncol+nphase,
                             (_ntst)*nphase*_ncol+nphase+1);
    mp3=bvpFunc->CreateMatrix((_ntst)*nphase*_ncol+nphase+1,
                             (_ntst)*nphase*_ncol+nphase+1);
    bvpFunc->Init(staFunc,nphase,_ntst,_ncol,nbc,nint,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);
      M=MemFree(M);
      Re=staFunc->DestroyVector(Re);
      Im=staFunc->DestroyVector(Im);
      StdFun=staFunc->DestroyVector(StdFun);
      Mod=staFunc->DestroyVector(Mod);
      Arg=staFunc->DestroyVector(Arg);
      Rhs=MemFree(Rhs);
/* destroy global arrays */
      A=staFunc->DestroyMatrix(A);
      B=staFunc->DestroyMatrix(B);
      BM=staFunc->DestroyMatrix(BM);
      BP=staFunc->DestroyMatrix(BP);
      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);
      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);
      mp1=bvpFunc->DestroyMatrix(mp1);
    }
  active=MemFree(active);
}

/**********************************************/
/* Starters to continue the period doubling 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) pd_DefPD (Vector x, Vector y);
Entry(Int2) pd_JacDefPD (Vector x, bvpMatrix mp);
Entry(Int2) pd_ProcDefault(Int2 Ind, Vector x, Vector v1, CharPtr *msg);
Local(Int2) SystemJac (Vector X, Matrix A);



Entry(Int2) pd_Starter(StagenDataPtr stagenptr) {
  Int2 I,IB,IB1,IC,IC1,J,JJ,JP1,K,K1,K2,L,L1;
  Int2 i,j;
  VAR RLCUR,DT,DDT,u,S,SJ,T,RN;
  Vector v1=NULL,DDP=NULL;
  Int2 rntst,rncol;
  Int2 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 SetInitCycle */
      staUtil->ErrMessage("No way to start from a user-supplied PD cycle");
      return 1;
    } else {	/* no SetInitCycle function */
      if (!stagenData->BifDataInOk) {
	  staUtil->ErrMessage("No period-doubling cycle.\n"
            "Pick up a computed PD-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,nint,nfree);

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

    v1=staFunc->CreateVector((rntst)*nphase*rncol+nphase+nfree);

      /* v1 - critical solution to variational BVP */
      MemCopy(v1,stagenData->BifDataInPtr+2+(rntst*rncol+1)*nphase+2,
                                            sizeof(VAR)*(rntst*rncol+1)*nphase);
      if (rntst!=_ntst || rncol!=_ncol) { 
        /* Interpolation */
        DDP=staFunc->CreateVector(1+nappend);
        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,nint,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]);
        }
        bvpFunc->SetVectorElementFC(x0,nphase,staData->Period[0]);
        for (i=0; i<=nappend+1; i++) bvpFunc->SetVectorElementFC(x0,nphase+1+i,staData->P[active[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];
      }

   /* setup global V1 */   
        for (i=0; i<_ntst; i++) {
          for (j=0; j<nphase*_ncol; j++) {
            bvpFunc->SetVectorElementFA(V1,i,j,bvpFunc->GetVectorElementFA(v0,i,j));
          }
        }
        for (j=0; j<nphase; j++) {
          bvpFunc->SetVectorElementFC(V1,j,bvpFunc->GetVectorElementFC(v0,j));
        }
   
  /* compute the adjoint null-vector W1 */
  /* initialize bvp algebra with nint=nfree=0 */
     bvpFunc->Init(staFunc,nphase,_ntst,_ncol,nbc,0,0);
     mp1=bvpFunc->CreateMatrix((_ntst)*nphase*_ncol+nphase,(_ntst)*nphase*_ncol+nphase);
     bvpFunc->ClearMatrix(mp1);

  /* Initialize */
  for (I=0; I<NTST; I++) {
    for (J=0; J<nphase*NCOL; J++) {
      UPS(I,J)=bvpFunc->GetVectorElementFA(x0,I,J);   
    }
  }
  for (J=0; J<nphase; J++) UPS(NTST,J)=bvpFunc->GetVectorElementFC(x0,J);
  RLCUR=bvpFunc->GetVectorElementFC(x0,nphase);       /* period  */
  for (K=0; K<=nappend+1; K++) xp[nphase+K]=bvpFunc->GetVectorElementFC(x0,nphase+K+1); /* active parameters */

  /* Generate the matrix of adjoint equation*/
  /* Generate AA : */
       for (JJ=0; JJ<NTST; JJ++) {
          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);
          }
          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;
             }

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

             IC1=IC*NDIM;
             for (IB=0; IB<NCP1; IB++) {
                IB1=IB*NDIM;
                for (I=0; I<NDIM; I++) {
                   bvpFunc->SetMatrixAElement(mp1,IB1+I,IC1+I,JJ,WPLOC(IB,IC));
                   for (K=0; K<NDIM; K++) {
		      u=bvpFunc->GetMatrixAElement(mp1,IB1+K,IC1+I,JJ)+WT(IB,IC)*RLCUR*A[K][I];
                      bvpFunc->SetMatrixAElement(mp1,IB1+K,IC1+I,JJ,u);
		      /* scaled by T=RLCUR */
                   }
                }
             }
          }
       }
    
  /*     Generate CC */
  /*     Boundary conditions : */
  if (NBC > 0) {
     for (I=0; I<NBC; I++) {
       bvpFunc->SetMatrixCElement(mp1,I,I,0,1.0);
       bvpFunc->SetMatrixCElement(mp1,NRA+I,I,NTST-1,1.0);
     }
  }
  /* compute the adjoint null-vector W1 */

  staFunc->ClearVector(W1);
  bvpFunc->SingSolve(mp1,W1);

  /* make L2-norm of W1 equal to 1.0 */
  for (I=0; I<NTST; I++) {
    for (J=0; J<nphase*NCOL; J++) {
      UPS(I,J)=bvpFunc->GetVectorElementFA(W1,I,J);   
    }
  }
  for (J=0; J<nphase; J++) UPS(NTST,J)=bvpFunc->GetVectorElementFC(W1,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));
*/

  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;
      }
  }
  staFunc->MultiplyVectorScalar(W1,1.0/S,W1);

    /* Specify the direction vector */
    /* Reinitialize BVP algebra package with the new _ntst & _ncol */
    bvpFunc->Init(staFunc,nphase,_ntst,_ncol,nbc,nint,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(v1);
    staFunc->ClearVector(v0);
    for (j=(_ntst)*nphase*_ncol+nphase+nfree-1; j>=0; j--) {
      v1[j]=1.0;
      Ret=pd_ProcDefault(0, x0, v1, NULL); /* Setup UPOLDP */
      if (Ret == 0) {
        bvpFunc->SetVectorElementFC(v0,nphase+2+nappend,stagenData->Forward ? 1 : -1);
        bvpFunc->Solve(D1,v0);
bvpFunc->PrintVector(v0,"v0:");
       break;
      };
      v1[j]=0.0;
    };
    if (Ret) {
       staUtil->ErrMessage(
       "Unable to find a tangent vector to the PD 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;
}

#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<=nappend+1; i++) P[active[i]]=xp[nphase+i];
}

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

Local(void) CopyToM(void) {
  MemCopy(M,Mod,sizeof(VAR)*nphase);
  MemCopy(M+nphase,Arg,sizeof(VAR)*nphase);
  MemCopy(M+2*nphase,Re,sizeof(VAR)*nphase);
  MemCopy(M+3*nphase,Im,sizeof(VAR)*nphase);
}
/***************************/
/* User-defined functions  */
Entry(Int2) pd_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) pd_SystemRHS(Vector xp, Vector y) {
  CopyToP(xp); 
  stagenData->Rhs(xp,P,&ZERO,y);	
  return 0;
}

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

Local(VAR) Time,period;

Entry(Int2) pd_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;
      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_MULT]=M;
      CopyToM();
      indirect[CL_PERIOD]=&period;
      period=bvpFunc->GetVectorElementFC((Vector)vpoint,nphase);
      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(pd_SystemRHS, nphase, X, _dx, A)) return(1);
  }
  return(0);
}

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

/*    Boundary conditions for periodic solution */
Local(void) bcps(Vector ubc0, Vector ubc1, Vector fbc) {
Int2 I;
  for (I=0; I<NDIM; I++) FBC(I)=UBC0(I)-UBC1(I);
  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 limit cycle PD  */
Entry(Int2) pd_DefPD (Vector x, Vector y) {

Int2 I,I1,J,J1,K,K1,K2,JJ,JP1,IB,IB1,IC,IC1,L,L1,M;
VAR  T,RN,S,SJ,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=bvpFunc->GetVectorElementFC(x,nphase);     /* period  */
  for (K=0; K<=nappend+1; K++) xp[NDIM+K]=bvpFunc->GetVectorElementFC(x,NDIM+1+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;
      }
      pd_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(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));
	}   /* - -> + */
      }
    }
  }
/*     Flip condition:    */
    bvpFunc->Init(staFunc,nphase,_ntst,_ncol,nbc,0,1);
    staFunc->ClearVector(v2);
    staFunc->CopyVectorElements(V1,0,v2,0,(_ntst)*nphase*_ncol+nphase);  /* V1 is global */

/*  Generate AA :  */
       for (JJ=0; JJ<NTST; JJ++) {
          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);
          }
          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;
             }

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

             IC1=IC*NDIM;
             for (IB=0; IB<NCP1; IB++) {
                IB1=IB*NDIM;
                for (I=0; I<NDIM; I++) {
                   bvpFunc->SetMatrixAElement(mp2,IB1+I,IC1+I,JJ,WPLOC(IB,IC));
                   for (K=0; K<NDIM; K++) {
		      u=bvpFunc->GetMatrixAElement(mp2,IB1+K,IC1+I,JJ)-WT(IB,IC)*RLCUR*A[I][K];
                      bvpFunc->SetMatrixAElement(mp2,IB1+K,IC1+I,JJ,u);
		      /* scaled by RLCUR=T */
                   }
                }
             }
          }
       }
    
/*  Generate CC   */
/*  Boundary conditions :  */
  if (NBC > 0) {
     for (I=0; I<NBC; I++) {
       bvpFunc->SetMatrixCElement(mp2,I,I,0,1.0);
       bvpFunc->SetMatrixCElement(mp2,NRA+I,I,NTST-1,1.0);
     }
  }
  bvpFunc->AppendMatrixVector(mp2,v2,mp3);

  for (I=0; I<NTST; I++) {
    for (J=0; J<nphase*NCOL; J++) {
      bvpFunc->SetMatrixBElement(mp3,0,J,I,bvpFunc->GetVectorElementFA(W1,I,J));   /* W1 is global */
    }
  }
  for (J=0; J<nphase; J++) bvpFunc->SetMatrixDElement(mp3,0,J,bvpFunc->GetVectorElementFC(W1,J));
  
  w2[(_ntst)*nphase*_ncol+nphase]=1.0;
  bvpFunc->Decomp(mp3);
  bvpFunc->Solve(mp3,w2);

  y[NDIM*NCOL*NTST+NDIM+1]=w2[(_ntst)*nphase*_ncol+nphase];
  bvpFunc->Init(staFunc,nphase,_ntst,_ncol,nbc,nint,nfree);
  
/*     Appended user functions   */  
  if (nappend) pd_DefUser(x,y+NDIM*NCOL*NTST+NDIM+2);
 
  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

/***************************************************/
Entry(Int2) lc_G (Vector x, Vector y) {
Int2 I,IB,IB1,IC,IC1,J,JJ,JP1,K,K1,K2,L,L1;
Int2 i,j;
VAR RLCUR,DT,DDT,u,S,SJ,T,RN;

  /* 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=bvpFunc->GetVectorElementFC(x,nphase);       /* period  */

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

  bvpFunc->Init(staFunc,nphase,_ntst,_ncol,nbc,0,0);
  bvpFunc->ClearMatrix(mp1);
  /* Generate AA : */
       for (JJ=0; JJ<NTST; JJ++) {
          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);
          }
          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;
             }

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

             IC1=IC*NDIM;
             for (IB=0; IB<NCP1; IB++) {
                IB1=IB*NDIM;
                for (I=0; I<NDIM; I++) {
                   bvpFunc->SetMatrixAElement(mp1,IB1+I,IC1+I,JJ,WPLOC(IB,IC));
                   for (K=0; K<NDIM; K++) {
		      u=bvpFunc->GetMatrixAElement(mp1,IB1+K,IC1+I,JJ)-WT(IB,IC)*RLCUR*A[I][K];
                      bvpFunc->SetMatrixAElement(mp1,IB1+K,IC1+I,JJ,u);
		      /* scaled by RLCUR=T */
                   }
                }
             }
          }
       }
    
  /*     Generate CC */
  /*     Boundary conditions : */
  if (NBC > 0) {
     for (I=0; I<NBC; I++) {
       bvpFunc->SetMatrixCElement(mp1,I,I,0,1.0);
       bvpFunc->SetMatrixCElement(mp1,NRA+I,I,NTST-1,1.0);
     }
  }
  bvpFunc->MultiplyMatrixVector(mp1,V1,vv1);   /* V1,W1 are assumed to be global */
  y[0]=-staFunc->ScalProd(W1,vv1)/staFunc->ScalProd(V1,W1);
  bvpFunc->Init(staFunc,nphase,_ntst,_ncol,nbc,nint,nfree);
  return 0;
}
/* Jacobian of defining functions for limit cycle PD */

Entry(Int2) pd_JacDefPD (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;  

/* 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=bvpFunc->GetVectorElementFC(x,nphase);     /* period  */
  for (K=0; K<=nappend+1; K++) xp[NDIM+K]=bvpFunc->GetVectorElementFC(x,NDIM+1+K);  /* active parameters */    

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

       for (JJ=0; JJ<NTST; JJ++) {
          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);
          }
          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;
             }

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

             IC1=IC*NDIM;
             for (IB=0; IB<NCP1; IB++) {
                IB1=IB*NDIM;
                for (I=0; I<NDIM; I++) {
                   bvpFunc->SetMatrixAElement(mp,IB1+I,IC1+I,JJ,WPLOC(IB,IC));
                   for (K=0; K<NDIM; K++) {
		      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);
		      /* scaled by RLCUR=T */
                   }
                }
             }

             for (I=0; I<NDIM; I++) {
	       bvpFunc->SetMatrixBElement(mp,0,IC1+I,JJ,-F(I));
	       for (K=0; K<=nappend+1; K++) 
	         bvpFunc->SetMatrixBElement(mp,1+K,IC1+I,JJ,-RLCUR*A[I][NDIM+K]);
	     }
          }
       }
    
/*     Generate CC, DD :  */
/*     Boundary conditions : */

  if (NBC > 0) {
     for (I=0; I<NBC; I++) {
       bvpFunc->SetMatrixCElement(mp,I,I,0,1.0);
       bvpFunc->SetMatrixCElement(mp,NRA+I,I,NTST-1,-1.0);
     }
  }     
    
/*     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));
        }
      }
    }
  }
  
/*     Flip bifurcation condition     */

  staFunc->Der(lc_G, 1, x, _dx, MM);    /* gradient computation */
  
      for (JJ=0; JJ<nphase*_ncol*_ntst+nphase+nfree; JJ++) x1[JJ]=MM[0][JJ];
      for (I=0; I<_ncol*nphase; I++) 
        bvpFunc->SetMatrixCElement(mp,I,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,NBC+1,K,0.5*bvpFunc->GetVectorElementFA(x1,K,I));
	  bvpFunc->SetMatrixCElement(mp,nphase*_ncol+I,NBC+1,K-1,0.5*bvpFunc->GetVectorElementFA(x1,K,I));
	}
        for (I=nphase; I<_ncol*nphase; I++)
          bvpFunc->SetMatrixCElement(mp,I,NBC+1,K,bvpFunc->GetVectorElementFA(x1,K,I));
        for (I=_ncol*nphase; I<(_ncol+1)*nphase; I++) {
          bvpFunc->SetMatrixCElement(mp,I,NBC+1,K,0.5*bvpFunc->GetVectorElementFA(x1,K+1,I-nphase*_ncol));
          bvpFunc->SetMatrixCElement(mp,I-nphase*_ncol,NBC+1,K+1,0.5*bvpFunc->GetVectorElementFA(x1,K+1,I-nphase*_ncol));
	}
    }
    for (I=0; I<nphase; I++) {
      bvpFunc->SetMatrixCElement(mp,I,NBC+1,_ntst-1,0.5*bvpFunc->GetVectorElementFA(x1,_ntst-1,I));
      bvpFunc->SetMatrixCElement(mp,nphase*_ncol+I,NBC+1,_ntst-2,0.5*bvpFunc->GetVectorElementFA(x1,_ntst-1,I));
    }
    for (I=nphase; I<_ncol*nphase; I++)
      bvpFunc->SetMatrixCElement(mp,I,NBC+1,_ntst-1,bvpFunc->GetVectorElementFA(x1,_ntst-1,I));
    for (I=_ncol*nphase; I<(_ncol+1)*nphase; I++)
      bvpFunc->SetMatrixCElement(mp,I,NBC+1,_ntst-1,bvpFunc->GetVectorElementFC(x1,I-nphase*_ncol));
    for (I=0; I<nfree; I++)
      bvpFunc->SetMatrixDElement(mp,I,NBC+1,bvpFunc->GetVectorElementFC(x1,I+nphase));


/* CC and DD if nappend */
  if (nappend) {
    staFunc->Der(pd_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+2,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+2,K,0.5*bvpFunc->GetVectorElementFA(x1,K,I));
	  bvpFunc->SetMatrixCElement(mp,nphase*_ncol+I,J+NBC+2,K-1,0.5*bvpFunc->GetVectorElementFA(x1,K,I));
	}
        for (I=nphase; I<_ncol*nphase; I++)
          bvpFunc->SetMatrixCElement(mp,I,J+NBC+2,K,bvpFunc->GetVectorElementFA(x1,K,I));
        for (I=_ncol*nphase; I<(_ncol+1)*nphase; I++) {
          bvpFunc->SetMatrixCElement(mp,I,J+NBC+2,K,0.5*bvpFunc->GetVectorElementFA(x1,K+1,I-nphase*_ncol));
          bvpFunc->SetMatrixCElement(mp,I-nphase*_ncol,J+NBC+2,K+1,0.5*bvpFunc->GetVectorElementFA(x1,K+1,I-nphase*_ncol));
	}
    }
    for (I=0; I<nphase; I++) {
      bvpFunc->SetMatrixCElement(mp,I,J+NBC+2,_ntst-1,0.5*bvpFunc->GetVectorElementFA(x1,_ntst-1,I));
      bvpFunc->SetMatrixCElement(mp,nphase*_ncol+I,J+NBC+2,_ntst-2,0.5*bvpFunc->GetVectorElementFA(x1,_ntst-1,I));
    }
    for (I=nphase; I<_ncol*nphase; I++)
      bvpFunc->SetMatrixCElement(mp,I,J+NBC+2,_ntst-1,bvpFunc->GetVectorElementFA(x1,_ntst-1,I));
    for (I=_ncol*nphase; I<(_ncol+1)*nphase; I++)
      bvpFunc->SetMatrixCElement(mp,I,J+NBC+2,_ntst-1,bvpFunc->GetVectorElementFC(x1,I-nphase*_ncol));
    for (I=0; I<nfree; I++)
      bvpFunc->SetMatrixDElement(mp,I,J+NBC+2,bvpFunc->GetVectorElementFC(x1,I+nphase));
    }
  } 
  return 0;
}

#ifdef WIN32
static int _USERENTRY pd_SortMult(const void *f, const void *s) {
#else
Entry(int) pd_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) pd_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=bvpFunc->GetVectorElementFC(x,NDIM);   	/* period  */
  for (I=0; I<=nappend+1; I++) xp[NDIM+I]=bvpFunc->GetVectorElementFC(x,NDIM+1+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);
       pd_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);
  pd_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);
  pd_JacDefPD(x,D);
  bvpFunc->AppendMatrixVector(D,v1,D1);
  Ret=bvpFunc->Decomp(D1); 
  
/* Multipliers */
   if (staData->multcomp) { 
    for (I=0; I<nphase; I++) {
      for (J=0; J<nphase; J++) {
        B[I][J]=bvpFunc->GetMatrixP1Element(D1,I,J);
      }
    }
    staFunc->Decomp(B);
    for (J=0; J<nphase; J++) {
      for (I=0; I<nphase; I++) {
        frhs[I]=-bvpFunc->GetMatrixP0Element(D1,I,J);
      }
      staFunc->Solve(B,frhs);
      for (I=0; I<nphase; I++) {
        BM[I][J]=frhs[I];
      }
    }
    staFunc->EigenValues(BM,Re,Im);
    for (I=0; I<nphase; I++) {
      M[2*I]=Re[I];
      M[2*I+1]=Im[I];
    }
    qsort(M,nphase,2*sizeof(VAR),pd_SortMult);
    for (I=0; I<nphase; I++) {
      Re[I]=M[2*I];
      Im[I]=M[2*I+1];
    }
    for (I=0; I<nphase; I++) {
      Mod[I]=sqrt(Re[I]*Re[I]+Im[I]*Im[I]);
      if (fabs(Re[I]/Mod[I]) > 0.5) {
        Arg[I]=atan(fabs(Im[I]/Re[I]));
      } else {
        Arg[I]=PID2-atan(fabs(Re[I]/Im[I]));
      }
      if (Re[I] < 0) Arg[I]=PI-Arg[I];
      if (Im[I] < 0) Arg[I]=-Arg[I];
      Arg[I]*=SCALE;
    }
  }
  MemCopy(x1,x,sizeof(VAR)*(_ntst*nphase*_ncol+nphase+nfree));     /* for the last point */

  return Ret;
}

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

Local(Char) buf[80];

/*********************************/
/* Test and processing functions */
/* Dummy test function */
Entry(Int2) pd_TestDummy(Vector x, Vector v, FloatHiPtr res) {
int i;
  *res=1.0;
  test[0]=*res;
  return 0;
}

Entry(Int2) pd_ProcDummy(Int2 Ind, Vector x, Vector v, CharPtr *msg) {
  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 periodicity */    /* CYCLE SPECIFIC !!! */
  for (I=0; I<NDIM; I++) HD[lntst][I]=HD[0][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=0.5*(TM(lntst)-TM(lntst-1) + TM(1)-TM(0));
    }
    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) pd_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);
}