/*	bb.c bouncing ball program
	copyright 1985 by Nicholas B. Tufillaro
	Date written: 25 July 1985
	Date last modified: 5 August 1987
	This program simulates the dynamics of a bouncing ball subject to
	repeated collisions with an oscillating wall.
	The INPUT is:
			delta v0 Amplitude Frequency damping cycles
	where

	delta: the initial position of ball between 0 and 1, this is the
		   phase of forcing frequency that you start at (phase mod 2*PI)
	v0:	   the initial velocity of the ball, this must be greater than
		   the initial velocity of the wall.
	A:	   Amplitude of the oscillating wall
	Freq:  Frequency of the oscillating wall in hertz
	damp:  (0-1) the impact parameter describing the energy loss per
		   collision. No energy loss (conservative case) when d = 1,
		   maximum dissipation occurs at d = 0.
	cycles:the total length the simulation should be run in terms of 
		   the number of forcing oscillations.
		   Units: CGS assumed
		   Compile with: cc bb.c -lm -O -o bb
	Bugs:
*/

#include 
#include 

/* CONSTANTS (CGS Units) */

#define STEPSPERCYCLE		(256)
#define TOLERANCE			(1e-12)
#define MAXITERATIONS		(1024)
#define PI					(3.14159265358979323846)
#define G					(981)	/* earth's gravitational constant */
#define STUCK				(-1)
#define EIGHTH				(0.125)

/* Macros */

#define	max(A, B)	((A) > (B) ? (A) : (B))
#define min(A, B)	((A) < (B) ? (A) : (B))

/*	Comments of variables
	t is time since last impact
	ti is time of last impact
	tau is ti + t, time since start of simulation
	xi is position of ball at last impact
	vi is velocity of ball at last impact
	w is velocity of wall
	v is velocity of ball
*/

/* Global Variables */

double delta;	/* initial phase (position) of ball */
double v0;		/* initial ball velocity */
double A;		/* table amplitude */
double freq;	/* frequency of forcing */
double damp;	/* impact parameter */
double omega;	/* angular frequency 2*PI*f = 2*PI/T */
double T;		/* period of forcing */
double cycles;	/* length of simulation */

/* Functions */

double s(), w(), x(), v(), d(), acc();
double find_step();
double checkstep();
double root();
double fmod(), asin();

main()
{
	int stuckcount;
	double t, ti, tau, dt, tstop, xi, vi, tj, xj, vj;
	double t_alpha, t_beta, t_ph;  /* variables for sticking case */

	/* read input parameters */
	scanf("%lf%lf%lf%lf%lf%lf", &delta, &v0, &A, &freq, &damp, &cycles);
	T = 1.0/freq;
	tstop = T*cycles;
	omega = 2*PI*freq;
	delta = delta/freq;
	if( v0 < w(0.0) ) {
		printf("Error: Initial velocity less than wall velocity\n"); 
		printf("Wall velocity: %g\n", w(0.0));
		exit();
	}

	t = 0; ti = 0; tau = ti + t; xi = s(tau); vi = v0;
	dt = find_step(ti, xi, vi);
	t = dt; tau = ti + t; stuckcount = 0;
	
	while( tau <= tstop) {
		t += dt; tau = ti + t;
		if( impact(tau, t, xi, vi)) {
			t = root(t-dt, t, t, xi, vi, ti);
			tj = ti + t;
			xj = s(tj);
			vj = (1+damp)*w(tj) - damp*v(t,vi);
			t = 0; xi = xj; vi = vj; ti = tj;
			dt = find_step(ti, xi, vi);
			if (dt  == STUCK  ) { /* sticking solution */
				if (A*omega*omega < G) {
					printf("Stuck forever with table\n");
						exit();
				}
				if(fabs(G/(A*omega*omega)) < 1.0)
					t_alpha = (1.0/omega)*asin(G/(A*omega*omega));
				else
					t_alpha = T*0.25;
				t_beta = 0.5*T-t_alpha;
				dt = (t_beta-t_alpha)/STEPSPERCYCLE;
				t_ph = fmod(ti+delta,T);
				if(!((t_ph > t_alpha) && (t_ph < t_beta))) {
					stuckcount +=1;
					if (t_alpha < t_ph)
						tj = T + ti + t_alpha - t_ph;
					else
						tj = ti + t_alpha - t_ph;
					xj = s(tj); vj = w(tj);
					t = 0; xi = xj; vi = vj; ti = tj;
				}
				if(stuckcount == 2) {
				  	printf("%g %g\n", fmod(tj/T,1.0), v(t,vj));
					printf("Ball Stuck Twice\n");
					exit();
				}
			}
			if(tau > tstop/10.0)
				printf("%g %g\n", fmod(tj/T,1.0), v(t,vj));
			dt = checkstep(dt,ti,xi,vi);
			t = dt; tau = ti + dt;
		}
	}
}

double s(tau)	/* wall position */
double tau;
{
	return(A*(sin(omega*(tau+delta))+1));
}

double w(tau)	/* wall velocity */
double tau;
{
	return(A*omega*cos(omega*(tau+delta)));
}

double acc(tau)		/* table acceleration */
double tau;
{
	return(-A*omega*omega*sin(omega*(tau+delta)));
}

double x(t,xi,vi)	/* ball position */
double t, xi, vi;
{
	return(xi+vi*t-0.5*G*t*t);
}

double v(t,vi)		/* ball velocity */
double t, vi;
{
	return(vi-G*t);
}

double d(tau,t,xi,vi)	/* distance between ball and wall */
double tau, t, xi, vi;
{
	return(x(t,xi,vi)-s(tau));
}

int impact(tau,t,xi,vi)	/* find when ball is below wall */
double tau,t,xi,vi;
{
	return(d(tau,t,xi,vi) <= 0.0);
}

/* pick a good step for finding zero */
double find_step(ti,xi,vi)
double ti,xi,vi;
{
	double t_max, tstep;

	if(vi-w(ti) <= 0.0) { /* should be alpha = 0, or sticking */
		return(STUCK);
	}
	if(-acc(ti) - G != 0.0)
		t_max = fabs((w(ti)-vi)/(-acc(ti)-G));
	else 
		t_max = T/STEPSPERCYCLE;
	if(t_max < T*TOLERANCE) {
		return(STUCK);
	}
	tstep = min(EIGHTH*t_max,T/STEPSPERCYCLE);
	return(tstep);
}

double root(a,b,t,xi,vi,ti)
double a,b;
double  t, xi, vi, ti;
{
	double m;
	int count;
	count = 0;
	while(1) {
	count += 1; 
	if(count > MAXITERATIONS) {
		printf("ERROR: infinite loop in root\n");        
		exit();
	}
	if(d(ti+a,a,xi,vi)*d(ti+b,b,xi,vi) > 0.0) {
		printf("root finding error: no zero on interval\n");
		exit();
	}
	m = (a+b)/2.0;
	if((d(ti+m,m,xi,vi) == 0.0 ) || (b-a) < T*TOLERANCE)
		return(m);
	else if ((d(ti+a,a,xi,vi)*d(ti+m,m,xi,vi)) < 0.0 )
		b = m;
	else
		a = m;
	}
}

double checkstep(dt,ti,xi,vi)
double dt, ti, xi, vi;
{
	int count;

	for(count=0; d(ti+dt,dt,xi,vi) < 0.0; ++count) {
		dt = EIGHTH*dt;
		if (count > 10) {
			printf("Error: Can't calculate dt\n");
			exit();
		}
	}
	return(dt);
}