/*
   dd__o
   Any initial data-->OrbitCurve 
*/

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

#include "stagen.h"

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

#include "dd__o.h"

/*
   IMPORTANT: all headers of integrators must have identical two first fields
   in their data structures: X0 and ActiveUserTest.
*/
#include "_integr.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_DELAY	3
#define CL_PAR		4
#define CL_UTEST	5


Local(StagenDataPtr) stagenData=NULL;	/* global copy of Starter's parameter passed by content */
Local(dd__o_DataPtr) staData=NULL;	/* global copy of our own data area */
Local(StdLinAlgDataPtr) staFunc=NULL;	/* global copy of pointer to standard linea algebra functions */
Local(UtilitiesDataPtr) staUtil=NULL;	/* global copy of pointer to utility functions */
Local(Int2) npar,nphase,ndel,nmesh,nappend;	
					/* dims of par and phase spaces, number of delays, 
                                           number of mesh points, number of appended parameters =0 */
Local(Vector) x1=NULL;			/* ptr to vector - the first point produced by starter */
Local(VAR) t,Maxdel;			/* time and maximal fabs delay ax*/
Local(FloatHiPtr) P=NULL;		/* current set of params used by Decoder */
Local(FloatHiPtr) JJJ;                  /* current complete Jacobian */
Local(Boolean) NumDer1=TRUE;		/* TRUE-compute derivatives numericaly */

/* workspace */
Local(Vector) X,FF,BB,CC,DD,Xdel,X1,FF1,BB1,CC1,DD1; 
Int2 i,j,imax;
VAR *Tnew,*Xnew;
/* end workspace */            

/* Compute interpolation coefficients */
/* staData->Jinterpol: 0 - linear interpolation, 1 - cubic spline */
Local(Int2) Spline (Vector x, Vector f, Vector b, Vector c, Vector d) {
Int2 i,n,n1,n2;
VAR t;

staFunc->PrintVector(x,"Spline:\n x=");
staFunc->PrintVector(f,"Spline:\n f=");

  n=staFunc->GetVectorDim(x);
  n1=n-1;
  n2=n-2;
  switch (staData->Jinterpol) {
    case 1:		/* cubic spline */
      d[0]=x[1]-x[0];
      c[1]=(f[1]-f[0])/d[0];
      for (i=1; i<n1; i++) {	
	d[i]=x[i+1]-x[i];
	b[i]=2.0*(x[i+1]-x[i-1]);
	c[i+1]=(f[i+1]-f[i])/d[i];
	c[i]=c[i+1]-c[i];
      }
      b[0]=-d[0];
      b[n1]=-d[n2];
      c[0]=c[2]/(x[3]-x[1])-c[1]/(x[2]-x[0]);
      c[n1]=c[n2]/(x[n1]-x[n1-2])-c[n1-2]/(x[n2]-x[n1-3]);
      c[0]=c[0]*d[0]*(d[0]/(x[3]-x[0]));  
      c[n1]=-c[n1]*d[n2]*(d[n2]/(x[n1]-x[n1-3]));
      for (i=1; i<n; i++) {
	t=d[i-1]/b[i-1];
	b[i]=b[i]-t*d[i-1];
	c[i]=c[i]-t*c[i-1];
      }
      c[n1]=c[n1]/b[n1];
      for (i=n2; i>-1; i--)
	c[i]=(c[i]-d[i]*c[i+1])/b[i];
      b[n1]=(f[n1]-f[n2])/d[n2]+d[n2]*(c[n2]+2.0*c[n1]);
      for (i=0; i<n1; i++) {
	b[i]=(f[i+1]-f[i])/d[i]-d[i]*(c[i+1]+2.0*c[i]);
	d[i]=(c[i+1]-c[i])/d[i];
	c[i]=3.0*c[i];
      }
      c[n1]=3.0*c[n1];
      d[n1]=d[n2];
      break;
    case 0:		/* linear interpolation */
      for (i=0; i<n1; i++) b[i]=(f[i+1]-f[i])/(x[i+1]-x[i]);
      break;
    }
  return 0;
}
/* Recover the function value at point z */
Local(Int2) Fspline (Vector x, Vector f, Vector b, Vector c, Vector d, VAR z, VAR* ff) {
Int2 i,n;
VAR t;

  n=staFunc->GetVectorDim(x);

  for (i=0; i<n && z>=x[i++]; );	/* the body is empty here! */
  /* ASSERT z<x[i] */
  i-=2;

  t=z-x[i];

  switch (staData->Jinterpol) {
    case 1:	/* cubic spline */
      *ff=f[i]+t*(b[i]+t*(c[i]+t*d[i]));
      break;
    case 0:	/* linear interpolation */
      *ff=f[i]+t*b[i];
      break;
  }
  
  return 0;
}

