/*************************************************/ 
/*This program is a auto time step               */
/*Runge-Kutta method using by Fehlberg formula   */
/* modified at 18/11/2003/						 */
/*************************************************/

#include<stdio.h>
#include<stdlib.h>
#include<string.h>
#include<math.h>

void CFILE();
void FEHL(int LINDEX,int N,double Y0[],double YN[]);
void FUNC(double T,double X[],int N);
void DOUT();
void PARM();
void PLO(unsigned int I);
void RG2(double XE,double ABSERR,int NUM);
void SOLV(double T,double Y[]);
void INIT();
void OFILE();
int ICHEK(unsigned int i, unsigned int j,unsigned int k);
int INDEX(char a[],char b[]);
double DSIGN(double a,double b);
double FU(int I,double T,double X[]);
FILE *fp1,*fp2;
unsigned int NS,ING;
unsigned int ITC;
unsigned int ND;
unsigned int NT,NOT;
unsigned int NOUT;
double ERR[130];
double T0,T1,TI;
extern double H,X[130],T;
extern double DX[130];
double DE[40],DF[40];
extern double OP[100];
double PT[4];
extern double PA[999];
double XX[100];
char LABEL[100][20];
int ERRNUM[130];

main(){
  unsigned int i,j=0,k=0;
  int NUM;
  double q,ABSERR;


  T0=0;T1=0;TI=0;
  H=0;T=0;
  for(i=0;i<=129;i++)X[i]=0.0;
  for(i=0;i<=129;i++)DX[i]=0.0;
  for(i=0;i<=39;i++)DE[i]=0.0;
  for(i=0;i<=39;i++)DF[i]=0.0;
  for(i=0;i<=99;i++)OP[i]=0.0;
  for(i=0;i<=3;i++)PT[i]=0.0;
  for(i=0;i<=299;i++)PT[i]=0.0;
  for(i=0;i<=99;i++)XX[i]=0.0;
  OFILE();
  PARM();
  INIT();

  q=0.001;/*absolute acceptable error*/
  ABSERR=q;
  DOUT();
  PLO(0);
  NUM=1;

  RG2(T1,ABSERR,NUM);
  NOT=NUM;
  CFILE();
 
}

void SOLV(double T,double Y[]){
/* SOLVING CONSTRAINT EQUATIONS
   IF DEVIATION RATIO IS LESS THAN RELATIVE ERROR(D0)
   THEN THE PROGRAM OBTAINS SOLUTION */

  int i,I0,I1,ICHK;
  double DE1[40],DF1[40],D0,D1,D2;
  if(ND!=0){
    I0=ND/2;
    D0=1.0e-08;
/* D0 : RELATIVE ERROR */
    while(1){
      ICHK=0;
/* ICHK : INDICATOR FOR TERMINATION
        =1 ONE MORE REITERATION
        =0 TEMINATION */
      for(i=0;i<=I0-1;i++){
	D1=fabs(DE[i])*D0;
	I1=2*i-1;
	DE1[i]=FU(I1,T,Y);
	D2=fabs(DE1[i]-DE[i]);
	if(D2>D1) ICHK=1;
	DE[i]=DE1[i];
	
	D1=fabs(DF[i])*D0;
	if(D2>D1) ICHK=1;
	DF[i]=DF1[i];
      }
      if(ICHK==0) break;
    }
  }
}

