/*
 * robot arm using SDL2
 *
 * This program shows how to composite modeling transformations
 * to draw translated and rotated hierarchical models.
 * Interaction:  pressing the s and e keys (shoulder and elbow)
 * alters the rotation of the robot arm shoulder and elbow.
 * Also left and right plates of the gripper.
 *
 * Originally based on the Redbook robot arm example.
 */
#include <GL/gl.h>
#include <GL/glu.h>
#include <SDL2/SDL.h>
#include <stdbool.h>
#include <stdlib.h>
#include <math.h>

// Globals
bool debug = true;
float shoulder, elbow, wrist, left_plate, right_plate;
SDL_Window *window;
const float gripper_increment = .2;


/* 
 * We reimplement GLUT's glutPostRedisplay() function.  Why? Because
 * it allows us to easily request a redraw, without having to worry
 * about doing redundant redraws of the scene. We use the
 * wantRedisplay variable as a latch that is cleared when a redraw is
 * done.
 */
int wantRedisplay = 1;
void postRedisplay()
{
  wantRedisplay = 1;
}

void quit(int code)
{
  SDL_DestroyWindow(window);
  SDL_Quit();
  exit(code);
}

// OpenGL initialisation
void init(void) 
{
  glClearColor (0.0, 0.0, 0.0, 0.0);
  glShadeModel (GL_FLAT);
  glColor3f(1.0, 1.0, 1.0);
}


// Draws a wireframe box
void myWireBox(float l, float h)
{
  glPushMatrix();
  glScalef(0.5, 0.5, 0.5);
  glScalef(l, h, 1.0);
  glBegin(GL_LINE_LOOP);
  glVertex2f(-1.0, -1.0);
  glVertex2f(1.0, -1.0);
  glVertex2f(1.0, 1.0);
  glVertex2f(-1.0, 1.0);
  glEnd();
  glPopMatrix();
}

// Draws a wireframe diamond - a rotated box
void myWireDiamond(float l, float h)
{
  glPushMatrix();
    glScalef(l, h, 1.0);
    glScalef(1.0/sqrt(2.0), 1.0/sqrt(2.0), 1.0);
    glRotatef(45.0, 0.0, 0.0, 1.0);
    myWireBox(1.0, 1.0);
  glPopMatrix();
}

// Draws a robot arm
void display(void)
{
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  glColor3f(1.0, 1.0, 1.0);

  glPushMatrix();
  // Shoulder 
  glTranslatef(7.0, 4.0, 0.0);
  glRotatef(shoulder, 0.0, 0.0, 1.0);
  // Upper arm
  glTranslatef(2.0, 0.0, 0.0);
  myWireDiamond(4.0, 2.0);
  // Elbow
  glTranslatef(2.0, 0.0, 0.0);
  glRotatef(elbow, 0.0, 0.0, 1.0);
  // Lower arm
  glTranslatef(0.0, -2.0, 0.0);
  myWireDiamond(2.0, 4.0);
  // Wrist
  glTranslatef(0.0, -2.0, 0.0);
  glRotatef(wrist, 0.0, 0.0, 1.0);
  // Hand
  glTranslatef(0.0, -0.25, 0.0);
  myWireBox(2.0, 0.5);
  // Gripper: left and right plates
  glPushMatrix();
  glTranslatef(-0.7+left_plate, -1.0, 0.0);
  myWireBox(0.5, 1.25);
  glPopMatrix();
  glPushMatrix();
  glTranslatef(0.75-right_plate, -1.0, 0.0);
  myWireBox(0.5, 1.25);
  glPopMatrix();
  glPopMatrix();

  // Does the same thing as glutSwapBuffers
  SDL_GL_SwapWindow(window);
 
  // Check for OpenGL errors at least once per frame
  int err;
  while ((err = glGetError()) != GL_NO_ERROR) {
    printf("display: %s\n",gluErrorString(err));
  }
}

void reshape(int w, int h)
{
  glViewport(0, 0, (GLsizei) w, (GLsizei) h); 
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glOrtho(-5.0, 20.0, -5.0, 20.0, 0.1, 100.0);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  glTranslatef (0.0, 0.0, -5.0); 
}

// Key down events
void keyDown(SDL_KeyboardEvent *e)
{
  switch (e->keysym.sym) {
  case SDLK_ESCAPE:
    quit(0);
    break;
  case SDLK_LCTRL:
    printf("LCTRL\n");
  case SDLK_s:
    if (e->keysym.mod & (KMOD_LSHIFT | KMOD_RSHIFT | KMOD_CAPS))
      shoulder = ((int)shoulder + 5) % 360;
    else
      shoulder = ((int)shoulder - 5) % 360;
    postRedisplay();
    break;
  case SDLK_e:
    if (e->keysym.mod & (KMOD_LSHIFT | KMOD_RSHIFT | KMOD_CAPS))
      elbow = ((int)elbow + 5) % 360;
    else
      elbow = ((int)elbow - 5) % 360;
    postRedisplay();
    break;
  case SDLK_l:
    if (e->keysym.mod & (KMOD_LSHIFT | KMOD_RSHIFT | KMOD_CAPS))
      left_plate += gripper_increment;
    else
      left_plate -= gripper_increment;
    postRedisplay();
    break;
  case SDLK_r:
    if (e->keysym.mod & (KMOD_LSHIFT | KMOD_RSHIFT | KMOD_CAPS))
      right_plate += gripper_increment;
    else
      right_plate -= gripper_increment;
    postRedisplay();
    break;
  default:
    break;
  }
}

