/* The name of this program is pendulum.c */
/* Created by AUTOLEV 3.2 on Wed Jun  9 15:24:36 2004 */

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

/* Header Files Added */
#include <array.h>
#include <chplot.h>
/* Header Files Added */

#define  _NAN      9.99999999999999E+305

/* Constant Definitions Added */
#define POINTS     101 // Number of data points
/* Constant Definitions Added */

void     eqns1     (double T, double VAR[], double VARp[], char boundary);
void     output    (FILE *Fptr[], double T);
void     readf     (FILE *Fp, char message[], double *next, ...);
void     pgets     (FILE *Fp, char line[], double *x);
void     writef    (FILE *Fp, char format[], ...);
int      kutta     (void(*eqns)(double, double*, double*, char),
                    int numy, double y[], double *t, double integstp,
                    double abserr, double relerr, char com);

double   A1,B1,B2,B3,G,LA,LB,MA,MB;
double   Q1,Q2,U1,U2;
double   Q1p,Q2p,U1p,U2p;
double   Pi,DEGtoRAD,RADtoDEG;

/* variables added */
class CPlot plot;
int count = 0;
double time[POINTS];
double angles[POINTS];
/* variables added */

/* ................................ MAIN ............................. */
int      main         (void)
{
FILE     *Fptr[3];
char     string[256];
int      iprint, printint, iloop;
double   T, tinitial,tfinal,integstp,abserr,relerr,_printint;
double   VAR[4];

/* Open input and output files */
for(iloop=0;  iloop<=2;  iloop++)
  {
  if( !iloop ) strcpy(string, "pendulum.in");
  else sprintf(string, "pendulum.%d", iloop);
  if( (Fptr[iloop] = fopen(string, iloop ? "w" : "r")) == NULL)
    {printf("Error: unable to open file %s\n", string);  exit(0);}
  }
 
/* Read message from input file */
readf(Fptr[0],string,NULL);

/* Read values of constants from input file */
readf(Fptr[0],NULL,&A1,&B1,&B2,&B3,&G,&LA,&LB,&MA,&MB,NULL);

/* Read the initial value of each variable from input file */
readf(Fptr[0],NULL,&Q1,&Q2,&U1,&U2,NULL);

/* Read integration parameters from input file */
readf(Fptr[0],NULL,&tinitial,&tfinal,&integstp,&_printint,&abserr,&relerr,NULL);
printint = (int)_printint;

/* Write heading(s) to output file(s) */
fprintf(stdout,  " FILE: pendulum.1\n\n *** %s\n\n", string);
fprintf(stdout,  "        T             Q2\n"
                 "     (UNITS)         (DEG)\n\n" );
/* Add # to these statements to comment out headings */
/* Necessary when plotting using the output files */
fprintf(Fptr[1], "# FILE: pendulum.1\n#\n# *** %s#\n#\n", string);
fprintf(Fptr[1], "#        T             Q2\n"
                 "#     (UNITS)         (DEG)\n#\n" );
fprintf(Fptr[2], "# FILE: pendulum.2\n#\n# *** %s#\n#\n", string);
fprintf(Fptr[2], "#        T             Q1\n"
                 "#     (UNITS)         (DEG)\n#\n" );
/* Add # to these statements to comment out headings */

/* Degrees to radians conversion */
  Pi       = 3.14159265358979323846264338327950288419716939937510582;
  DEGtoRAD = 0.01745329251994329576923690768488612713442871888541725;
  RADtoDEG = 57.2957795130823208767981548141051703324054724665643215;
  Q1 = Q1*DEGtoRAD;
  Q2 = Q2*DEGtoRAD;

/* Evaluate constants */
/* Initialize time, print counter, variables array for integrator */
  T      = tinitial;
  iprint = 0;
  VAR[0] = Q1;
  VAR[1] = Q2;
  VAR[2] = U1;
  VAR[3] = U2;

/* Initalize numerical integrator, with call to eqns1 at T=TINITIAL */
kutta(eqns1, 4, VAR, &T, integstp, abserr, relerr, 0);

/* Numerically integrate; print results */
while(1)
  {
  if( tfinal>=tinitial && T+.01*integstp>=tfinal) iprint=-7;
  if( tfinal<=tinitial && T+.01*integstp<=tfinal) iprint=-7;
  if( iprint <= 0 )
    {
    output(Fptr, T);
    if( iprint == -7 ) break;
    iprint = printint;
    }
  if( !kutta(eqns1, 4, VAR, &T, integstp, abserr, relerr, 1) )
    {
    output(Fptr, T);
    for(iloop=0;  iloop<=2;  iloop++)
      fputs( "\n\tError: Numerical integration failed to converge\n",
              iloop ? Fptr[iloop]:stdout);
    break;
    }
  iprint--;
  }

/* Inform user of input and output filename(s) */
puts( "\n Input is in the file pendulum.in" );
puts( "\n Output is in the file(s) pendulum.i  (i=1,2)" );
puts( "\n The output quantities and associated files are listed in file "
      "pendulum.dir\n" );

/* Added to Close file and plot results */
fclose(Fptr[0]);
fclose(Fptr[1]);
fclose(Fptr[2]);

plotxy(time, angles, "q2 vs. time", "t (s)", "Angle (deg)", &plot);

plot.plotting();

plot.outputType(PLOT_OUTPUTTYPE_FILE, "postscript eps color", "plot.eps");
/* or for .png file */
/* plot1.outputType(PLOT_OUTPUTTYPE_FILE, "png color", "plot.png"); */

plot.plotting();
/* Added to Close file and plot results */

return 0;
}


