#include <GL/glut.h>

#include <time.h>
#include <string>
#include <iostream>

#define _USE_MATH_DEFINES
#include <math.h>

#include "C_MatchBloxEngine.h"
#include "message_input.h"

#include "C_3DObject.h"
#include "C_Environment.h"
#include "C_Hand.h"
#include "C_Block.h"
#include "C_Box.h"
#include "C_Log.h"
#include "bitmap.h"
#include "message_queue.h"
#include "message_input.h"


C_MatchBloxEngine::C_MatchBloxEngine(const char *f_strModelPath,
                                     const char *f_strLogFile,
                                     GameSettings f_GameSettings)
: m_GameSettings(f_GameSettings)
{
  //create logger
  
  
  //Load models
  if (LoadModels(f_strModelPath))
  {
    //set state to initialised;
    m_State = ES_INITIALISED;
  }
  else
  {
    m_State = ES_ERROR;
  }

  //initialise a random seed
  srand ( time(NULL) );

  //init vars
  m_CurrentBox = BS_SMALL;

  //init the world bounding box
  m_WorldBox.m_Min = Vect3D_t(-15.0, -5.0, -15.0);
  m_WorldBox.m_Max = Vect3D_t(15.0, 15.0, 15.0);

  //m_DotHist.clear();
  //m_DotHistSize = 15;
}

C_MatchBloxEngine::~C_MatchBloxEngine()
{
  //destroy logger
  
  //delete models
  DeleteModels();
}

GameResult C_MatchBloxEngine::ProcessMsgs(void)
{
  struct messageq_s *message;
  
  while (message = messageq_get(MESSAGE_RENDERER))
  {
    switch(message->sender)
    {
    case MESSAGE_INPUT_KEYBOARD:
      break;
    case MESSAGE_INPUT_MOUSE:
      break;
    case MESSAGE_INPUT_WIIMOTE:
      //get message payload
      //??how to interpret the message payload?? 
      //wiimote ir dots have a range of 1024x768 in xy which can easily be mapped to world xycoords
      //for the z coordinate: we know the size of the sensorbar, the reach of a human arm is <1m so 
      //if we map the initial z distance (in mm) -iz- (taken in ES_GET_READY) to be 0 and -z to be iz - 250mm
      //and +z to be iz + 250 mm we have 3d input!!
      //process button presses

      //process ir data
      input_payload_wiimote *l_pMsg;
      l_pMsg = (input_payload_wiimote*)message->payload;

      //for(int i=0; i<4; i++)
      {
        if (l_pMsg->btns && WIIMOTE_BUTTON_A)
        {
          //init depth
          Vect3D_t l_relPos;
          if (CalcWiimoteRelativeCursorPos(l_pMsg, &l_relPos))
          {
            m_dInitialWiimoteDist = l_relPos.z;
          }
          else
          {
            m_dInitialWiimoteDist = 500.0;
          }
        }
        else
        {
          Vect3D_t l_WorldPos;
          if (ConvertWiimoteToWorld(l_pMsg, &l_WorldPos))
          {
            m_pBlock[0]->SetPos(l_WorldPos);
          }
        }
        
          //std::cout << "Dot["<<i<<"] ("<< l_pMsg->Dot[i].RawX <<","<< l_pMsg->Dot[i].RawY <<")\n";
      }

      //if (m_State == ES_GET_READY)
      //{
      //  //if player has pressed the A button
      //  //take the initial z distance

      //  //perhaps we should do a countdown 3.. 2.. 1.. GO! but this requires another engine state
      //}
      //if (m_State == ES_PLAYING_GRAB_BLOCK || m_State == ES_PLAYING_PUT_BLOCK)
      //{
      //  //convert ir data to cursor pos in world coordinates

      //  Vect3D_t tmpPos(l_pMsg->x, l_pMsg->y, l_pMsg->z);
      //  CursorMove(tmpPos);
      //}
      break;
    default:
      std::cout << "Undefined message! Sender: " << message->sender << std::endl;
    }
  }

  return GR_BUSY;
}

