/* rd_int1.c - Implicit Euler integrator */
/* Last modified : September 25, 1998 */

#include <values.h>

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

#include "stagen.h"

#include "_linalg.h"
#include "_rdalg.h"

#include "_util.h"

#include "rd_int1.h"

#include "rd__o.h" 

/* Ptr to Data area passed to generator by content.
   Its structure is defined in stagen.h */
Local(StagenDataPtr) genData;
Local(StagenDataPtr) stagenData=NULL;
Local(rd__o_DataPtr) staData=NULL;

#define NumSing (stagen->ProcFuncNum-1)	/* 0-th is a default processor */

enum {          /* used as subscripts to stagenData->AuxFunc[] */
      RHS_I=-1,
      RHSD1_I,
      RHSD2_I,
      RHSD3_I,
      RHSD4_I,
      RHSD5_I,
      BC0_I,
      BC0D1_I,
      BC0D2_I,
      BC0D3_I,
      BC0D4_I,
      BC0D5_I,
      BC1_I,
      BC1D1_I,
      BC1D2_I,
      BC1D3_I,
      BC1D5_I
};
                                                                                                            
/* The pointer to Generator's functional parameters
   points to the following structure
   (names of functions must be listed in appropriate
    generator's description files under /genfunc section
    in proper order). */
typedef struct {
  PrintVectorPtr	PrintVector;
  CreateVectorPtr       CreateVector;
  DestroyVectorPtr      DestroyVector;
  GetVectorDimPtr       GetVectorDim;
  CopyVectorPtr         CopyVector;
  ScalProdPtr           ScalProd;
  NormOfVectorPtr	NormOfVector;
  NormalizeVectorPtr    NormalizeVector;
  AddScaledVectorPtr    AddScaledVector;
  CopyVectorElementsPtr CopyVectorElements;
} ContFuncPars, PNTR ContFuncParsPtr;

typedef EntryPtr(int,BCPtr,(VARPTR u, VARPTR ux, VARPTR p, VARPTR t, VARPTR f)); 
typedef EntryPtr(VARPTR,BCDPtr,(VARPTR u, VARPTR ux, VARPTR p, VARPTR t));
typedef EntryPtr(Int2,AdapterFuncPtr,(Vector xx, Vector yy));

/* Pointer to a structure containing pointers to functional parameters */
Local(ContFuncParsPtr) contFunc;

/* Pointer to a integrator's own data area */
Local(IntegrDataPtr) contData;

Local(StdLinAlgDataPtr) staFunc=NULL;
Local(RdLinAlgDataPtr) rdFunc=NULL;
Local(UtilitiesDataPtr) staUtil=NULL;   /* global copy of pointer to utility functions */
Local(VAR) zero=0.0;
Local(FloatHiPtr) P=NULL;

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

#define RHS        genData->DefFunc[0]

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

#define _Tint     contData->Tint	/* Interval of integration */
#define _epsuf    contData->epsuf       /* user-function tolerance  */

#define ActiveUTestFuns     contData->ActiveUserTest

					/* continuation data */
#define _h0       contData->h0      
#define _eps      contData->eps
#define _dx 	  contData->dx
#define _maxiter  contData->maxiter
#define _hmin     contData->hmin
#define _hmax     contData->hmax
#define _nadapt   contData->nadapt
#define nUTest    genData->udFuncNum
#define TestIter  contData->IterTestFuns

#define DefaultProcessor(p) ((ProcFuncPtr)genData->ProcFunc[0])(0,p,NULL,NULL);

Local(int) IndZero;     	/* zero test function index  */
Local(VAR) CurTint;		/* current end time */
Local(VAR) _h;
Local(VAR) _hlast;		/* last (uncutted) step */
Local(Boolean) NumDer1=TRUE;	/* TRUE-compute derivatives numericaly */
Local(int) nmesh, nphase, npar;
Local(int) np;                 /* number of points along curve segment */
Local(int) npc;                /* total number of points along curve */

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

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

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


/* Message Processing */

Local(Int2) MsgProc(Pointtype type, Vector p, char *text) {
  /* PostProcessing */
  return genData->ProcessPoint(type, p, text);
}

/***************/
/* StopCheck   */
Local(Boolean) LastPoint;

Local(int) Checker(Vector X) {
  if (LastPoint) {    
    if (genData->Forward) {
      CurTint+=fabs(_Tint);
    } else {
      CurTint-=fabs(_Tint);
    }
    return 1;	
  }

  _hlast=_h;
  if (X[nmesh*nphase] == CurTint) return 1;
  
  if (((_h > 0)&&(X[nmesh*nphase]+_h > CurTint))||((_h < 0)&&(X[nmesh*nphase]+_h < CurTint))) {
    _h=CurTint-X[nmesh*nphase];
    LastPoint=TRUE;
  }
  return 0;
}

Local(Int1Ptr) IndTestFuns;
Local(int) *Index;
Local(int) *IndexR;

Local(Vector) TestValues;           	 /* values of User Functions  */
Local(Vector) *SuspiciousPoints;
Local(Vector) X,Xm,Xp,X4,X4p,X4m,F,F4,F4p,F4m,Fm,Fp,Fn,Fv,Fo,Fs,H4,Z,XM,XMT,U_0,U_X,U_XX,U_Xm,U_Xp; /* work vectors */
Local(VAR) Xi, delta;
Local(Matrix) JAC,JacV, JacW, fi;
Local(rdMatrix) J;