#ifdef WIN32
static int _USERENTRY o_SortCmp(const void *f, const void *s) {
#else
Entry(int) o_SortCmp(const void *f, const void *s) {
#endif
  if (*(VARPTR )f==*(VARPTR )s) return 0;
  return (*(VARPTR )f<*(VARPTR )s) ? -1 : 1;
}

/* Initialize function common for Starter and Decoder */
Local(Int2) Init(StagenDataPtr stagenptr) {
  /* Set local pointers to data area and functions array */
  stagenData=stagenptr;
  staData=(dd__o_DataPtr)stagenptr->StaPtr;
  /* Use standard linea algebra */
  staFunc=(StdLinAlgDataPtr)stagenptr->StdLinAlg;
  staUtil=(UtilitiesDataPtr)stagenptr->Utilities;
  /* Get dimensions of Phase, Delay and Par spaces */
  npar=*stagenptr->ClassDim[CL_PAR];
  nphase=*stagenptr->ClassDim[CL_PHASE];
  ndel=*stagenptr->ClassDim[CL_DELAY];
  
  /* Call udf SetInitPoint function, if any */
  if (!stagenData->ContDataGenLen && FuncParActive(staData->SetInitPoint)) {	
    /* there is UF to set InitPoint */
    Vector tmp;
    VAR PNTR work=NULL;
    Int2 Index_P;
    Boolean error=FALSE, negative=FALSE;
    Index_P=stagenptr->ParIndex(staData,P);
    staData->nmeshold=0;
    staData->time0=MemGet(sizeof(VAR)*1,FALSE);
    staData->coord0=MemGet(sizeof(VAR)*nphase,FALSE);
    if (FuncParCall(staData->SetInitPoint)(-1,staData)) {	/* init (e.g. fopen) */
	error=TRUE;
	staUtil->ErrMessage("Nonzero return code from function SetInitPoint (when ip=-1)"); 
    } else {       
      while (FuncParCall(staData->SetInitPoint)(staData->nmeshold,staData)==0) { /* while there is a point */
        (staData->nmeshold)++;
        if (work) work=MemMore(work,sizeof(VAR)*(nphase+1)*staData->nmeshold);
        else work=MemGet(sizeof(VAR)*(nphase+1),FALSE);
        MemCopy(work+(nphase+1)*(staData->nmeshold-1),staData->time0,sizeof(VAR)*1);
        MemCopy(work+(nphase+1)*(staData->nmeshold-1)+1,staData->coord0,sizeof(VAR)*nphase);
      }
      FuncParCall(staData->SetInitPoint)(-2,staData);	/* term (e.g. fclose) */
    }
    staData->coord0=MemFree(staData->coord0);
    staData->time0=MemFree(staData->time0);
    qsort(work,staData->nmeshold,sizeof(VAR)*(nphase+1),o_SortCmp);
    staData->Tau=MemFree(staData->Tau);
    staData->X=MemFree(staData->X);
    staData->lenT=sizeof(VAR)*staData->nmeshold;
    staData->Tau=MemGet(staData->lenT,FALSE);		/* old mesh points */
    staData->lenX=sizeof(VAR)*nphase*staData->nmeshold;
    staData->X=MemGet(staData->lenX,FALSE);	/* old components */
    for (i=0; i<staData->nmeshold; i++) {
      staData->Tau[i]=work[i*(nphase+1)];
      MemCopy(staData->X+i*nphase,work+i*(nphase+1)+1,sizeof(VAR)*nphase);
    }   
    work=MemFree(work);

    /* find maximal delay: the delay interval [-Maxdel,0]*/
    imax=0;
    Maxdel=0.0;
    if (!staData->DT==NULL) {
    Maxdel=staData->DT[0];
      for (i=0; i<ndel; i++) {
        if (staData->DT[i]<0) negative=TRUE;
        if (staData->DT[i]>Maxdel) {
          imax=i;
          Maxdel=staData->DT[i];
        }   
      }
    }

    /* check the input */
    if (staData->nmeshold<1) {
      error=TRUE;
      staUtil->ErrMessage("Nmesh must be positive");
    }
    if (!error && negative) {
      error=TRUE;
      staUtil->ErrMessage("All delays must be nonnegative");
    }
    if (!error && staData->Tau[0]!=-Maxdel) {
      error=TRUE;
      staUtil->ErrMessage("The left mesh point must correspond to the maximal delay");
    }
    if (!error && staData->Tau[staData->nmeshold-1]!=0.0) {
      error=TRUE;
      staUtil->ErrMessage("The right mesh point must be zero");
    }
    for (i=1; i<staData->nmeshold; i++)
      if (!error && staData->Tau[i-1]==staData->Tau[i]) {
	error=TRUE;
	staUtil->ErrMessage("Two identical mesh points");
	break;
      }
    if (error) {
      staData->Tau=MemFree(staData->Tau);
      staData->X=MemFree(staData->X);
      staData->lenT=0;
      staData->lenX=0;
      return 10;
    }
    stagenptr->UpdatePar(Index_P);
  } else {
    if (staData->Tau==NULL) {
      staUtil->ErrMessage("The initial point was not selected.\n"
                          "Specify InitPointProc or pickup a computed point.");
      return 1;
    }
  }

   staData->lenT=sizeof(VAR)*staData->nmesh;
   staData->lenX=sizeof(VAR)*staData->nmesh*nphase;
   Tnew=MemGet(staData->lenT,FALSE);	
   Xnew=MemGet(staData->lenX,FALSE);	
  
  /* interpolate to the reqested number of mesh points */
  if (staData->nmeshold!=staData->nmesh) {
    FF=staFunc->CreateVector(staData->nmeshold);
    X=staFunc->CreateVector(staData->nmeshold);
    BB=staFunc->CreateVector(staData->nmeshold);
    CC=staFunc->CreateVector(staData->nmeshold);
    DD=staFunc->CreateVector(staData->nmeshold);
    MemCopy(X,staData->Tau,sizeof(VAR)*staData->nmeshold);
    for (i=0; i<staData->nmesh; i++)
      Tnew[i]=-Maxdel+i*Maxdel/(staData->nmesh-1);
    /* componentwise spline */
    for (i=0; i<nphase; i++) {
      for (j=0; j<staData->nmeshold; j++)
	FF[j]=staData->X[i+j*nphase];
      Spline(X,FF,BB,CC,DD);
      for (j=0; j<staData->nmesh; j++)
	Fspline (X, FF, BB, CC, DD, Tnew[j], &Xnew[i+j*nphase]);  
    }
    FF=staFunc->DestroyVector(FF);
    X=staFunc->DestroyVector(X);
    BB=staFunc->DestroyVector(BB);
    CC=staFunc->DestroyVector(CC);
    DD=staFunc->DestroyVector(DD);
    staData->Tau=MemFree(staData->Tau);
    staData->X=MemFree(staData->X);
    staData->Tau=Tnew;
    staData->X=Xnew;
    staData->nmeshold=staData->nmesh;
  }
  
  
/********
 FF=staFunc->CreateVector(staData->nmesh*nphase);
 for (i=0; i<nphase; i++) {
      for (j=0; j<staData->nmesh; j++) FF[j]=staData->X[i+j*nphase];
 }
 staFunc->PrintVector(FF,"staData->X:");
 FF=staFunc->DestroyVector(FF);
********/

  /* Do all the checks related to active parameters and find them 
  for (i=0,nappend=0; i<nUTest; i++) if (ActiveUTestFuns[i]==UDF_APPEND) nappend++;
  active=MemNew(sizeof(Int2)*(nappend+1));    
  if (staUtil->CheckParameters(staData->Active,npar,active,1+nappend)) {
    active=MemFree(active);
    return 1;
  } */
  /* Checks related to analytical/numerical differentiation of RHS */
  if (!stagenData->Der1 && staData->dx <= 0) {
    staUtil->ErrMessage("Analytical Jacoby matrix is undefined.\nRecompile system or set positive 'Increment'");
    return 2;
  }
  NumDer1=stagenData->Der1==NULL;
  if (!stagenData->Der2 && staData->dx <= 0) {
    staUtil->ErrMessage("Analytical Hessian matrix is undefined.\nRecompile system or set positive 'Increment'");
    /* active=MemFree(active); */
    return 3;
  }
  if (!stagenData->Der3 && staData->dx <= 0) {
    staUtil->ErrMessage("Analytical matrix of third derivatives is undefined.\nRecompile system or set positive 'Increment'");
    /* active=MemFree(active); */
    return 4;
  }
  if (staData->dx <= 0 && nappend > 0) {
    staUtil->ErrMessage("Set positive 'Increment'");
    /* active=MemFree(active); */
    return 5;
  }
  if (staData->nmesh<1) {
    staUtil->ErrMessage("Set positive 'nmesh'");
    /* active=MemFree(active); */
    return 6;
  }
  nmesh=staData->nmesh;
  FF1=staFunc->CreateVector(nmesh);   
  X1=staFunc->CreateVector(nmesh);
  BB1=staFunc->CreateVector(nmesh);
  CC1=staFunc->CreateVector(nmesh);
  DD1=staFunc->CreateVector(nmesh);
  FF=staFunc->CreateVector(nmesh+1);   /* new point is added */
  X=staFunc->CreateVector(nmesh+1);
  BB=staFunc->CreateVector(nmesh+1);
  CC=staFunc->CreateVector(nmesh+1);
  DD=staFunc->CreateVector(nmesh+1);
  
  Xdel=staFunc->CreateVector(nphase*(ndel+1));
		/* Xdel has the following structure:
		   x(0) x(\tau_1) ... x(\tau_ndel),
		   ndel is the number of delays.
		*/
		
  /* Create a copy parameters */
  if (!P) {
    P=MemNew(sizeof(FloatHi)*npar);
    MemCopy(P,staData->P,sizeof(FloatHi)*npar);
    /* Allocate memory for values of rhs used by Decoder/Define/Tests */
    x1=staFunc->CreateVector(nphase+1);   /* [0] - time */
  }
  stagenData->BifDataOutLen=0;
  ((IntegrDataPtr)stagenptr->GenPtr)->ActiveUserTest=staData->ActiveUserTest;
  return 0;
}

Local(void) Term (Boolean OK) {
    if (OK&&P&&(stagenData->Reason==RF_CLEAN)) 
        stagenData->ProcessPoint(stagenData->ProcFuncNum, x1, "Last point");
    if (P) {
      P=MemFree(P);
      x1=staFunc->DestroyVector(x1);
      FF=staFunc->DestroyVector(FF);
      X=staFunc->DestroyVector(X);
      BB=staFunc->DestroyVector(BB);
      CC=staFunc->DestroyVector(CC);
      DD=staFunc->DestroyVector(DD);
      FF1=staFunc->DestroyVector(FF1);
      X1=staFunc->DestroyVector(X1);
      BB1=staFunc->DestroyVector(BB1);
      CC1=staFunc->DestroyVector(CC1);
      DD1=staFunc->DestroyVector(DD1);
      Tnew=MemFree(Tnew);
      Xnew=MemFree(Xnew);
    }
}

Entry(Int2) o_ProcDefault(Int2 Ind, Vector x, Vector v1, CharPtr *msg);

/* Any point  -> Orbit Starter */

Entry(Int2) o_Starter(StagenDataPtr stagenptr) {
  Int2 i,Ret;
  if (!stagenptr) {	/* this is request to terminate */
    Term(TRUE);
    return 0;
  }

  if (Init(stagenptr)) return 1;	/* initialize local variables */
  
/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

{{{

* Test RHS *
Vector x,p,f;
int i,j;
x=staFunc->CreateVector(nphase*(ndel+1));
p=staFunc->CreateVector(npar);
f=staFunc->CreateVector(nphase);
* x has the following structure:
   x(0) x(\tau_1) ... x(\tau_ndel),
   ndel is the number of delays.
*
* suppose nphase=ndel=2, npar=0 *
x[0]=0.123; x[1]=0.234;	* 'undelayed' phase var - x(t),y(t) *
x[2]=0.345; x[3]=0.456;	* x(t-\tau_1), y(t-\tau_1) *
x[4]=0.567; x[5]=0.678;	* x(t-\tau_2), y(t-\tau_2) *
stagenData->Rhs(x,p,NULL,f);	* NULL is for time *
printf("RHS: %g %g\n",f[0],f[1]);

* Test Der1 *
*
  Each line of Jac has derivatives in respect to:
  x(0) x(\tau_1) ... x(\tau_ndel) par
  in that order. Total nphase+nphase*ndel+npar values.
*
JJJ=stagenData->Der1(x,p,NULL);	* NULL is for time *
printf("Jac:\n");
for (i=0; i<nphase; i++) {
  for (j=0; j<nphase+nphase*ndel+npar; j++)
    printf(" %g",(JJJ+i*(nphase+nphase*ndel+npar))[j]);
  printf("\n");
}

* End of Test *
staFunc->DestroyVector(x);
staFunc->DestroyVector(p);
staFunc->DestroyVector(f);
return 1;

}}}

*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/



  /* Create the first point for generator */
  x1[0]=*stagenptr->ClassDim[CL_TIME] ? *staData->T : *staData->Tdef; 
  MemCopy(x1+1,staData->X+(nmesh-1)*nphase,sizeof(FloatHi)*nphase);  
  ((IntegrDataPtr)stagenptr->GenPtr)->X0=x1;
  /* That's all */
  return 0;
}

/***********/
/* Decoder */
Entry(Int2) o_Decoder(VoidPtr vpoint, FloatHiPtr PNTR indirect, Int2 oper) {
printf("Decoder...\n");
  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 1;
    default:		/* extract next M-point from G-point pointed by vpoint */
      /* vpoint is a Vector */
      indirect[CL_TIME]=(VAR *)vpoint;
      indirect[CL_PHASE]=(VAR *)vpoint+1;
      indirect[CL_PAR]=P;
      indirect[CL_DELAY]=staData->DT;
  }
  return 0;
}

