/*
-----------------------------------------------------------------------------
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.
-----------------------------------------------------------------------------
*/


/*
 Optitrack camera library based on Camera SDK from NaturalPoint
 First version : may 2011
 Author : Aymeric Suteau
*/

/*! @defgroup grpoptitrack Scol functions definition for Optitrack camera object
 *  Scol functions definition for Optitrack camera object
 *  @{
 */
/** @} */


#include "plugin.h"

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

#include "cameralibrary.h"
using namespace CameraLibrary;


//!Scol machine declaration for MM macros
cbmachine ww;
HWND HScol = 0;

//! Optitrack Objects in Scol
int OBJ_OPTITRACK_SCOL;

//===== CB Optitrack CONNECTED ===
int SCOL_OPTITRACK_CAMERA_CONNECTED_CB = 0;
int OPTITRACK_CAMERA_CONNECTED_CB;

//===== CB Optitrack DISCONNECTED ===
int SCOL_OPTITRACK_CAMERA_DISCONNECTED_CB = 1;
int OPTITRACK_CAMERA_DISCONNECTED_CB;


/*! \mainpage NaturalPoint Optitrack Scol Plugin
 *
 * \section intro_sec Introduction
 * This plugin allows Scol Virtual Machine to manage NaturalPoint Optitrack V100:R2 camera.
 * <br/>This plugin is based on Camera SDK available here : http://media.naturalpoint.com/software/OptiTrack_Camera_SDK.1.1.0.Final.exe
 * 
*/


//!Scol CallBack for ObjOptitrackCamera destruction
int destroyOptitrackObj(mmachine m, int handsys, int objTab) 
{
	// Read the first element of a TAB element (table of objects)
  OptitrackCamera* optitrackObject = (OptitrackCamera*) MMfetch(m, MTOP(objTab), 0);

	if (optitrackObject != 0)
  {
    try
    {
      OptitrackCameraManager::GetInstance()->RemoveCamera(optitrackObject);
    }
    catch (ExceptionOptitrack& e)
    {
      MMechostr(MSKDEBUG, "%s", e.GetFullDescription().c_str());
    }
  }

	// Write the first element of a TAB element
	MMstore(m, MTOP(objTab), 0, 0);

	// Display debug message
	MMechostr(MSKDEBUG, "ObjOptitrackCamera destroyed.\n");
	return 0;
}


/*! @ingroup grpoptitrack
* \brief _CROptitrackCamera : Open and initialize Optitrack camera
*
* <b>Prototype:</b> fun [Chn] ObjOptitrackCamera
*
* \param Chn : current channel
*
* \return ObjOptitrackCamera : ObjOptitrackCamera object if success, NIL otherwise 
*/
int _CROptitrackCamera(mmachine m) 
{
	#ifdef	_SCOL_DEBUG_
		MMechostr(MSKDEBUG,"_CROptitrackCamera\n");
	#endif

	// Declare local variables
	int k = 0;

	// Create OptitrackObject instance
  OptitrackCamera* newOptitrackDevice = 0;
  if (OptitrackCameraManager::GetInstance() == 0)
  {
    MMset(m, 0, NIL);					// NIL on the stack
    return 0;
  }


  // DEBUG: Get camera manager singleton
  //OptitrackCameraManager::GetInstance();


  try
  {
    newOptitrackDevice = OptitrackCameraManager::GetInstance()->AddCamera();
  }
  catch (ExceptionOptitrack& e)
  {
    MMechostr(MSKDEBUG, "%s", e.GetFullDescription().c_str());
    MMset(m, 0, NIL);					// NIL on the stack
    return 0;
  }

  if (newOptitrackDevice == 0) 
  {
		MMechostr(MSKDEBUG, "_CRobjOptitrack ...creation failed\n");
		SAFE_DELETE(newOptitrackDevice);
		MMset(m, 0, NIL);					// NIL on the stack
		return 0;
	}
	MMechostr(MSKDEBUG, "_CRobjOptitrack ...creation successful\n");

	int objTab = MMmalloc(m, 1, TYPETAB);
	if (objTab == NIL)
  {
		MMechostr(MSKDEBUG, "_CRobjOptitrack ...MMmalloc failed\n");
		SAFE_DELETE(newOptitrackDevice); 
		MMset(m, 0, NIL);					// NIL on the stack
    return 0;
	}
	MMechostr(MSKDEBUG, "_CRobjOptitrack ...MMmalloc successful\n");
  
	// Push the table of object into the stack
	MMstore(m, objTab, 0, (int)newOptitrackDevice);
	MMpush(m, PTOM(objTab));

	// Create a new scol object
	k = OBJcreate(m, OBJ_OPTITRACK_SCOL, (int)newOptitrackDevice, 0, 0);
	MMechostr(MSKDEBUG, "_CRobjOptitrackDevice ...object creation successful\n");

	#ifdef	_SCOL_DEBUG_
		MMechostr(MSKDEBUG,"ok\n");
	#endif
	// Return object
	return k;
}