Local(void) Term (void) {
Int2 i;
  MemFree(IndTestFuns);
  for (i=0; i<nUTest; i++) {
    contFunc->DestroyVector(SuspiciousPoints[i]);
  }
  MemFree(SuspiciousPoints);
  MemFree(Index);
  MemFree(IndexR);
  if (P) MemFree(P);
  contFunc->DestroyVector(TestValues);
  contFunc->DestroyVector(X);
  contFunc->DestroyVector(X4);
  contFunc->DestroyVector(X4p);  
  contFunc->DestroyVector(X4m);  
  contFunc->DestroyVector(F);
  contFunc->DestroyVector(F4);  
  contFunc->DestroyVector(F4p);  
  contFunc->DestroyVector(F4m);  
  contFunc->DestroyVector(Fm);  
  contFunc->DestroyVector(Fp);  
  contFunc->DestroyVector(Fn);  
  contFunc->DestroyVector(Fv);    
  contFunc->DestroyVector(Fo);    
  staFunc->DestroyVector(Fs);  
  contFunc->DestroyVector(H4);    
  contFunc->DestroyVector(Z);
  contFunc->DestroyVector(XM);  
  contFunc->DestroyVector(XMT);    
  contFunc->DestroyVector(U_0);
  contFunc->DestroyVector(U_X);
  contFunc->DestroyVector(U_XX);
  contFunc->DestroyVector(Xp);  
  contFunc->DestroyVector(Xm);  
  contFunc->DestroyVector(U_Xp);  
  contFunc->DestroyVector(U_Xm);  
  JAC=staFunc->DestroyMatrix(JAC);  
  JacW=staFunc->DestroyMatrix(JacW);
  JacV=staFunc->DestroyMatrix(JacV);
  J=rdFunc->DestroyMatrix(J);
}		

/* Copy values of analitical derivatives to a[i],b[i],c[i] */
Local(void) CopyToMatrixU(Matrix tm, VARPTR sa, Int2 firstcol, Int2 rowlen) {
  Int2 i;
  for (i=0; i<nphase; i++)
    MemCopy(tm[i],sa+i*rowlen+firstcol,sizeof(VAR)*nphase);
}
        
/* f0(u,.,.,.) */
Entry(Int2) ie_BC0u(VARPTR u0, Vector f) {
 if (((BCPtr)(stagenData->AuxFunc[BC0_I]))(u0,U_X,P,&zero,f)) return 1;
 return 0;
}

/* f0(.,u_x,.,.) */
Entry(Int2) ie_BC0u_x(VARPTR  u_x0, Vector f) {
  if (((BCPtr)(stagenData->AuxFunc[BC0_I]))(X4,u_x0,P,&zero,f))        /* left boundary condition */
    return 1;
  return 0;
}
        
/* F(u,.,.,.,.,.) */
Entry(Int2)  ie_RHSu(VARPTR  u, Vector f) {
  if (stagenData->Rhs(u,U_X,U_XX,&Xi,P,&zero,f))                /* equation */
    return 1;
  return 0;
}
        
/* F(.,u_x,.,.,.,.) */
Entry(Int2)  ie_RHSu_x(VARPTR  u_x, Vector f) {
  if (stagenData->Rhs(U_0,u_x,U_XX,&Xi,P,&zero,f))              /* d(equation)/d(u') */
    return 1;
  return 0;
}
                
/* F(.,.,u_xx,.,.,.) */
Entry(Int2)  ie_RHSu_xx(VARPTR  u_xx, Vector f) {
  if (stagenData->Rhs(U_0,U_X,u_xx,&Xi,P,&zero,f))              /* d(equation)/d(u'') */
    return 1;
  return 0;
}

/* f1(u,.,.,.) */  
Entry(Int2) ie_BC1u(VARPTR  u1, Vector f) {
  if (((BCPtr)(stagenData->AuxFunc[BC1_I]))(u1,U_X,P,&zero,f))  /* right boundary condition */
    return 1;
  return 0;
}
        
/* f1(.,u_x,.,.) */
Entry(Int2) ie_BC1u_x(VARPTR  u_x1, Vector f) {
  if (((BCPtr)(stagenData->AuxFunc[BC1_I]))(X4+(nmesh-1)*nphase,u_x1,P,&zero,f)) 
    return 1;
  return 0;
}

