#include <GL\glut.h>

#define _USE_MATH_DEFINES
#include <math.h>

#include "stereoheadtrackfrustum.h"
#include "typedefs.h"
#include "message_input.h"

bool CalcEyePosInMM(input_payload_wiimote *f_pWiimote, EyeOrigin_t f_Eye, FrustumParms_t &f_FrustumParms, Vect3D_t &f_HeadPosInMM)
{
  Vect3D_t l_Dot[2];

  if (f_pWiimote->posDataValid)
  {
    l_Dot[0] = Vect3D_t(f_pWiimote->SensorBarDot[0].sx, f_pWiimote->SensorBarDot[0].sy, 0.0);
    l_Dot[1] = Vect3D_t(f_pWiimote->SensorBarDot[1].sx, f_pWiimote->SensorBarDot[1].sy, 0.0);

    //invert the y-axis
    //double l_dCamViewHeight = 2.0 * f_FrustumParms.m_dCameraYCenter;
    l_Dot[0].x = 1016.0 - l_Dot[0].x;
    l_Dot[1].x = 1016.0 - l_Dot[1].x;

    //calculate the distance of the head (in mm)
    double l_dHeadDistInMM = f_pWiimote->Zdist,
           l_dDistanceFactor = f_FrustumParms.m_dEyeDistMM / f_FrustumParms.m_dHeadTrackLedDist;
    Vect3D_t  l_EyeCam,    //the coords of the eye camera (in wiimote ir-camera coords)
              l_HeadCenter = (l_Dot[0] + l_Dot[1]) / 2.0;

    //determine the eye position in camera coordinates
    switch(f_Eye)
    {
    case STEREO_LEFT_EYE:
    case STEREO_RIGHT_EYE:
        //in order to determine the position of the left and right eyes, we first determine which IR dot is the left one (then 
        //we also know the right dot). Then we determine the center on the line between the two dots and the direction vector from 
        //the center to the left/right dot, and add the vector multiplied by the factor between the led distance and eye distance, to
        //the center coordinates.
        l_EyeCam = l_HeadCenter + (l_Dot[(int)f_Eye] - l_HeadCenter) * l_dDistanceFactor;
      break;

    case MONO_CENTER:
    default:
        l_EyeCam = l_HeadCenter;
      break;
    }

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

    //correct the y angle
    l_dEyeYAngle += f_FrustumParms.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.
    f_HeadPosInMM.x = sin(l_dEyeXAngle) * l_dHeadDistInMM;
    f_HeadPosInMM.y = sin(l_dEyeYAngle) * l_dHeadDistInMM - f_FrustumParms.m_dCameraYOffset;
    f_HeadPosInMM.z = sqrt(l_dHeadDistInMM*l_dHeadDistInMM - 
                              (f_HeadPosInMM.x*f_HeadPosInMM.x + f_HeadPosInMM.y*f_HeadPosInMM.y)
                             ); //pythagoras
    
    return true;
  }
  
  return false;
}

void SetFrustum(FrustumParms_t &f_FrustumParms, Vect3D_t &f_HeadPosInMM, bool f_bTranslateEye)
{
  //convert the eye position from mm to world coordinates
  double l_dMmPerWorldCoord = f_FrustumParms.m_dScreenHeightMM / f_FrustumParms.m_dScreenHeightWorld;
  Vect3D_t l_Eye = f_HeadPosInMM/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 (center) and translate the modelview
  //matrix to (-l_dEyeX, -l_dEyeY, -l_dEyeZ), such that the world origin is in the center of the screen

  //in order to allow objects to appear in front of the screen we need to move the near clipping plane closer to the eye position,
  //without changing the frustum. 
  double l_dHalfHeight = 0.5 * f_FrustumParms.m_dScreenHeightWorld,
         l_dHalfWidth = l_dHalfHeight * f_FrustumParms.m_dScreenAspect;
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  GLdouble  l_dZNear  = 1.0,
            l_dFactor = l_dZNear/l_Eye.z,
            l_dLeft   = (-l_Eye.x - l_dHalfWidth) * l_dFactor,
            l_dRight  = (-l_Eye.x + l_dHalfWidth) * l_dFactor,
            l_dBottom = (-l_Eye.y - l_dHalfHeight) * l_dFactor,
            l_dTop    = (-l_Eye.y + l_dHalfHeight) * l_dFactor;

  glFrustum(l_dLeft, l_dRight, l_dBottom, l_dTop, l_dZNear, l_Eye.z + 100.0);

  if (f_bTranslateEye)
    glTranslated(-l_Eye.x, -l_Eye.y, -l_Eye.z); //move the frustum back to the position of the eye

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
}