/*! @ingroup grpoptitrack
* \brief _DSOptitrackCamera : Destroy ObjOptitrackCamera
*
* <b>Prototype:</b> fun [objOptitrack] I
*
* \param ObjOptitrackCamera : object to destroy
*
* \return I : 1 if success, NIL otherwise 
*/
int _DSOptitrackCamera(mmachine m)
{
#ifdef	_SCOL_DEBUG_
	MMechostr(MSKDEBUG,"_DSOptitrackCamera\n");
#endif
	
	int objTab = MMget(m, 0);
	if ( objTab == NIL ) 
  {
    MMset(m, 0, NIL); 
    return 0;
  }


  // DEBUG: Destroy camera manager singleton
  //OptitrackCameraManager::GetInstance()->Kill();

	
	OBJdelTM(m, OBJ_OPTITRACK_SCOL, objTab);
	MMset(m, 0, ITOM(1));

#ifdef	_SCOL_DEBUG_
	MMechostr(MSKDEBUG,"ok\n");
#endif
	return 0;
}


/*! @ingroup grpoptitrack
* \brief _CBOptitrackCameraConnected : This function set the Callback for Connection from Optitrack camera
*
* <b>Prototype:</b> fun [ObjOptitrackCamera fun [ObjOptitrackCamera u0] u1 u0] ObjOptitrackCamera
* \param ObjOptitrackCamera : the current ObjOptitrackCamera
* \param fun [ObjOptitrackCamera u0] : SCOL CallBack function to call
* \param u1 : user parameter
* \param u0 : user parameter
*
* \return I : ObjOptitrackCamera if success
*/
int _CBOptitrackCameraConnected(mmachine m)
{
	return OBJaddreflex(m, OBJ_OPTITRACK_SCOL, SCOL_OPTITRACK_CAMERA_CONNECTED_CB);
}

int getOptitrackCameraConnectedCb(mmachine m, HWND h, unsigned msg, UINT id, LONG param, int *ret)
{
	int k = 0;
  OptitrackCamera* cameraObj = (OptitrackCamera*) id;
	
  if (OBJbeginreflex(m, OBJ_OPTITRACK_SCOL, (int)cameraObj, SCOL_OPTITRACK_CAMERA_CONNECTED_CB))
		return 0;
	if (k = OBJcallreflex(m, 0)) return k;
	return k;
}


/*! @ingroup grpoptitrack
* \brief _CBOptitrackCameraDisconnected : This function set the Callback for Disconnection from Optitrack camera
*
* <b>Prototype:</b> fun [ObjOptitrackCamera fun [ObjOptitrackCamera u0] u1 u0] ObjOptitrackCamera
* \param ObjOptitrackCamera : the current ObjOptitrackCamera
* \param fun [ObjOptitrackCamera u0] : SCOL CallBack function to call
* \param u1 : user parameter
* \param u0 : user parameter
*
* \return I : ObjOptitrackCamera if success
*/
int _CBOptitrackCameraDisconnected(mmachine m)
{
	return OBJaddreflex(m, OBJ_OPTITRACK_SCOL, SCOL_OPTITRACK_CAMERA_DISCONNECTED_CB);
}

int getOptitrackCameraDisconnectedCb(mmachine m, HWND h, unsigned msg, UINT id, LONG param, int *ret)
{
	int k = 0;
  OptitrackCamera* cameraObj = (OptitrackCamera*) id;
	
  if (OBJbeginreflex(m, OBJ_OPTITRACK_SCOL, (int)cameraObj, SCOL_OPTITRACK_CAMERA_DISCONNECTED_CB))
		return 0;
	if (k = OBJcallreflex(m, 0)) return k;
	return k;
}


/*! @ingroup grpoptitrack
* \brief _OptitrackCameraSetIntensity : Change the intensity of an Optitrack camera
*
* <b>Prototype:</b> fun [ObjOptitrack I] I
* \param ObjOptitrack : the current ObjOptitrack
* \param I : new intensity (default 0)
*
* \return I : 0 if success, NIL otherwise
*/
int _OptitrackCameraSetIntensity(mmachine m) 
{
#ifdef _SCOL_DEBUG_
	MMechostr(MSKDEBUG,"_OptitrackCameraSetIntensity\n");
#endif
  
  int intensity = MTOI(MMpull(m));
	int obj = MMget(m, 0);
	if (obj == NIL)
  {
		MMechostr(MSKDEBUG, "ObjOptitrack NIL\n");
		MMset(m, 0, NIL);
		return 0;
	}
  
  OptitrackCamera* cameraObj = (OptitrackCamera*) MMfetch(m, MTOP(obj), 0);
	if (cameraObj == 0)
  {
		MMset(m, 0, NIL);
		return 0;
	}

  cameraObj->SetIntensity(intensity);

  MMset(m, 0, ITOM(0));

#ifdef	_SCOL_DEBUG_
	MMechostr(MSKDEBUG,"ok\n");
#endif
	return 0;
}


