/* demo.c - */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <unistd.h>
#include <GL/glut.h>

void *font = GLUT_STROKE_ROMAN;

static int draw_count=0;

#define DRAW_Q (draw_count >= 1)
#define DRAW_P (draw_count >= 2)
#define DRAW_PRED (draw_count >=3)
#define DRAW_F (draw_count >= 4)
#define DRAW_G (draw_count >= 5)
#define ANIMATE (draw_count > 5)


static double c1[4][2] = {
  {0.3, 0.15},
  {0.4, 0.3},
  {0.25, 0.5},
  {0.3, 0.65}};
static double c2[4][2] = {
  {0.3, 0.65},
  {0.4, 0.6},
  {0.7, 0.7},
  {0.8, 0.65}};


static double p_pos=0.15, q_pos=0.42;
void draw_fg(double p[2], double q[2], double pred[2]);
int idle_count = 0;

#define NUMSTEPS 200

void linear(double a[2], double b[2], double t, double dest[2])
{
  int i;
  for (i = 0; i < 2; i++) 
    dest[i] = a[i] + t * (b[i] - a[i]);
}

void cubic(double pts[4][2], double t, double dest[2])
{
  double temp[3][2];
  linear(pts[0], pts[1], t, temp[0]);
  linear(pts[1], pts[2], t, temp[1]);
  linear(pts[2], pts[3], t, temp[2]);
  linear(temp[0], temp[1], t, temp[0]);
  linear(temp[1], temp[2], t, temp[1]);
  linear(temp[0], temp[1], t, dest);
}

void tangent(double pts[4][2], double t, double dest[2])
{
  double temp[3][2];
  linear(pts[0], pts[1], t, temp[0]);
  linear(pts[1], pts[2], t, temp[1]);
  linear(pts[2], pts[3], t, temp[2]);
  linear(temp[0], temp[1], t, temp[0]);
  linear(temp[1], temp[2], t, temp[1]);
  
  dest[0] = temp[0][0] - temp[1][0];
  dest[1] = temp[0][1] - temp[1][1];
}

void
compute(double p[2], double q[2], double pred[2], double tang[2])
{
  int i;
  double len, dp, diff[2];
  
  if (p_pos < 0.5) {
    cubic(c1, p_pos * 2, p);
  } else {
    cubic(c2, (p_pos - 0.5) * 2, p);
  }

  if (q_pos < 0.5) {
    cubic(c1, 2.0 * q_pos, q);
    tangent(c1, 2.0 * q_pos, tang);
  } else {
    cubic(c2, 2.0 * (q_pos - 0.5), q);
    tangent(c2, 2.0 * (q_pos - 0.5), tang);
  }

  len = hypot(tang[0], tang[1]);
  for (i = 0; i < 2; i++)   tang[i] /= len;
  for (i = 0; i < 2; i++)   diff[i] = p[i] - q[i];
  dp = (diff[0] * tang[0] + diff[1] * tang[1]);
  for (i = 0; i < 2; i++)   pred[i] = q[i] + tang[i] * dp;

}

