src/Plugin.cpp

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

/*
 SpacePoint Fusion library based on USB HID library from WDK
 First version : april 2011
 Author : Aymeric Suteau
*/

// Include Header File
#include "Plugin.h"

cbmachine ww;
HWND HScol = NULL;      

int OBJFUSIONSCOL;

//===== CB Connected ===
int SCOL_FUSION_CONNECTED_CB          = 0;
int FUSION_CONNECTED_CB;

//===== CB Disconnected ===
int SCOL_FUSION_DISCONNECTED_CB       = 1;
int FUSION_DISCONNECTED_CB;

//===== CB New orientation data ===
int SCOL_FUSION_ORIENTATION_DATA_CB   = 2;
int FUSION_ORIENTATION_DATA_CB;

//===== CB New raw data ===
int SCOL_FUSION_RAW_DATA_CB           = 3;
int FUSION_RAW_DATA_CB;

//===== CB Button pressed/released ===
int SCOL_FUSION_BUTTON_CB             = 4;
int FUSION_BUTTON_CB;



int destroyFusionObj(mmachine m, int handsys, int fusionTab) 
{
        // Read the first element of a TAB element (table of objects)
        Fusion* FusionObj = (Fusion*) MMfetch(m, MTOP(fusionTab), 0);

        // Safely dispose of "FusionObj" pointer
        SAFE_DELETE(FusionObj);

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

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


int _OpenFusionDevice(mmachine m) 
{
        #ifdef  _SCOL_DEBUG_
                MMechostr(MSKDEBUG,"_OpenFusionDevice\n");
        #endif

        // Declare local variables
        int k = 0;
        int dataType;

  // Test channel
  if (MMget(m, 1) == NIL)   // NOTE: Channel is the first parameter, so on the top of the stack
  {   
    MMechostr(MSKDEBUG,"_OpenFusionDevice : channel NIL\n");
    m->pp += 1;
    return 0;
  }

  // Retrieve data type (raw or orientation)
        if ((dataType = MTOI(MMpull(m))) == NIL)
                dataType = ORIENTATION_DATA;  // Default data mode : orientation

        // Create Fusion instance
        Fusion * fusion = new Fusion();
  MMechostr(MSKDEBUG, "_OpenFusionDevice ...new SpacePoint Fusion instance created !\n");

        // Open Fusion device on USB port
  if (!fusion->OpenDevice(dataType)) 
  {
                MMechostr(MSKDEBUG, "_OpenFusionDevice ...initialization failed\n");
                SAFE_DELETE(fusion);
                MMpull(m);                                                              // Pull the channel
                MMpush(m, NIL);                                         // Push NIL on the stack
                return 0;
        }
 
        // Allocate a block in the stack for a table of Fusion objects
        int fusionTab = MMmalloc(m, 1, TYPETAB);
        if (fusionTab == NIL) 
  {
                MMechostr(MSKDEBUG, "_OpenFusionDevice ...MMmalloc failed\n");
                SAFE_DELETE(fusion); 
                MMpull(m);                                                              // Pull the channel
                return MMpush(m, NIL);          // Push NIL on the stack
        }
        MMechostr(MSKDEBUG, "_OpenFusionDevice ...MMmalloc successful\n");

        // Push the TAB Fusion object into the stack
        MMstore(m, fusionTab, 0, (int)fusion);
        MMpush(m, PTOM(fusionTab));

        // Create a new SpacePoint Fusion object
        k = OBJcreate(m, OBJFUSIONSCOL, (int)fusion, NULL, NULL);
        MMechostr(MSKDEBUG, "_OpenFusionDevice ...object creation successful\n");

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

        // Return Fusion object
        return k;
}


int _CloseFusionDevice(mmachine m) 
{
        #ifdef  _SCOL_DEBUG_
                MMechostr(MSKDEBUG,"_CloseFusionDevice\n");
  #endif

        // Get the table of Fusion objects into the stack (without pulling it)
        int fusionTab = MTOP(MMget(m, 0));
  MMechostr(MSKDEBUG, "_CloseFusionDevice\n");
        if (fusionTab == NIL)
  {
                MMechostr(MSKDEBUG, "_CloseFusionDevice ...ObjFusion NIL\n");
                MMset(m, 0, NIL);
                return 0;
        }

        // Destroy Fusion object according to its type and magma object
  OBJdelTM(m, OBJFUSIONSCOL, PTOM(fusionTab));

        // Reinitialize the stack
        MMset(m, 0, ITOM(0));

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

        return 0;
}


int _CBFusionConnected(mmachine m)
{
        return OBJaddreflex(m, OBJFUSIONSCOL, SCOL_FUSION_CONNECTED_CB);
}

int getFusionConnectedCb(mmachine m, HWND h, unsigned msg, UINT id, LONG param, int *ret)
{
        int k = 0;
        Fusion * FusionObj = (Fusion*) id;
        
        // OBJbeginreflex(mmachine, objecttype, objectptr, cbtype)
        if (OBJbeginreflex(m, OBJFUSIONSCOL, (int)FusionObj, SCOL_FUSION_CONNECTED_CB))
                return 0;

        return (OBJcallreflex(m, 0));
}


int _CBFusionDisconnected(mmachine m)
{
        return OBJaddreflex(m, OBJFUSIONSCOL, SCOL_FUSION_DISCONNECTED_CB);
}

int getFusionDisconnectedCb(mmachine m, HWND h, unsigned msg, UINT id, LONG param, int *ret)
{
        int k = 0;
        Fusion * FusionObj = (Fusion*) id;
        
        // OBJbeginreflex(mmachine, objecttype, objectptr, cbtype)
        if (OBJbeginreflex(m, OBJFUSIONSCOL, (int)FusionObj, SCOL_FUSION_DISCONNECTED_CB))
                return 0;

        return (OBJcallreflex(m, 0));
}


int _CBFusionOrientationData(mmachine m) 
{
        // Add a reflex
        MMechostr(MSKDEBUG, "_CBFusionOrientationData ...adding reflex\n");
        return OBJaddreflex(m, OBJFUSIONSCOL, SCOL_FUSION_ORIENTATION_DATA_CB);
}


int getFusionOrientationDataCb(mmachine m, HWND h, unsigned msg, UINT id, LONG param, int *ret) 
{
        int k = 0;
        float accX = 0, accY = 0, accZ = 0;                           // Initialize acceleration values

        // Cast id parameter to GloveObj type
        Fusion * FusionObj = (Fusion*) id;
  
        // Use : OBJbeginreflex(mmachine, type of object, ptr object, callback type)
        if (OBJbeginreflex(m, OBJFUSIONSCOL, (int)FusionObj, SCOL_FUSION_ORIENTATION_DATA_CB)) 
  {
                //MMechostr(MSKDEBUG, "getFusionOrientationDataCb ...ObjFusion not found\n");
                return 0;
        }

        // Retrieve acceleration values
  accX = FusionObj->fAcceleration[0];
  accY = FusionObj->fAcceleration[1];
  accZ = FusionObj->fAcceleration[2];

        // 1. Create tuple for acceleration values
        int tupleAcc = MMmalloc(m, 3, TYPETAB);
        if (tupleAcc == NIL) {
                MMpush(m, NIL);         // Reinitialize the stack       
                return MERRMEM;         // Return an error code (-1)
        }
        // Write TAB object field by field
        MMstore(m, tupleAcc, 0, FTOM(accX));
        MMstore(m, tupleAcc, 1, FTOM(accY));
  MMstore(m, tupleAcc, 2, FTOM(accZ));

        // Push tuple on the stack
  MMpush(m, PTOM(tupleAcc));

        // 2. Create tuple for quaternion values
        int tupleQuat = MMmalloc(m, 4, TYPETAB);
        if (tupleQuat == NIL) 
  {
                MMpush(m, NIL);
                return MERRMEM;
        }
        MMstore(m, tupleQuat, 0, FTOM(FusionObj->fQuaternion.x));
        MMstore(m, tupleQuat, 1, FTOM(FusionObj->fQuaternion.y));
  MMstore(m, tupleQuat, 2, FTOM(FusionObj->fQuaternion.z));
  MMstore(m, tupleQuat, 3, FTOM(FusionObj->fQuaternion.w));
  MMpush(m, PTOM(tupleQuat));

        // Call reflex previously defined (second parameter = number of user parameters)
        k = OBJcallreflex(m, 2);
        return k;
}


int _CBFusionRawData(mmachine m) 
{
        // Add a reflex
        MMechostr(MSKDEBUG, "_CBFusionRawData ...adding reflex\n");
        return OBJaddreflex(m, OBJFUSIONSCOL, SCOL_FUSION_RAW_DATA_CB);
}


int getFusionRawDataCb(mmachine m, HWND h, unsigned msg, UINT id, LONG param, int *ret) 
{
        int k = 0;
        int magX = 0, magY = 0, magZ = 0;
  int accX = 0, accY = 0, accZ = 0;
  int gyrR = 0, gyrP = 0, gyrY = 0;
  int res1 = 0, res2 = 0;
        int buttonL = 0, buttonR = 0;

        // Cast id parameter to GloveObj type
        Fusion * FusionObj = (Fusion*) id;
  
        // Use : OBJbeginreflex(mmachine, type of object, ptr object, callback type)
        if (OBJbeginreflex(m, OBJFUSIONSCOL, (int)FusionObj, SCOL_FUSION_RAW_DATA_CB)) 
  {
                //MMechostr(MSKDEBUG, "getFusionRawDataCb ...ObjFusion not found\n");
                return 0;
        }

  // Retrieve magnetometer values
  magX = FusionObj->iRawAxes[0];
  magY = FusionObj->iRawAxes[1];
  magZ = FusionObj->iRawAxes[2];
        
        // Retrieve accelerometer values
  accX = FusionObj->iRawAxes[3];
  accY = FusionObj->iRawAxes[4];
  accZ = FusionObj->iRawAxes[5];

  // Retrieve gyroscope values
  gyrR = FusionObj->iRawAxes[6];
  gyrP = FusionObj->iRawAxes[7];
  gyrY = FusionObj->iRawAxes[8];

  // Retrieve results 1 and 2 values
  res1 = FusionObj->iRawAxes[9];
  res2 = FusionObj->iRawAxes[10];

        // 1. Create tuple for magnetometer values
        int tupleMag = MMmalloc(m, 3, TYPETAB);
        if (tupleMag == NIL) 
  {
                MMpush(m, NIL);         // Reinitialize the stack       
                return MERRMEM;         // Return an error code (-1)
        }
        // Write TAB object field by field
  MMstore(m, tupleMag, 0, ITOM(magX));
        MMstore(m, tupleMag, 1, ITOM(magY));
  MMstore(m, tupleMag, 2, ITOM(magZ));

        // Push tuple on the stack
  MMpush(m, PTOM(tupleMag));

        // 2. Create tuple for accelerometer values
  int tupleAcc = MMmalloc(m, 3, TYPETAB);
        if (tupleAcc == NIL) 
  {
                MMpush(m, NIL);         // Reinitialize the stack       
                return MERRMEM;         // Return an error code (-1)
        }
  MMstore(m, tupleAcc, 0, ITOM(accX));
        MMstore(m, tupleAcc, 1, ITOM(accY));
  MMstore(m, tupleAcc, 2, ITOM(accZ));
  MMpush(m, PTOM(tupleAcc));
  
  // 3. Create tuple for gyroscope values
  int tupleGyr = MMmalloc(m, 3, TYPETAB);
        if (tupleGyr == NIL) 
  {
                MMpush(m, NIL);         // Reinitialize the stack       
                return MERRMEM;         // Return an error code (-1)
        }
  MMstore(m, tupleGyr, 0, ITOM(gyrR));
        MMstore(m, tupleGyr, 1, ITOM(gyrP));
  MMstore(m, tupleGyr, 2, ITOM(gyrY));
  MMpush(m, PTOM(tupleGyr));

  // 4. Create tuple for results 1 and 2 values
  int tupleRes = MMmalloc(m, 2, TYPETAB);
        if (tupleRes == NIL) 
  {
                MMpush(m, NIL);         // Reinitialize the stack       
                return MERRMEM;         // Return an error code (-1)
        }
  MMstore(m, tupleRes, 0, ITOM(res1));
  MMstore(m, tupleRes, 1, ITOM(res2));
  MMpush(m, PTOM(tupleRes));

        // Call reflex previously defined (second parameter = number of user parameters)
        k = OBJcallreflex(m, 4);
        return k;
}


int _CBFusionButton(mmachine m) 
{
        // Add a reflex
        MMechostr(MSKDEBUG, "_CBFusionButton ...adding reflex\n");
        return OBJaddreflex(m, OBJFUSIONSCOL, SCOL_FUSION_BUTTON_CB);
}


int getFusionButtonCb(mmachine m, HWND h, unsigned msg, UINT id, LONG param, int *ret) 
{
        int k = 0;
        int fusionButtons = 0;

        // Cast id parameter to GloveObj type
        Fusion * FusionObj = (Fusion*) id;
  
        // Use : OBJbeginreflex(mmachine, type of object, ptr object, callback type)
        if (OBJbeginreflex(m, OBJFUSIONSCOL, (int)FusionObj, SCOL_FUSION_BUTTON_CB)) 
  {
                MMechostr(MSKDEBUG, "getFusionButtonCb ...ObjFusion not found\n");
                return 0;
        }

  // Retrieve button values
  if (FusionObj->iButton[0])
    fusionButtons |= 1;
  if (FusionObj->iButton[1])
    fusionButtons |= 2;

  // Store Fusion buttons values into the stack
  MMpush(m, ITOM(fusionButtons));

        // Call reflex previously defined
        k = OBJcallreflex(m, 1);
        return k;
}




#define NbTplPKG 8


char    *TplName[NbTplPKG] =
{
        "ObjFusion",
  "_OpenFusionDevice",
        "_CloseFusionDevice",
  "_CBFusionConnected",
  "_CBFusionDisconnected",
  "_CBFusionOrientationData",
  "_CBFusionRawData",
  "_CBFusionButton"
};


int (*TplFunc[NbTplPKG])(mmachine m)=
{
        NULL,                                                                                                                                                           // ObjFusion
  _OpenFusionDevice,                                                                                                    // _OpenFusionDevice
        _CloseFusionDevice,                                                                                                     // _CloseFusionDevice
  _CBFusionConnected,                         // _CBFusionConnected
  _CBFusionDisconnected,                      // _CBFusionDisconnected
  _CBFusionOrientationData,                   // _CBFusionOrientationData
  _CBFusionRawData,                           // _CBFusionRawData
  _CBFusionButton                             // _CBFusionButton
};


int TplNArg[NbTplPKG]=
{
        TYPTYPE,                                                                                                                                                // ObjFusion
  2,                                                                                                                                                                    // _OpenFusionDevice
        1,                                                                                                                                                                      // _CloseFusionDevice
  3,                                          // _CBFusionConnected
  3,                                          // _CBFusionDisconnected
  3,                                          // _CBFusionOrientationData
  3,                                          // _CBFusionRawData
  3                                           // _CBFusionButton
};


char* TplType[NbTplPKG]=
{
        NULL,                                                                                                                                                                                                     // ObjFusion
  "fun [Chn I] ObjFusion",                                                                                                                        // _OpenFusionDevice
        "fun [ObjFusion] I",                                                                                                                                // _CloseFusionDevice
  "fun [ObjFusion fun [ObjFusion u0] u1 u0] ObjFusion",                                     // _CBFusionConnected
  "fun [ObjFusion fun [ObjFusion u0] u1 u0] ObjFusion",                                     // _CBFusionDisconnected
  "fun [ObjFusion fun [ObjFusion u0 [F F F] [F F F F]] u1 u0] ObjFusion",                 // _CBFusionOrientationData
  "fun [ObjFusion fun [ObjFusion u0 [I I I] [I I I] [I I I] [I I]] u1 u0] ObjFusion",   // _CBFusionRawData
  "fun [ObjFusion fun [ObjFusion u0 I] u1 u0] ObjFusion"                                // _CBFusionButton
};


// Everything inside _cond and _endcond is ignored by doxygen

int LoadFusion(mmachine m) 
{
  // Return variable for PKhardpak function
        int k;

        // Declare a new type of object ("OBJFUSIONSCOL")
        OBJFUSIONSCOL = OBJregister(5, 1, destroyFusionObj, "OBJFUSIONSCOL");

  // Get new user events and associate these events with a callback
  FUSION_CONNECTED_CB = OBJgetUserEvent();
  OBJdefEvent(FUSION_CONNECTED_CB, (int (__cdecl *)(struct Mmachine *, int, unsigned int, int, int, int *))getFusionConnectedCb);

  FUSION_DISCONNECTED_CB = OBJgetUserEvent();
  OBJdefEvent(FUSION_DISCONNECTED_CB, (int (__cdecl *)(struct Mmachine *, int, unsigned int, int, int, int *))getFusionDisconnectedCb);

  FUSION_ORIENTATION_DATA_CB = OBJgetUserEvent();
        OBJdefEvent(FUSION_ORIENTATION_DATA_CB, (int (__cdecl *)(struct Mmachine *, int, unsigned int, int, int, int *))getFusionOrientationDataCb);

  FUSION_RAW_DATA_CB = OBJgetUserEvent();
        OBJdefEvent(FUSION_RAW_DATA_CB, (int (__cdecl *)(struct Mmachine *, int, unsigned int, int, int, int *))getFusionRawDataCb);

  FUSION_BUTTON_CB = OBJgetUserEvent();
        OBJdefEvent(FUSION_BUTTON_CB, (int (__cdecl *)(struct Mmachine *, int, unsigned int, int, int, int *))getFusionButtonCb);

        // Load package
        k = PKhardpak(m, "FusionEngine", NbTplPKG, TplName, TplFunc, TplNArg, TplType);
        return k;
}


extern "C" __declspec (dllexport) int SCOLloadFUSION(mmachine m, cbmachine w) 
{
        SCOLinitplugin(w);

        // Get Scol window handle (for message callback)
        HScol = (HWND)SCgetExtra("hscol");

        // Display debug message
        MMechostr(MSKDEBUG, "SCOLloadFUSION loading SpacePoint Fusion DLL ...\n");
        
        LoadFusion(m);
        return 0;
}


extern "C" __declspec (dllexport) int SCOLfreeFUSION() 
{
        // Display debug messages
        MMechostr(MSKDEBUG, "Release SpacePoint Fusion DLL\n");
        MMechostr(MSKDEBUG, "SpacePoint Fusion DLL CLOSED...\n\n" );
        return 0;
}