/* ................................ EQNS1 ............................. */
void     eqns1        (double T, double VAR[], double VARp[], char boundary)
{

/* Update variables after integration step */
  Q1 = VAR[0];
  Q2 = VAR[1];
  U1 = VAR[2];
  U2 = VAR[3];

  Q1p = U1;
  Q2p = U2;
  U1p = -(G*LA*MA*sin(Q1)+G*LB*MB*sin(Q1)-2*(B1-B2)*sin(Q2)*cos(Q2)*U1*U2)/(
  A1+B1+MA*pow(LA,2)+MB*pow(LB,2)-(B1-B2)*pow(sin(Q2),2));
  U2p = -(B1-B2)*sin(Q2)*cos(Q2)*pow(U1,2)/B3;

/* Update derivative array prior to integration step */
  VARp[0] = Q1p;
  VARp[1] = Q2p;
  VARp[2] = U1p;
  VARp[3] = U2p;

}


/* ................................ OUTPUT ............................. */
void     output (FILE *Fptr[], double T)
{
int      i1;

/* Evaluate output quantities */
/* Write output to screen and to output file(s) */
  writef(stdout, " %- 14.6E", T,(Q2*RADtoDEG),_NAN);
  writef(Fptr[1]," %- 14.6E", T,(Q2*RADtoDEG),_NAN);
  writef(Fptr[2]," %- 14.6E", T,(Q1*RADtoDEG),_NAN);

/* Added arrays for plotting */
  time[count] = T;
  angles[count] = (Q2*RADtoDEG);

  count++;
/* Added arrays for plotting */
}


/*................................... READF ................................*/
void     readf     (FILE *Fp, char message[], double *next, ...) 
{
va_list  args;                                   /* Variable Argument List  */
char     line[256];

if( message )
  {
  pgets(Fp,line,NULL);   pgets(Fp,line,NULL);   pgets(Fp,message,NULL);
  pgets(Fp,line,NULL);   pgets(Fp,line,NULL);
  }
else                                             /* Get number from file    */
  {
  for( va_start(args,next);  next;  next=va_arg(args,double *) )
    pgets(Fp,line,next);
  va_end(args);                        /* Help function make normal return  */
  }
pgets(Fp,line,NULL);                   /* Always get a newline at the end   */
}


/*................................... PGETS ................................*/
void     pgets     (FILE *Fp, char line[], double *x)
{
static   long      line_number=0;
char               *s1;

line_number++;
if( !fgets(line,256,Fp) )
  {
  printf("\n Unable to read line %ld of input file."
         "\n Premature end of file found while reading input file\n", line_number);
  exit(0);
  }
if( !x ) return;                                 /* No number expected      */
if( strlen(line) >= 60 )                         /* Expecting number here   */
  {
  *x = strtod(line+59,&s1);                      /* Get double number       */
  while( isspace(*s1) )  s1++;                   /* Skip white space to end */
  if( !*s1 ) return;                             /* Check for valid number  */
  }
printf("\n An error occured while reading line %ld of the input file."
       "\n The program was expecting to find one double precision number"
       "\n beginning with the 60th character in the following line:\n\n%s\n",
           line_number, line );
exit(0);
}