/* Euler modification */
Local(Int2) OneStep(VAR h) {	/* X -> X4 */
   Int2 i,ipoint,j,k,b,c,Ret;
   Int2 N=nmesh-1, nspace=1; 
   VAR hi,Hi,si,etam,eta,etap,phim,phi,phip,psim,psi,psip,phil0,phil1,phil2,phirN,phirN1,phirN2; 
   VARPTR dm;
   Matrix ma,mb,mc,md;

/* calculation of F in u */
   RHS(X,F);

   staFunc->ClearVector(X4);
/* calculate u tildes (in internal points, i.e. 1..(nmesh-1)*nphase) 
   and store in U_0 */
   staFunc->ClearVector(U_0);
   c=1;
   for (i=1;i<N;i++,c+=nphase) {
     b=i*nphase;
     hi=XM[i]-XM[i-1];     
     Hi=XM[i+1]-XM[i];
     si=hi+Hi;
     etam=(hi-Hi)*(Hi+si)/(9*hi*si);
     eta=(hi+si)*(Hi+si)/(9*hi*Hi);
     etap=(Hi-hi)*(hi+si)/(9*Hi*si);
     for (j=0; j<nphase; j++) {
       U_0[j+c]=etam*X[b-nphase+j]+eta*X[b+j]+etap*X[b+nphase+j];
     }
   }
   U_0[nmesh*nphase-1]=X[nmesh*nphase-1];

   staFunc->CopyVector(X,X4); 
/* perform adaptation step in internal mesh points */
   for (i=0;i<(nmesh-2)*nphase;i++) {
     X4[i+nphase]=U_0[i+1]+h*F[nphase+i];
   }

/* increment time */
   X4[nmesh*nphase]=X[nmesh*nphase]+h;  
 
/* calculate x tildes (1..nmesh-2) and store in XMT */
   XMT[0]=XM[0];
   for (i=1; i<N; i++) {
     Hi=XM[i+1]-XM[i];
     hi=XM[i]-XM[i-1];
     XMT[i]=XM[i]+(Hi-hi)/3;
   }
   XMT[N]=XM[N];
   MemCopy(contData->X1,XMT,nmesh*sizeof(VAR));  

/* calculate u_x in left point */
   phil0=(2.0*XMT[0]-XMT[1]-XMT[2])/((XMT[1]-XMT[0])*(XMT[2]-XMT[0]));
   phil1=(XMT[2]-XMT[0])/((XMT[1]-XMT[0])*(XMT[2]-XMT[1]));
   phil2=(XMT[0]-XMT[1])/((XMT[2]-XMT[1])*(XMT[2]-XMT[0]));
   for (j=0; j<nphase; j++)
     U_X[j]=phil0*X[j]+phil1*X4[j+nphase]+phil2*X4[j+2*nphase];

/* create Jacobian Matrix */
   if (NumDer1) {
    contFunc->CopyVector(X,Xm);
    contFunc->CopyVector(X,Xp);
    contFunc->CopyVector(U_X,U_Xm);
    contFunc->CopyVector(U_X,U_Xp);
    for (i=0; i<nphase; i++) {
      for (j=0; j<nphase; j++) {
        Xm[j+1]-=_dx;
        Xp[j+1]+=_dx;
        U_Xm[j]=phil0*(X[j]-_dx)+phil1*X4[j+nphase]+phil2*X4[j+2*nphase];
        U_Xp[j]=phil0*(X[j]+_dx)+phil1*X4[j+nphase]+phil2*X4[j+2*nphase];
        if (((BCPtr)stagenData->AuxFunc[BC0_I])(Xm+1,U_Xm,P,&zero,Fm)) { exit; }       
        if (((BCPtr)stagenData->AuxFunc[BC0_I])(Xp+1,U_Xp,P,&zero,Fp)) { exit; }
        JAC[i][j]=(Fp[i]-Fm[i])/(_dx+_dx);
        Xm[j+1]+=_dx;
        Xp[j+1]-=_dx;
        U_Xm[j]=U_X[j];
        U_Xp[j]=U_X[j];
      } 
    }
   } else { /* symbolic derivatives */
     dm=((BCDPtr)(stagenData->AuxFunc[BC0D1_I]))(X,U_X,P,&zero);
     CopyToMatrixU(JacW,dm,0,2*nphase+npar);
     CopyToMatrixU(JAC,dm,nphase,2*nphase+npar);
     staFunc->AddScaledMatrix(JacW,JAC,phil0,JAC);
   } 
   Ret=staFunc->Decomp(JAC);
   if (Ret) {
     staUtil->ErrMessage("before recomp of u0 : No convergence in Newton iteration");
     Term();
     return 1;
   } 
 
/* perform recomputation by Newton on u0 */
   for (i=0;i<nphase;i++) {
     X4[i]=X[i];
   }
   staFunc->ClearVector(Fs);
   for (k=0;k<_maxiter;k++) {
     /* calculation of f0 */
     if (((BCPtr)stagenData->AuxFunc[BC0_I])(X4,U_X,P,&zero,Fs)) exit; 
     staFunc->Solve(JAC,Fs); 
     delta=staFunc->NormOfVector(Fs);
     for (j=0;j<nphase;j++) {
       X4[j]=X4[j]-Fs[j];
       U_X[j]=phil0*X4[j]+phil1*X4[j+nphase]+phil2*X4[j+2*nphase];
     }  
     if (delta < _eps) {
       break;
     }  
     if (NumDer1) {
      contFunc->CopyVector(X4,Xm);
      contFunc->CopyVector(X4,Xp);
      contFunc->CopyVector(U_X,U_Xm);
      contFunc->CopyVector(U_X,U_Xp);
      for (i=0; i<nphase; i++) {
        for (j=0; j<nphase; j++) {
          Xm[j+1]-=_dx;
          Xp[j+1]+=_dx;
          U_Xm[j]=phil0*(X4[j]-_dx)+phil1*X4[j+nphase]+phil2*X4[j+2*nphase];
          U_Xp[j]=phil0*(X4[j]+_dx)+phil1*X4[j+nphase]+phil2*X4[j+2*nphase];
          if (((BCPtr)stagenData->AuxFunc[BC0_I])(Xm+1,U_Xm,P,&zero,Fm)) exit;       
          if (((BCPtr)stagenData->AuxFunc[BC0_I])(Xp+1,U_Xp,P,&zero,Fp)) exit;
          JAC[i][j]=(Fp[i]-Fm[i])/(_dx+_dx);
          Xm[j+1]+=_dx;
          Xp[j+1]-=_dx;
          U_Xm[j]=U_X[j];
          U_Xp[j]=U_X[j];
        } 
      }
     } else { /* symbolic derivatives */
         dm=((BCDPtr)(stagenData->AuxFunc[BC0D1_I]))(X4,U_X,P,&zero);
         CopyToMatrixU(JacW,dm,0,2*nphase+npar);
         CopyToMatrixU(JAC,dm,nphase,2*nphase+npar);
         staFunc->AddScaledMatrix(JacW,JAC,phil0,JAC);
     } 
     Ret=staFunc->Decomp(JAC);
     if (Ret) {
      staUtil->ErrMessage("before u_x in right point : No convergence in Newton iteration");
      Term();
      return 1;
     } 
   }
   
/* calculate u_x in right point */
   phirN2=(XMT[N]-XMT[N-1])/((XMT[N-1]-XMT[N-2])*(XMT[N]-XMT[N-2]));
   phirN1=-(XMT[N]-XMT[N-2])/((XMT[N-1]-XMT[N-2])*(XMT[N]-XMT[N-1]));
   phirN=(2.0*(XMT[N]-XMT[N-1])+(XMT[N-1]-XMT[N-2]))/((XMT[N]-XMT[N-1])*(XMT[N]-XMT[N-2]));
   for (j=0; j<nphase; j++)
     U_X[j]=phirN2*X4[j+(nmesh-3)*nphase]+phirN1*X4[j+(nmesh-2)*nphase]+phirN*X[j+(nmesh-1)*nphase];

/* create Jacobian Matrix */
   if (NumDer1) {
    contFunc->CopyVector(X,Xm);
    contFunc->CopyVector(X,Xp);
    contFunc->CopyVector(U_X,U_Xm);
    contFunc->CopyVector(U_X,U_Xp);
    for (i=0; i<nphase; i++) {
      for (j=0; j<nphase; j++) {
        Xm[j+1]-=_dx;
        Xp[j+1]+=_dx;
        U_Xm[j]=phirN2*X4[j+(nmesh-3)*nphase]+phirN1*X4[j+(nmesh-2)*nphase]+phirN*(X[j+(nmesh-1)*nphase]-_dx);
        U_Xp[j]=phirN2*X4[j+(nmesh-3)*nphase]+phirN1*X4[j+(nmesh-2)*nphase]+phirN*(X[j+(nmesh-1)*nphase]+_dx);
        if (((BCPtr)stagenData->AuxFunc[BC1_I])(Xm+1,U_Xm,P,&zero,Fm)) exit;       
        if (((BCPtr)stagenData->AuxFunc[BC1_I])(Xp+1,U_Xp,P,&zero,Fp)) exit;
        JAC[i][j]=(Fp[i]-Fm[i])/(_dx+_dx);
        Xm[j+1]+=_dx;
        Xp[j+1]-=_dx;
        U_Xm[j]=U_X[j];
        U_Xp[j]=U_X[j];
      } 
    }
   } else { /* symbolic derivatives */
       dm=((BCDPtr)(stagenData->AuxFunc[BC1D1_I]))(X4+N*nphase,U_X,P,&zero);
       CopyToMatrixU(JacW,dm,0,2*nphase+npar);
       CopyToMatrixU(JAC,dm,nphase,2*nphase+npar);
       staFunc->AddScaledMatrix(JacW,JAC,phirN,JAC);
   } 
   Ret=staFunc->Decomp(JAC);
   if (Ret) {
      staUtil->ErrMessage("No convergence in Newton iteration");
      Term();
      return 1;
   } 

/* perform recomputation by Newton on uN */
   for (i=0;i<nphase;i++) {
     X4[(nmesh-1)*nphase+i]=X[(nmesh-1)*nphase+i];
   }
   staFunc->ClearVector(Fs);
   for (k=0;k<_maxiter;k++) {
     if (((BCPtr)stagenData->AuxFunc[BC1_I])(X4+(nmesh-1)*nphase,U_X,P,&zero,Fs))
      exit;
     staFunc->Solve(JAC,Fs); 
     delta=staFunc->NormOfVector(Fs);
     for (j=0;j<nphase;j++) {
       X4[j+(nmesh-1)*nphase]=X4[j+(nmesh-1)*nphase]-Fs[j];
       U_X[j]=phirN2*X4[j+(nmesh-3)*nphase]+phirN1*X4[j+(nmesh-2)*nphase]+phirN*X4[j+(nmesh-1)*nphase];
     }  
     if (delta < _eps) {
       break;
     }  
     if (NumDer1) {
      contFunc->CopyVector(X4,Xm);
      contFunc->CopyVector(X4,Xp);
      contFunc->CopyVector(U_X,U_Xm);
      contFunc->CopyVector(U_X,U_Xp);
      for (i=0; i<nphase; i++) {
        for (j=0; j<nphase; j++) {
          Xm[j+1]-=_dx;
          Xp[j+1]+=_dx;
          U_Xm[j]=phirN2*X4[j+(nmesh-3)*nphase]+phirN1*X4[j+(nmesh-2)*nphase]+phirN*(X4[j+(nmesh-1)*nphase]-_dx);
          U_Xp[j]=phirN2*X4[j+(nmesh-3)*nphase]+phirN1*X4[j+(nmesh-2)*nphase]+phirN*(X4[j+(nmesh-1)*nphase]+_dx);
          if (((BCPtr)stagenData->AuxFunc[BC1_I])(Xm+1,U_Xm,P,&zero,Fm)) exit;       
          if (((BCPtr)stagenData->AuxFunc[BC1_I])(Xp+1,U_Xp,P,&zero,Fp)) exit;
          JAC[i][j]=(Fp[i]-Fm[i])/(_dx+_dx);
          Xm[j+1]+=_dx;
          Xp[j+1]-=_dx;
          U_Xm[j]=U_X[j];
          U_Xp[j]=U_X[j];
        } 
      }
     } else {
         dm=((BCDPtr)(stagenData->AuxFunc[BC1D1_I]))(X4+N*nphase,U_X,P,&zero);
         CopyToMatrixU(JacW,dm,0,2*nphase+npar);
         CopyToMatrixU(JAC,dm,nphase,2*nphase+npar);
         staFunc->AddScaledMatrix(JacW,JAC,phirN,JAC);
     } 
     Ret=staFunc->Decomp(JAC);
     if (Ret) {
      staUtil->ErrMessage("No convergence in Newton iteration");
      Term();
      return 1;
     } 
   }

/* copy Explicit Euler predicted values */
  staFunc->CopyVector(X4,Fp);

/* Implicit Euler scheme */
  for (k=0;k<_maxiter;k++) {
    rdFunc->ClearMatrix(J);
    staFunc->ClearVector(Fm);    
    if (NumDer1) {  
      /* left */  
       for (j=0; j<nphase; j++)
         U_X[j]=phil0*X4[j]+phil1*X4[j+nphase]+phil2*X4[j+2*nphase];
       ma=rdFunc->GetMatrixA(J,0);
       mb=rdFunc->GetMatrixB(J,0);
       mc=rdFunc->GetMatrixC(J,0);
       if (staFunc->DerA((DiffFun)ie_BC0u, nphase, X4, _dx, JacW)) return 1;  
       if (staFunc->DerA((DiffFun)ie_BC0u_x, nphase, U_X, _dx, ma)) return 1;
       staFunc->CopyMatrix(ma,mb);
       staFunc->CopyMatrix(ma,mc);
       staFunc->AddScaledMatrix(JacW,ma,phil0,ma);
       staFunc->AddScaledMatrix(NULL,mb,phil1,mb);
       staFunc->AddScaledMatrix(NULL,mc,phil2,mc);
      /* internal */ 
       for (ipoint=1;ipoint<N;ipoint++) {
         ma=rdFunc->GetMatrixA(J,ipoint);
         mb=rdFunc->GetMatrixB(J,ipoint);
         mc=rdFunc->GetMatrixC(J,ipoint);
         b=nphase*ipoint;
         hi=XM[ipoint]-XM[ipoint-1];
         Hi=XM[ipoint+1]-XM[ipoint];
         si=hi+Hi;
         phim=-(hi+si)/(3*hi*si);
         phi=(Hi-hi)/(3*hi*Hi);
         phip=(Hi+si)/(3*Hi*si);
         psim=2/(hi*si);
         psi=-2/(hi*Hi);
         psip=2/(Hi*si);
         etam=(hi-Hi)*(Hi+si)/(9*hi*si);
         eta=(hi+si)*(Hi+si)/(9*hi*Hi);
         etap=(Hi-hi)*(hi+si)/(9*Hi*si);

         Xi=XM[ipoint]+(Hi-hi)/3;
         for (j=0; j<nphase; j++) {
           U_0[j]= etam*X4[b-nphase+j]+eta*X4[b+j]+etap*X4[b+nphase+j];
           U_X[j]= phim*X4[b-nphase+j]+phi*X4[b+j]+phip*X4[b+nphase+j];
           U_XX[j]=psim*X4[b-nphase+j]+psi*X4[b+j]+psip*X4[b+nphase+j];      
         }
         for (i=ipoint*nphase; i<(ipoint+1)*nphase;i++) {
           Fm[i]=-U_0[i-ipoint*nphase];
         }      
         if(staFunc->DerA((DiffFun)ie_RHSu,nphase,U_0,_dx,JacV)) return 1;
         if(staFunc->DerA((DiffFun)ie_RHSu_x,nphase,U_X,_dx,JacW)) return 1;
         if(staFunc->DerA((DiffFun)ie_RHSu_xx,nphase,U_XX,_dx,ma)) return 1;

         if(stagenData->Rhs(U_0,U_X,U_XX,&Xi,P,&zero,Fn+b)) return 2;
         for (j=0; j<nphase; j++) {
           U_0[j]= etam*X[b-nphase+j]+eta*X[b+j]+etap*X[b+nphase+j];
           U_X[j]= phim*X[b-nphase+j]+phi*X[b+j]+phip*X[b+nphase+j];
           U_XX[j]=psim*X[b-nphase+j]+psi*X[b+j]+psip*X[b+nphase+j];      
         }    
         for (i=ipoint*nphase; i<(ipoint+1)*nphase;i++) {
           Fm[i]+=U_0[i-ipoint*nphase];
         }                    
         if(stagenData->Rhs(U_0,U_X,U_XX,&Xi,P,&zero,Fo+b)) return 2;      
         for (i=ipoint*nphase; i<(ipoint+1)*nphase;i++) {
           Fm[i]+=h*Fn[i];
         }                    
         staFunc->AddScaledMatrix(NULL,ma,psi,mb);
         staFunc->AddScaledMatrix(NULL,ma,psip,mc);
         staFunc->AddScaledMatrix(NULL,ma,psim,ma);
         staFunc->AddScaledMatrix(ma,JacW,phim,ma);
         staFunc->AddScaledMatrix(ma,JacV,etam,ma);
         staFunc->AddScaledMatrix(mb,JacW,phi,mb);
         staFunc->AddScaledMatrix(mb,JacV,eta,mb);
         staFunc->AddScaledMatrix(mc,JacW,phip,mc);
         staFunc->AddScaledMatrix(mc,JacV,etap,mc);
         for (i=0;i<nphase;i++) {
           for(j=0;j<nphase;j++) {
             ma[i][j]=-ma[i][j]*h;
             mb[i][j]=-mb[i][j]*h;
             mc[i][j]=-mc[i][j]*h;
           }
         }
         for(j=0;j<nphase;j++) {
          ma[j][j]+=etam;
          mb[j][j]+=eta;
          mc[j][j]+=etap;
         }
       }    
      /* right */
       for (j=0; j<nphase; j++)
         U_X[j]=phirN2*X4[j+(nmesh-3)*nphase]+phirN1*X4[j+(nmesh-2)*nphase]+phirN*X4[j+(nmesh-1)*nphase];
       ma=rdFunc->GetMatrixA(J,N);
       mb=rdFunc->GetMatrixB(J,N);
       mc=rdFunc->GetMatrixC(J,N);
       if (staFunc->DerA((DiffFun)ie_BC1u, nphase, X4+N*nphase, _dx, JacW)) return 1;  
       if (staFunc->DerA((DiffFun)ie_BC1u_x,nphase, U_X, _dx, mc)) return 1;
       staFunc->CopyMatrix(mc,mb);
       staFunc->CopyMatrix(mc,ma);
       staFunc->AddScaledMatrix(JacW,mc,phirN,mc);
       staFunc->AddScaledMatrix(NULL,mb,phirN1,mb);
       staFunc->AddScaledMatrix(NULL,ma,phirN2,ma);
    } else { /* symbolical part */
      /* left */
       for (j=0; j<nphase; j++)
         U_X[j]=phil0*X4[j]+phil1*X4[j+nphase]+phil2*X4[j+2*nphase];
       ma=rdFunc->GetMatrixA(J,0);
       mb=rdFunc->GetMatrixB(J,0);
       mc=rdFunc->GetMatrixC(J,0);
       dm=((BCDPtr)(stagenData->AuxFunc[BC0D1_I]))(X4,U_X,P,&zero);
       if (!dm) return 11;
       CopyToMatrixU(JacW,dm,0,2*nphase+npar);
       CopyToMatrixU(ma,dm,nphase,2*nphase+npar);
       staFunc->CopyMatrix(ma,mb);
       staFunc->CopyMatrix(ma,mc);
       staFunc->AddScaledMatrix(JacW,ma,phil0,ma);
       staFunc->AddScaledMatrix(NULL,mb,phil1,mb);
       staFunc->AddScaledMatrix(NULL,mc,phil2,mc); 
     /* internal */
        for (ipoint=1;ipoint<N;ipoint++) {
         ma=rdFunc->GetMatrixA(J,ipoint);
         mb=rdFunc->GetMatrixB(J,ipoint);
         mc=rdFunc->GetMatrixC(J,ipoint);
         b=nphase*ipoint;
         hi=XM[ipoint]-XM[ipoint-1];
         Hi=XM[ipoint+1]-XM[ipoint];
         si=hi+Hi;
         phim=-(hi+si)/(3*hi*si);
         phi=(Hi-hi)/(3*hi*Hi);
         phip=(Hi+si)/(3*Hi*si);
         psim=2/(hi*si);
         psi=-2/(hi*Hi);
         psip=2/(Hi*si);
         etam=(hi-Hi)*(Hi+si)/(9*hi*si);
         eta=(hi+si)*(Hi+si)/(9*hi*Hi);
         etap=(Hi-hi)*(hi+si)/(9*Hi*si);

         Xi=XM[ipoint]+(Hi-hi)/3;
         for (j=0; j<nphase; j++) {
           U_0[j]= etam*X4[b-nphase+j]+eta*X4[b+j]+etap*X4[b+nphase+j];
           U_X[j]= phim*X4[b-nphase+j]+phi*X4[b+j]+phip*X4[b+nphase+j];
           U_XX[j]=psim*X4[b-nphase+j]+psi*X4[b+j]+psip*X4[b+nphase+j];      
         }
         for (i=ipoint*nphase; i<(ipoint+1)*nphase;i++) {
           Fm[i]=-U_0[i-ipoint*nphase];
         }
         dm=stagenData->Der1(U_0,U_X,U_XX,&Xi,P,&zero);
         if (!dm) return 12;
         CopyToMatrixU(JacV,dm,0,3*nphase+nspace+npar);
         CopyToMatrixU(JacW,dm,nphase,3*nphase+nspace+npar);
         CopyToMatrixU(ma,dm,2*nphase,3*nphase+nspace+npar);
         if(stagenData->Rhs(U_0,U_X,U_XX,&Xi,P,&zero,Fn+b)) return 2;
         for (j=0; j<nphase; j++) {
           U_0[j]= etam*X[b-nphase+j]+eta*X[b+j]+etap*X[b+nphase+j];
           U_X[j]= phim*X[b-nphase+j]+phi*X[b+j]+phip*X[b+nphase+j];
           U_XX[j]=psim*X[b-nphase+j]+psi*X[b+j]+psip*X[b+nphase+j];      
         }    
         for (i=ipoint*nphase; i<(ipoint+1)*nphase;i++) {
           Fm[i]+=U_0[i-ipoint*nphase];
         }                    
         if(stagenData->Rhs(U_0,U_X,U_XX,&Xi,P,&zero,Fo+b)) return 2;      
         for (i=ipoint*nphase; i<(ipoint+1)*nphase;i++) {
           Fm[i]+=h*Fn[i];
         }                    
         staFunc->AddScaledMatrix(NULL,ma,psi,mb);
         staFunc->AddScaledMatrix(NULL,ma,psip,mc);
         staFunc->AddScaledMatrix(NULL,ma,psim,ma);
         staFunc->AddScaledMatrix(ma,JacW,phim,ma);
         staFunc->AddScaledMatrix(ma,JacV,etam,ma);
         staFunc->AddScaledMatrix(mb,JacW,phi,mb);
         staFunc->AddScaledMatrix(mb,JacV,eta,mb);
         staFunc->AddScaledMatrix(mc,JacW,phip,mc);
         staFunc->AddScaledMatrix(mc,JacV,etap,mc);
         for (i=0;i<nphase;i++) {
           for(j=0;j<nphase;j++) {
             ma[i][j]=-ma[i][j]*h;
             mb[i][j]=-mb[i][j]*h;
             mc[i][j]=-mc[i][j]*h;
           }
         }
         for(j=0;j<nphase;j++) {
          ma[j][j]+=etam;
          mb[j][j]+=eta;
          mc[j][j]+=etap;
         }
        } /* end of internal */
       /* right */  
       for (j=0; j<nphase; j++)
         U_X[j]=phirN2*X4[j+(nmesh-3)*nphase]+phirN1*X4[j+(nmesh-2)*nphase]+phirN*X4[j+(nmesh-1)*nphase];
       ma=rdFunc->GetMatrixA(J,N);
       mb=rdFunc->GetMatrixB(J,N);
       mc=rdFunc->GetMatrixC(J,N);
       dm=((BCDPtr)(stagenData->AuxFunc[BC1D1_I]))(X4+N*nphase,U_X,P,&zero);
       if (!dm) return 13;
       CopyToMatrixU(JacW,dm,0,2*nphase+npar);
       CopyToMatrixU(mc,dm,nphase,2*nphase+npar);
       staFunc->CopyMatrix(mc,mb);
       staFunc->CopyMatrix(mc,ma);
       staFunc->AddScaledMatrix(JacW,mc,phirN,mc);
       staFunc->AddScaledMatrix(NULL,mb,phirN1,mb);
       staFunc->AddScaledMatrix(NULL,ma,phirN2,ma);
    }   
    fi=rdFunc->GetMatrixFi(J);
    fi[0][0]=1.0;    
    rdFunc->Decomp(J);
    rdFunc->Solve(J,Fm);
    for (i=0; i<(N+1)*nphase; i++) {
      X4[i]+=Fm[i];
    }
    delta=staFunc->NormOfVector(Fm);
    if (delta < _eps) break;
  }
  return 0;
}

