#include <values.h>

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


#include "stagen.h"

#include "_linalg.h"

#include "_util.h"

#include "_conti.h"

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

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

/* 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;
  PrintMatrixPtr	PrintMatrix;
  CreateMatrixPtr       CreateMatrix;
  DestroyMatrixPtr      DestroyMatrix;
  CopyMatrixPtr         CopyMatrix;
  MultiplyMatrixVectorPtr MultiplyMatrixVector;
  AppendMatrixVectorPtr AppendMatrixVector;
  SolvePtr              Solve;
  DecompPtr             Decomp;
} ContFuncPars, PNTR ContFuncParsPtr;

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

/* Pointer to a continuator's own data area */
Local(ContDataPtr) contData;

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

#define BIG_VALUE  1.0

/***************************/
#define _maxit   contData->maxit      /* corrector data  */
#define _maxnewt contData->maxnewt
#define _epsx    contData->epsx
#define _epsf    contData->epsf
#define _epsz    contData->epsz
#define _ASG     contData->ActiveSing

#define ActiveUTestFuns     contData->ActiveUserTest
					/* continuation data */
#define _h0           contData->h0      
#define _h            contData->hcur    /* current step size */
#define _hmin         contData->hmin
#define _hmax         contData->hmax

#define _maxnpt		contData->maxnpt
#define _nclosed	contData->nclosed
#define _nadapt         contData->nadapt
#define _outlevel       contData->outlevel

#define nTest         genData->TestFuncNum
#define TestIter      contData->IterTestFuns
#define TestType      contData->TypeTestFuns
#define TestZero      contData->ZeroTestFuns

#define nUTest       genData->udFuncNum

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

Local(int) np;                 /* number of points along curve segment */
Local(int) npc;                /* total number of points along curve */

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

typedef EntryPtr(Int2,DefFuncPtr,(Vector xx, Vector yy));
typedef EntryPtr(Int2,TestFuncPtr,(Vector xx, Vector vv, FloatHiPtr TV));
typedef EntryPtr(Int2,ProcFuncPtr,(Int2 Ind, Vector xx,  Vector vv, CharPtr *msg));
typedef EntryPtr(Int2,AdapterFuncPtr,(Vector xx, Vector yy));


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


/* Message Processing */

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

Local(Matrix) A,B0,B;      /* global matrices and vectors used in iterations */
Local(Vector) F,W1,Z1;
Local(BoolPtr) ActiveTestFuns;

/*********************************************/
/*  Newton-chord - Moore-Penrose Corrector   */

Local(int) newtmpc (int Ind,      /* Ind=0 - adapt tangent; Ind=1 - fixed tangent */
	   Vector X0,             /* Predicted point  */ 
           Vector V0,             /* Predicted normalized tangent vector */          
	   Vector X,              /* Corrected point */ 
           Vector V,              /* Corrected normalized tanget vector */
	   int *it)
{
    char msgtxt[80];

    Int2 n=contFunc->GetVectorDim(X0);	/* space dimension */

    VAR errx, errf;
    VAR TstVl2;
    Int2 i;

    contFunc->CopyVector(X0,X);
    contFunc->CopyVector(V0,V);
/*  Main loop  */
    for (*it=1; *it<=_maxit; (*it)++) {
	if (genData->DefFunc[0](X, F)) return (1);
	errf = contFunc->NormOfVector(F);   /* f-error */
/* printf("epsf=%g\n",errf); */
/*  compute or restore decomposed Jacobian  */
	if (*it <= _maxnewt) {
           contFunc->CopyVector(X,Z1);
	   if (genData->DefFunc[1](Z1, A)) return (1);
	   contFunc->AppendMatrixVector(A,V,B);
           if (contFunc->Decomp(B)) return(3);    /* singularity check */
           contFunc->CopyMatrix(B,B0); 

/*  compute and normalize the new kernel vector  */
           if (Ind == 0) {
              contFunc->MultiplyMatrixVector(A,V,W1);
              contFunc->CopyVectorElements(W1,0,Z1,0,n-1);
	      Z1[n-1]=0.0;
              contFunc->Solve(B,Z1);
              contFunc->AddScaledVector(V,Z1,-1,V);
              contFunc->NormalizeVector(V);
           }
	} else {
	   contFunc->CopyMatrix(B0,B);
	}
/*  solve linear problem for displacement  */
        contFunc->CopyVectorElements(F,0,Z1,0,n-1);
        Z1[n-1]=0.0;
	contFunc->Solve(B,Z1);

	errx = contFunc->NormOfVector(Z1);   /* x-error */  
        contFunc->AddScaledVector(X,Z1,-1,X);    /* correction  */
        if (_outlevel>1) {
	   DefaultProcessor(1,X,V); 		/* do not compute global data; only for visualization */
	   for (i=0; i<nTest; i++) 
             if (ActiveTestFuns[i])                               /* for visialization */
                ((TestFuncPtr)(genData->TestFunc[i]))(X,V,&TstVl2);
           /* User-defined test functions */
           for (i=0; i<nUTest; i++) 
             if (ActiveUTestFuns[i]==UDF_MONITOR || ActiveUTestFuns[i]==UDF_DETECT) 
	        genData->udFunc(X,i,&TstVl2);
           sprintf(msgtxt,"* Correction: it=%i |Var|=%e |Fun|=%e",*it,errx,errf);
           MsgProc(255,X,msgtxt);
        }
	if (errx < _epsx && errf < _epsf) break;
    }

    if (*it <= _maxit)  return (0);
    return (2);
}