void
display()
{
  int i;
  double pt[2], p[2], q[2], pred[2], tang[2];
  double len;

  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  glHint(GL_LINE_SMOOTH_HINT,GL_NICEST);
  glEnable(GL_BLEND);
  glEnable(GL_LINE_SMOOTH);
  glEnable(GL_POLYGON_SMOOTH);
  glEnable(GL_POINT_SMOOTH);

  glClear(GL_COLOR_BUFFER_BIT);
  glColor3f(0.0, 0.0, 0.0);

  glBegin(GL_LINE_STRIP);
  for (i = 0; i < NUMSTEPS; i++) {
    double t = ((double) i) / NUMSTEPS;
    cubic(c1, t, pt);
    glVertex2d(pt[0], pt[1]);
  }
  for (i = 1; i < NUMSTEPS; i++) {
    double t = ((double) i) / NUMSTEPS;
    cubic(c2, t, pt);
    glVertex2d(pt[0], pt[1]);
  }
  glEnd();

  compute(p, q, pred, tang);

  if (DRAW_G) {
    // line P to PRED
    glColor3f(0.2, 0.2, 1.0);
    glBegin(GL_LINES);
    glVertex2d(p[0], p[1]);
    glVertex2d(pred[0], pred[1]);
    glEnd();
  }

  if (DRAW_P) {
    // P
    glColor3f(1.0, 0.0, 0.0);
    glBegin(GL_POINTS);
    glVertex2d(p[0], p[1]);
    glEnd();
    
    glLineWidth(4.0);
    glPushMatrix();
    glTranslatef(p[0]+0.02, p[1], 0.0);
    glScalef(0.0004, 0.0004, 0.002);
    glutStrokeCharacter(font, 'p');
    glPopMatrix();
    glLineWidth(7.0);
  }


  if (DRAW_Q) {
    // Q
    glColor3f(0.0, 0.75, 0.0);
    glBegin(GL_POINTS);
    glVertex2d(q[0], q[1]);
    glEnd();
  }

  len = hypot(tang[0], tang[1]);
  tang[0] /= len;
  tang[1] /= len;
  tang[0] *= .1;
  tang[1] *= .1;


  if (DRAW_PRED) {
    // Line Q to PRED
    // make tang point from p to pred
    if ((tang[0] * (pred[0] - q[0]) +
         tang[1] * (pred[1] - q[1])) < 0.0) {
      tang[0] = -tang[0];
      tang[1] = -tang[1];
    }

    glColor3f(0.0, 0.75, 0.0);
    glBegin(GL_LINES);
    glVertex2d(pred[0] + tang[0], pred[1]+ tang[1]);
    glVertex2d(q[0] - tang[0], q[1] - tang[1]);
    glEnd();
  } else if (DRAW_Q) {
    glBegin(GL_LINES);
    glVertex2d(q[0] + tang[0], q[1]+ tang[1]);
    glVertex2d(q[0] - tang[0], q[1] - tang[1]);
    glEnd();
    
  }

  if (DRAW_Q) {
    glLineWidth(4.0);
    glPushMatrix();
    glTranslatef(q[0]+0.02, q[1], 0.0);
    glScalef(0.0004, 0.0004, 0.002);
    glutStrokeCharacter(font, 'q');
    glPopMatrix();
    glLineWidth(7.0);
  }
  
  if (DRAW_F && (! DRAW_G)) {
    glColor3f(1.0, 0.0, 0.0);
    glBegin(GL_LINES);
    glVertex2d(q[0], q[1]);
    glVertex2d(p[0], p[1]);
    glEnd();
  }

  // times
  glLineWidth(4.0);
  glColor3f(0.0, 0.0, 0.0);
  glPushMatrix();
  glTranslatef(c1[0][0]-0.1, c1[0][1]-0.03, 0.0);
  glScalef(0.0004, 0.0004, 0.002);
  glutStrokeCharacter(font, 't');
  glutStrokeCharacter(font, '=');
  glutStrokeCharacter(font, '0');
  glPopMatrix();

  glPushMatrix();
  glTranslatef(c2[3][0], c2[3][1]+0.04, 0.0);
  glScalef(0.0004, 0.0004, 0.002);
  glutStrokeCharacter(font, 't');
  glutStrokeCharacter(font, '=');
  glutStrokeCharacter(font, '1');
  glPopMatrix();
  glLineWidth(7.0);
  
  if (DRAW_PRED) {
    // PRED
    glColor3f(0.2, 0.2, 1.0);
    glPointSize(30.0);
    glBegin(GL_POINTS);
    glVertex2d(pred[0], pred[1]);
    glEnd();
    glPointSize(10.0);
  }

  draw_fg(p, q, pred);

  glutSwapBuffers();
}

