/******************************************************************************
 *
 *  EZ GL Demo.   Pendulums.
 */
#include "EZ.h"
#include <stdio.h>
#include <math.h>

static void Simulate();
static void StopSimulate();
static void hslider_callback(); 
static void hslider_callback3(); 
static void radioButton_callback(); 
static int  which_pendulum = 0;
static void hslider_callback1();  /* Damping coef  */

extern void exit();

static float  xyz[300][4];         /* the spring        */
static double pPosition[10];       /* position of the pendulums  */
static float  pendulum[10][3];     /* head and tail of pendulums */
static float  aspect = 1.0;         /* www/hhh */
/*
 * data to control the configuration of the spring
 */
int StepIs0 = 0;                       /* control vars      */
double  TotalTime;                     /* Time or phase     */
double  KKK = 0.1;                     /* coupling const    */
double  XXX[5] = {0.5, 0.0,0.0,0.0,0.0};

#define  PLength (1.0)
#define  PRadius (0.1)

/*===================================================================
 * 
 * 5 Identical pendulums connected by  weightless 'weak' springs
 * 
 *==================================================================*/

void  ROC(cstate,derivs, dim)
     double *cstate, *derivs;
     int dim; 
{
  double dtmpa[5],dtmpb[5],dtmpc[5], dtmp;
  double sines[5], cosines[5];
  int i, j;

  derivs[0] = cstate[5];
  derivs[1] = cstate[6];
  derivs[2] = cstate[7];
  derivs[3] = cstate[8];
  derivs[4] = cstate[9];
  for(i = 0; i < 5; i++)
    {
      cosines[i] = cos(cstate[i]);
      sines[i] =  sin(cstate[i]);
    }
  for(i = 0; i < 4; i++)  
    {
      j = i+1;
      dtmpa[i] = (sines[i] - sines[j] - 2.5);
      dtmpb[i] = (cosines[i] - cosines[j]);
      dtmpc[i] = sqrt(dtmpa[i] * dtmpa[i] + dtmpb[i] * dtmpb[i]);
    }
  dtmp = dtmpc[0] == 0.0 ? 0.0 :
    2.0 * (dtmpc[0] - 2.5)/ dtmpc[0] * (dtmpa[0] * cosines[0] - dtmpb[0] * sines[0]);
  derivs[5] = -KKK * ( dtmp) - sines[0];

  dtmp = 2.0 *((dtmpc[0] == 0.0? 0.0 :
		(dtmpc[0] - 2.5)/ dtmpc[0] * ( -dtmpa[0] * cosines[1] + dtmpb[0] * sines[1])) +
	       ( dtmpc[1] == 0.0 ? 0.0 :
		(dtmpc[1] - 2.5)/ dtmpc[1] * (dtmpa[1] * cosines[1] - dtmpb[1] * sines[1])));
  derivs[6] =  -KKK * ( dtmp) - sines[1];

  dtmp = 2.0 * ((dtmpc[1] == 0.0 ? 0.0 :
		(dtmpc[1] - 2.5)/ dtmpc[1] * ( -dtmpa[1] * cosines[2] + dtmpb[1] * sines[2])) +
		(dtmpc[2] == 0.0 ? 0.0 :
		 (dtmpc[2] - 2.5)/ dtmpc[2] * (dtmpa[2] * cosines[2] - dtmpb[2] * sines[2])));
  derivs[7] =  -KKK * ( dtmp) - sines[2];

  dtmp = 2.0 * ((dtmpc[2] == 0.0 ? 0.0 :
		 (dtmpc[2] - 2.5)/ dtmpc[2] * ( -dtmpa[2] * cosines[3] + dtmpb[2] * sines[3])) +
		(dtmpc[3] == 0.0 ? 0.0 :
		 (dtmpc[3] - 2.5)/ dtmpc[3] * (dtmpa[3] * cosines[3] - dtmpb[3] * sines[3])));
  derivs[8] =  -KKK * (dtmp) - sines[3];

  dtmp = 2.0 * (dtmpc[3] == 0.0 ? 0.0 :
		(dtmpc[3] - 2.5)/ dtmpc[3] * ( -dtmpa[3] * cosines[4] + dtmpb[3] * sines[4]));
  derivs[9] =  -KKK * ( dtmp) - sines[4];
}

/***************************************************/
static void event_handle();
static void make_data();
static void draw();
static void InitMaterials();