/*! @ingroup grpoptitrack
* \brief _OptitrackCameraGetIntensity : Return the current intensity of an Optitrack camera
*
* <b>Prototype:</b> fun [ObjOptitrack] I
* \param ObjOptitrack : the current ObjOptitrack
*
* \return I : current intensity, NIL otherwise
*/
int _OptitrackCameraGetIntensity(mmachine m) 
{
#ifdef _SCOL_DEBUG_
	MMechostr(MSKDEBUG,"_OptitrackCameraGetIntensity\n");
#endif
  
	int obj = MMget(m, 0);
	if (obj == NIL)
  {
		MMechostr(MSKDEBUG, "ObjOptitrack NIL\n");
		MMset(m, 0, NIL);
		return 0;
	}
 
  OptitrackCamera* cameraObj = (OptitrackCamera*) MMfetch(m, MTOP(obj), 0);
	if (cameraObj == 0)
  {
		MMset(m, 0, NIL);
		return 0;
	}

  MMset(m, 0, ITOM(cameraObj->GetIntensity()));

#ifdef	_SCOL_DEBUG_
	MMechostr(MSKDEBUG,"ok\n");
#endif
	return 0;
}


/*! @ingroup grpoptitrack
* \brief _OptitrackCameraSetExposure : Change the exposure of an Optitrack camera
*
* <b>Prototype:</b> fun [ObjOptitrack I] I
* \param ObjOptitrack : the current ObjOptitrack
* \param I : new exposure (default 0)
*
* \return I : 0 if success, NIL otherwise
*/
int _OptitrackCameraSetExposure(mmachine m) 
{
#ifdef _SCOL_DEBUG_
	MMechostr(MSKDEBUG,"_OptitrackCameraSetExposure\n");
#endif
  
  int exposure = MTOI(MMpull(m));
	int obj = MMget(m, 0);
	if (obj == NIL)
  {
		MMechostr(MSKDEBUG, "ObjOptitrack NIL\n");
		MMset(m, 0, NIL);
		return 0;
	}
  
  OptitrackCamera* cameraObj = (OptitrackCamera*) MMfetch(m, MTOP(obj), 0);
	if (cameraObj == 0)
  {
		MMset(m, 0, NIL);
		return 0;
	}

  cameraObj->SetExposure(exposure);

  MMset(m, 0, ITOM(0));

#ifdef	_SCOL_DEBUG_
	MMechostr(MSKDEBUG,"ok\n");
#endif
	return 0;
}


/*! @ingroup grpoptitrack
* \brief _OptitrackCameraGetExposure : Return the current exposure of an Optitrack camera
*
* <b>Prototype:</b> fun [ObjOptitrack] I
* \param ObjOptitrack : the current ObjOptitrack
*
* \return I : current exposure, NIL otherwise
*/
int _OptitrackCameraGetExposure(mmachine m) 
{
#ifdef _SCOL_DEBUG_
	MMechostr(MSKDEBUG,"_OptitrackCameraGetExposure\n");
#endif
  
	int obj = MMget(m, 0);
	if (obj == NIL)
  {
		MMechostr(MSKDEBUG, "ObjOptitrack NIL\n");
		MMset(m, 0, NIL);
		return 0;
	}
 
  OptitrackCamera* cameraObj = (OptitrackCamera*) MMfetch(m, MTOP(obj), 0);
	if (cameraObj == 0)
  {
		MMset(m, 0, NIL);
		return 0;
	}

  MMset(m, 0, ITOM(cameraObj->GetExposure()));

#ifdef	_SCOL_DEBUG_
	MMechostr(MSKDEBUG,"ok\n");
#endif
	return 0;
}


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////																											                                                         ////
///										                  DECLARATION OF FUNCTIONS (FOR SCOL)											                    ///
////																											                                                         ////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//! Nb of Scol functions or types
#define NbOptitrackPKG	9

