/******************************************************************************
 *
 *  EZ GL Demo.  The Pendulum.
 */
#include "EZ.h"
#include <math.h>

static void Simulate();
static void StopSimulate();
static void hslider_callback0();  /* Length        */
static void hslider_callback2();  /* Position      */
static void hslider_callback3();  /* Damping coef  */

void my_exit()
{
  EZ_Shutdown();
  exit(0);
}

static int    counter = 0;         
static double pPosition[4];        /* position of the pendulum   */
static float  trace[512][3];       /* trajectory of the pendulum */
static float pendulum[2][3];
static float TheHeight;

/*
 * data to control the configuration of the spring
 */
int StepIs0 = 0;                       /* control vars      */
double  TotalTime, TimeScale = 0.4;   /* Time or phase     */
double  PLength = 3.0, PRadius = 0.35;
double  KKK = 1.0, FFF = 0.0;

/*============================================================
 * 
 * OK, let's solve an ODE
 *               x" = M g sin(x)   (x = angle)
 * with friction or whatever, add a term proportional to x', so
 *
 *               x" = -K sin(x) - F x'   (K and F are constants)
 * Or
 * 
 *               x' = y
 *               y' = -K sin(x) - F y
 *===========================================================*/
static double  X0 = 0.5;

void  ROC(cstate,derivs, dim)
     double *cstate, *derivs;
     int dim;  /* dim = 2 for our case */
{
  derivs[0] = cstate[1];
  derivs[1] = -KKK * sin(cstate[0]) - FFF *cstate[1];
}

/***************************************************/
static void event_handle();
static void make_data();
static void draw();
static void InitMaterials();
/***************************************************
 *
 * Foging stuff, simulate depthcue.
 */
static float aa[2] = { -44.0, 0};
static float bb[2] = { 10.0, 0};
static float cc[2] = { 4.0, 0};
static float fogc[4] = {0.0, 0.0,0.0,1.0};
static float fogd[1] = {3.0};
static float foge[1] = {(float)EZ_LINEAR};
static float fogs[1] = {0.4};
static float fogE[1] = {0.7};
static float cmapmode[3] = {16.0,100.0,130.0};