// Key up events
void keyUp(SDL_KeyboardEvent *e)
{
}

void eventDispatcher()
{
  SDL_Event e;

  // Handle events
  while (SDL_PollEvent(&e)) {
    switch (e.type) {
    case SDL_QUIT:
      if (debug)
	printf("Quit\n");
      quit(0);
      break;
    case SDL_MOUSEMOTION:
      if (debug)
	printf("Mouse moved by %d,%d to (%d,%d)\n", 
	       e.motion.xrel, e.motion.yrel, e.motion.x, e.motion.y);
      break;
    case SDL_MOUSEBUTTONDOWN:
      if (debug)
	printf("Mouse button %d pressed at (%d,%d)\n",
	       e.button.button, e.button.x, e.button.y);
      break;
    case SDL_KEYDOWN:
      keyDown(&e.key);
      break;
    case SDL_WINDOWEVENT:
      if (debug)
	printf("Window event %d\n", e.window.event);
      switch (e.window.event)
	{
	case SDL_WINDOWEVENT_SHOWN:
	  if (debug)
	    SDL_Log("Window %d shown", e.window.windowID);
	  break;
	case SDL_WINDOWEVENT_SIZE_CHANGED:
	  if (debug)
	    printf("SDL_WINDOWEVENT_SIZE_CHANGED\n");
	  break;
	case SDL_WINDOWEVENT_RESIZED:
	  if (debug)
	    printf("SDL_WINDOWEVENT_RESIZED.\n");
	  if (e.window.windowID == SDL_GetWindowID(window)) {
	    SDL_SetWindowSize(window, e.window.data1, e.window.data2);
	    reshape(e.window.data1, e.window.data2);
	    postRedisplay();
	  }
	  break;
	case SDL_WINDOWEVENT_CLOSE:
	  if (debug)
	    printf("Window close event\n");
	  break;
	default:
	  break;
	}
      break;
    default:
      break;
    }
  }
}

// Used to update application state e.g. compute physics, game AI
void update()
{
}

/* 
 * Since we no longer have glutMainLoop() to do all the work for us,
 * we now have to do it ourselves. Good and bad. Good in that we have
 * more control, bad in that we have to do more work. I think that
 * good outweighs bad by a long shot :)
 *
 * This is essentially what GLUT's main loop does.
 */
void mainLoop()
{
  while (1) {
    eventDispatcher();
    if (wantRedisplay) {
      display();
      wantRedisplay = 0;
    }
    update();
  }
}

/*
 * Function for setting up the SDL window. You don't have to do it in
 * a function like this. You can stick it directly in main() if you
 * wish.  
 * 
 * Do not put any OpenGL calls before this function is called! 
 */
int initGraphics()
{
  SDL_GL_SetAttribute(SDL_GL_RED_SIZE, 8);
  SDL_GL_SetAttribute(SDL_GL_GREEN_SIZE, 8);
  SDL_GL_SetAttribute(SDL_GL_BLUE_SIZE, 8);
  SDL_GL_SetAttribute(SDL_GL_DEPTH_SIZE, 24);
  SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1);

  window =
    SDL_CreateWindow("Robot Arm Using SDL2",
		     SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED,
		     640, 480, SDL_WINDOW_OPENGL | SDL_WINDOW_RESIZABLE);
  if (!window) {
    fprintf(stderr, "%s:%d: create window failed: %s\n", 
	    __FILE__, __LINE__, SDL_GetError());
    exit(EXIT_FAILURE);
  }

  SDL_GLContext mainGLContext = SDL_GL_CreateContext(window);
  if (mainGLContext == 0) {
    fprintf(stderr, "%s:%d: create context failed: %s\n", 
	    __FILE__, __LINE__, SDL_GetError());
    exit(EXIT_FAILURE);
  }

  int w, h;
  SDL_GetWindowSize(window, &w, &h);
  reshape(w, h);

  return 0;
}

/* 
 * This is automatically called when the program exits, thanks to
 * "atexit()" in main(). This is a good place to do your program
 * shutdown and free memory, etc.
 */
void sys_shutdown()
{
  SDL_Quit();
}

    
int main(int argc, char** argv)
{

  if (SDL_Init(SDL_INIT_VIDEO) < 0) {
    fprintf(stderr, "%s:%d: unable to init SDL: %s\n",
	    __FILE__, __LINE__, SDL_GetError());
    exit(EXIT_FAILURE);
  }

  // Set up the window and OpenGL rendering context 
  if (initGraphics()) {
      SDL_Quit();
      return EXIT_FAILURE;
  }

  // OpenGL initialisation, must be done before any OpenGL calls
  init();

  atexit(sys_shutdown);

  mainLoop();
  
  return EXIT_SUCCESS;
}