/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com

Copyright (c) 2010 I-maginer

This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt

You may alternatively use this source under the terms of a specific version of
the OpenSpace3D Unrestricted License provided you have obtained such a license from
I-maginer.
-----------------------------------------------------------------------------
*/

/*! \class OptitrackCamera
  *  \brief Optitrack device handling.
  *  \
  *  \version 1.0
  *  \date may 2011
  */


#include "OptitrackCamera.h"
#include "../plugin.h"
#include "../core/Exception.h"

// Initialize singleton
OptitrackCameraManager* OptitrackCameraManager::_singleton = NULL;

/*
Manager handling the list of cameras
*/
OptitrackCameraManager::OptitrackCameraManager()
{
  // Retrieve camera manager
  MMechostr(MSKDEBUG, ">>> Waiting for cameras to spin up... ");
  manager = &CameraLibrary::CameraManager::X();

  // Initialize camera library
  manager->WaitForInitialization();
  if (manager->AreCamerasInitialized())
  {
    MMechostr(MSKDEBUG, "complete.\n");
  }
  else
  {
    MMechostr(MSKDEBUG, "failed!\n");
  }
}

OptitrackCameraManager::~OptitrackCameraManager()
{
  // Release camera manager
  if (manager != 0)
  {
    listOfCameras.clear();
    manager->Shutdown();
    manager = 0; 
  }
}

OptitrackCameraManager* OptitrackCameraManager::GetInstance()
{
  if (NULL == _singleton)
  {
    _singleton = new OptitrackCameraManager();
  }
  return _singleton;
}

void OptitrackCameraManager::Kill()
{
  SAFE_DELETE(_singleton);
}

CameraLibrary::CameraManager* OptitrackCameraManager::GetManager()
{
  return manager;
}

void OptitrackCameraManager::AddValidCamera(OptitrackCamera* camera)
{
  OptitrackCameraMap::iterator iCameraSearched = listOfCameras.find(camera->GetId());
  if (iCameraSearched == listOfCameras.end())
  {
    listOfCameras.insert(std::make_pair(camera->GetId(), camera));
  }
  else
  {
    OPTITRACK_EXCEPT(ExceptionOptitrack, "Optitrack camera already exists", "OptitrackCameraManager::AddValidCamera");
  }
}

void OptitrackCameraManager::RemoveValidCamera(OptitrackCamera* camera)
{
  OptitrackCameraMap::iterator iCameraSearched = listOfCameras.find(camera->GetId());
  if (iCameraSearched != listOfCameras.end())
  {
    listOfCameras.erase(iCameraSearched);
    SAFE_DELETE(camera);
  }
  else
  {
    OPTITRACK_EXCEPT(ExceptionOptitrack, "Can't remove Optitrack camera", "OptitrackCameraManager::RemoveValidCamera");
  }
}

bool OptitrackCameraManager::GetValidCamera(OptitrackCamera* camera)
{
  OptitrackCameraMap::iterator iCameraSearched = listOfCameras.find(camera->GetId());
  if (iCameraSearched != listOfCameras.end())
  {
    return true;
  }
  else
  {
    return false;
  }
}

void OptitrackCameraManager::RemoveCamera(OptitrackCamera* camera)
{
  if ((manager != 0) && (GetValidCamera(camera)))
  {
    RemoveValidCamera(camera);
    SAFE_DELETE(camera);
  }
}

unsigned int OptitrackCameraManager::CreateId()
{
  int idFound = 1;    // OptiTrack UID starts at 1
  while (listOfCameras.find(idFound) != listOfCameras.end())
  {
    idFound++;
  }
  return idFound;
}


/*
  Single camera 
*/
OptitrackCamera* OptitrackCameraManager::AddCamera()
{
  if (manager != 0)
  {
    OptitrackCamera* camera = new OptitrackCamera(CreateId());
    AddValidCamera(camera);
    return camera;
  }
  else
  {
    return 0;
  }
}

OptitrackCamera::OptitrackCamera(unsigned int id) : Thread()
{
  mId = id;
  mIntensity = 15;  // range: [1;15]
  mExposure = 255;  // range: [1;480]

  // Start Boost threading
  bConnected = false;
  StartThreading(boost::bind(&OptitrackCamera::GoThread, this));
  MMechostr(MSKDEBUG, ">>> Thread started...\n");

  // Add the current camera to the device manager
  OpenCamera();
  /*if (mCamera)
    TODO: register event handler */
} 

OptitrackCamera::OptitrackCamera() : Thread()
{
  // Forbiden (private).
}

OptitrackCamera::~OptitrackCamera()
{
  // Release camera
  if (mCamera)
  {
    // TODO: Unregister event handler
    mCamera->Release();
    mCamera = 0;
  }

  // Stop Boost threading
  StopThreading();
}

// Getters
unsigned int OptitrackCamera::GetId()
{
  return mId;
}

int OptitrackCamera::GetIntensity()
{
  return mIntensity;
}

int OptitrackCamera::GetExposure()
{
  return mExposure;
}

bool OptitrackCamera::IsConnected()
{
  return bConnected;
}

// Setters
void OptitrackCamera::SetIntensity(int intensity)
{
  mIntensity = intensity;
}

void OptitrackCamera::SetExposure(int exposure)
{
  mExposure = exposure;
}

// Open camera
void OptitrackCamera::OpenCamera()
{
  if (CameraLibrary::CameraManager::X().AreCamerasInitialized())
  {
    mCamera = OptitrackCameraManager::GetInstance()->GetManager()->GetCamera(GetId());   // Find a camera by Id
    //mCamera = OptitrackCameraManager::GetInstance()->GetManager()->GetCamera();
    if (!mCamera)
    {
      OPTITRACK_EXCEPT(ExceptionOptitrack, "Can't open camera", "OptitrackCamera::OpenCamera");
    }
    else
    {
      PostMessage(HScol, OPTITRACK_CAMERA_CONNECTED_CB, (int)this, NULL);
      bConnected = true;
    }
  }
  else
  {
    OPTITRACK_EXCEPT(ExceptionOptitrack, "Cameras not initialized", "OptitrackCamera::OpenCamera");
  }

  // Start camera
  mCamera->Start();
  MMechostr(MSKDEBUG, ">>> Camera started...\n");

  // DEBUG: Display camera ID
  mCamera->SetNumeric(true, GetId());
  MMechostr(MSKDEBUG, ">>> Camera Id set...\n");
}

void OptitrackCamera::CloseCamera()
{
  // Stop and release the device
  mCamera->Stop();
  mCamera->Release();

  if (mCamera)
  {
    SAFE_DELETE(mCamera);
  }

  PostMessage(HScol, OPTITRACK_CAMERA_DISCONNECTED_CB, (int)this, NULL);
}

// Handle Boost threading
void OptitrackCamera::GoThread()
{
  try
  {
    while(IsRunning())
    {
      // TODO: Handle camera update
      if (IsConnected())
      {
        // Retrieve a new mFrame from the camera
        mFrame = mCamera->GetLatestFrame();
        MMechostr(MSKDEBUG, "Frame retrieved.\n");
        if (mFrame)
        {
          // Release mFrame
          mFrame->Release();
        }
      }
    }
  }
  catch (ExceptionOptitrack& e)
  {
    MMechostr(MSKDEBUG, "%s", e.GetFullDescription().c_str());
  }
}