static EZ_Widget *StopButton;
main(int argc, char **argv)
{
  EZ_Widget *frame, *canvas, *tmp, *tmpframe, *tmpF, *sliders[4];
  /*
   * 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, 520, EZ_HEIGHT, 600, 
		     EZ_ORIENTATION, EZ_VERTICAL,
		     EZ_BACKGROUND, "bisque2",
		     EZ_FILL_MODE, EZ_FILL_BOTH, 0);

  tmp = EZ_CreateLabel(frame,"Simple Pendulum");  /* 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_HEIGHT, 0, EZ_WIDTH,0,EZ_SIDE, EZ_RIGHT,
		     EZ_ORIENTATION, EZ_VERTICAL_CENTER, 0);

  tmpF = EZ_CreateFrame(tmpframe, NULL);
  EZ_ConfigureWidget(tmpF,  EZ_STACKING, EZ_HORIZONTAL_RIGHT, 0);
  tmp = EZ_CreateLabel(tmpF, "Length");
  EZ_ConfigureWidget(tmp,  EZ_WIDTH, 0,0);
  sliders[0] = EZ_CreateSlider(tmpF, NULL, 1.0, 4.0, 3.0, 
			       EZ_HORIZONTAL_SLIDER);
  EZ_ConfigureWidget(sliders[0], EZ_WIDTH, 350,
		     EZ_CALL_BACK, hslider_callback0, NULL,0);

  tmpF = EZ_CreateFrame(tmpframe,NULL);
  EZ_ConfigureWidget(tmpF,  EZ_STACKING, EZ_HORIZONTAL_RIGHT, 0);
  tmp = EZ_CreateLabel(tmpF, "Initial Position");
  EZ_ConfigureWidget(tmp,  EZ_WIDTH, 0,0);
  sliders[2] = EZ_CreateSlider(tmpF, NULL, 0.0, 3.1415926535, 0.5, 
			       EZ_HORIZONTAL_SLIDER);
  EZ_ConfigureWidget(sliders[2], EZ_WIDTH, 350,
		     EZ_CALL_BACK, hslider_callback2, NULL, 0);

  tmpF = EZ_CreateFrame(tmpframe,NULL);
  EZ_ConfigureWidget(tmpF,  EZ_STACKING, EZ_HORIZONTAL_RIGHT, 0);
  tmp = EZ_CreateLabel(tmpF, "Damping Coefficient");
  EZ_ConfigureWidget(tmp,  EZ_WIDTH, 0,0);
  EZ_ConfigureWidget(tmp, EZ_FOREGROUND, "blue", 0);
  sliders[3] = EZ_CreateSlider(tmpF, NULL, 0.0, 0.2, 0.0, 
			       EZ_HORIZONTAL_SLIDER);
  EZ_ConfigureWidget(sliders[3], EZ_WIDTH, 350,
		     EZ_CALL_BACK, hslider_callback3, NULL, 0);
  
  tmpframe = EZ_CreateFrame(tmpframe, NULL);
  EZ_ConfigureWidget(tmpframe, EZ_EXPAND,1,
		     EZ_ORIENTATION, EZ_HORIZONTAL_CENTER, 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, my_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);
    ftmp = (float)www/ (float)hhh;
    EZ_Ortho(-5.5 * ftmp,5.5 *ftmp,-3.0,8.0, 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_FOG);
  EZ_Fogfv(EZ_FOG_COLOR,fogc);
  EZ_Fogfv(EZ_FOG_DENSITY,fogd);
  EZ_Fogfv(EZ_FOG_MODE,foge);
  EZ_Fogfv(EZ_FOG_START,fogs);
  EZ_Fogfv(EZ_FOG_END,fogE);
  EZ_Fogfv(EZ_FOG_INDEX,aa);
  EZ_Enable(EZ_DITHER);
*/
  pPosition[0] = X0;
  pPosition[1] = 0.0;
  pendulum[0][0] = 0.0;
  pendulum[0][1] = 0.0;
  pendulum[0][2] = 3.0;
  draw(0);
  EZ_EventMainLoop();
}
/****************************************************************/

static void hslider_callback0(widget, ddd)
     EZ_Widget *widget; void *ddd;
{
  StepIs0 = 1;
  PLength  =  (double) EZ_GetSliderValue(widget);  
  KKK = PLength;
  EZ_ConfigureWidget(StopButton, EZ_FOREGROUND, "red", 0);
  draw(0);
}

static void hslider_callback2(widget, ddd)
     EZ_Widget *widget; void *ddd;
{
  StepIs0 = 1;
  pPosition[0] = X0  = (double) EZ_GetSliderValue(widget);  
  EZ_ConfigureWidget(StopButton, EZ_FOREGROUND, "red", 0);
  draw(0);
}