void C_MatchBloxEngine::Render_Basics(unsigned int f_uiElapsedTime)
{
  glPushMatrix();

  //set camera pitch
  glRotated(20.0, 1.0, 0.0, 0.0);
  //rotate the environment map about the y axis
  glRotated((GLdouble)f_uiElapsedTime/1000.0, 0.0, 1.0, 0.0);
  m_pEnvMap->Render();

  glPopMatrix();

  glPushMatrix();
  

  //move the camera backwards
  glTranslated(0.0, 0.0, -16.0);
  glRotated(20.0, 1.0, 0.0, 0.0);
  

  

  //if (m_pCurrentSession)
  //{
    m_pBox[(int)m_CurrentBox]->Render();
    m_pBlock[0]->Render(f_uiElapsedTime);
   /* for (int i=0; i<4; i++)
      m_pBlock[i]->Render(f_uiElapsedTime);*/
  //  m_pHand->Render(f_uiElapsedTime);
  //}

  glPopMatrix();
}

void C_MatchBloxEngine::Render(unsigned int f_uiElapsedTime)
{
  switch (m_State)
  {
  case ES_INITIALISED:
    Render_Basics(f_uiElapsedTime);
  break;

  case ES_ERROR:
    //render a red cube
    glPushMatrix();
    glPushAttrib(GL_ENABLE_BIT);
    glDisable(GL_LIGHTING);
    glColor3d(1.0, 0.0, 0.0);
    glTranslated(0.0, 0.0, -5.0);
    glutSolidCube(5.0);
    glPopAttrib();
    glPopMatrix();
  break;

  case ES_GET_READY:
    Render_Basics(f_uiElapsedTime);
    //render some GET READY text
  break;

  case ES_PLAYING_GRAB_BLOCK:
    Render_Basics(f_uiElapsedTime);
  break;

  case ES_PLAYING_PUT_BLOCK:
    Render_Basics(f_uiElapsedTime);
  break;

  case ES_PAUSED:
    Render_Basics(f_uiElapsedTime);
    //render menu??
  break;

  case ES_FINISHED:
    //render results...
  break;
  }

}

bool C_MatchBloxEngine::NewGame(int f_iUserID, int f_iGameId, BoxSize f_BS)
{
  if(m_State == ES_INITIALISED)
  {
    m_CurrentBox = f_BS;

    //prepare a session struct for administration
    m_pCurrentSession = new GameSession(m_GameSettings.m_iNrOfTurns, f_BS);

    //randomize the box tiles
    m_pBox[(int)m_CurrentBox]->RandomizeTiles();
    
    //set state to GET READY
    m_State = ES_GET_READY; 
  
    return true;
  }

  return false;
}

bool C_MatchBloxEngine::StartGame()
{
  if (m_State == ES_GET_READY)
  {
    //start the session timer
    m_CurrentBlock = m_pCurrentSession->StartSession();

    //move a block into the sky
    m_pBlock[(int)m_CurrentBlock ]->SetPos(0.0, 0.0, 15.0);

    m_State = ES_PLAYING_GRAB_BLOCK;

    return true;
  }
  return false;
}

bool C_MatchBloxEngine::Pause()
{
  //only pause when playing
  if (m_State == ES_PLAYING_GRAB_BLOCK ||
      m_State == ES_PLAYING_PUT_BLOCK)
  {
    //save current state
    m_SavedState = m_State;

    m_pCurrentSession->PauseSession();
    
    //set current state to paused
    m_State = ES_PAUSED;

    return true;    
  }

  return false;
}

bool C_MatchBloxEngine::Resume()
{
  if (m_State == ES_PAUSED)
  {
    //restore previous state
    m_State = m_SavedState;

    //restore timer
    m_pCurrentSession->ResumeSession();

    return true;
  }
  return false;
}

bool C_MatchBloxEngine::Abort()
{
  //abort when not in error or init state
  if (m_State != ES_ERROR && m_State != ES_INITIALISED)
  {
    //set state to initialised
    m_State = ES_INITIALISED;

    //delete session (if there is any)
    if (m_pCurrentSession)
    {
      delete m_pCurrentSession;
      m_pCurrentSession = NULL;
    }

    return true;
  }
  return false;
}

