#include <wiimote.h>
#include <GL\glew.h>
#include <GL\glut.h>
#include <stdlib.h>

#include "typedefs.h"
#include "stereoheadtrackfrustum.h"
#include "scenerenderer.h"

extern GameState_t g_GameState;

void ExitProgram()
{
  printf("Finished!\n");


  //we prolly should do some cleanup
  if (g_GameState.m_pTrackingWiimote != NULL)
  {
    g_GameState.m_pTrackingWiimote->Disconnect();
    delete g_GameState.m_pTrackingWiimote;
  }

  exit(0);
}

inline void GSConvScaleChange(int index, GLfloat delta)
{
  g_GameState.m_GSConvScale[index] += delta;
  if (g_GameState.m_bStereoEnabled) 
    glUniform3fv(g_GameState.m_GSConvScaleLoc, 1, g_GameState.m_GSConvScale);
}

void Keyboard(unsigned char f_cKey, int f_iX, int f_iY)
{
  switch(f_cKey)
  {
  case 'q':
  case 'Q':
  case 27 :
    ExitProgram();
    break;

  case 's':
  case 'S': //enable /disable stereo vision
    g_GameState.m_bStereoEnabled = !g_GameState.m_bStereoEnabled;
    break;

  case 'h':
  case 'H': //enable disable head tracking
    if (g_GameState.m_pTrackingWiimote != NULL && g_GameState.m_pTrackingWiimote->IsConnected())
    {
      g_GameState.m_bHeadTrackingEnabled = !g_GameState.m_bHeadTrackingEnabled;
    }
    break;

  case ' ':
    ////set y angle correction to current y angle of the the center of the two ir dots
    //if (g_Wiimote.IsConnected())
    //{
    //  wiimote_state::ir::dot l_Dot[2];  //shorthand to the ir dots
    //  //check if we see at least 2 IR dots
    //  if (FindIRDots(&g_Wiimote.IR, l_Dot))
    //  {      
    //    //calculate the distance of the head (in mm)
    //    double  l_dHeadDistInMM = CalcHeadDistInMM(l_Dot, &g_GameState.m_HeadTrackParms), //the distance between the head and camera (in mm)
    //            l_dEyeYCam = (double)(l_Dot[0].RawY + l_Dot[1].RawY) / 2.0;
    //  
    //    // of the eye relative to the camera 
    //    g_GameState.m_HeadTrackParms.m_dYAngleCorrection = g_GameState.m_HeadTrackParms.m_dRadPerCameraPixel * (l_dEyeYCam - g_GameState.m_HeadTrackParms.m_dCameraYCenter);
    //  }
    //}
    break;

  case 'u': GSConvScaleChange(0,  0.05); break;
  case 'j': GSConvScaleChange(0, -0.05); break;

  case 'i': GSConvScaleChange(1,  0.05); break;
  case 'k': GSConvScaleChange(1, -0.05); break;

  case 'o': GSConvScaleChange(2,  0.05); break;
  case 'l': GSConvScaleChange(2, -0.05); break;
  }
}