/**************/
/* System RHS */
Entry(Int2) o_DefRHS (Vector x, Vector y) {

  if (Maxdel==0.0) { 	/* no delays */
    MemCopy(Xdel,staData->X,sizeof(VAR)*nphase);
  } else {  		/* componentwise spline for delay times */
    MemCopy(X1,staData->Tau,sizeof(VAR)*nmesh);
    for (i=0; i<nphase; i++) {
      for (j=0; j<nmesh; j++) {
        FF1[j]=staData->X[i+j*nphase];
      }
      Spline(X1,FF1,BB1,CC1,DD1);
printf("tdel=%g\n",x[0]-staData->DT[0]);
      for (j=0; j<ndel+1; j++) Fspline(X1,FF1,BB1,CC1,DD1,x[0]-staData->DT[j],&Xdel[i+j*nphase]);  
    }
  }
staFunc->PrintVector(Xdel,"Xdel=");
  stagenData->Rhs(Xdel,P,&x[0],y+1);	/* x[0] - time */
  return 0;
}

/**************/
/* System Jacobian */
Entry(Int2) o_DefJAC (Vector x, Matrix Jac) {
  int i,j;
  JJJ=stagenData->Der1(x+1,P,&x[0]);    /* 0-component is for time */
  for(i=0;i<nphase;i++) MemCopy(Jac[i],JJJ+(nphase+npar)*i,sizeof(FloatHi)*nphase);
  return 0;
}