void PLO(unsigned int I){
  unsigned int j,k;
  char CDIV[2]={','};

  if(I==0){
    fprintf(fp2,"TIME         ,");
	printf("TIME          ");
    for(j=0;j<=NOUT-1;j++){
      if(j==NOUT-1){
                for(k=0;k<strlen(LABEL[j+1]);k++) 
                  if(LABEL[j+1][k]==' ') LABEL[j+1][k]='\0';
		fprintf(fp2," %s", LABEL[j+1]);
		printf(" %s",LABEL[j+1]);
	  }else{
                for(k=0;k<strlen(LABEL[j+1]);k++) 
                  if(LABEL[j+1][k]==' ') LABEL[j+1][k]='\0';
		fprintf(fp2,"%s%s",LABEL[j+1],CDIV);
		printf(" %s%s",LABEL[j+1],CDIV);
	  }
	  if(!((j+1)%4)){

	    printf("\n");
        printf("              ");
	  }
	}
    fprintf(fp2,"\n");
    fprintf(fp2,"%e%s",T,CDIV);
	printf("\n");
    printf("%e%s",T,CDIV);
    for(j=0;j<=NOUT-1;j++){
	  if(j==NOUT-1){
		if(OP[j]>=0){
		  fprintf(fp2," %e",OP[j]);
		  printf(" %e",OP[j]);
		}else{
		  fprintf(fp2,"%e",OP[j]);
		  printf("%e",OP[j]);
		}
	  }else{
		if(OP[j]>=0){
		  fprintf(fp2," %e%s",OP[j],CDIV);
		  printf(" %e%s",OP[j],CDIV);
		}else{
		  fprintf(fp2,"%e%s",OP[j],CDIV);
		  printf("%e%s",OP[j],CDIV);
		}
	  }
	  if(!((j+1)%4)){
	  printf("\n");
	  printf("              ");
	  }
	}
    fprintf(fp2,"\n");
	printf("\n");
  }else{
    fprintf(fp2,"%e%s",T,CDIV);
	printf("%e%s",T,CDIV);
    for(j=0;j<=NOUT-1;j++){
	  if(j==NOUT-1){
		if(OP[j]>=0){
		  fprintf(fp2," %e",OP[j]);
		  printf(" %e",OP[j]);
		}else{
		  fprintf(fp2,"%e",OP[j]);
		  printf("%e",OP[j]);
		}
	  }else{
		if(OP[j]>=0){
		  fprintf(fp2," %e%s",OP[j],CDIV);
		  printf(" %e%s",OP[j],CDIV);
		}else{
		  fprintf(fp2,"%e%s",OP[j],CDIV);
		  printf("%e%s",OP[j],CDIV);
		}
	  }
	  if(!((j+1)%4)){
		printf("\n");
        printf("              ");
	  }
    }
    fprintf(fp2,"\n");
	printf("\n");
  }
}
  
