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

#define _USE_MATH_DEFINES
#include <math.h>

struct HeadTrackWiimotes
{
  wiimote** m_ppWiimotes;
  int m_iConnected; 
} g_Wiimotes;

struct HeadTrackParameters
{
  double m_dHeadTrackLedDist;       //distance between leds on head in millimeters
  double m_dRadPerCameraPixel;      //radians per camera pixel
  double m_dCameraXCenter;          //the coordinates of the center of the camera
  double m_dCameraYCenter;          //
  double m_dYAngleCorrection;       //the correction added to the verticle angle measured by the camera (in radians)
  double m_dCameraYOffset;          //the offset in Y direction of the camera relative to the center of the screen (in mm)
  double m_dScreenHeightMM;         //the height of the screen (in mm)
  double m_dScreenAspect;           //the aspect ratio of the screen (width/height)
  double m_dScreenHeightWorld;      //the height of the screen (in world coordinates)
} g_HeadTrackParms;

enum EyeOrigin {
    STEREO_LEFT_EYE,
    STEREO_RIGHT_EYE,
    MONO_CENTER
};

inline void FindIRDots(const ir_t *f_pIR, const ir_dot_t *f_pDot[2])
{
  //get the first two visible IR dots
  for(int i = 0, n = 0; i < 4 && n < 2; i++)
  {
    if (f_pIR->dot[i].visible)
    {
      f_pDot[n] = &f_pIR->dot[i];
      n++;
    }
  }
}

inline double CalcHeadDistInMM(const ir_dot_t *f_pDot[2], HeadTrackParameters *f_pHTParms)
{
  double  l_dX = f_pDot[0]->rx - f_pDot[1]->rx,            //difference in x coordinates
          l_dY = f_pDot[0]->ry - f_pDot[1]->ry,            //difference in y coordinates
          l_dDotDist = sqrt(l_dX*l_dX + l_dY*l_dY),        //distance between ir dots (in camera pixels)
          l_dDotAngle = f_pHTParms->m_dRadPerCameraPixel * l_dDotDist; //the angle between the lines from the camera through the two ir dots (in radians)

  return (0.5 * f_pHTParms->m_dHeadTrackLedDist) / tan(0.5 * l_dDotAngle); //the distance between the head and camera (in mm)
}

