/*
 * Extended robot arm example with simple keyframe,
 * linear-interpolation based animation.  
 * Geoff Leach. April 2002.
 *
 * Added code for frame rate meter.
 * Geoff Leach. April 2004
 *
 * Used separate function to perform linear interpolation.
 * Changed some variable names.
 * Tidied up some warnings.
 * Geoff Leach. April 2005.
 *
 * Separate function for finding keyframes to interpolate
 * Interpolator struct
 * Geoff Leach. May 2015
 *
 */
/* $Id: robot-anim.c,v 1.11 2015/05/13 03:16:07 gl Exp gl $ */

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <GL/glut.h>

#define PRINT_NFRAMES
#undef TRACE
#undef DEBUG

float shoulder = 0.0, elbow = 0.0;
float elapsedTime = 0.0;
int frameNo = 0;

/* Animation variables. */
typedef struct {
  float time;
  float value;
} keyFrame;
keyFrame shoulderKeyFrame[] = {
};
const int maxKeyFrames = 100;
typedef struct {
  int nKeyFrames;
  keyFrame keyFrames[10];
  float startTime;
} interpolator;
interpolator shoulderInterpolator = {
  5,
  {
    {0.0, 0.0}, 
    {1.0, 90.0}, 
    {2.0, 45.0}, 
    {3.0, 90.0}, 
    {5.0, 0.0},
  },
  -1.0
};

const float milli = 1000.0;

void init(void) 
{
  glClearColor (0.0, 0.0, 0.0, 0.0);
  glShadeModel (GL_FLAT);
}

void displayFrameRate(int frameNo)
{
  static const float interval = 0.1;
  static float frameRateInterval = 0.0;
  static int frameNoStartInterval = 0;
  static float elapsedTimeStartInterval = 0.0;
  static char buffer[] = "        fps";
  int len, i;
 
  /* No frame rate until at least 'interval' of time. */
  if (elapsedTime < interval)
    return;

  /* Only update frame rate every 'interval' of time. */
  if (elapsedTime > elapsedTimeStartInterval + interval) {
    frameRateInterval = (frameNo - frameNoStartInterval) / 
      (elapsedTime - elapsedTimeStartInterval);
    elapsedTimeStartInterval = elapsedTime;
    frameNoStartInterval = frameNo;
  }

  /* Update frame rate string */
  sprintf(buffer, "%6.1f", frameRateInterval);
  buffer[6] = ' ';      /* Overwrite null from sprintf. */
#ifdef DEBUG_FRAMERATE
  printf("displayFrameRate: frameNo %d elapsedTime %f frameRateInterval %f\n",
         frameNo, elapsedTime, frameRateInterval);
#endif

  /* Draw frame rate string */
  glRasterPos3f(0.0,-1.0,0.0);
  len = (int)strlen(buffer);
  for (i = 0; i < len; i++)
    glutBitmapCharacter(GLUT_BITMAP_HELVETICA_12, buffer[i]);
}

void display(void)
{
  glClear (GL_COLOR_BUFFER_BIT);
  glPushMatrix();
  glTranslatef (-1.0, 0.0, 0.0);
  glRotatef (shoulder, 0.0, 0.0, 1.0);
  glTranslatef (1.0, 0.0, 0.0);
  glPushMatrix();
  glScalef (2.0, 0.4, 1.0);
  glutWireCube (1.0);
  glPopMatrix();
  glTranslatef (1.0, 0.0, 0.0);
  glRotatef (elbow, 0.0, 0.0, 1.0);
  glTranslatef (1.0, 0.0, 0.0);
  glPushMatrix();
  glScalef (2.0, 0.4, 1.0);
  glutWireCube (1.0);
  glPopMatrix();
  glPopMatrix();

  displayFrameRate(frameNo);

  glutSwapBuffers();

  /* Increment frame counter. */
  frameNo++;
#ifdef DEBUG_FRAMERATE
  printf("frameNo: %d\n", frameNo);
#endif
}