/* .................................. WRITEF .............................. */
void     writef       (FILE *Fp, char format[], ...)
{
va_list  args;                                   /* Variable Argument List  */
double   next;                                   /* Current Place in List   */

va_start(args,format);                           /* args start after format */
while( (next=va_arg(args,double)) != _NAN )  fprintf(Fp, format, next);
va_end(args);                                    /* End of variable list    */
fprintf(Fp, "\n");                               /* End with newline        */
}


/*****************************************************************************
C**                                                                         **
C** PURPOSE  Solves a set of first order ordinary differential equations    **
C**          of the form dy(i)/dt = F(t,y(1), ..., y(numeqns) (i = 1,       **
C**          ..., numeqns)                                                  **
C**                                                                         **
C** INPUT                                                                   **
C**    eqns: Subroutine that evaluates dy(i)/dt (i = 1, ..., numeqns), the  **
C**          first derivatives of y(1), ..., y(numeqns) with respect to t   **
C**                                                                         **
C** numeqns: The number of differential equations to be solved              **
C**                                                                         **
C**       y: One-dimensional array whose elements are y(1), ..., y(numeqns) **
C**                                                                         **
C**       t: Independent variable                                           **
C**                                                                         **
C** integstp: Maximum integration stepsize                                  **
C**                                                                         **
C**  abserr: Allowable absolute error in y(i)  (i=1, ..., numeqns)          **
C**                                                                         **
C**  relerr: Allowable relative error in y(i)  (i=1, ..., numeqns)          **
C**                                                                         **
C**     com: When com = 2, the Kutta-Merson algorithm (L. Fox, Numerical    **
C**          Solutions of Ordinary and Partial Differential Equations,      **
C**          Palo Alto: Addison-Wesley, 1962, pp. 24-25) is employed to     **
C**          perform the numerical solution of the differential equations.  **
C**          Accordingly, dy(i)/dt (i = 1, ..., numeqns) are evaluated at   **
C**          every integration boundary, including those at Tinitial,       **
C**          Tfinal, and ones created when integstp is halved to satisfy    **
C**          the requirements imposed by abserr and relerr.  Integration    **
C**          is self-starting at each boundary, and the occurrence, at      **
C**          boundaries, of discontinuities in derivatives does not lead    **
C**          to failure of the integration process.                         **
C**                                                                         **
C**          When com = 1, a modified form of the Kutta-Merson algorithm    **
C**          is employed.  It is nearly 20% faster than the one used when   **
C**          com = 2 because no recalculation of derivatives at inte-       **
C**          gration boundaries between Tinitial and Tfinal takes place.    **
C**          Integration is self-starting at Tinitial and Tfinal only.      **
C**          Integration may fail if any of dy(i)/dt (i = 1, ..., numeqns)  **
C**          is discontinuous between Tinitial and Tfinal.                  **
C**                                                                         **
C**          When com = 0, the function eqns is called and dy(i)/dt         **
C**          (i = 1, ..., numeqns) are evaluated, but no integration        **
C**          is performed.                                                  **
C**                                                                         **
C** OUTPUT                                                                  **
C**          The value of t+integstp is returned in t, and the values of    **
C**          y(i) at t+integstp are returned in y.                          **
C**                                                                         **
C** SOURCE                                                                  **
C**          Copyright 1995 by Paul C. Mitiguy, Thomas R. Kane, David A.    **
C**          Levinson, and David B. Schaechter.  Permission is granted      **
C**          to copy, modify, and distribute this subroutine, provided      **
C**          that this copyright notice appear.                             **
C**                                                                         **
C****************************************************************************/
int      kutta        ( void(*eqns)(double, double*, double*, char),
                        int numy, double y[], double *t,
                        double integstp, double abserr, double relerr, char com )
{
static double  f0[100], f1[100], f2[100], y1[100], y2[100];
static int     numcuts = 20;                     /* Max # cuts of integstp  */
static double  hc = 0;                           /* Last value of stepsize  */
char           entry = 1;                        /* Just entered routine    */
char           stepdouble;                       /* Double the stepsize     */
double         tfinal = *t + integstp;           /* Time at end of full step*/
double         error, test;                      /* Testing error criterion */
double         tt, h, h2, h3, h6, h8;
int            i;

if( !com ) { (*eqns)(*t,y,f0,1);  return 1;}     /* Fill array f0 and return*/
if( numy == 0)  { hc = integstp;  return 1;}     /* Check for initial entry */
if( integstp == 0)  return 0;                    /* Cannot integrate forward*/
if( hc*integstp < 0 ) hc = -hc;                  /* Integrate backward      */        
else if( hc == 0 )    hc = integstp;             /* Maybe initial entry     */
h  = hc;                                         /* Current stepsize        */
tt = *t + h;                                     /* Terminal time this step */
*t = tfinal;                                     /* Return updated t value  */

beginning:
while( tt+h != tt )                              /* Check round-off problems*/
  {
  h2 = h * 0.5;                                            /* Half    of h  */
  h3 = h / 3.0;                                            /* Third   of h  */
  h6 = h / 6.0;                                            /* Sixth   of h  */
  h8 = h * 0.125;                                          /* Eighth  of h  */
  if( com==2 || entry)                                     
 {(*eqns)( tt-h,     y, f0, 1 );   entry=0; }              /* Boundary here */
  for( i=0;  i<numy;  i++)  y1[i] = y[i] + h3*f0[i];
  (*eqns)( tt-2*h3, y1, f1, 0 );
  for( i=0;  i<numy;  i++)  y1[i] = y[i] + h6*(f0[i] + f1[i]);
  (*eqns)( tt-2*h3, y1, f1, 0 );
  for( i=0;  i<numy;  i++)  y1[i] = y[i] + h8*(f0[i] + 3*f1[i]);
  (*eqns)( tt-h2,   y1, f2, 0 );
  for( i=0;  i<numy;  i++)  y1[i] = y[i] + h2*(f0[i] - 3*f1[i] + 4*f2[i]);
  (*eqns)( tt,      y1, f1, 0 );
  for( i=0;  i<numy;  i++)  y2[i] = y[i] + h6*(f0[i] + 4*f2[i] + f1[i]);
  stepdouble = 1;                                /* Assume need to double   */
  for(i=0;  i<numy;  i++)                        /* Check all equations     */
    {
    error = fabs( y1[i] - y2[i] ) * 0.2;         /* Error in estimate       */
    test  = fabs( y1[i] ) * relerr;              /* For relative error      */
    if( error >= test  &&  error >= abserr )     /* Error criterion not met?*/
      {                                          /* Restart w/ half stepsize*/
      hc = h = h2;                               /* Halve the stepsize      */
      tt -= h2;                                  /* Change terminal time    */
      if( numcuts-- > 0 )  goto beginning;       /* Back to beginning       */
      printf("\n THE STEPSIZE HAS BEEN HALVED TOO MANY TIMES; T = %15.8E"
             "\n NUMERICAL INTEGRATION FAILED TO CONVERGE.\n", *t=tt-h );
      (*eqns)(*t,y,f0,0);                        /* Fill for error display  */
      return 0;
      }
    if( stepdouble && 64*error>test && 64*error>abserr ) stepdouble = 0;
    }
  for(i=0;  i<numy;  i++)  y[i] = y2[i];         /* Update y for next step  */
  if( stepdouble && fabs(h+h)<=fabs(integstp) && fabs(tt+h+h)<=fabs(tfinal) )
    {hc=(h+=h);  numcuts++;}                     /* Double the stepsize     */
  if( tt == tfinal )                             /* End of integration      */
    { (*eqns)(tfinal,y,f0,2);  return 1;}        /* Derivatives at tfinal   */
  tt += h;                                       /* Update terminal time    */
  /*** If next jump puts tt past or very close to tfinal, adjust h and tt ***/
  if( (h>0 && tt>tfinal-0.1*h) || (h<0 && tt<tfinal-0.1*h) )
    { h = tfinal-(tt-h);  tt = tfinal; }         /* Adjust for last jump    */
  if(com == 1)                                   /* Approx this derivative  */
    for(i=0;  i<numy;  i++)  f0[i] = f1[i];
 }
printf("\nTHE STEPSIZE OF %15.7E IS TOO SMALL RELATIVE TO THE TERMINAL TIME"
       "\nOF %15.7E.  INTEGRATION HALTED BECAUSE OF NUMERICAL ROUND-OFF.\n"
       "\nTHE STEPSIZE MAY HAVE BEEN CUT TOO MANY TIMES.\n\n", h, *t=tt);
(*eqns)(*t,y,f0,0);                              /* Fill for error display  */
return 0;
}