#define SIGMA_F 0.2
#define SIGMA_G 0.02

void
draw_fg(double p[2], double q[2], double pred[2])
{
  int i;
  double fval, gval, len, delta[2], temp;
  static double idlevals[NUMSTEPS];

  for (i = 0; i < 2; i++) delta[i] = p[i] - q[i];
  len = hypot(delta[0], delta[1]);
  fval = exp(- len * len / SIGMA_F);

  if (DRAW_F) {
    glColor3f(0.0, 0.0, 0.0);
    glPushMatrix();
    glTranslatef(1.0, 0.7, 0.0);
    glScalef(0.2, 0.2, 0.2);
    
    glLineWidth(4.0);
    glPushMatrix();
    glTranslatef(0.0, 1.1, 0.0);
    glScalef(0.002, 0.002, 0.002);
    glutStrokeCharacter(font, 's');
    glutStrokeCharacter(font, 'p');
    glutStrokeCharacter(font, 'a');
    glutStrokeCharacter(font, 't');
    glutStrokeCharacter(font, 'i');
    glutStrokeCharacter(font, 'a');
    glutStrokeCharacter(font, 'l');
    glPopMatrix();
    glLineWidth(7.0);
    
    glBegin(GL_LINES);
    glVertex2d(0.0, 0.0);
    glVertex2d(0.0, 1.0);
    glVertex2d(0.0, 0.0);
    glVertex2d(1.0, 0.0);
    glEnd();
    
    glLineWidth(7.0);
    glColor3f(1.0, 0.0, 0.0);
    glBegin(GL_LINES);
    glVertex2d(len, -0.05);
    glVertex2d(len, fval);
    glEnd();
    glLineWidth(7);
    glColor3f(0.0, 0.0, 0.0);
    
    glBegin(GL_LINE_STRIP);
    for (i = 0; i < 20; i++) {
      double len = ((double) i) / 20.0;
      
      temp = exp(- len * len / SIGMA_F);
      glVertex2d(len, temp);
    }
    glEnd();
    
    glPopMatrix();
  }

  if (DRAW_G) {
    for (i = 0; i < 2; i++) delta[i] = p[i] - pred[i];
    len = hypot(delta[0], delta[1]);
    gval = exp(- len * len / SIGMA_G);
    
    glColor3f(0.0, 0.0, 0.0);
    glPushMatrix();
    glTranslatef(1.0, 0.4, 0.0);
    glScalef(0.2, 0.2, 0.2);
    
    glLineWidth(4.0);
    glPushMatrix();
    glTranslatef(0.0, 1.1, 0.0);
    glScalef(0.002, 0.002, 0.002);
    glutStrokeCharacter(font, 'i');
    glutStrokeCharacter(font, 'n');
    glutStrokeCharacter(font, 'f');
    glutStrokeCharacter(font, 'l');
    glutStrokeCharacter(font, 'u');
    glutStrokeCharacter(font, 'e');
    glutStrokeCharacter(font, 'n');
    glutStrokeCharacter(font, 'c');
    glutStrokeCharacter(font, 'e');
    glPopMatrix();
    glLineWidth(7.0);
    
    glBegin(GL_LINES);
    glVertex2d(0.0, 0.0);
    glVertex2d(0.0, 1.0);
    glVertex2d(0.0, 0.0);
    glVertex2d(1.0, 0.0);
    glEnd();
    
    glLineWidth(7.0);
    glColor3f(0.2, 0.2, 1.0);
    glBegin(GL_LINES);
    glVertex2d(len, -0.05);
    glVertex2d(len, gval);
    glEnd();
    glLineWidth(7);
    glColor3f(0.0, 0.0, 0.0);


    glBegin(GL_LINE_STRIP);
    for (i = 0; i < 20; i++) {
      double len = ((double) i) / 20.0;
      
      temp = exp(- len * len / SIGMA_G);
      glVertex2d(len, temp);
    }
    glEnd();
    
    glPopMatrix();
  }

  if (! ANIMATE) return;

  glPushMatrix();
  glTranslatef(1.0, 0.1, 0.0);
  glScalef(0.2, 0.2, 0.2);

  glLineWidth(4.0);
  glPushMatrix();
  glTranslatef(0.0, 1.1, 0.0);
  glScalef(0.002, 0.002, 0.002);
  glutStrokeCharacter(font, 'w');
  glutStrokeCharacter(font, 'e');
  glutStrokeCharacter(font, 'i');
  glutStrokeCharacter(font, 'g');
  glutStrokeCharacter(font, 'h');
  glutStrokeCharacter(font, 't');
  glutStrokeCharacter(font, '(');
  glutStrokeCharacter(font, 't');
  glutStrokeCharacter(font, ')');
  glPopMatrix();
  glLineWidth(7.0);

  glBegin(GL_LINES);
  glVertex2d(0.0, 0.0);
  glVertex2d(0.0, 1.0);
  glVertex2d(0.0, 0.0);
  glVertex2d(1.0, 0.0);
  glVertex2d(0.5, -0.1);
  glVertex2d(0.5, 0.1);
  glEnd();
  if (idle_count) {
    idlevals[NUMSTEPS-idle_count] = fval * gval;

    glBegin(GL_LINE_STRIP);
    for (i = 1; i <= NUMSTEPS - idle_count; i++)
      glVertex2d(((double) i) / ((double) NUMSTEPS), idlevals[i]);
    glEnd();
  } else {
    glBegin(GL_LINE_STRIP);
    for (i = 1; i < NUMSTEPS; i++)
      glVertex2d(((double) i) / ((double) NUMSTEPS), idlevals[i]);
    glEnd();
  }    

  glPopMatrix();

}