bool C_MatchBloxEngine::LoadModels(const char* f_strModelDir)
{
  MatProps_t   l_Mat;
  std::string l_BaseName = f_strModelDir;

  //create the environment mapped cube
  m_pEnvMap = new C_Environment("envmaps/brightday2_", 50.0);

  //load the bitmaps for the textures
  LoadTexture((l_BaseName + "/wood1.bmp").c_str(), m_uiWood1Tex);
  LoadTexture((l_BaseName + "/wood2.bmp").c_str(), m_uiWood2Tex);
  LoadTexture((l_BaseName + "/wood3.bmp").c_str(), m_uiWood3Tex);

  //load the block models
  //red squares
  l_Mat.setAmb(1.0, 0.0, 0.0, 1.0);
  l_Mat.setDif(1.0, 0.0, 0.0, 1.0);
  m_pBlock[BT_SQUARE] = new C_Block((l_BaseName + "/square.obj").c_str(),
                                    m_uiWood1Tex, l_Mat);
  if (!m_pBlock[BT_SQUARE]->Initialized()) return false;

  //yellow cricles
  l_Mat.setAmb(0.0, 1.0, 1.0, 1.0);
  l_Mat.setDif(0.0, 1.0, 1.0, 1.0);
  m_pBlock[BT_CIRCLE] = new C_Block((l_BaseName + "/circle.obj").c_str(),
                                    m_uiWood1Tex, l_Mat);
  if (!m_pBlock[BT_CIRCLE]->Initialized()) return false;

  //green triangles
  l_Mat.setAmb(0.0, 1.0, 0.0, 1.0);
  l_Mat.setDif(0.0, 1.0, 0.0, 1.0);
  m_pBlock[BT_TRIANGLE] = new C_Block((l_BaseName + "/triangle.obj").c_str(),
                                    m_uiWood1Tex, l_Mat);
  if (!m_pBlock[BT_TRIANGLE]->Initialized()) return false;

  //blue crosses
  l_Mat.setAmb(0.0, 0.0, 1.0, 1.0);
  l_Mat.setDif(0.0, 0.0, 1.0, 1.0);
  m_pBlock[BT_CROSS] = new C_Block((l_BaseName + "/cross.obj").c_str(),
                                    m_uiWood1Tex, l_Mat);
  if (!m_pBlock[BT_CROSS]->Initialized()) return false;

  
  //load the hand???
  
  
  //Load the box tiles
  l_Mat.setAmb(1.0, 1.0, 1.0, 1.0);
  l_Mat.setDif(1.0, 1.0, 1.0, 1.0);
  
  m_pTiles[BT_SQUARE]   = new C_3DObject((l_BaseName + "/tile_square.obj").c_str(),
                                         m_uiWood3Tex, l_Mat);
  if (!m_pTiles[BT_SQUARE]->Initialized()) return false;

  m_pTiles[BT_CIRCLE]   = new C_3DObject((l_BaseName + "/tile_circle.obj").c_str(),
                                         m_uiWood3Tex, l_Mat);
  if (!m_pTiles[BT_CIRCLE]->Initialized()) return false;

  m_pTiles[BT_TRIANGLE] = new C_3DObject((l_BaseName + "/tile_triangle.obj").c_str(),
                                         m_uiWood3Tex, l_Mat);
  if (!m_pTiles[BT_TRIANGLE]->Initialized()) return false;

  m_pTiles[BT_CROSS]   = new C_3DObject((l_BaseName + "/tile_cross.obj").c_str(),
                                         m_uiWood3Tex, l_Mat);
  if (!m_pTiles[BT_CROSS]->Initialized()) return false;

  m_pTiles[4]          = new C_3DObject((l_BaseName + "/tile_no_hole.obj").c_str(),
                                         m_uiWood3Tex, l_Mat);
  if (!m_pTiles[4]->Initialized()) return false;

  
  //Load the box models
  m_pBox[0] = new C_Box((l_BaseName + "/box_small.obj").c_str(),
                        m_uiWood2Tex, l_Mat, 2, 2, m_pTiles);
  if (!m_pBox[0]->Initialized()) return false;
  
  m_pBox[1] = new C_Box((l_BaseName + "/box_med.obj").c_str(),
                        m_uiWood2Tex, l_Mat, 4, 2, m_pTiles);
  if (!m_pBox[1]->Initialized()) return false;

  m_pBox[2] = new C_Box((l_BaseName + "/box_large.obj").c_str(),
                        m_uiWood2Tex, l_Mat, 4, 4, m_pTiles);
  if (!m_pBox[2]->Initialized()) return false;

  
  return true;
}

