#include <values.h>

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

#include "stagen.h"

#include "_linalg.h"

#include "_util.h"

#include "_integr.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;
  NormalizeVectorPtr    NormalizeVector;
  AddScaledVectorPtr    AddScaledVector;
  CopyVectorElementsPtr CopyVectorElements;
} ContFuncPars, PNTR ContFuncParsPtr;

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

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

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

#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 nUTest        genData->udFuncNum
#define TestIter      contData->IterTestFuns

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

Local(VAR) CurTint;	/* current end time */
Local(VAR) _h;		/* current step */
Local(VAR) _hlast;	/* last (uncutted) step */

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

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[0] == CurTint) return 1;
  
  if (((_h > 0)&&(X[0]+_h > CurTint))||((_h < 0)&&(X[0]+_h < CurTint))) {
    _h=CurTint-X[0];
    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,X4,F,Z;			/* work vectors */

Local(void) Term (void) {
Int2 i;
  MemFree(IndTestFuns);
  for (i=0; i<nUTest; i++) {
    contFunc->DestroyVector(SuspiciousPoints[i]);
  }
  MemFree(SuspiciousPoints);
  MemFree(Index);
  MemFree(IndexR);
  contFunc->DestroyVector(TestValues);
  contFunc->DestroyVector(X);
  contFunc->DestroyVector(X4);
  contFunc->DestroyVector(F);
  contFunc->DestroyVector(Z);
}		

/* Euler modification */
Local(void) OneStep(VAR h) {	/* X -> X4 */
printf("OneStep:\n");

   RHS(X,F);
   contFunc->AddScaledVector(X,F,h,X4);
   X4[0]=X[0]+h;
}

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


  Int2 n;		        /* space dimension */
  VAR TstVl1,TstVl2,TV,tau,tau1,tau2;
  int i,iu,j,l,resit,ii,iter;
  int Cont=1,Ret=0;
  Boolean ZeroTest;

  CharPtr msgtext;
  char msgtxt[80];
  
  if (!stagen) {
    Term();
    return Ret;
  }
  genData=stagen;
  contData=(IntegrDataPtr)stagen->GenPtr;
  contFunc=(ContFuncParsPtr)stagen->GenFunc;

  LastPoint=FALSE;
  
  IndTestFuns=MemNew(nUTest);
  SuspiciousPoints=MemNew(nUTest*sizeof(Vector *));
  Index=MemNew(nUTest*sizeof(int));
  IndexR=MemNew(nUTest*sizeof(int));

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

  X = contFunc->CreateVector(n);
  X4 = contFunc->CreateVector(n);
  F = contFunc->CreateVector(n);
  Z = contFunc->CreateVector(n);


/* Orbit extension */

  if (genData->ContDataGenLen) { 
    MemCopy(X,genData->ContDataGenPtr,n*sizeof(VAR));
    _h=genData->ContDataGenPtr[n];
    CurTint=genData->ContDataGenPtr[n+1];
    genData->ContDataGenPtr=MemFree(genData->ContDataGenPtr);  
  } else {
    if (genData->Forward) {
      CurTint=((FloatHiPtr)(contData->X0))[0]+fabs(_Tint);
      _h=fabs(_h0);
    } else {
      CurTint=((FloatHiPtr)(contData->X0))[0]-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;
      }
    }
  /* Regular step */
  while (Cont) {

    contFunc->CopyVector(Z,X);   /* new point */

    /* Issue the message */
    if (MsgProc(0,X,NULL)) {
       Cont=0;
       continue;
    }
printf("AfterMsgProc:\n");


   /* STEP */    
again:
    OneStep(_h);
    contFunc->CopyVector(X4,Z);
    DefaultProcessor(Z);
    
    /* Location zeros of User Test Funstions */
    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;
    }
  } /* while */
final:
  DefaultProcessor(X);
  /* assert: V is the tangent vector at the last found point X */
  genData->ContDataGenLen=(n+2)*sizeof(VAR);
  genData->ContDataGenPtr=MemNew(genData->ContDataGenLen);
  MemCopy(genData->ContDataGenPtr,X,n*sizeof(VAR));
  genData->ContDataGenPtr[n]=_hlast;
  genData->ContDataGenPtr[n+1]=CurTint;

  Term();
  
  return (Ret);
}