/* Entry point to Implicit Euler orbit generator */
Entry(Int2) ie_OrbitCont(StagenDataPtr stagen) {

  Int2 n,b;		        /* space dimension */
  VAR TstVl1,TstVl2,TV,tau,tau1,tau2,E,etam,eta,etap,hi,Hi,si;
  int i,iu,j,k,l,m,resit,ii,iter;
  int Cont=1,Ret=0;
  Boolean ZeroTest;
  Boolean cond;

  CharPtr msgtext;
  char msgtxt[80];

  staFunc=(StdLinAlgDataPtr)stagen->StdLinAlg;
  rdFunc=(RdLinAlgDataPtr)stagen->StaFunc;
  staData=(rd__o_DataPtr)stagen->StaPtr;
  staUtil=(UtilitiesDataPtr)stagen->Utilities;

  if (!stagen) {
    Term();
    return Ret;
  }
  genData=stagen;
  stagenData=stagen;
  contData=(IntegrDataPtr)stagen->GenPtr;
  contFunc=(ContFuncParsPtr)stagen->GenFunc;

  LastPoint=FALSE;
  
  NumDer1=stagenData->Der1==NULL;
  
  IndTestFuns=MemNew(nUTest);
  SuspiciousPoints=MemNew(nUTest*sizeof(Vector *));
  Index=MemNew(nUTest*sizeof(int));
  IndexR=MemNew(nUTest*sizeof(int));

  n=contFunc->GetVectorDim(contData->X0);
  nmesh=contFunc->GetVectorDim(contData->X1); 
  nphase=*stagen->ClassDim[3];
  npar=*stagen->ClassDim[4];
  X = contFunc->CreateVector(n);
  X4 = contFunc->CreateVector(n);
  X4p = contFunc->CreateVector(n);  
  X4m = contFunc->CreateVector(n);  
  F = contFunc->CreateVector(n);
  F4 = contFunc->CreateVector(n);  
  F4p = contFunc->CreateVector(n);  
  F4m = contFunc->CreateVector(n);  
  Fm = contFunc->CreateVector(n);  
  Fp = contFunc->CreateVector(n);
  Fn = contFunc->CreateVector(n);
  Fv = contFunc->CreateVector(n-1-2*nphase);
  Fo = contFunc->CreateVector(n);  
  Fs = staFunc->CreateVector(nphase);  
  H4 = contFunc->CreateVector(n);
  Z = contFunc->CreateVector(n);
  XM = staFunc->CreateVector(nmesh);
  MemCopy(XM,staData->X,nmesh*sizeof(VAR));  
  XMT = staFunc->CreateVector(nmesh); 
  U_0 = staFunc->CreateVector(nphase*nmesh+1);   
  U_X = staFunc->CreateVector(nphase);
  U_XX = staFunc->CreateVector(nphase);
  P=MemNew(npar*sizeof(FloatHi));
  MemCopy(P,staData->P,npar*sizeof(FloatHi));
  Xp = contFunc->CreateVector(n);  
  Xm = contFunc->CreateVector(n);
  U_Xp = staFunc->CreateVector(nphase);
  U_Xm = staFunc->CreateVector(nphase);
  J=rdFunc->CreateMatrix((nmesh+1)*nphase+1,(nmesh+1)*nphase+1);
  JAC=staFunc->CreateMatrix(nphase,nphase);
  JacW=staFunc->CreateMatrix(nphase,nphase);
  JacV=staFunc->CreateMatrix(nphase,nphase);  
  /* Curve "continuation" */
  if (genData->ContDataGenLen) { 
    MemCopy(X,genData->ContDataGenPtr,n*sizeof(VAR));
    _h=genData->ContDataGenPtr[n];
    CurTint=genData->ContDataGenPtr[n+1];
    npc=(int)genData->ContDataGenPtr[n+2];    
    genData->ContDataGenPtr=MemFree(genData->ContDataGenPtr);  
  } else {
    npc=0;
    if (genData->Forward) {
      CurTint=((FloatHiPtr)(contData->X0))[nmesh*nphase]+fabs(_Tint);
      _h=fabs(_h0);
    } else {
      CurTint=((FloatHiPtr)(contData->X0))[nmesh*nphase]-fabs(_Tint);
      _h=-fabs(_h0);
    }
    contFunc->CopyVector(contData->X0,X);
  }
  TestValues=contFunc->CreateVector(nUTest);
  contFunc->CopyVector(X,Z);   

  /* User test function values at the first point  */
  for (i=0; i<nUTest; i++) 
    if (ActiveUTestFuns[i]==UDF_MONITOR || ActiveUTestFuns[i]==UDF_DETECT) {
      ii=(genData->udFunc)(X,i,&TestValues[i]);
      if (ii) { 
        MsgProc(0,X,"Undefined user test function in the first point");
        Ret=2;
        goto final;
      }
    }
    
  np=0;  
    
  /* Regular step */
  while (Cont) {
    contFunc->CopyVector(Z,X);   /* new point */

    /* Issue the message */
    if (MsgProc(0,X,NULL)) {
       Cont=0;
       continue;
    }

   /* STEP */    
again:
    if (OneStep(_h)) return 1;
    contFunc->CopyVector(X4,Z);

    /* Location zeros of User Test Functions */
    ZeroTest=FALSE;		/* No vanished test functions */
    for (iu=0; iu<nUTest; iu++) {
      Index[iu]=iu;
      IndTestFuns[iu]=1;
      if (ActiveUTestFuns[iu]==UDF_MONITOR) {
        (genData->udFunc)(Z,iu,&TstVl2);
      }
      if (ActiveUTestFuns[iu]==UDF_DETECT) {
        ii=(genData->udFunc)(Z,iu,&TstVl2);
        if (ii) continue;
        TstVl1=TestValues[iu];
        TestValues[iu]=TstVl2;
	resit=1;
        if (TstVl1 == 0) {
	  ZeroTest=TRUE;
          IndTestFuns[iu]=0; 
          resit=0;
          SuspiciousPoints[iu]=contFunc->CopyVector(X,SuspiciousPoints[iu]); /* save */
        }
        if (TstVl1*TstVl2 < 0.0) {  /* change of sign detected */
	  ZeroTest=TRUE;
          /* location of zero by secant iterations */
	  tau1=0;
	  tau2=_h; 
	  for (iter=0; iter<TestIter; iter++) {
	    tau=(tau1+tau2)/2.0;
	    OneStep(tau);
            if (genData->udFunc(X4,iu,&TV)) {
              resit=2;
              break;
            }
	    if (fabs(TV)<_epsuf) {
	      resit=0;
	      break;
	    }
            if (TV*TstVl1 < 0.0) {
              TstVl2=TV;
              tau2=tau;
            } else {
              TstVl1=TV;
              tau1=tau;
            }  	
          }
          SuspiciousPoints[iu]=contFunc->CopyVector(X4,SuspiciousPoints[iu]); /* save */
          switch(resit) {
            case 0:  /* OK  */
              IndTestFuns[iu]=0; 
              break; 
            case 1:   /* no convergence of zero location */
            case 2:   /* can not compute user function */
              IndTestFuns[iu]=-1;
              break;
          }
        }
      }
    }	 
    if (ZeroTest) {
      /* sorting of zeros with respect to time X[0] */
      for (i=0; i<nUTest; i++)
	if (IndTestFuns[i]<=0)	/* zero test func */
	  for (j=1; j<nUTest; j++)
	    if (IndTestFuns[j]<=0)
              if (_h>0 ? (SuspiciousPoints[i][0]>SuspiciousPoints[j][0]) : 
				   (SuspiciousPoints[i][0]<SuspiciousPoints[j][0])) {
		 l=Index[i];
		 Index[i]=Index[j];
		 Index[j]=l;
	      }
      for (i=0; i<nUTest; i++) IndexR[Index[i]]=i;
      for (i=0; i<nUTest; i++)	{
        ii=IndexR[i];
	if (IndTestFuns[i]==0) {
          sprintf(msgtxt,"Zero of user function number %i",(int)ii+1);
          msgtext=msgtxt;
          for (j=0; j<nUTest; j++) 
            if (ActiveUTestFuns[j]==UDF_MONITOR || ActiveUTestFuns[j]==UDF_DETECT) {
	      if ((genData->udFunc)(SuspiciousPoints[ii],j,&TstVl2)) { 
		MsgProc(0,SuspiciousPoints[ii],"Undefined user test function");
		Ret=2;
		goto final;
	       }
             }
	   if (MsgProc(USERPOINT|(ii),SuspiciousPoints[ii],msgtext)) {
             Cont=0;
             goto end;
           }
	 }	/* if  */
      }	/* for i=0; */
      /* Recompute values of all test function at the next regular point */
      for (i=0; i<nUTest; i++) 
        if (ActiveUTestFuns[i]==UDF_MONITOR || ActiveUTestFuns[i]==UDF_DETECT) 
	  genData->udFunc(Z,i,&TstVl2);
    }	/* if (ZeroTest) */

end: ;
    /* Stop check  */
    switch (Checker(Z)) {
      case 1:		/* end of the interval */
        Cont=0;
	MsgProc(0,Z,NULL);        
	contFunc->CopyVector(Z,X);	
	continue;
    }

  /* Step size control */
     RHS(X4,F4);
     staFunc->CopyVector(X4,X4p);
     staFunc->CopyVector(X4,X4m);
     for (i=nphase;i<(nmesh-1)*nphase;i++) {
       X4p[i]+=_dx*F4[i];
       X4m[i]-=_dx*F4[i];
     }
     RHS(X4p,F4p);
     RHS(X4m,F4m);
  /* Gx G */
     for (i=0;i<n-1-2*nphase;i++) { /* -1 for time, -2*nphase to exclude boundaries */
       Fv[i]=(F4p[nphase+i]-F4m[nphase+i])/(2.0*_dx);
     }

     delta=rdFunc->NormOfVector(Fv);      
     delta*=(_h*_h/2.0);
   
     if (delta > _eps) {
       _h/=2.0;
       if (fabs(_h)<_hmin && !LastPoint) {
           MsgProc(0,X,"Current step size too small");
           break;
       }
       goto again;
     } else {
         if (delta < _eps/2) _h*=1.3;
         if (fabs(_h)>_hmax) _h= _h<0 ? -_hmax : _hmax;
     } 
     
    /* Adaptation of mesh */
     if (_nadapt > 0) {
        if (np%_nadapt == 0) {
           ((AdapterFuncPtr)(genData->AdapterFunc[0]))(Z,XM);
        }
     }
     np++;
     npc++;    
  } /* while */
final:
  DefaultProcessor(X);
  /* assert: V is the tangent vector at the last found point X */
  genData->ContDataGenLen=(n+3)*sizeof(VAR);
  genData->ContDataGenPtr=MemNew(genData->ContDataGenLen);
  MemCopy(genData->ContDataGenPtr,X,n*sizeof(VAR));
  genData->ContDataGenPtr[n]=_hlast;
  genData->ContDataGenPtr[n+1]=CurTint;
  genData->ContDataGenPtr[n+2]=(VAR)npc;
  Term();
  return (Ret);
}