/*********************/
/* Default processor */
Entry(Int2) o_ProcDefault(Int2 Ind, Vector x, Vector v1, CharPtr *msg) {
  /* Assert: v1==NULL, msg==NULL, Ind==0 */

staFunc->PrintVector(x,"DefaultProc: x=");  

  MemCopy(x1,x,sizeof(VAR)*(nphase+1));     /* for the last point */

  if (Maxdel==0.0) { /* case of no delays: Maxdel=0, nmesh=1 */
     staData->Tau[0]=x[0];
     MemCopy(staData->X,x+1,sizeof(VAR)*nphase);
     return 0;
  }

/* Double call of Default Proc */
  if (staData->Tau[nmesh-1]==x[0]) return 0;

  for (i=0; i<nmesh; i++) Tnew[i]=x[0]-Maxdel+i*Maxdel/(nmesh-1);  
  
  /* componentwise spline */
  MemCopy(X,staData->Tau,sizeof(VAR)*nmesh);
  X[nmesh]=x[0];
staFunc->PrintVector(X,"X="); 
  for (i=0; i<nphase; i++) {
    for (j=0; j<nmesh; j++) {
      FF[j]=staData->X[i+j*nphase];
    }
    FF[nmesh]=x[1+i];
    Spline(X,FF,BB,CC,DD);
    for (j=0; j<nmesh; j++) Fspline (X, FF, BB, CC, DD, Tnew[j], &Xnew[i+j*nphase]);  
  }  
  MemCopy(staData->Tau,Tnew,sizeof(VAR)*nmesh);
  MemCopy(staData->X,Xnew,sizeof(VAR)*nmesh*nphase);
  return 0;
}