/*************************************************************************/
static EZ_Widget *StopButton, *InitSlider;
main(int argc, char **argv)
{
  EZ_Widget *frame, *canvas, *tmp, *tmpframe, *tmpF, *tmpG, *slider;
  /*
   * Create a 3DCanvas, configure it and display it.
   * This must be done before invoking any routines
   * from the EZ graphics library.
   */
  make_data();
  EZ_Initialize(argc, argv, 1);          /* initialize       */
  frame = EZ_CreateFrame(NULL,NULL);     /* a toplevel frame */
  EZ_ConfigureWidget(frame, EZ_WIDTH, 600, EZ_HEIGHT, 0, 
		     EZ_ORIENTATION, EZ_VERTICAL,
		     EZ_BACKGROUND, "bisque2",
		     EZ_FILL_MODE, EZ_FILL_BOTH, 0);

  tmp = EZ_CreateLabel(frame,"Coupled Pendulums");  /* a label  */
  EZ_ConfigureWidget(tmp, EZ_WIDTH, 0, EZ_HEIGHT, 0,
		     EZ_FOREGROUND, "red",
		     EZ_FONT_NAME, "-Adobe-Times-Bold-I-Normal--*-180-*-*-*-*-*-*",
		     0);
  
  canvas = EZ_Create3DCanvas(frame);              /* a canvas */
  EZ_ConfigureWidget(canvas,  EZ_BORDER_WIDTH, 4,
		     EZ_EVENT_HANDLE, event_handle, NULL, 0);

  /* some interfacing widgets */
  tmpframe = EZ_CreateFrame(frame, NULL);
  EZ_ConfigureWidget(tmpframe, EZ_WIDTH, 0, EZ_HEIGHT, 0,
		     EZ_FILL_MODE, EZ_FILL_HORIZONTALLY,
		     EZ_ORIENTATION, EZ_VERTICAL_CENTER, 0);

  tmpF = EZ_CreateFrame(tmpframe, "Set Initials");
  tmpG = EZ_CreateFrame(tmpF, NULL);
  EZ_ConfigureWidget(tmpG,  EZ_STACKING, EZ_VERTICAL, 0);
  {
    int i;
    EZ_Widget *rb;
    char  tmpstr[16];

    for(i = 0; i < 5; i++)
      {
	sprintf(tmpstr, "Pendulum %d",i);
	rb =  EZ_CreateRadioButton(tmpG, tmpstr,-1, 0, i);
	EZ_SetWidgetCallBack(rb, radioButton_callback, NULL);
      }
    EZ_SetRadioButtonGroupVariableValue(rb, which_pendulum);
  }
  InitSlider = EZ_CreateSlider(tmpF, NULL, -1.5707963267948966, 3.2, 0.5, 
			   EZ_HORIZONTAL_SLIDER);
  EZ_ConfigureWidget(InitSlider,EZ_CALL_BACK, hslider_callback, NULL, 0);

  tmpF = EZ_CreateFrame(tmpframe,NULL);
  EZ_ConfigureWidget(tmpF,  EZ_STACKING, EZ_HORIZONTAL_RIGHT,
		     EZ_EXPAND, 1, EZ_FILL_MODE, EZ_FILL_HORIZONTALLY,0);
  tmp = EZ_CreateLabel(tmpF, "Spring Stiffness");
  EZ_ConfigureWidget(tmp, EZ_FOREGROUND, "blue", 0);
  slider = EZ_CreateSlider(tmpF, NULL, 0.0, 2.0, 0.1, 
			   EZ_HORIZONTAL_SLIDER);
  EZ_ConfigureWidget(slider, EZ_CALL_BACK, hslider_callback3, NULL, 0);
  
  tmpframe = EZ_CreateFrame(tmpframe, NULL);
  EZ_ConfigureWidget(tmpframe, EZ_EXPAND, 1, 0);
  tmp = EZ_CreateButton(tmpframe, "Start", 0);
  EZ_SetWidgetCallBack(tmp, Simulate, NULL);
  StopButton = tmp = EZ_CreateButton(tmpframe, "Stop", 1);
  EZ_SetWidgetCallBack(tmp, StopSimulate, NULL);
  tmp = EZ_CreateButton(tmpframe, "Quit", 0);
  EZ_SetWidgetCallBack(tmp, (EZ_CallBack)exit, NULL);
  
  EZ_DisplayWidget(frame);

  /* global GL modes */
  EZ_AutoSelectBackBuffer();
  EZ_DrawBuffer(EZ_BACK);
  EZ_RGBMode();

  InitMaterials();
  EZ_Enable(EZ_CULL_FACE);
/*  EZ_Enable(EZ_DEPTH_TEST);*/
  EZ_ShadeModel(EZ_SMOOTH);

  {
    /* matrices */
    int www,hhh;
    float ftmp;

    EZ_MatrixMode(EZ_PROJECTION);
    EZ_LoadIdentity();
    EZ_Get3DCanvasSize(canvas, &www,&hhh);
    aspect = 6.5 * (float)hhh/ (float)www;
    EZ_Ortho(-6.5, 6.5, -0.5, 1.5, 4.0, 8.0);
    EZ_MatrixMode(EZ_MODELVIEW);
    EZ_LoadIdentity();
    EZ_LookAt(6.0,0.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0);
  }

  EZ_Enable(EZ_DITHER);

  pPosition[0] = XXX[0];
  pPosition[1] = XXX[1];
  pPosition[2] = XXX[2];
  pPosition[3] = XXX[3];
  pPosition[4] = XXX[4];
  pPosition[5] = 0.0;
  pPosition[6] = 0.0;
  pPosition[7] = 0.0;
  pPosition[8] = 0.0;
  pPosition[9] = 0.0;

  {
    int i;
    float ftmp = -5.5;
    for(i = 0; i < 10; i += 2)
      {
	pendulum[i][0] = 0.0;
	pendulum[i][1] = ftmp;
	pendulum[i][2] = 1.0;
	pendulum[i+1][0] = 0.0;
	ftmp += 2.5;
      }
  }
  EZ_EventMainLoop();
}
/****************************************************************/
static void radioButton_callback(widget) 
     EZ_Widget *widget;
{
  which_pendulum =  EZ_GetRadioButtonGroupVariableValue(widget);
  EZ_SetSliderValue(InitSlider, XXX[which_pendulum]);
}