void C_MatchBloxEngine::DeleteModels()
{
  //delete objects
  delete m_pEnvMap;
  delete m_pBlock[0];
  delete m_pBlock[1];
  delete m_pBlock[2];
  delete m_pBlock[3];
  //delete m_pHand; 
  delete m_pBox[0];
  delete m_pBox[1];
  delete m_pBox[2];
  delete m_pTiles[0];
  delete m_pTiles[1];
  delete m_pTiles[2];
  delete m_pTiles[3];
  delete m_pTiles[4];

  //delete textures
  glDeleteTextures(1, &m_uiWood1Tex);
  glDeleteTextures(1, &m_uiWood2Tex);
  glDeleteTextures(1, &m_uiWood3Tex);
}

void C_MatchBloxEngine::LoadTexture(const char* f_BmpName, GLuint &f_uiTexHandle)
{
  BitmapStruct l_Bmp;

  l_Bmp = BitmapLoad((char*)f_BmpName);
  f_uiTexHandle = (GLuint)l_Bmp.m_iImageId;
  
  glBindTexture(GL_TEXTURE_2D, f_uiTexHandle);

  //set the texture paramaters
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
}

void C_MatchBloxEngine::CursorMove(Vect3D_t &f_NewCursorPos)
{
  //check the state to see what 3d object currently has to be considered to 
  //be the cursor currently is and which bounding box we need to check for overlap
  BoundingBox_t l_CursBBox;
  bool          l_bCollision = false; //indicates a collission has happend

  switch (m_State)
  {
  case ES_PLAYING_PUT_BLOCK:
    //cursor is the current block figure (that is being held by the player)
    //translate the bounding box of the block with the cursor position 
    l_CursBBox = m_pBlock[m_CurrentBlock]->GetBoundingBox() + f_NewCursorPos;
    break;

  default:
    //cursor is the hand object
    l_CursBBox = m_pHand->GetBoundingBox() + f_NewCursorPos;
    break;
  }

  //now we have the bounding box of the cursor which we use to do some simple hit tests
  
  //make sure the cursor is still completely contained in the world bounding box
  if (!m_WorldBox.Contains(l_CursBBox))
  {
    //restore the previous cursor position, because the new one is invalid
    //... nothing to do actually
    //maybe generate some vibration event and collision animation???
    l_bCollision = true;
  }
  else
  {
    //check for bounding box overlap with the static elements in the scene 
    //actually only the block box
    if (m_pBox[m_CurrentBox]->GetAbsBoundBox().Overlap(l_CursBBox))
    {
      //overlap with the block box, a collision is very likely so set 
      //collision to true and reset to false when the player is putting
      //the block in the right hole (if the player is holding a block
      //that is, if the player isn't then there is certainly a collision)
      l_bCollision = true;
      
      //check if we are holding a block that has to be put in the box
      if (m_State == ES_PLAYING_PUT_BLOCK)
      {
        

      }
      else
      {
        //we are not holding a block so every contact with the 
        //block box is a collision
        l_bCollision = true;
      }
    }
  }

}