void reshape (int w, int h)
{
  printf("%d %d %f\n", w, h, (GLfloat) w/(GLfloat) h);
  glViewport (0, 0, (GLsizei) w, (GLsizei) h); 
  glMatrixMode (GL_PROJECTION);
  glLoadIdentity ();
  gluPerspective(65.0, (GLfloat) w/(GLfloat) h, 1.0, 20.0);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  glTranslatef (0.0, 0.0, -5.0);
}

float lerp(float t0, float v0, float t1, float v1, float t)
{
  float v;

  /* Linear interpolation, or lerp. */
  v = v0 + (t - t0) * (v1 - v0) / (t1 - t0);

  return v;
}

int findInterval(keyFrame kf[], int n, float t)
{
  int i;

  /* Use linear search (better approaches possible). */
  for (i = n-1; i >=0; i--) {
#ifdef DEBUG
    printf("findInterval: kf[%d].time %f\n", i, kf[i].time);
#endif
    if (t > kf[i].time)
      break;
  }

  return i;
}

/* Callback for animation. */
void animate(void)
{
  float elapsedTimeShoulderAnimation;
  int i;

#ifdef TRACE
  printf("animate\n");
#endif

  elapsedTime = glutGet(GLUT_ELAPSED_TIME) / milli;
#ifdef TRACE
  printf("elapsedTime %f\n", elapsedTime);
#endif

  elapsedTimeShoulderAnimation = elapsedTime - shoulderInterpolator.startTime;
#ifdef DEBUG
  printf("elapsedTimeShoulderAnimation %f\n", elapsedTimeShoulderAnimation);
#endif

  /* Keyframe calculations */
  if (elapsedTimeShoulderAnimation >= 
      shoulderInterpolator.keyFrames[shoulderInterpolator.nKeyFrames-1].time) {
    /* Finished. */
    i = shoulderInterpolator.nKeyFrames;
    glutIdleFunc(NULL);
  } else {
    /* Find key frames to interpolate. */
    i = findInterval(shoulderInterpolator.keyFrames,
		     shoulderInterpolator.nKeyFrames,
		     elapsedTimeShoulderAnimation);
  }

  /* Use linear interpolation to work out intermediate value. */
  if (i >= shoulderInterpolator.nKeyFrames)
    shoulder = shoulderInterpolator.keyFrames[shoulderInterpolator.nKeyFrames-1].value;
  else
    shoulder = lerp(shoulderInterpolator.keyFrames[i].time,
		    shoulderInterpolator.keyFrames[i].value,
		    shoulderInterpolator.keyFrames[i+1].time,
		    shoulderInterpolator.keyFrames[i+1].value,
		    elapsedTimeShoulderAnimation);


#ifdef TRACE
  printf("elapsedTimeShoulderAnimation %f shoulder %f\n", 
         elapsedTimeShoulderAnimation, shoulder);
#endif

  glutPostRedisplay();
}

/* ARGSUSED1 */
void keyboard (unsigned char key, int x, int y)
{
  switch (key) {
  case 'a':
    shoulderInterpolator.startTime = glutGet(GLUT_ELAPSED_TIME) / milli;
    glutIdleFunc(animate);
    glutPostRedisplay();
    break;
  case 's':
    shoulder += 5.0;
    if (shoulder > 360.0) shoulder -= 360.0;
    glutPostRedisplay();
    break;
  case 'S':
    shoulder -= 5.0;
    if (shoulder < -360.0) shoulder += 360.0;
    glutPostRedisplay();
    break;
  case 'e':
    elbow += 5.0;
    glutPostRedisplay();
    break;
  case 'E':
    elbow -= 5.0;
    if (elbow < -360.0) elbow += 360.0;
    glutPostRedisplay();
    break;
  case 27:
#ifdef PRINT_NFRAMES
    printf("nframes = %d\n",frameNo);
#endif
    exit(0);
    break;
  default:
    break;
  }
}

int main(int argc, char** argv)
{
  glutInit(&argc, argv);
  glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB);
  glutInitWindowSize (300, 300); 
  glutInitWindowPosition (100, 100);
  glutCreateWindow (argv[0]);
  init();
  glutDisplayFunc(display); 
  glutReshapeFunc(reshape);
  glutKeyboardFunc(keyboard);
  glutMainLoop();
  return 0;
}