static void hslider_callback(widget)
     EZ_Widget *widget;
{
  int i;
  StepIs0 = 1;
  for(i = 0; i < 5; i++)
    pPosition[i]=  XXX[i];
  
  pPosition[which_pendulum]= XXX[which_pendulum] = 
    (double) EZ_GetSliderValue(widget);  

  EZ_ConfigureWidget(StopButton, EZ_FOREGROUND, "red", 0);
  draw(0);
}


static void hslider_callback3(widget)
     EZ_Widget *widget;
{
  int i;
  StepIs0 = 1;
  for(i = 0; i < 5; i++)
    pPosition[i]=  XXX[i];

  KKK = (double) EZ_GetSliderValue(widget);  
  EZ_ConfigureWidget(StopButton, EZ_FOREGROUND, "red", 0);
  draw(0);
}

static void StopSimulate()
{
  StepIs0 = 1;
  EZ_ConfigureWidget(StopButton, EZ_FOREGROUND, "red", 0);
}

static void Simulate()
{
  int i;
  float ftmp;
  double Htry = 0.2;
  double Hnext;

  for(i = 0; i < 5; i++)
    {
      pPosition[i] = XXX[i];
      pPosition[5+i] = 0.0;
    }    
  StepIs0 = 0;
  EZ_ConfigureWidget(StopButton, EZ_FOREGROUND, "black", 0);

  TotalTime = 0.0;
  while(StepIs0 == 0)
    {
      rkqc(pPosition, Htry, &Hnext,ROC, 10);
      Htry = Hnext;

      if(TotalTime > 15000.0) StepIs0 = 1; /* don't run forever */
      draw(1);
      EZ_ServiceEvents();
    }
}