void RG2(double XE,double ABSERR,int NUM){
	int i,ITEMAX,N,ISTIFF,ICOUNT,IER,ITER,LINDEX;
	double EPSMIN,RELERR,TW,TOUT,TOL,TOLDY,TT,ERRET,H1;
	double HMIN;
	double YH0[130],YHN[130],YL0[130],ET[130],YLN[130];
	double ERRMAX;
	double NTIME;
	ITEMAX=1000000;
	EPSMIN=1.0e-15;
	RELERR=0.0e+0;
	TW=(T1-T0)/NOT;
	TOUT=T+TW;

	N=NS+ING;
	for(i=0;i<N;i++) ERR[i]=0.0;
	/* INITIALIZATION */
	for(i=0;i<N;i++) YL0[i]=X[i];
	/* 初期刻み値　H 決定*/
	SOLV(T,YL0);
	FUNC(T,YL0,N);
	TOLDY=10.0e+74;
	for(i=0;i<N;i++){
		if(DX[i]!=0.0){
			TOL=RELERR*fabs(YL0[i])+ABSERR;
		if(TOLDY>(TOL/fabs(DX[i])))TOLDY=TOL/fabs(DX[i]);
		}
	}
	HMIN = EPSMIN*(XE-T);
	if(TOLDY == 10.0e+74)
		H=HMIN;
	else{
		if((XE-T)/100.0 > pow(TOLDY,0.2) )
			H= pow(TOLDY,0.2);
		else
			H= (XE-T)/100.0;
	}
	/*ここを書き直す 18/11/2003
	HMIN=EPSMIN*(XE-T)
	if(TOLDY==10.0D+74)
	H=HMIN;
	else
	H = MIN((XE - T) / 100.0D+0, TOLDY ** 0.2D+0)
	*/

	ISTIFF=0;
	ICOUNT=0;
	IER=0;

	/* MAIN ITERATION */
	for(ITER=1;ITER<=10*ITEMAX;ITER++){
		for(i=0;i<N;i++){
			X[i]=YL0[i];
			YLN[i]=X[i];
		}

		TT=T;
		LINDEX=1;
		FEHL(LINDEX,N,YL0,YLN);

		ERRET=0.0;
		for(i=0;i<N;i++){
			ET[i]=RELERR*0.5*(fabs(YL0[i])+fabs(YLN[i]))+ABSERR;

			if(ET[i]==0.0) goto LBL2;
			else{
				if(ERRET<(fabs(ERR[i])/ET[i])) ERRET=fabs(ERR[i])/ET[i];
			}
		}
		
		ERRMAX=0.0;
	for(i=0;i<N;i++)if(fabs(ERRMAX)<fabs(ERR[i]))ERRMAX=fabs(ERR[i]);
	
	if(ERRET>=1.0){
/* UNSUCCESSFUL STEP H resetting */
	  T=TT;
	  if(ERRET>=59049.0) H=0.1*H;
	  else H=0.9*H/pow(ERRET,0.25);
	  if(H<=EPSMIN) 
		  goto LBL3;
	}else{

      if((TT+H)<XE){
/* SUCCESSFUL STEP (X + H < THE END POINT) */
    	for(i=0;i<N;i++){
		  YH0[i]=YL0[i];
		  YL0[i]=YLN[i];
		}
		H1=H;
LBL:	if(TOUT<=(TT+H1)){
		  T=TT;
		  H=TOUT-T;
		  LINDEX=0;
		  FEHL(LINDEX,N,YH0,YHN);
		  DOUT();
		  PLO(NUM);
		  NUM++;
		  TOUT+=TW;
		  goto LBL;
		}
	    H=H1;
	    T=TT+H;
/* CHOOSE NEXT STEP */
		NTIME=H;
	    if(ERRET<=1.889568e-4) 
			H=0.9*H*pow(ABSERR*H/TW/ERRMAX,0.25);
	    else 
			H=0.9*H*pow(ABSERR*H/TW/ERRMAX,0.25);


		if(H>=TW/1000*1.2) H=TW/1000*1.2;

	  }else{
/* SUCCESSFUL STEP (X + H > = THE END POINT */
		for(i=0;i<N;i++)YH0[i]=YL0[i];
	    while(TOUT<=XE){
		  T=TT;
		  H=TOUT-T;
		  LINDEX=0;
		  FEHL(LINDEX,N,YH0,YHN);
		  DOUT();
		  PLO(NUM);
		  NUM++;
		  TOUT+=TW;
		}
	    T=TT;
	    H=XE-T;
	    LINDEX=0;
	    FEHL(LINDEX,N,YH0,YHN);
        DOUT();
	    PLO(NUM);
	    return;
	  }
	}
  }
/* error code */
  IER=10000;
  printf(" (SUBR.-RKFDS) TROUBLE(TOO MANY ITERATIONS) AT X = %e\n",T);
  return;
LBL2: IER=20000;
  printf(" (SUBR.-RKFDS) TROUBLE(INAPPROPRIATE ERROR TOLERANCE) AT X = %e\n",T);
  return;
LBL3: IER=30000;
  printf(" (SUBR.-RKFDS) TROUBLE(TOO SMALL STEP SIZE) AT X = %e\n",T);
  return;
}