void SetHeadTrackedFrustum(EyeOrigin f_Eye)
{
  //check if there are any wiimotes connected
  if (g_Wiimotes.m_iConnected > 0)
  {
    //check if we see at least 2 IR dots
    if (g_Wiimotes.m_ppWiimotes[0]->ir.num_dots >= 2)
    {
      const ir_dot_t *l_pDot[2];  //shorthand to the ir dots
      FindIRDots(&g_Wiimotes.m_ppWiimotes[0]->ir, l_pDot);
      
      //calculate the distance of the head (in mm)
      double l_dHeadDistInMM = CalcHeadDistInMM(l_pDot, &g_HeadTrackParms); //the distance between the head and camera (in mm)

      double l_dEyeXCam,
             l_dEyeYCam;
      //determine the eye position in camera coordinates
      switch(f_Eye)
      {
      case STEREO_LEFT_EYE:
      case STEREO_RIGHT_EYE:
          //determine the lower left point
          int i ;
          if (l_pDot[0]->rx < l_pDot[1]->rx || (l_pDot[0]->rx == l_pDot[1]->rx && l_pDot[0]->ry < l_pDot[1]->ry))
            //0 is the left eye 1 is the right eye
            i = (f_Eye==STEREO_LEFT_EYE? 0: 1);
          else
            //1 is the left eye 0 is the right eye
            i = (f_Eye==STEREO_LEFT_EYE? 1: 0);
          l_dEyeXCam = (double)l_pDot[i]->rx;
          l_dEyeYCam = (double)l_pDot[i]->ry;
        break;

      case MONO_CENTER:
      default:
          l_dEyeXCam = (double)(l_pDot[0]->rx + l_pDot[1]->rx) / 2.0;
          l_dEyeYCam = (double)(l_pDot[0]->ry + l_pDot[1]->ry) / 2.0;
        break;
      }

      //calculate the x and y angles of the eye relative to the camera 
      double l_dEyeXAngle = g_HeadTrackParms.m_dRadPerCameraPixel * (l_dEyeXCam - g_HeadTrackParms.m_dCameraXCenter),
             l_dEyeYAngle = g_HeadTrackParms.m_dRadPerCameraPixel * (l_dEyeYCam - g_HeadTrackParms.m_dCameraYCenter);

      //correct the y angle
      l_dEyeYAngle += g_HeadTrackParms.m_dYAngleCorrection;

      //calculate the the x,y,z coordinates of the eye relative to the screen (in mm), note that we assume that the camera
      //is placed above or underneath the center of the screen, so for the x axis we dont correct the position, but for the 
      //y axis we have to take the y-offset of the camera relative to the center of the screen into account.
      double l_dEyeX = sin(l_dEyeXAngle) * l_dHeadDistInMM,
             l_dEyeY = sin(l_dEyeYAngle) * l_dHeadDistInMM - g_HeadTrackParms.m_dCameraYOffset,
             l_dEyeZ = sqrt(l_dHeadDistInMM*l_dHeadDistInMM - (l_dEyeX*l_dEyeX + l_dEyeY*l_dEyeY)); //pythagoras

      //convert the mm to world coordinates
      double l_dMmPerWorldCoord = g_HeadTrackParms.m_dScreenHeightMM / g_HeadTrackParms.m_dScreenHeightWorld;
      l_dEyeX /= l_dMmPerWorldCoord;
      l_dEyeY /= l_dMmPerWorldCoord;
      l_dEyeZ /= l_dMmPerWorldCoord;

      //now set a frustum with the near clipping plane at (-1/2 width, -1/2 height, 0), (-1/2 width, -1/2 height, 0) with the 
      //eye at position (l_dEyeX, l_dEyeY, l_dEyeZ). But because OpenGL expects the eye at position (0,0,0) when specifying a frustum,
      //we have to specify our near clipping plane with (-l_dEyeX, -l_dEyeY, -l_dEyeZ) as its origin and translate the modelview
      //matrix to (-l_dEyeX, -l_dEyeY, -l_dEyeZ), such that the world origin is in the center of the screen
      double l_dHalfHeight = 0.5 * g_HeadTrackParms.m_dScreenHeightWorld,
             l_dHalfWidth = l_dHalfHeight * g_HeadTrackParms.m_dScreenAspect;
      glMatrixMode(GL_PROJECTION);
      glLoadIdentity();
      GLdouble  l_dZNear  = 1.0,
                l_dFactor = l_dZNear/l_dEyeZ,
                l_dLeft   = (-l_dEyeX - l_dHalfWidth) * l_dFactor,
                l_dRight  = (-l_dEyeX + l_dHalfWidth) * l_dFactor,
                l_dBottom = (-l_dEyeY - l_dHalfHeight) * l_dFactor,
                l_dTop    = (-l_dEyeY + l_dHalfHeight) * l_dFactor;

      glFrustum( l_dLeft, l_dRight, l_dBottom, l_dTop, l_dZNear, l_dEyeZ + 100.0);
      glMatrixMode(GL_MODELVIEW);
      glLoadIdentity();
      glTranslated(-l_dEyeX, -l_dEyeY, -l_dEyeZ);
    }
  }
}

void DrawRasterBox()
{
  double l_dHalfHeight = 0.5 * g_HeadTrackParms.m_dScreenHeightWorld,
         l_dHalfWidth = l_dHalfHeight * g_HeadTrackParms.m_dScreenAspect,
         l_dDH = g_HeadTrackParms.m_dScreenHeightWorld/20.0,
         l_dDW = l_dDH * g_HeadTrackParms.m_dScreenAspect,
         l_dBoxDepth = g_HeadTrackParms.m_dScreenHeightWorld,
         l_dVariable;
  
  for (int i = 0; i <= 20; i++)
  {
    l_dVariable = -l_dHalfHeight + (double)i * l_dDH;
    //draw U on xz
    glBegin(GL_LINE_STRIP);
    glColor3d(0.0, 0.0, 1.0);
    glVertex3d(-l_dHalfWidth, l_dVariable, 0.0);
    glVertex3d(-l_dHalfWidth, l_dVariable, -l_dBoxDepth);
    glVertex3d(l_dHalfWidth, l_dVariable, -l_dBoxDepth);
    glVertex3d(l_dHalfWidth, l_dVariable, 0.0);
    glEnd();

    //draw U on yz
    l_dVariable = -l_dHalfWidth + (double)i * l_dDW;
    glBegin(GL_LINE_STRIP);
    glVertex3d(l_dVariable, -l_dHalfHeight, 0.0);
    glVertex3d(l_dVariable, -l_dHalfHeight, -l_dBoxDepth);
    glVertex3d(l_dVariable, l_dHalfHeight, -l_dBoxDepth);
    glVertex3d(l_dVariable, l_dHalfHeight, 0.0);
    glEnd();

    //draw square on xy
    l_dVariable = (double)i * -l_dDH;
    glBegin(GL_LINE_LOOP);
    glVertex3d(-l_dHalfWidth, -l_dHalfHeight, l_dVariable);
    glVertex3d(l_dHalfWidth, -l_dHalfHeight, l_dVariable);
    glVertex3d(l_dHalfWidth, l_dHalfHeight, l_dVariable);
    glVertex3d(-l_dHalfWidth, l_dHalfHeight, l_dVariable);
    glEnd();
  }
}

