#include <values.h>

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

#include "stagen.h"

#include "_linalg.h"

#include "_util.h"

#include "_integr1.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 continuator's own data area */
Local(IntegrDataPtr) contData;

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

#define RHS        genData->DefFunc[0]

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

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

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

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

Local(VAR) CurTint;		/* current maximum of time */
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(Int2) _h_Index;		/* for update value of current step */

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) X,F,Z;
Local(Vector) X1,X2,X3,X4,X5,F1,F2,F3,F4;  
Local(Vector) Temp;		/* work vectors */
Local(Vector) * SuspiciousPoints;

/* Runge-Kutta with Merson modification */
Local(void) OneStep(VAR h) {	/* X -> X4 */
  RHS(X,F);
  contFunc->AddScaledVector(X,F,h/3.0,X1);
  X1[0]+=h/3.0;
  RHS(X1,F1);
  contFunc->AddScaledVector(F,F1,1.0,Temp);
  contFunc->AddScaledVector(X,Temp,h/6.0,X2); 
  X2[0]=X1[0];
  RHS(X2,F2);
  contFunc->AddScaledVector(F,F2,3.0,Temp);
  contFunc->AddScaledVector(X,Temp,h/8.0,X3); 
  X3[0]=X[0]+h/2.0;
  RHS(X3,F3);
  contFunc->AddScaledVector(F,F2,-3.0,Temp);
  contFunc->AddScaledVector(Temp,F3,4.0,Temp); 
  contFunc->AddScaledVector(X,Temp,h/2.0,X4);
  X4[0]=X[0]+h;
}

Local(void) Term(void) {
Int2 i;
  IndTestFuns=MemFree(IndTestFuns);
  for (i=0; i<nUTest; i++) {
    contFunc->DestroyVector(SuspiciousPoints[i]);
  }
  SuspiciousPoints=MemFree(SuspiciousPoints);
  Index=MemFree(Index);
  IndexR=MemFree(IndexR);
  contFunc->DestroyVector(TestValues);
  contFunc->DestroyVector(X);
  contFunc->DestroyVector(X1);
  contFunc->DestroyVector(X2);
  contFunc->DestroyVector(X3);
  contFunc->DestroyVector(X4);
  contFunc->DestroyVector(X5);
  contFunc->DestroyVector(F);
  contFunc->DestroyVector(F1);
  contFunc->DestroyVector(F2);
  contFunc->DestroyVector(F3);
  contFunc->DestroyVector(F4);
  contFunc->DestroyVector(Z);
  contFunc->DestroyVector(Temp);
}

/* Entry point to the generator */
Entry(Int2) rk_OrbitCont(StagenDataPtr stagen) {


  Int2 n;		        /* space dimension */
  VAR TstVl1,TstVl2,TV,tau,tau1,tau2,E;
  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);
  X1 = contFunc->CreateVector(n);
  X2 = contFunc->CreateVector(n);
  X3 = contFunc->CreateVector(n);
  X4 = contFunc->CreateVector(n);
  X5 = contFunc->CreateVector(n);
  F = contFunc->CreateVector(n);
  F1 = contFunc->CreateVector(n);
  F2 = contFunc->CreateVector(n);
  F3 = contFunc->CreateVector(n);
  F4 = contFunc->CreateVector(n);
  Z=contFunc->CopyVector(X,NULL);


/* Curve "continuation" */
  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);   

  Temp = contFunc->CreateVector(n);

  _h_Index=stagen->ParIndex(contData,hcur);	/* hcur is a real name of a continuator's data structute field */
  stagen->UpdatePar(_h_Index);

  /* 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 and tangent vector */

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

   /* STEP */    
again:
    if (fabs(_h)<_hmin && !LastPoint) {
      MsgProc(0,X,"Current step size too small");
      break;
    }
    OneStep(_h);
    RHS(X4,F4);
    contFunc->AddScaledVector(F,F3,4.0,Temp);
    contFunc->AddScaledVector(Temp,F4,1.0,Temp); 
    contFunc->AddScaledVector(X,Temp,_h/6.0,X5);
    X5[0]=X4[0];
    contFunc->AddScaledVector(X4,X5,-1.0,Temp);
    E=sqrt(contFunc->ScalProd(Temp,Temp)); 
    if (E>_eps) {
      _h/=2;
      stagen->UpdatePar(_h_Index);
      goto again;
    } else {
      contFunc->CopyVector(X4,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) */

/* Step size control */
    if (E<_eps/50.0) {
      _h*=2;
      if (fabs(_h)>_hmax) _h= _h<0 ? -_hmax : _hmax;
      stagen->UpdatePar(_h_Index);
    }
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);
}