//bool C_MatchBloxEngine::CursorMove_PutBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CursBBox)
//{
//  //check if the block is being put in the right hole
//  //by testing whether the position of the cursor is close 
//  //enough to the center of the correct hole
//  //note that the hole positions are relative to the position
//  //of the box
//  bool     l_bCollision = false;
//  Vect3D_t l_AbsHolePos = m_pBox[m_CurrentBox]->GetPos() + 
//                          m_pTiles[m_CurrentBlock]->GetPos();
//  Vect3D_t l_PosDif = l_AbsHolePos - f_CursPos;
//  double   l_dXZProximity = l_PosDif.x * l_PosDif.x + 
//                            l_PosDif.y * l_PosDif.y;
//
//  if (l_dXZProximity < m_GameSettings.m_dMinProximity)
//  {
//    //the block the player is holding is considered to be in the right hole
//    //l_bCollision = false;
//   
//    //?? would it not be better to check for bounding box intersection with hole?
//    l_bCollision = !(m_pTiles[m_CurrentBlock]->GetAbsBoundBox() + 
//                     m_pBox[m_CurrentBox]->GetPos()).Overlap(f_CursBBox);
//
//    //now check if the block is far enough in the hole to count as a point
//    if (l_PosDif.z > m_GameSettings.m_dHoleDepth)
//    {
//      //yipee!! we have got a winner!!
//      m_CurrentBlock = m_pCurrentSession->NewTurn();
//
//      //m_GameState
//    }
//
//
//   
//  }
//
//  return false;
//}

//bool C_MatchBloxEngine::CursorMove_GrabBlock(Vect3D_t &f_CursPos, BoundingBox_t &f_CursBBox)
//{
//  //grabbing a block: just check for bounding box intersection
//  if (m_pBlock[(int)m_CurrentBlock]->GetAbsBoundBox().Overlap(f_CursBBox))
//  {
//    
//  } 
//  return true;
//}


bool C_MatchBloxEngine::FindIRDots(input_payload_wiimote *f_pWiimote, ir_dot_t f_Dot[2])
{
  //find the pair of ir dots with the largest squared distance
  int mdist = 0, //max length
      dist2,dx,dy,
      dot0, dot1;
  
  //loop through all combinations
  for(int i = 0; i < 4; i++)
  {
    for (int j = i+1; j < 4; j++)
    {
      //check if the ir dots are found
      if (f_pWiimote->ir.dot[i].visible && 
          f_pWiimote->ir.dot[j].visible)
      {
        //compute the squared distance
        dx = f_pWiimote->ir.dot[i].rx - f_pWiimote->ir.dot[j].rx;
        dy = f_pWiimote->ir.dot[i].ry - f_pWiimote->ir.dot[j].ry;
        dist2 = dx*dx + dy*dy;

        if (dist2 > mdist)
        {
          mdist = dist2;
          dot0 = i;
          dot1 = j;
          //std::cout << "(" << i << "," << j <<") " << mdist << std::endl;
        }
      }
    }
  }

  if (mdist > 0)
  {
    std::cout << "Winner - (" << dot0 << "," << dot1 <<") " << mdist << " numdots: " << (int)f_pWiimote->ir.num_dots << std::endl;
    //left down is f_Dot[0]
    if (f_pWiimote->ir.dot[dot0].rx < f_pWiimote->ir.dot[dot1].ry ||
        (f_pWiimote->ir.dot[dot0].rx == f_pWiimote->ir.dot[dot1].ry &&
         f_pWiimote->ir.dot[dot0].rx < f_pWiimote->ir.dot[dot1].ry))
    {
      f_Dot[0] = f_pWiimote->ir.dot[dot0];
      f_Dot[1] = f_pWiimote->ir.dot[dot1];
    }
    else
    {
      f_Dot[0] = f_pWiimote->ir.dot[dot1];
      f_Dot[1] = f_pWiimote->ir.dot[dot0];
    }
    
    return true;
  }
  else
  {
    //std::cout << "nothing...\n";
    return false;
  }
}