/***************/
/* StopCheck   */

Local(Vector) WWWWW=NULL;

Local(int) Checker(Vector X0, Vector X) {

  VAR dist;
  if (++np>_maxnpt) return 1;	/* number of points */
  if (++npc>_nclosed) {
    WWWWW=contFunc->CopyVector(X,WWWWW);
    contFunc->AddScaledVector(WWWWW,X0,-1,WWWWW);
    dist=contFunc->ScalProd(WWWWW,WWWWW);
    dist=sqrt(dist);
    if (dist<_h) {   /*  h - current step size  */
      return 2;			/* closed curve */
    }
  }
  return 0;
}

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

Local(Int2) NextSubscr (Int2 s) {
  Int2 i,j,m;
  m=s<nTest ? nTest : nTest+nUTest;
  for (i=s; i<m; i++) {
    j=Index[i];
    if (j>=0 && IndTestFuns[j]<=0)
      return i;
  }
  return -1;
}

Local(Vector) TestValues;            /* values of Test Functions  */
Local(Vector) X,Y=NULL,Z=NULL,ZZ,V,W=NULL,WW;
Local(Vector) X1=NULL,X2=NULL,V1=NULL,V2=NULL,DX,Temp,TempV;  /* work vectors */
Local(Vector) *SuspiciousPoints;
Local(Vector) *SuspiciousVectors;
Local(int) *IndexR;
Local(BoolPtr) CurSing;
Local(Int1Ptr) CurInd;

Local(void) Term (void) {
Int2 i;
  ActiveTestFuns=MemFree(ActiveTestFuns);
  IndTestFuns=MemFree(IndTestFuns);
  CurInd=MemFree(CurInd);
  for (i=0; i<nTest+nUTest; i++) {
    contFunc->DestroyVector(SuspiciousPoints[i]);
    contFunc->DestroyVector(SuspiciousVectors[i]);
  }
  SuspiciousPoints=MemFree(SuspiciousPoints);
  SuspiciousVectors=MemFree(SuspiciousVectors);
  Index=MemFree(Index);
  IndexR=MemFree(IndexR);
  CurSing=MemFree(CurSing);
  contFunc->DestroyVector(TestValues);
  contFunc->DestroyVector(X);
  Y=contFunc->DestroyVector(Y);
  contFunc->DestroyVector(V);
  Z=contFunc->DestroyVector(Z);
  W=contFunc->DestroyVector(W);
  contFunc->DestroyVector(ZZ);
  contFunc->DestroyVector(WW);
  contFunc->DestroyVector(Temp);
  contFunc->DestroyVector(F);
  X1 = contFunc->DestroyVector(X1);
  V1 = contFunc->DestroyVector(V1);
  X2 = contFunc->DestroyVector(X2);
  V2 = contFunc->DestroyVector(V2);
  TempV = contFunc->DestroyVector(TempV);
  DX = contFunc->DestroyVector(DX);
  contFunc->DestroyVector(W1);
  contFunc->DestroyVector(Z1);
  WWWWW=contFunc->DestroyVector(WWWWW);	/* Stop check */
  contFunc->DestroyMatrix(A);
  contFunc->DestroyMatrix(B);
  contFunc->DestroyMatrix(B0);
}