void FEHL(int LINDEX,int N,double Y0[],double YN[]){
  int i;
  double TT;
  double AK1[130],AK2[130],AK3[130],AK4[130],AK5[130],AK6[130];
  double W[130];
  double ONE=1.0,TWO=2.0,THR=3.0,TWL=12.0;
  double AL2,AL3,AL4,AL5,AL6,B21,B31,B32;
  double B41,B42,B43,B51,B52,B53,B54,B61,B62,B63,B64,B65;
  double GA1,GA3,GA4,GA5,GA6,DA1,DA3,DA4,DA5,DA6;
  AL2=ONE/4;AL3=THR/8;AL4=TWL/13;AL5=ONE,AL6=ONE/2;
  B21=ONE/4;B31=THR/32;B32=9.0/32;
  B41=1932.0/2197;B42=-7200.0/2197;
  B43=7296.0/2197;B51=439.0/216;
  B52=-8.0;B53=3680.0/513;
  B54=-845.0/4104;B61=-8.0/27;
  B62=2.0;B63=-3544.0/2565;
  B64=1859.0/4104;B65=-11.0/40;
  GA1=16.0/135;GA3=6656.0/12825;
  GA4=28561.0/56430;
  GA5=-9.0/50;GA6=TWO/55;
  DA1=ONE/360;DA3=-128.0/4275;
  DA4=-2197.0/75240;
  DA5=ONE/50;DA6=TWO/55;

/*-extra-*/
  for(i=0;i<N;i++) ERRNUM[i]=0;
/*------*/
  TT=T;
  for(i=0;i<N;i++) X[i]=Y0[i];
  SOLV(T,Y0);
  FUNC(T,Y0,N);
  for(i=0;i<N;i++){
	AK1[i]=DX[i];
	W[i]=Y0[i]+H*B21*AK1[i];
  }


  T=TT+AL2*H;
  SOLV(T,W);
  FUNC(T,W,N);

  for(i=0;i<N;i++){
	AK2[i]=DX[i];
	W[i]=Y0[i]+H*(B31*AK1[i]+B32*AK2[i]);

  }

  T=TT+AL3*H;
  SOLV(T,W);
  FUNC(T,W,N);

  for(i=0;i<N;i++){
	AK3[i]=DX[i];
	W[i]=Y0[i]+H*(B41*AK1[i]+B42*AK2[i]+B43*AK3[i]);
  }


  T=TT+AL4*H;
  SOLV(T,W);
  FUNC(T,W,N);
  for(i=0;i<N;i++){
	AK4[i]=DX[i];
	W[i]=Y0[i]+H*(B51*AK1[i]+B52*AK2[i]+B53*AK3[i]+B54*AK4[i]);
  }
  T=TT+AL5*H;
  SOLV(T,W);
  FUNC(T,W,N);
  for(i=0;i<N;i++){
	AK5[i]=DX[i];
	W[i]=Y0[i]+H*(B61*AK1[i]+B62*AK2[i]+B63*AK3[i]+B64*AK4[i]+B65*AK5[i]);
  }

  T=TT+AL6*H;
  SOLV(T,W);
  FUNC(T,W,N);
  for(i=0;i<N;i++){
	AK6[i]=DX[i];
	  YN[i]=X[i]+H*(GA1*AK1[i]+GA3*AK3[i]+GA4*AK4[i]+GA5*AK5[i]+GA6*AK6[i]);
	  X[i]=YN[i];
  }
  T=TT+H;
  SOLV(T,X);
  if(LINDEX==1){
	for(i=0;i<N;i++){
	  if(ERRNUM[i]!=0)ERR[i]=0.0;
	  else ERR[i]=H*(DA1*AK1[i]+DA3*AK3[i]+DA4*AK4[i]+DA5*AK5[i]+DA6*AK6[i]);
	}

	return;
  }else return;
}
		