void
idle()
{
  if (idle_count == 0) {
    glutIdleFunc(NULL);
    return;
  }
  idle_count--;
  p_pos = 1.0 - (idle_count / ((double) NUMSTEPS));
  usleep(10000);
  glutPostRedisplay();
}

double win_width, win_height;

void
reshape(int w, int h)
{
  glViewport(0, 0, w, h);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluOrtho2D(0, ((double) w) / h, 0, 1.0);
  glMatrixMode(GL_MODELVIEW);
  win_width = w;
  win_height = h;
}


int dragging_p;
void
mouse(int button, int state, int x, int y)
{
  if (state != GLUT_DOWN) return;
  if (x < win_width / 2) {
    dragging_p = 1;
    p_pos = 1.0 - ((double) y) / win_height;
  } else {
    dragging_p = 0;
    q_pos = 1.0 - ((double) y) / win_height;
  }

  glutPostRedisplay();
}

void motion(int x, int y)
{
  if (dragging_p) {
    p_pos = 1.0 - ((double) y) / win_height;
  } else {
    q_pos = 1.0 - ((double) y) / win_height;
  }
  glutPostRedisplay();
}

void
key(unsigned char k, int x, int y)
{
  if (k == 27) exit(0);
  else {
    draw_count++;
    if (ANIMATE) {
      idle_count = NUMSTEPS;
      p_pos = 0.0;
      glutIdleFunc(idle);
    } else {
      glutPostRedisplay();
    }
  }
}

int
main(int argc, char **argv)
{
  glutInitWindowSize(400, 400);
  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA);
  glutCreateWindow("Bilateral Demo");

  glClearColor(1.0, 1.0, 1.0, 1.0);
  glPointSize(10.0);
  glLineWidth(7.0);
  glutReshapeFunc(reshape);
  glutMouseFunc(mouse);
  glutMotionFunc(motion);
  glutDisplayFunc(display);
  glutKeyboardFunc(key);
  glutFullScreen();

  glutMainLoop();
  return 0;
}