/*!
*	Scol function names
*/
char	*OptitrackName[NbOptitrackPKG] =
{	
	"ObjOptitrackCamera",
	"_CROptitrackCamera",
  "_DSOptitrackCamera",
  "_CBOptitrackCameraConnected",
  "_CBOptitrackCameraDisconnected",
  "_OptitrackCameraSetIntensity",
  "_OptitrackCameraGetIntensity",
  "_OptitrackCameraSetExposure",
  "_OptitrackCameraGetExposure"
};

/*!
*	Pointers to C functions that manipulate the VM for each scol function previously defined
*/
int (*OptitrackFunc[NbOptitrackPKG])(mmachine m)=
{
  NULL,
  _CROptitrackCamera,
  _DSOptitrackCamera,
  _CBOptitrackCameraConnected,
  _CBOptitrackCameraDisconnected,
  _OptitrackCameraSetIntensity,
  _OptitrackCameraGetIntensity,
  _OptitrackCameraSetExposure,
  _OptitrackCameraGetExposure
};

/*!
*	Nb of arguments of each scol function
*/
int OptitrackNArg[NbOptitrackPKG]=
{
  TYPTYPE,
	1,        // _CROptitrackCamera
  1,        // _DSOptitrackCamera
  3,        // _CBOptitrackCameraConnected
  3,        // _CBOptitrackCameraDisconnected
  2,        // _OptitrackCameraSetIntensity
  1,        // _OptitrackCameraGetIntensity
  2,        // _OptitrackCameraSetExposure
  1         // _OptitrackCameraGetExposure
};

/*!
*	Prototypes of the scol functions
*/
char* TplType[NbOptitrackPKG]=
{
	NULL,
	"fun [Chn] ObjOptitrackCamera",         						                                  // _CROptitrackCamera
  "fun [ObjOptitrackCamera] I",         						                                    // _DSOptitrackCamera
  "fun [ObjOptitrackCamera fun [ObjOptitrackCamera u0] u1 u0] ObjOptitrackCamera",      // _CBOptitrackCameraConnected
  "fun [ObjOptitrackCamera fun [ObjOptitrackCamera u0] u1 u0] ObjOptitrackCamera",      // _CBOptitrackCameraDisconnected
  "fun [ObjOptitrackCamera I] I",                                                       // _OptitrackCameraSetIntensity
  "fun [ObjOptitrackCamera] I",                                                         // _OptitrackCameraGetIntensity
  "fun [ObjOptitrackCamera I] I",                                                       // _OptitrackCameraSetExposure
  "fun [ObjOptitrackCamera] I"                                                          // _OptitrackCameraGetExposure
};


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////																											                                                         ////
///												                        DLL CALL FUNCTIONS													                      ///
////																											                                                         ////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Everything inside _cond and _endcond is ignored by doxygen
//! \cond

/*!
* \brief Load the Optitrack camera plugin
*
* \param mmachine : scol machine structure
*
* \return int : 0 if success, error code otherwise
*/
int LoadPlugin(mmachine m) {
	int k = 0;

	// Declare a new type of object ("OBJ_KINECT_OPTITRACK")
  OBJ_OPTITRACK_SCOL = OBJregister(2, 1, destroyOptitrackObj, "OBJ_OPTITRACK_SCOL");

  //******************************    Callbacks for Optitrack Obj    *******************************************************
  OPTITRACK_CAMERA_CONNECTED_CB = OBJgetUserEvent();
  OBJdefEvent(OPTITRACK_CAMERA_CONNECTED_CB, (int (__cdecl *)(struct Mmachine *,int,unsigned int,int,int,int *))getOptitrackCameraConnectedCb);

  OPTITRACK_CAMERA_DISCONNECTED_CB = OBJgetUserEvent();
  OBJdefEvent(OPTITRACK_CAMERA_DISCONNECTED_CB, (int (__cdecl *)(struct Mmachine *,int,unsigned int,int,int,int *))getOptitrackCameraDisconnectedCb);

  // Load package
  k = PKhardpak(m, "OptitrackEngine", NbOptitrackPKG, OptitrackName, OptitrackFunc, OptitrackNArg, TplType);
	return k;
}
//! \endcond


/*! 
* \brief Starting point of the DLL
*/
extern "C" __declspec (dllexport) int ScolLoadPlugin(mmachine m, cbmachine w)
{
  SCOLinitplugin(w);
  
	// Get Scol window handle (for message callback)
  HScol = (HWND)SCgetExtra("hscol");
  OptitrackCameraManager::GetInstance();

  LoadPlugin(m);
	return 0;
}

/*! 
* \brief Ending point of the DLL        
*/
extern "C" __declspec (dllexport) int ScolUnloadPlugin()
{
  // Free main Singleton
  OptitrackCameraManager::GetInstance()->Kill();

	return 0;
}