void Display(void)
{
  //set perspective correction according to the head position
  SetHeadTrackedFrustum(MONO_CENTER);

  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  DrawRasterBox();

  glPushMatrix();
    glColor3d(1.0, 0.0, 0.0);
    glTranslated(-5.0, 5.0, -15.0);
    glutSolidCube(5.0);
    glColor3d(0.0, 1.0, 0.0);
    glTranslated(10.0, 0.0, 0.0);
    glutSolidCube(5.0);
    glColor3d(0.0, 0.0, 1.0);
    glTranslated(0.0, -10.0, 0.0);
    glutSolidCube(5.0);
    glColor3d(1.0, 1.0, 0.0);
    glTranslated(-10.0, 0.0, 0.0);
    glutSolidCube(5.0);
  glPopMatrix();

  glPushMatrix();
    glColor3d(1.0, 1.0, 0.0);
    glTranslated(-5.0, 5.0, -5.0);
    glutSolidCube(5.0);
    glColor3d(0.0, 0.0, 1.0);
    glTranslated(10.0, 0.0, 0.0);
    glutSolidCube(5.0);
    glColor3d(0.0, 1.0, 0.0);
    glTranslated(0.0, -10.0, 0.0);
    glutSolidCube(5.0);
    glColor3d(1.0, 0.0, 0.0);
    glTranslated(-10.0, 0.0, 0.0);
    glutSolidCube(5.0);
  glPopMatrix();

  glColor3d(1.0, 0.0, 1.0);

  /*glPushMatrix();
    glTranslated(0.0, 0.0, -10.0);
    glutSolidSphere(5.0, 8, 16);
  glPopMatrix();*/

  glPushMatrix();
  glTranslated(0.0, 0.0, 5.0);
  glScaled(1.0, 1.0, 10.0);
  glutSolidCube(2.0);
  glPopMatrix();


  /*glPushAttrib(GL_LIGHTING_BIT | GL_DEPTH_BUFFER_BIT);
  glDisable(GL_LIGHTING);
  glDisable(GL_DEPTH_TEST);*/
  /*glColor3d(1.0, 0.0, 0.0);
  glRasterPos2i(0, 9);
  char l_String[] = "TEST!!\0";
  for (int i=0; i<6; i++)
   glutBitmapCharacter( GLUT_BITMAP_HELVETICA_18, l_String[i]);*/
  //glPopAttrib();

  glutSwapBuffers();

}

void PollWiiMotes()
{
  //wiimotes report back to the host with a frequency of 100 herz
  if (wiiuse_poll(g_Wiimotes.m_ppWiimotes, 1))
  {
    for (int i=0; i < g_Wiimotes.m_iConnected; i++)
    {
      switch (g_Wiimotes.m_ppWiimotes[i]->event)
      {
      //default:
        //nothing??
      }
    }
  }
}

void Idle(void)
{
  PollWiiMotes();
  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_HeadTrackParms.m_dScreenHeightWorld,
         l_dHalfWidth = l_dHalfHeight * g_HeadTrackParms.m_dScreenAspect,
         l_zNear = 2.0 * g_HeadTrackParms.m_dScreenHeightWorld;

  glFrustum(-l_dHalfWidth, l_dHalfWidth, -l_dHalfHeight, l_dHalfHeight, l_zNear, l_zNear + 1000.0);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  glTranslated(0.0, 0.0, -l_zNear);
}