bool C_MatchBloxEngine::CalcWiimoteRelativeCursorPos(input_payload_wiimote *f_pWiimote, Vect3D_t *f_pRelPos)
{
  //get two ir dots
  ir_dot_t l_Dot[2];

  if (!FindIRDots(f_pWiimote, l_Dot))
    return false;

  //dotpair l_dp(l_Dot[0], l_Dot[1]);
  //m_DotHist.push_front(l_dp);
  //if (m_DotHist.size() > m_DotHistSize)
  //  m_DotHist.pop_back();

  ////calculate the average dots
  //l_Dot[0].RawX = l_Dot[0].RawY = l_Dot[1].RawX = l_Dot[1].RawY = 0;
  //for(list<dotpair>::iterator it = m_DotHist.begin(); it != m_DotHist.end(); it++)
  //{
  //  l_Dot[0].RawX += it->first.RawX; l_Dot[0].RawY += it->first.RawY;
  //  l_Dot[1].RawX += it->second.RawX; l_Dot[1].RawY += it->second.RawY;
  //}
  //l_Dot[0].RawX /= (double)m_DotHist.size(); l_Dot[0].RawY /= (double)m_DotHist.size();
  //l_Dot[1].RawX /= (double)m_DotHist.size(); l_Dot[1].RawY /= (double)m_DotHist.size();


  //invert the x and y axis
  l_Dot[0].rx = 1016 - l_Dot[0].ry;
  l_Dot[1].rx = 1016 - l_Dot[1].ry;
  l_Dot[0].rx =  760 - l_Dot[0].ry;
  l_Dot[1].rx =  760 - l_Dot[1].ry;

  double  l_dX = (double)l_Dot[0].rx - l_Dot[1].rx,            //difference in x coordinates
          l_dY = (double)l_Dot[0].ry - l_Dot[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_dRadPerPixel = ( 41.0 * (M_PI/180.0) ) /1016.0,        //radians per camera pixel: x view angel = 41 deg, 1016 pixels
          l_dDotAngle = l_dRadPerPixel * l_dDotDist;               //the angle between the lines from the camera through the two 
                                                                   //ir dots (in radians)

  f_pRelPos->x = (double)(l_Dot[0].rx + l_Dot[1].rx)/2.0; //camera x coordinate [0,1016]
  f_pRelPos->y = (double)(l_Dot[0].ry + l_Dot[1].ry)/2.0; //camera y coordinate [0,760]
  f_pRelPos->z = (0.5 * 205.0) / tan(0.5 * l_dDotAngle); //the distance between the sensorbar and wiimote (in mm)
  //std::cout << "(" << f_pRelPos->x << "," << f_pRelPos->y << "," << f_pRelPos->z << ")\n";
  
  return true;
}

bool C_MatchBloxEngine::ConvertWiimoteToWorld(input_payload_wiimote *f_pWiimote, Vect3D_t *f_pWorldPos)
{
  Vect3D_t l_RelPos;

  if (!CalcWiimoteRelativeCursorPos(f_pWiimote, &l_RelPos))
    return false;

  //use the world bounding box dimensions to convert the relative position to world coordinates

  //z is in mm, cap it to 250mm from the initial wiimote distance
  //using the initial distance
  l_RelPos.z = l_RelPos.z - m_dInitialWiimoteDist + 250;
  if (l_RelPos.z < 0.0) l_RelPos.z = 0.0;
  else if (l_RelPos.z > 500.0) l_RelPos.z = 500.0;

  //Vect3D_t l_WorldSize(m_WorldBox.m_Max - m_WorldBox.m_Min);
  Vect3D_t l_WorldSize;
  l_WorldSize.x = m_WorldBox.m_Max.x - m_WorldBox.m_Min.x;
  l_WorldSize.y = m_WorldBox.m_Max.y - m_WorldBox.m_Min.y;
  l_WorldSize.z = m_WorldBox.m_Max.z - m_WorldBox.m_Min.z;

  f_pWorldPos->x = m_WorldBox.m_Min.x + ((l_RelPos.x * l_WorldSize.x)/1016.0); //1016 pixels in x
  f_pWorldPos->y = m_WorldBox.m_Min.y + ((l_RelPos.y * l_WorldSize.y)/ 760.0); // 769 pixels in y
  f_pWorldPos->z = m_WorldBox.m_Min.z + ((l_RelPos.z * l_WorldSize.z)/ 500.0);//  500 mm in z

  //std::cout << "(" << f_pWorldPos->x << "," << f_pWorldPos->y << "," << f_pWorldPos->z << ")\n";
  return true;
}