static void draw(tag)
     int tag;
{
  int i, j, k;
  float ftmp;

  for(i = 0; i < 5; i++)
    {
      j = i+i;
      k = j+1;
      pendulum[k][1] = (float)PLength * sin(pPosition[i]) + pendulum[j][1];
      pendulum[k][2] = 1.0 - (float)PLength * cos(pPosition[i]);
    }
  
  EZ_Clear(EZ_COLOR_BUFFER_BIT | EZ_DEPTH_BUFFER_BIT);
  /*
   * draw the spring.
   */
  {
    float yy, zz, ftmp, angle;
    int i, j, k;

    EZ_Color3f(0.0,0.4,0.0);
    for(i = 1; i < 9; i += 2)
      {
	j = i+2;
	yy = pendulum[j][1] - pendulum[i][1];
	zz = pendulum[j][2] - pendulum[i][2];
	ftmp = sqrt(yy*yy + zz * zz);
	angle = asin(zz/ftmp);
	angle = angle * (180.0/3.14159265358979); 
	EZ_PushMatrix();
	EZ_Translate(pendulum[i][0], pendulum[i][1], pendulum[i][2]);
	EZ_Rotate(angle, 1.0,0.0,0.0);
	EZ_Scale(1.0, ftmp,1.0);

	EZ_Begin(EZ_LINE_STRIP);
	for(k = 0; k < 300; k++)
	  EZ_Vertex3fv(xyz[k]);
	EZ_End();
	EZ_PopMatrix();       
      }    
  }
  /* draw the pendulums */
  for(i = 0; i < 10; i += 2)
    {
      EZ_Color3f(0.5,0.5,0.0);
      EZ_Begin(EZ_LINES);
      EZ_Vertex3fv(pendulum[i]);
      EZ_Vertex3fv(pendulum[i+1]);
      EZ_End(); 
    }

  EZ_Enable(EZ_LIGHTING);
  for(i = 1; i < 10; i += 2)
    {
      EZ_PushMatrix();
      EZ_Translate(pendulum[i][0],pendulum[i][1],pendulum[i][2]);
      EZ_Scale(1.0, aspect, 1.0);
      EZ_Sphere(EZ_SPHERE_QUAD, 3, 0.0,0.0,0.0,PRadius);
      EZ_PopMatrix();
    }
  EZ_Disable(EZ_LIGHTING);

  EZ_SwapBuffers();
}

static void event_handle(EZ_Widget *canvas, void *vd, int eventType, XEvent *xev)
{
  switch(eventType) {
  case EZ_REDRAW:
  case EZ_RESIZE:
    {
      int www,hhh;
      float ftmp;
      EZ_MatrixMode(EZ_PROJECTION);
      EZ_LoadIdentity();
      EZ_Get3DCanvasSize(canvas, &www,&hhh);
      aspect = 6.5 * (float)hhh/ (float)www;
      EZ_Ortho(-6.5,6.5,-0.5,1.5, 4.0, 8.0);
      EZ_MatrixMode(EZ_MODELVIEW);
    }
    draw(0);
    break;
  case EZ_KEY_PRESS:
      switch(EZ_PressedKey)
	{
	case EZ_ESCAPE_KEY:
	  exit(0);
	  break;
	}
    break;
  default:
    break;
  }
}
/***************************************************************/
static void make_data()
{
  float y,dt, dy,t;
  int i, j;
  
  dt = 6.28/16.0;
  dy = 1.0/300.0;
  t = 0.0;
  y = 0.0;

  for(i = 0; i < 300; i++)
    {
      xyz[i][0] = cos(t) * 0.04;
      xyz[i][1] = y;
      xyz[i][2] = sin(t) * 0.041;;
      xyz[i][3] = 0.01;
      t += dt;
      y += dy;
    }
}
/***************************************************************/  

static void InitMaterials()
{
    static float ambient[] = {0.1, 0.1, 0.1, 1.0};
    static float diffuse[] = {0.9, 1.0, 1.0, 1.0};
    static float position0[] = {2.7, 2.7, 2.7, 0.0};

    static float front_mat_shininess[] = {60.0};
    static float front_mat_specular[] = {1., 1., 1., 1.0};
    static float front_mat_diffuse[] = {0.9, 0.28, 0.38, 1.0};
    static float lmodel_ambient[] = {1.0, 1.0, 1.0, 1.0};

    EZ_Lightfv(EZ_LIGHT0, EZ_AMBIENT, ambient);
    EZ_Lightfv(EZ_LIGHT0, EZ_DIFFUSE, diffuse);
    EZ_Lightfv(EZ_LIGHT0, EZ_POSITION, position0);

    EZ_Enable(EZ_LIGHT0);
    
    EZ_LightModelfv(EZ_LIGHT_MODEL_AMBIENT, lmodel_ambient);

    EZ_Materialfv(EZ_FRONT_AND_BACK, EZ_SHININESS, front_mat_shininess);
    EZ_Materialfv(EZ_FRONT_AND_BACK, EZ_SPECULAR, front_mat_specular);
    EZ_Materialfv(EZ_FRONT_AND_BACK, EZ_DIFFUSE, front_mat_diffuse);
}


/*********************************************************************
 * 
 *                 Runge-Kutta integrator
 *
 *********************************************************************/

#include <stdio.h>
#include <math.h>
#define ONE_OVER_6 0.166666666666666666667
#define FCOR       0.066666666666666666667
#define DIM  16
#define MAX(x,y) ((x)>(y)?(x):(y))
#define PGROW (-0.20)
#define PSHRNK (-0.25)