void ExitProgram()
{
  //disconnect wiimotes
  for (int i=0; i<g_Wiimotes.m_iConnected; i++)
  {
    wiiuse_disconnect(g_Wiimotes.m_ppWiimotes[i]);
  }

  wiiuse_cleanup(g_Wiimotes.m_ppWiimotes, g_Wiimotes.m_iConnected);

  printf("Finished!\n");

  exit(0);
}

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

  case ' ':
    //set y angle correction to current y angle of the the center of the two ir dots
    if (g_Wiimotes.m_iConnected > 0)
    {
      if (g_Wiimotes.m_ppWiimotes[0]->ir.num_dots >= 2)
      {
        const ir_dot_t *l_pDot[2];  //shorthand to the ir dots
        FindIRDots(&g_Wiimotes.m_ppWiimotes[0]->ir, l_pDot);
      
        //calculate the distance of the head (in mm)
        double  l_dHeadDistInMM = CalcHeadDistInMM(l_pDot, &g_HeadTrackParms), //the distance between the head and camera (in mm)
                l_dEyeYCam = (double)(l_pDot[0]->ry + l_pDot[1]->ry) / 2.0;
      
      // of the eye relative to the camera 
      g_HeadTrackParms.m_dYAngleCorrection = g_HeadTrackParms.m_dRadPerCameraPixel * (l_dEyeYCam - g_HeadTrackParms.m_dCameraYCenter);
      }
    }
    break;
  }
}

int main(int argc, char **argv)
{ 
  //init GLUT
  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH);
  glutCreateWindow("HeadTrackDemo");

  //init OpenGL
  glEnable(GL_LIGHTING);
  glEnable(GL_LIGHT0);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_COLOR_MATERIAL);
  glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE);
  glClearColor(1.0, 1.0, 1.0, 1.0);

  ////initialise the parameters needed for perspective correction when head tracking
  //g_HeadTrackParms.m_dHeadTrackLedDist = 205.0; 
  //g_HeadTrackParms.m_dRadPerCameraPixel = (41.0*(M_PI/180.0))/1024.0;
  //g_HeadTrackParms.m_dCameraXCenter = 1024.0/2.0;
  //g_HeadTrackParms.m_dCameraYCenter = 768.0/2.0;
  //g_HeadTrackParms.m_dYAngleCorrection = 0.0; //to be initialised correctly
  //g_HeadTrackParms.m_dCameraYOffset = 130.0;
  //g_HeadTrackParms.m_dScreenHeightMM = 210.0;
  //g_HeadTrackParms.m_dScreenAspect = 4.0/3.0;
  //g_HeadTrackParms.m_dScreenHeightWorld = 20.0;


  //initialise the parameters needed for perspective correction when head tracking
  g_HeadTrackParms.m_dHeadTrackLedDist = 205.0; 
  g_HeadTrackParms.m_dRadPerCameraPixel = (41.0*(M_PI/180.0))/1024.0;
  g_HeadTrackParms.m_dCameraXCenter = 1024.0/2.0;
  g_HeadTrackParms.m_dCameraYCenter = 768.0/2.0;
  g_HeadTrackParms.m_dYAngleCorrection = 0.0; //to be initialised correctly
  g_HeadTrackParms.m_dCameraYOffset = 205.0;
  g_HeadTrackParms.m_dScreenHeightMM = 320.0;
  g_HeadTrackParms.m_dScreenAspect = 16.0/10.0;
  g_HeadTrackParms.m_dScreenHeightWorld = 20.0;
 
  //connect to the wiimote(s)
  g_Wiimotes.m_ppWiimotes = wiiuse_init(1);

#ifdef WIN32
  wiiuse_set_bluetooth_stack(g_Wiimotes.m_ppWiimotes, 1, WIIUSE_STACK_MS);
#endif //WIN32

  int found = wiiuse_find(g_Wiimotes.m_ppWiimotes, 1, 30);
  g_Wiimotes.m_iConnected = wiiuse_connect(g_Wiimotes.m_ppWiimotes , 1); 
  if (g_Wiimotes.m_iConnected) 
  {
    printf("Connected to %i wiimotes (of %i found).\n", g_Wiimotes.m_iConnected, found); 

    wiiuse_set_leds(g_Wiimotes.m_ppWiimotes[0], WIIMOTE_LED_1 | WIIMOTE_LED_2 | WIIMOTE_LED_3 | WIIMOTE_LED_4);
    wiiuse_rumble(g_Wiimotes.m_ppWiimotes[0], 1);
    //wiiuse_motion_sensing(g_Wiimotes.m_ppWiimotes[0], 1);
    wiiuse_set_ir(g_Wiimotes.m_ppWiimotes[0], 1);
    Sleep(200);
    wiiuse_rumble(g_Wiimotes.m_ppWiimotes[0], 0);
  }
  else 
  { 
    printf("Failed to connect to any wiimote.\n"); 
    return 0; 
  }

  //register GLUT callback routines
  glutDisplayFunc(Display);
  glutIdleFunc(Idle);
  glutReshapeFunc(Reshape);
  glutKeyboardFunc(Keyboard);
  
  glutFullScreen();

  glutMainLoop();

  return 0;
}