void Display(void)
{
  glClear(GL_COLOR_BUFFER_BIT);

  double l_d3EyePosInMM[3];
  bool   l_bEyeLocated;

  if (g_GameState.m_bStereoEnabled)
  {
    //enable grayscale shader
    glUseProgram(g_GameState.m_GreyScaleShaderProgram);
    //set the conversion scale
    glUniform3fv(g_GameState.m_GSConvScaleLoc, 1, g_GameState.m_GSConvScale);
    
    for (int CurrentEye=0; CurrentEye<2; CurrentEye++) //STEREO_LEFT_EYE = 0, STEREO_RIGHT_EYE = 1
    {
      l_bEyeLocated = false;
      if (g_GameState.m_bHeadTrackingEnabled)
      {
        l_bEyeLocated = CalcEyePosInMM(g_GameState.m_pTrackingWiimote, 
                                       (EyeOrigin_t)CurrentEye, 
                                       g_GameState.m_FrustumParms, 
                                       l_d3EyePosInMM);
      }
      if(!l_bEyeLocated)
      {
        //set default eye coordinates
        l_d3EyePosInMM[0] = g_GameState.m_FrustumParms.m_dEyeDistMM * ((double)CurrentEye - 0.5);
        l_d3EyePosInMM[1] = 0.0;
        l_d3EyePosInMM[2] = g_GameState.m_FrustumParms.m_dDefHeadDistMM;
      }

      //set colormask
      if (CurrentEye == STEREO_LEFT_EYE)
      {
        glColorMask(GL_TRUE, GL_FALSE, GL_FALSE, GL_TRUE);
      }
      else
      {
        glColorMask(GL_FALSE, GL_TRUE, GL_TRUE, GL_TRUE);
      }

      //clear depth buffers
      glClear(GL_DEPTH_BUFFER_BIT);

      //set frustum
      SetFrustum(g_GameState.m_FrustumParms, l_d3EyePosInMM);

      RenderScene();
    }
    glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);
    //disable greyscale shader
    glUseProgram(0);
  }
  else // !g_GameState.m_bStereoEnabled
  {
    l_bEyeLocated = false;
    if (g_GameState.m_bHeadTrackingEnabled)
    {
      l_bEyeLocated = CalcEyePosInMM(g_GameState.m_pTrackingWiimote, 
                                     MONO_CENTER, 
                                     g_GameState.m_FrustumParms, 
                                     l_d3EyePosInMM);
    }
    if(!l_bEyeLocated)
    {
      //set default eye coordinates
      l_d3EyePosInMM[0] = 0.0;
      l_d3EyePosInMM[1] = 0.0;
      l_d3EyePosInMM[2] = g_GameState.m_FrustumParms.m_dDefHeadDistMM;
    }

    //clear depth buffer
    glClear(GL_DEPTH_BUFFER_BIT);

    //set frustum
    SetFrustum(g_GameState.m_FrustumParms, l_d3EyePosInMM);

    RenderScene();
  }

  GLdouble width = (GLdouble)glutGet(GLUT_SCREEN_WIDTH),
           height = (GLdouble)glutGet(GLUT_SCREEN_HEIGHT);
  glMatrixMode(GL_PROJECTION);
  glPushMatrix();
  glLoadIdentity();
  glOrtho(0.0, width, 0.0, height, -1.0, 1.0);
  glScaled(1.0, -1.0, 1.0);
  glTranslatef(0.0, -height, 0.0);
  
  glMatrixMode(GL_MODELVIEW);
  glPushAttrib(GL_LIGHTING_BIT);
  glShadeModel(GL_FLAT);
  glDisable(GL_LIGHTING);
  glPushMatrix();
  glLoadIdentity();
  glBegin(GL_QUADS);
  glColor3f(0.2f, 0.2f, 0.2f);
  glVertex3f(0.0f, 0.0f, -0.5f);
  glVertex3f(0.0f, 30.0f, -0.5f);
  glVertex3f(325.0f, 30.0f, -0.5f);
  glVertex3f(325.0f, 0.0f, -0.5f);
  glEnd();

  glColor3f(1.0, 1.0, 1.0);
  glRasterPos3f(5.0, 22.0, 0.5);
  char string[256];
  sprintf(string, "RGB conv scale: %4.3f, %4.3f, %4.3f", 
    g_GameState.m_GSConvScale[0],
    g_GameState.m_GSConvScale[1],
    g_GameState.m_GSConvScale[2]);
  int len = strlen(string);
  for (int i=0; i<len; i++)
    glutBitmapCharacter(GLUT_BITMAP_9_BY_15, string[i]);

  glPopMatrix();
  glMatrixMode(GL_PROJECTION);
  glPopMatrix();

  glPopAttrib();
  
  glutSwapBuffers();
} // Display


void Idle(void)
{
  if (g_GameState.m_pTrackingWiimote != NULL && g_GameState.m_pTrackingWiimote->IsConnected())
  {
    g_GameState.m_pTrackingWiimote->RefreshState();
  }

  glutPostRedisplay();
}

void Reshape(int f_iWidth, int f_iHeight)
{
  //set the new viewport dimension
  glViewport(0, 0, f_iWidth, f_iHeight);

  //should we set a new projection matrix?
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();

  double l_dHalfHeight = 0.5 * g_GameState.m_FrustumParms.m_dScreenHeightWorld,
         l_dHalfWidth = l_dHalfHeight * g_GameState.m_FrustumParms.m_dScreenAspect,
         l_zNear = 2.0 * g_GameState.m_FrustumParms.m_dScreenHeightWorld;

  glFrustum(-l_dHalfWidth, l_dHalfWidth, -l_dHalfHeight, l_dHalfHeight, l_zNear, l_zNear + 1000.0);
  glTranslated(0.0, 0.0, -l_zNear);

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
}