static void hslider_callback3(widget, ddd)
     EZ_Widget *widget; void *ddd;
{
  StepIs0 = 1;
  FFF = (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;

  pPosition[0] = X0;
  pPosition[1] = 0.0;
  TheHeight = 3.0 - (float)PLength * cos(pPosition[0]); 
/*
 * reset trajectory
 */
  TotalTime = 0.0;
  counter = 0;
  for(i = 0; i < 512; i++)
    {
      trace[i][1] = 0.0;
      trace[i][2] = TheHeight;
    }
    
  StepIs0 = 0;
  EZ_ConfigureWidget(StopButton, EZ_FOREGROUND, "black", 0);

  while(StepIs0 == 0)
    {
      rkqc(pPosition, Htry, &Hnext,ROC, 2);
      Htry = Hnext;

      trace[counter][1] = (float) TotalTime * TimeScale;
      trace[counter][2] = 3.0 - (float) PLength * cos(pPosition[0]);
      counter = (counter + 1) & 511;

      if(TotalTime > 500.0) StepIs0 = 1; /* don't run forever */
      draw(1);
      EZ_ServiceEvents();
    }
  EZ_ConfigureWidget(StopButton, EZ_FOREGROUND, "red", 0);
}


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


  pendulum[1][0] = 0.0;
  pendulum[1][1] = (float)PLength * sin(pPosition[0]);
  TheHeight = 3.0 - (float)PLength * cos(pPosition [0]);
  pendulum[1][2] = TheHeight;
  
  EZ_Clear(EZ_COLOR_BUFFER_BIT | EZ_DEPTH_BUFFER_BIT);
  EZ_PushMatrix();
  EZ_Translate(0.0,4.0, 0.0);
  EZ_Color3f(1.0,1.0,0.0);
  EZ_Begin(EZ_LINES);
  EZ_Vertex3fv(pendulum[0]);
  EZ_Vertex3fv(pendulum[1]);
  EZ_End(); 
  EZ_Enable(EZ_LIGHTING);
  EZ_Sphere(EZ_SPHERE_QUAD, 3,pendulum[1][0],pendulum[1][1],pendulum[1][2], 
	    PRadius);
  EZ_Disable(EZ_LIGHTING);

  if(tag)
    {
      EZ_Color3f(0.0, 0.4, 0.0);
      EZ_Begin(EZ_LINES);  
      EZ_Vertex3fv(pendulum[1]);
      EZ_Vertex3f(pendulum[1][0], -4.0,pendulum[1][2]);
      EZ_End();

      ftmp = -(float) TotalTime * TimeScale;
      EZ_Translate(0.0, ftmp -4.0, 0.0);
      EZ_Color3f(1.0,1.0,1.0);
      EZ_Begin(EZ_LINE_STRIP);
      for(i= 0; i < counter; i++)
	EZ_Vertex3fv(trace[i]);
      EZ_End(); 

      EZ_Begin(EZ_LINE_STRIP);
      for(i = counter; i < 512; i++)
	EZ_Vertex3fv(trace[i]);
      EZ_End(); 
    }
  EZ_PopMatrix();       

  EZ_SwapBuffers();
}

static void event_handle(EZ_Widget *canvas, void *data, 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);
      ftmp = (float)www/ (float)hhh;
      EZ_Ortho(-5.5 * ftmp,5.5 *ftmp,-3.0,8.0, 4.0, 8.0);
      EZ_MatrixMode(EZ_MODELVIEW);
    }
    draw(0);
    break;
  case EZ_KEY_PRESS:
      switch(EZ_PressedKey)
	{
	case 'P':
	  EZ_PolygonMode(EZ_FRONT, EZ_LINE);
	  EZ_Disable(EZ_DEPTH_TEST);
	  break;
	case 'p':
	  EZ_PolygonMode(EZ_FRONT, EZ_FILL);
	  EZ_Enable(EZ_DEPTH_TEST);
	  break;
	case EZ_ESCAPE_KEY:
	  exit(0);
	  break;
	}
    break;
  default:
    break;
  }
}
/***************************************************************/
static void make_data()
{

}
/***************************************************************/  

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 diffuse1[] = {.0, 1.0, 1.0, 1.0};
    static float position0[] = {2.7, 2.7, 2.7, 0.0};
    static float spot_dir[] = {-0.2, -0.2, -1.0, 0.0};
    static float spotE[] = {10.0};
    static float spotC[] = {6.0};

    static float position1[] = {-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 back_mat_shininess[] = {3.0};
    static float back_mat_specular[] = {1., 1., 1., 1.0};
    static float back_mat_diffuse[] = {1.0, 1.0, 0.2, 1.0};
    static float lmodel_ambient[] = {1.0, 1.0, 1.0, 1.0};
    static float lmodel_twoside[] = {0.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]);
}
/**************************************************************/