/* Entry point to the generator */
Entry(Int2) CurveCont(StagenDataPtr stagen) {
  Int2 n;		        /* space dimension */
  VAR TstVl1,TstVl2,TV,tau,dist,delta,maxcoord;
  VAR hnew;
  int it,itz;			/* number of performed correction iterations */
  int i,iu,j,k,l,m,resit,ii,iter,iii,jjj;
  int Cont=1,Ret=0;
  Int2 stf,utf;
  Boolean ZeroTest;
  Boolean cond;

  CharPtr msgtext;
  char msgtxt[80];

  Int2 _h_Index;		/* for update value of current step */

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

  ActiveTestFuns=MemNew(nTest);
  ((UtilitiesDataPtr)stagen->Utilities)->ActivateTestFuns(stagen,_ASG,ActiveTestFuns);
  CurSing=MemNew(NumSing+nUTest);

/*
printf("\nSingularities:");
for (i=0; i<NumSing; i++)
  printf(" %i",(int)_ASG[i]);
printf("\nTestFunc:\n");
for (i=0; i<stagen->TestFuncNum; i++)
  printf(" %i",(int)ActiveTestFuns[i]);
printf("\n\n");
*/

  IndTestFuns=MemNew(nTest+nUTest);
  CurInd=MemNew(nTest*nTest);
  SuspiciousPoints=MemNew((nTest+nUTest)*sizeof(Vector *));
  SuspiciousVectors=MemNew((nTest+nUTest)*sizeof(Vector *));
  Index=MemNew((nTest+nUTest)*sizeof(int));
  IndexR=MemNew((nTest+nUTest)*sizeof(int));

  n=contFunc->GetVectorDim(contData->X0);

  X = contFunc->CreateVector(n);
  V = contFunc->CreateVector(n);

/* Curve "extention" */
  if (genData->ContDataGenLen) { 
    MemCopy(X,genData->ContDataGenPtr,n*sizeof(VAR));
    MemCopy(V,genData->ContDataGenPtr+n,n*sizeof(VAR));
    MemCopy(contData->X0,genData->ContDataGenPtr+2*n,n*sizeof(VAR));
    MemCopy(contData->V0,genData->ContDataGenPtr+3*n,n*sizeof(VAR));

    _h=genData->ContDataGenPtr[4*n];
    npc=(int)genData->ContDataGenPtr[4*n+1];
    genData->ContDataGenPtr=MemFree(genData->ContDataGenPtr);  
  } else {
    npc=0;
    _h=_h0;
    contFunc->CopyVector(contData->X0,X);
    contFunc->CopyVector(contData->V0,V);
  }
  Z=contFunc->CopyVector(X,Z);
  W=contFunc->CopyVector(V,W);

  TestValues=contFunc->CreateVector(nTest+nUTest);

  ZZ = contFunc->CreateVector(n);
  WW = contFunc->CreateVector(n);

  Temp = contFunc->CreateVector(n);

  F = contFunc->CreateVector(n-1);     /* creation of global matrices and vectors for corrections */
  Z1 = contFunc->CreateVector(n);
  W1 = contFunc->CreateVector(n-1);
  A = contFunc->CreateMatrix (n-1,n);
  B = contFunc->CreateMatrix (n,n);
  B0 = contFunc->CreateMatrix (n,n);

  _h_Index=stagen->ParIndex(contData,hcur);	/* hcur is a real name of a continuator's data structute field */
  stagen->UpdatePar(_h_Index);
  np=0;
  
  /* Test function values at the initial point */ 
  DefaultProcessor(1,X,V); 
  for (i=0; i<nTest; i++) 
    if (ActiveTestFuns[i]) {
      if (TestZero[i] && !genData->ContDataGenLen) {
        TestValues[i]=0.0;
      } else {
        ii=((TestFuncPtr)(genData->TestFunc[i]))(X,V,&TestValues[i]);
        if (ii) {
           MsgProc(0,X,"Undefined test function in the first point");
           Ret=2;
           goto final;
        }
      }
    }
  /* 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+nTest]);
      if (ii) { 
        MsgProc(0,X,"Undefined user test function in the first point");
        Ret=2;
        goto final;
      }
    }

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

  /* Regular step */
  while (Cont) {

    contFunc->CopyVector(Z,X);   /* new point and tangent vector */
    contFunc->CopyVector(W,V);

    /* PREDICTION */
newpoint:
    Y=contFunc->AddScaledVector(X,V,_h,Y);
    if (_outlevel > 0) { 
	   DefaultProcessor(1,Y,V); 
	   for (i=0; i<nTest; i++) 
             if (ActiveTestFuns[i])                               /* for visialization */
                ((TestFuncPtr)(genData->TestFunc[i]))(Y,V,&TstVl2);
           /* User-defined test functions */
           for (i=0; i<nUTest; i++) 
             if (ActiveUTestFuns[i]==UDF_MONITOR || ActiveUTestFuns[i]==UDF_DETECT) 
	        genData->udFunc(Y,i,&TstVl2);
      if (MsgProc(255,Y,"* Prediction")) {
         Cont=0;
         continue;
      }
    } 

   /* CORRECTION */
    ii=newtmpc(0, Y, V, Z, W, &it);      /*  PUT BACK !!!! 1->0  (0-Moore-Penrose, 1-pseudoarchlength) */
again:
    switch (ii) {
      case 0:	/* OK */
        /* Smoothness test */
        tau=contFunc->ScalProd(V,W);
	if (np==0) tau=1.0;
        if (tau < 0.9) {  /* 0.7  */
  	  ii=4;
          goto again;
        }
        if (np==0) {
	  /* Test function values at the first point  */
          DefaultProcessor(1,Z,W); 
          for (i=0; i<nTest; i++) 
            if (ActiveTestFuns[i]) {
              ii=((TestFuncPtr)(genData->TestFunc[i]))(Z,W,&TestValues[i]);
              if (ii) {
                MsgProc(0,X,"Undefined test function in the first point");
                Ret=2;
                goto final;
              }
            }
          /* 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+nTest]);
              if (ii) { 
                MsgProc(0,X,"Undefined user test function in the first point");
                Ret=2;
                goto final;
              }
            }
	} /* endif np==0 */

        /* Location zeros of Test Funstions  */
	ZeroTest=FALSE;		/* No vanished test functions */
        for (i=0; i<nTest; i++) {
	  Index[i]=i;
          IndTestFuns[i]=1;
          if (ActiveTestFuns[i]) {
	    DefaultProcessor(1,Z,W);
            ii=((TestFuncPtr)(genData->TestFunc[i]))(Z,W,&TstVl2);
            if (ii) goto again;
            TstVl1=TestValues[i];
	    TestValues[i]=TstVl2;
            if (TstVl1 == 0) {
	       ZeroTest=TRUE;
               IndTestFuns[i]=0; 
               resit=0;
	       contFunc->CopyVector(X,ZZ);
               contFunc->CopyVector(V,WW);
               SuspiciousPoints[i]=contFunc->CopyVector(ZZ,SuspiciousPoints[i]); /* save */
               SuspiciousVectors[i]=contFunc->CopyVector(WW,SuspiciousVectors[i]); /* save */
            }
            if (TstVl1*TstVl2 < 0.0) {  /* change of sign detected */
	       ZeroTest=TRUE;
               /* location of zero by secant iterations */ 
	       X1 = contFunc->CopyVector(X,X1);
               V1 = contFunc->CopyVector(V,V1);
               X2 = contFunc->CopyVector(Z,X2);
	       V2 = contFunc->CopyVector(W,V2);
               TempV = contFunc->CreateVector(n);
               DX = contFunc->CreateVector(n);
      	       resit=1;		/* assuming no convergence */
               for (iter=1; iter<=TestIter; iter++) {
	         tau=0.5;  
/*	         tau=TstVl1/(TstVl1-TstVl2);    */
                 contFunc->AddScaledVector(X2,X1,-1.0,DX);
                 contFunc->AddScaledVector(X1,DX,tau,Temp);	
                 contFunc->AddScaledVector(V2,V1,-1.0,DX);
                 contFunc->AddScaledVector(V1,DX,tau,TempV);
                 if (_outlevel > 0) {
                    sprintf(msgtxt,"** Zero prediction");
                    MsgProc(255,Temp,msgtxt);
                 }
                 if (newtmpc(1, Temp, TempV, ZZ, WW, &itz)) {
                    contFunc->CopyVector(Temp,ZZ);
                    contFunc->CopyVector(TempV,WW);                                        		   
		 }
	         DefaultProcessor(1,ZZ,WW);
        	 if (((TestFuncPtr)(genData->TestFunc[i]))(ZZ,WW,&TV)) {
                    resit=3;
                    break;
                 }
                 contFunc->AddScaledVector(X1,ZZ,-1.0,DX);	
                 dist=contFunc->NormOfVector(DX);
	         delta=fabs(TV);
                 if (_outlevel > 0) {
                    sprintf(msgtxt,"** Zero location: itz=%i |Var|=%e |Test|=%e",iter,dist,delta);
                    MsgProc(255,ZZ,msgtxt);
                 }
                 if (dist < _epsx && (TestType[i] == 0 || delta < _epsz)) { 
                    resit=0;		   
	            break;
		 }
                 if (TV*TstVl1 < 0.0) {
                    /* TstVl2=TV; */
                    contFunc->CopyVector(ZZ,X2);
                    contFunc->CopyVector(WW,V2);                                       
                 } else {
                    /* TstVl1=TV; */
                    contFunc->CopyVector(ZZ,X1);
                    contFunc->CopyVector(WW,V1);  
                 }  	
               }
               if (resit == 1 && TestType[i]==1 && delta > BIG_VALUE) resit=4; 
               SuspiciousPoints[i]=contFunc->CopyVector(ZZ,SuspiciousPoints[i]); /* save */
               SuspiciousVectors[i]=contFunc->CopyVector(WW,SuspiciousVectors[i]); /* save */
               switch(resit) {
                  case 0:  /* OK  */
                     IndTestFuns[i]=0; 
                     break; 
                  case 1:   /* no convergence of zero location */
                  case 2:   /* no convergence of intermediate point location */
                  case 3:   /* undefined test function */
                     IndTestFuns[i]=-1;
                     break;
                  case 4:   /* no convergence in function value */
                     IndTestFuns[i]=1;
               }
	       X1 = contFunc->DestroyVector(X1);
	       V1 = contFunc->DestroyVector(V1);
	       X2 = contFunc->DestroyVector(X2);
	       V2 = contFunc->DestroyVector(V2);
               TempV = contFunc->DestroyVector(TempV);
               DX = contFunc->DestroyVector(DX);
            }
	  }
        }
        /* Location zeros of User Test Funstions */
        for (i=0,iu=nTest; i<nUTest; i++,iu++) {
	  Index[iu]=iu;
          IndTestFuns[iu]=1;
          if (ActiveUTestFuns[i]==UDF_MONITOR || ActiveUTestFuns[i]==UDF_DETECT) {
	    DefaultProcessor(1,Z,V);
            ii=(genData->udFunc)(Z,i,&TstVl2);
            if (ii) goto again;
            TstVl1=TestValues[iu];
	    TestValues[iu]=TstVl2;            
	  }
          if (ActiveUTestFuns[i]==UDF_DETECT) {
            if (TstVl1 == 0) {
	       ZeroTest=TRUE;
               IndTestFuns[iu]=0; 
               resit=0;
	       contFunc->CopyVector(X,ZZ);
               contFunc->CopyVector(V,WW);
               SuspiciousPoints[iu]=contFunc->CopyVector(ZZ,SuspiciousPoints[iu]); /* save */
               SuspiciousVectors[iu]=contFunc->CopyVector(WW,SuspiciousVectors[iu]); /* save */
            }
            if (TstVl1*TstVl2 < 0.0) {  /* change of sign detected */
	       ZeroTest=TRUE;
               /* location of zero by secant iterations */ 
	       X1 = contFunc->CopyVector(X,X1);
               V1 = contFunc->CopyVector(V,V1);
               X2 = contFunc->CopyVector(Z,X2);
	       V2 = contFunc->CopyVector(W,V2);
               TempV = contFunc->CreateVector(n);
               DX = contFunc->CreateVector(n);
      	       resit=1;		/* assuming no convergence */
               for (iter=1; iter<=TestIter; iter++) {
	         tau=TstVl1/(TstVl1-TstVl2); 
                 contFunc->AddScaledVector(X2,X1,-1.0,DX);
                 contFunc->AddScaledVector(X1,DX,tau,Temp);	
                 contFunc->AddScaledVector(V2,V1,-1.0,DX);
                 contFunc->AddScaledVector(V1,DX,tau,TempV);
                 if (_outlevel > 0) {
                    sprintf(msgtxt,"** User zero prediction");
                    MsgProc(255,Temp,msgtxt);
                 }
                 if (newtmpc(1, Temp, TempV, ZZ, WW, &itz)) {
                    contFunc->CopyVector(Temp,ZZ);
                    contFunc->CopyVector(TempV,WW);                                        		   
		 }
		 DefaultProcessor(1,ZZ,WW);
        	 if (genData->udFunc(ZZ,i,&TV)) {
                    resit=3;
                    break;
                 }
                 contFunc->AddScaledVector(X1,ZZ,-1.0,DX);	
                 dist=contFunc->NormOfVector(DX);
	         delta=fabs(TV);
                 if (_outlevel > 0) {
                    sprintf(msgtxt,"** User zero location: itz=%i |Var|=%e |Test|=%e",iter,dist,delta);
                    MsgProc(255,ZZ,msgtxt);
                 }
                 if (dist < _epsx) { 
                    resit=0;		   
	            break;
		 }
                 if (TV*TstVl1 < 0.0) {
                    TstVl2=TV;
                    contFunc->CopyVector(ZZ,X2);
                    contFunc->CopyVector(WW,V2);                                        
                 } else {
                    TstVl1=TV;
                    contFunc->CopyVector(ZZ,X1);
                    contFunc->CopyVector(WW,V1); 
                 }  	
               }
               SuspiciousPoints[iu]=contFunc->CopyVector(ZZ,SuspiciousPoints[iu]); /* save */
               SuspiciousVectors[iu]=contFunc->CopyVector(WW,SuspiciousVectors[iu]); /* save */
               switch(resit) {
                  case 0:  /* OK  */
                     IndTestFuns[iu]=0; 
                     break; 
                  case 1:   /* no convergence of zero location */
                  case 2:   /* no convergence of intermediate point location */
                  case 3:   /* undefined test function */
                     IndTestFuns[iu]=-1;
                     break;
               }
	       X1 = contFunc->DestroyVector(X1);
	       V1 = contFunc->DestroyVector(V1);
	       X2 = contFunc->DestroyVector(X2);
	       V2 = contFunc->DestroyVector(V2);
               TempV = contFunc->DestroyVector(TempV);
               DX = contFunc->DestroyVector(DX);
            }
	  }
        }
	if (ZeroTest) {
          /* sorting of zeros */
	  maxcoord=V[k=0];
	  for (i=1; i<n; i++)
	    if (fabs(maxcoord)<fabs(V[i]))
	      maxcoord=V[k=i];
	  for (i=0; i<nTest; i++)
	    if (IndTestFuns[i]<=0)	/* zero test func */
	      for (j=i+1; j<nTest; j++)
	        if (IndTestFuns[j]<=0)
		  if (maxcoord>0 ? (SuspiciousPoints[i][k]>SuspiciousPoints[j][k]) : 
				   (SuspiciousPoints[i][k]<SuspiciousPoints[j][k])) {
		    l=Index[i];
		    Index[i]=Index[j];
		    Index[j]=l;
		  }
	  for (i=nTest; i<nTest+nUTest; i++)
	    if (IndTestFuns[i]<=0)	/* zero test func */
	      for (j=nTest+1; j<nTest+nUTest; j++)
	        if (IndTestFuns[j]<=0)
		  if (maxcoord>0 ? (SuspiciousPoints[i][k]>SuspiciousPoints[j][k]) : 
				   (SuspiciousPoints[i][k]<SuspiciousPoints[j][k])) {
		    l=Index[i];
		    Index[i]=Index[j];
		    Index[j]=l;
		  }
          for (i=0; i<nTest+nUTest; i++) IndexR[Index[i]]=i;
/*
{ int qq;
printf("Index= ");
for (qq=0; qq<nTest; qq++) printf(" %i",Index[qq]);
printf("\n");
}
*/
          /* group and process zeroes */
	  for (iii=0; iii<nTest; ) {
	    i=IndexR[iii];
	    if (IndTestFuns[i]<=0) {
	      MemFill(CurInd+i*nTest,1,nTest);
	      (CurInd+i*nTest)[i]=IndTestFuns[i];
	      m=i;
	      jjj=iii;
	      do {
	        for (jjj++; jjj<nTest && IndTestFuns[j=IndexR[jjj]]>0; jjj++);
	        if (jjj>=nTest) break;
	        if (fabs(SuspiciousPoints[i][k]-SuspiciousPoints[j][k])>_h/10) break;
	        (CurInd+i*nTest)[j]=IndTestFuns[j];				
	        m=j;
	      } while (1);
	      if (i==m) {	/* there is the only point in the cluster */
	      } else {	/* 0.5*(left+right) */
		contFunc->AddScaledVector(SuspiciousPoints[m],SuspiciousPoints[i],-1.0,Temp);
	        contFunc->AddScaledVector(SuspiciousPoints[i],Temp,0.5,SuspiciousPoints[i]);
	        contFunc->AddScaledVector(SuspiciousVectors[m],SuspiciousVectors[i],-1.0,Temp);
	        contFunc->AddScaledVector(SuspiciousVectors[i],Temp,0.5,SuspiciousVectors[i]);
		for (ii=iii+1; ii<jjj; ii++) Index[ii]=-1;
	      }
              iii=jjj;
            } else iii++;
	  }	/* for */
	  /* Sort by merging and issue points */
	  stf=NextSubscr(0);
	  utf=NextSubscr(nTest);
	  while (stf>=0 || utf>=0) {
	    if (stf>=0) i=Index[stf];
	    if (utf>=0) j=Index[utf];
	    cond=((stf>=0 && utf>=0) &&
		  (maxcoord>0 ? (SuspiciousPoints[i][k]<SuspiciousPoints[j][k]) : 
				(SuspiciousPoints[i][k]>SuspiciousPoints[j][k]))) ||
		  (utf<0);
	    MemFill(CurSing,0,NumSing+nUTest);
	    if (cond) {
	      ((UtilitiesDataPtr)stagen->Utilities)->IndProcFunc(stagen,_ASG,CurInd+i*nTest,CurSing);
/**************
{ int qq;
printf("CurInd: ");
for (qq=0; qq<nTest; qq++) printf(" %i",(int)(CurInd+i*nTest)[qq]);
printf("\n");
printf("CurSing: ");
for (qq=0; qq<NumSing; qq++) printf(" %i",(int)(CurSing)[qq]);
printf("\n");
}
**************/
	      for (l=ii=0; ii<nTest; ii++)
		if (IndTestFuns[ii]<=0)
		  if (!l) l=IndTestFuns[ii];
	      ii=i;
	      if (++stf<nTest) stf=NextSubscr(stf); else stf=-1;
	    } else {
	      CurSing[NumSing+j-nTest]=TRUE;
	      ii=j;
	      utf=NextSubscr(utf+1);
	    }
	    contFunc->CopyVector(SuspiciousPoints[ii],ZZ);
	    contFunc->CopyVector(SuspiciousVectors[ii],WW);
	    DefaultProcessor(1,ZZ,WW);
	    for (ii=0; ii<NumSing+nUTest; ii++)	{
	      if (CurSing[ii]) {
	        if (ii<NumSing) {
		  ((ProcFuncPtr)genData->ProcFunc[ii+1])(l,ZZ,WW,&msgtext); /* Processor #0 is for Default Processor */
		} else {
		  DefaultProcessor(-1,ZZ,WW);  /* -1 means a zero of user-defined function */
		  sprintf(msgtxt,"Zero of user function %s",genData->udFuncName(ii-NumSing));
		  msgtext=msgtxt;
		}
                for (i=0; i<nTest; i++) 
                  if (ActiveTestFuns[i]) {
                    if (((TestFuncPtr)(genData->TestFunc[i]))(ZZ,WW,&TstVl2)) { 
                      MsgProc(0,ZZ,"Undefined test function");
		      Ret=2;
		      goto final;
                    }
                  }
                for (i=0; i<nUTest; i++) 
                  if (ActiveUTestFuns[i]==UDF_MONITOR || ActiveUTestFuns[i]==UDF_DETECT) {
		    if ((genData->udFunc)(ZZ,i,&TstVl2)) { 
		      MsgProc(0,ZZ,"Undefined user test function");
		      Ret=2;
		      goto final;
		    }
                  }
		if (MsgProc(ii<NumSing?ii+1:USERPOINT|(ii-NumSing),ZZ,msgtext)) {
		  DefaultProcessor(1,ZZ,WW);
		  contFunc->CopyVector(ZZ,Z);
		  contFunc->CopyVector(WW,W);
		  Cont=0;
		  goto end;
		}
	      }	/* if CurSing[ii] */
	    }	/* for ii=0; */
	  }	/* while */
	}/* if (ZeroTest) */

	 if (ZeroTest) {
	 /* Recompute values of all test functions at the next regular point */
	   DefaultProcessor(1,Z,W);
	   for (i=0; i<nTest; i++) 
             if (ActiveTestFuns[i])                               /* for visialization */
                ((TestFuncPtr)(genData->TestFunc[i]))(Z,W,&TstVl2);
           /* User-defined test functions */
           for (i=0; i<nUTest; i++) 
             if (ActiveUTestFuns[i]==UDF_MONITOR || ActiveUTestFuns[i]==UDF_DETECT) 
	        genData->udFunc(Z,i,&TstVl2);
	 }

	/* Step size control */
	hnew=_h;
	if (it<4) hnew=1.3*_h;
	if (hnew>=_hmax) hnew=_hmax;
	if (_h!=hnew) {
	  _h=hnew;
	  stagen->UpdatePar(_h_Index);
	}

	break;
      case 1:	/* function cannot be computed */
      case 2:	/* no convergence */
      case 3:	/* singular matrix in linear solver */
      case 4:   /* closeness test failure */
	_h*=0.5;
	stagen->UpdatePar(_h_Index);
	if (_h<_hmin) {
          Cont=0; 
	  Ret=1;
	  DefaultProcessor(1,X,V);
          MsgProc(0,X,"Current step size too small");
          continue;
        }
	goto newpoint;
    }
    /* ADAPTATION */
    if (_nadapt > 0) {
      if (np%_nadapt == 0) {
        ((AdapterFuncPtr)(genData->AdapterFunc[0]))(Z,W);
      }
    }
    /* Issue the message */
    DefaultProcessor(0,Z,W);  /* compute global data and store the point for visualization */
    if (MsgProc(0,Z,NULL)) {
      Cont=0;
    }
end: ;
    /* Stop check  */
    switch (Checker(contData->X0, Z)) {
      case 2:		/* closed curve */
	DefaultProcessor(1,Z,W); 
	MsgProc(0,Z,NULL);
	DefaultProcessor(0,contData->X0,contData->V0);
	   for (i=0; i<nTest; i++) 
             if (ActiveTestFuns[i])                               /* for visialization */
                ((TestFuncPtr)(genData->TestFunc[i]))(contData->X0,contData->V0,&TstVl2);
           /* User-defined test functions */
           for (i=0; i<nUTest; i++) 
             if (ActiveUTestFuns[i]==UDF_MONITOR || ActiveUTestFuns[i]==UDF_DETECT) 
	        genData->udFunc(contData->X0,i,&TstVl2);
	MsgProc(0,contData->X0,"Closed curve.");
	contFunc->CopyVector(contData->X0,Z);
	contFunc->CopyVector(contData->V0,W);
      case 1:		/* number of points */
        Cont=0;
    }
  }
final:
  /* assert: W is the tangent vector at the last found point Z */
  genData->ContDataGenLen=(4*n+2)*sizeof(VAR);
  genData->ContDataGenPtr=MemNew(genData->ContDataGenLen);
  MemCopy(genData->ContDataGenPtr,Z,n*sizeof(VAR));
  MemCopy(genData->ContDataGenPtr+n,W,n*sizeof(VAR));
  MemCopy(genData->ContDataGenPtr+2*n,contData->X0,n*sizeof(VAR));
  MemCopy(genData->ContDataGenPtr+3*n,contData->V0,n*sizeof(VAR));
  genData->ContDataGenPtr[4*n]=_h;
  genData->ContDataGenPtr[4*n+1]=(VAR)npc;

  Term();
  
  return (Ret);
}