#define SAFETY 0.9
#define ERRCON 0.0006
#define ONE 1.0
#define EPS 100000.0
typedef void (*vfunc)();
/**********************************************************************/
/*
 * StepIs0 is a flag used in the addaptive RK integrator (rkqc)
 * TotalTime is the accumulated time for the addaptive RK integrator
 */
extern int StepIs0;
extern double TotalTime; 
/*********************************************************************/
/* extern double yscale[], total_time; */

rkqc(yy,htry,hnext,derivs, dimension)    
     double *yy,htry,*hnext;
     vfunc derivs;
     int dimension;
{
  double dydx[DIM],ytemp[DIM],ysav[DIM],dysav[DIM];
  double step,errmax,hhl;
  int i;
  
  (*derivs)(yy,dydx, dimension);

  for(i=0; i< dimension;i++)
    {
      ysav[i]=yy[i];
      dysav[i]=dydx[i];
    }
  step = htry;
 begin:
  hhl=0.5*step;
  rk4_for_rkqc(ysav,dysav,hhl,ytemp,derivs, dimension);
  (*derivs)(ytemp,dydx, dimension);
  rk4_for_rkqc(ytemp,dydx,hhl,yy,derivs, dimension);
  StepIs0 = (step <= 0.0);
  rk4_for_rkqc(ysav,dysav,step,ytemp,derivs, dimension);
  errmax=0.0;
  for(i=0;i < dimension; i++)
    {
      double ftmp;
      ytemp[i]=yy[i]-ytemp[i];

      ftmp = fabs(ytemp[i]/* *yscale[i]*/ );
      errmax=MAX(errmax,ftmp);
    }
  errmax=errmax*EPS;
  if(errmax> ONE)
    {
      step=SAFETY*step*pow(errmax,PSHRNK);
      goto begin;
    }
  else if(errmax>ERRCON)
    *hnext=SAFETY*step*pow(errmax,PGROW);
  else
    {
      *hnext=4.0*step;
      if( *hnext > 20.0) *hnext = 20.0;
    }
  TotalTime += step;
  for(i=0; i<dimension; i++)
    yy[i]=yy[i]+ytemp[i]*FCOR;
}

rk4_for_rkqc(yy,dydx,step,yout,derivs, dimension)
     double *yy,*yout,*dydx,step;
     vfunc  derivs;
     int dimension;
{
  double yt[DIM],dyt[DIM],dym[DIM];
  double hhl, h6l;
  int i;

  hhl=0.5*step;
  h6l=step*ONE_OVER_6;

  for(i=0; i< dimension; i++) yt[i]=yy[i]+hhl*dydx[i];
  (*derivs)(yt,dyt, dimension);
  for(i=0; i< dimension; i++) yt[i]=yy[i]+hhl*dyt[i];
  (*derivs)(yt,dym, dimension);
  for(i=0; i< dimension; i++)
    {
      yt[i]=yy[i]+step*dym[i];
      dym[i]=dyt[i]+dym[i];
    }
  (*derivs)(yt,dyt, dimension);
  for(i=0; i< dimension; i++)
    yout[i]=yy[i]+h6l*(dydx[i]+dyt[i]+2.0*dym[i]);
}

/*****************************************************************
 *
 *             4th order Runge-Kutta
 *
 *****************************************************************/

rk4(pos,step,derivs, dimension)
     double *pos,step;
     vfunc derivs;
     int dimension;
{
  double  dydt[DIM],yt[DIM],dyt[DIM],dym[DIM];
  double  hhl,hl,h6l;
  int     i;

  hhl = 0.5*step;
  hl = step;
  h6l = step*ONE_OVER_6;

  (*derivs)(pos,dydt, dimension);  
  for(i=0; i<dimension; i++)
    yt[i]=pos[i]+hhl*dydt[i];
  (*derivs)(yt,dyt, dimension);  
  for(i=0; i<dimension; i++)
    yt[i]=pos[i]+hhl*dyt[i];
  (*derivs)(yt,dym, dimension);  
  for(i=0; i<dimension; i++)
    {
      yt[i]=pos[i]+hl*dym[i];  
      dym[i]=dyt[i]+dym[i];
    }
  (*derivs)(yt,dyt, dimension);  
  for(i=0; i<dimension; i++)    
    pos[i]=pos[i]+h6l*(dydt[i]+dyt[i]+2.0*dym[i]);
}
/**************************************************************/