void PARM(){
  int i,NO,N,I0;
  double V;
  char NA[4],NAM[20],LINE[72],DUMMY[20];
  
  NOUT=0;
  NOT=0;
  while(1){
    i=0;
    while((LINE[i]=fgetc(fp1))!='\n') i++;
    LINE[i]='\0';
    for(i=0;i<=2;i++) NA[i]=LINE[i];
    NA[i]='\0';
    if(strcmp(NA,"END")){
      if(!strcmp(NA,"PA ")){
	    for(i=0;i<=2;i++) DUMMY[i]=LINE[i+3];
	    DUMMY[i]='\0';
	    NO=atoi(DUMMY);
		i=0;
		while((DUMMY[i]=LINE[i+9])!=' ')i++;

	    DUMMY[i]='\0';
	    V=atof(DUMMY);
	    PA[NO]=V;
      }else{
	    if(!strcmp(NA,"SU ")){
	      for(i=0;i<=2;i++) DUMMY[i]=LINE[i+2];
	      DUMMY[i]='\0';
	      NO=atoi(DUMMY);
		  i=0;
		  while((NAM[i]=LINE[i+6])!=' ')i++;
	      NAM[i]='\0';
		}else{
	      if(!strcmp(NA,"LA ")){
	        for(i=0;i<=2;i++) DUMMY[i]=LINE[i+2];
	        DUMMY[i]='\0';
	        NO=atoi(DUMMY);
			i=0;
		    while((NAM[i]=LINE[i+7])!=' ')i++;
	        for(i=i;i<=12;i++)NAM[i]=' ';
			NAM[i]='\0';
	        NOUT++;
	        strcpy(LABEL[NO],NAM);
		  }else{
	        if(!strcmp(NA,"NS ")){
	          for(i=0;i<=4;i++) DUMMY[i]=LINE[i+5];
	          DUMMY[i]='\0';
	          NO=atoi(DUMMY);
	          NS=NO;
			}else{
	          if(!strcmp(NA,"IN ")){
		        for(i=0;i<=4;i++) DUMMY[i]=LINE[i+5];
		        DUMMY[i]='\0';
		        NO=atoi(DUMMY);
		        ING=NO;
		        N=NS+ING;
		        for(i=1;i<=N;i++) X[i-1]=0.0e-00;
			  }else{
		        if(!strcmp(NA,"ND ")){
		          for(i=0;i<=4;i++) DUMMY[i]=LINE[i+5];
		          DUMMY[i]='\0';
		          NO=atoi(DUMMY);
		          ND=NO;
		          if(ND>0){
		            I0=ND/2;
		            for(i=1;i<=I0;i++){
		            DE[i]=0.0e-00;
		            DF[i]=0.0e-00;
					}
				  }
				}else{
		          if(!strcmp(NA,"PT ")){
		            for(i=0;i<=2;i++) DUMMY[i]=LINE[i+2];
		            DUMMY[i]='\0';
		            NO=atoi(DUMMY);
					i=0;
		            while((DUMMY[i]=LINE[i+8])!=' ')i++;
		            DUMMY[i]='\0';
		            V=atof(DUMMY);
		            PT[NO]=V;
				  }else{
		            if(!strcmp(NA,"NO ")){
		              for(i=0;i<=11;i++) DUMMY[i]=LINE[i+5];
		              DUMMY[i]='\0';
		              NO=atoi(DUMMY);
		              NOT=NO;
					}else{
		              if(!strcmp(NA,"OP ")){
			            for(i=0;i<=11;i++) DUMMY[i]=LINE[i+5];
			            DUMMY[i]='\0';
			            NO=atoi(DUMMY);
			            NOUT=NO;
					  }else{
			            if(!strcmp(NA,"ST ")){
			              for(i=0;i<=2;i++) DUMMY[i]=LINE[i+2];
			              DUMMY[i]='\0';
			              NO=atoi(DUMMY);
						  i=0;
		                  while((DUMMY[i]=LINE[i+8])!=' ')i++;
			              DUMMY[i]='\0';
			              V=atof(DUMMY);
			              X[NO-1]=V;
						}else{
			              printf("      *** PARAMETER ERROR ***\n");
			              printf("      *** ILLEAGAL INPUT ***\n");
			              printf("      ...%s\n",LINE);
			              exit(1);
						}
					  }
					}
				  }
				}
			  }
			}
		  }
		}
      }
    }else{
      T0=PT[1];
      T1=PT[2];
      TI=PT[3];
      T=(T1-T0)/TI;
      NT=(int)T;
      if(NT<=NOT) NOT=NT;
      H=TI;
      T=T0;
      break;
    }
  }
}

int ICHEK(unsigned int i,unsigned int j,unsigned int k){
  int ICHEK;
  double TC;
  if(k==0){
    TC=((T1-T0)*j/NOT)/TI;
    ITC=(int)TC;
    k=1;
  }
  if(i<NT) ICHEK=i%ITC;
  else ICHEK=0;
  return(ICHEK);
}

void INIT(){
  SOLV(T,X);
}


void OFILE(){

  char DYCP[50],NACP[50];

  printf("PLEASE INPUT DATA FILE NAME SUCH AS A:/DAT/SIM.BGS **\n");
  strcpy(NACP, "temp");
  strcpy(DYCP, NACP);
  if((fp1=fopen(strcat(DYCP,".PAR"),"rt"))==NULL){
    printf("FILE NOT OPEN\n");
    exit(1);
  }
  strcpy(DYCP,NACP);
  if((fp2=fopen(strcat(DYCP,".csv"),"wt"))==NULL){
    printf("FILE NOT OPEN\n");
    exit(1);
  }
}

int INDEX(char a[],char b[]){
  int i=0,j=0,k,l,f=0;
  k=(int)strlen(a);
  l=(int)strlen(b);
  while(i<=k-1){
    if(a[i]==b[j]){
      if(j==l-1){
	f=1;break;
      } else j++;
    }else j=0;
    i++;
  }
  if(f) return(i+2-l);
  else return(0);
}

double DSIGN(double a,double b){
  if(b>=0) return(fabs(a));
  else return(-fabs(a));
}

void CFILE(){
  fclose(fp1);
  fclose(fp2);
}