scolplugin.cpp

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

Copyright (c) 2012 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

-----------------------------------------------------------------------------
*/

/*
 MYO by ThalmicLabs interface for Scol
 First version : May 2014
 Author : Bastien BOURINEAU - I-Maginer
*/

#include "SMyo.h"
#include <scolPlugin.h>
#include <scolMemoryHelper.hpp>

#ifdef SCOL_STATIC
extern cbmachine        ww;
extern mmachine mm;
#else
cbmachine       ww;
mmachine mm;
#endif

int OBJ_MYO_SCOL;

int MYO_CONNECTED_CB;
int SCOL_MYO_CONNECTED_CB = 0;

int MYO_DISCONNECTED_CB;
int SCOL_MYO_DISCONNECTED_CB = 1;

int MYO_ORIENTATION_CB;
int SCOL_MYO_ORIENTATION_CB = 2;

int MYO_ACCEL_CB;
int SCOL_MYO_ACCEL_CB = 3;

int MYO_POSE_CB;
int SCOL_MYO_POSE_CB = 4;

int MYO_ARMRECOGNIZED_CB;
int SCOL_MYO_ARMRECOGNIZED_CB = 5;

int MYO_ARMLOST_CB;
int SCOL_MYO_ARMLOST_CB = 6;

int destroyMyoObj(mmachine m, SCOL_PTR_TYPE handsys, int handscol) 
{
        // Read the first element of a TAB element (table of object)
  SMyo* myoInstance = MMgetPointer<SMyo*>(m, MTOP(handscol));

  // Safely dispose of "myoInstance" pointer
  if (myoInstance)
    SMyoManager::GetInstance()->RemoveMyoInstance(myoInstance);

  MMsetPointer(m, MTOP(handscol), NULL);

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


int _CRmyoDevice(mmachine m)
{
#ifdef _SCOL_DEBUG_
        MMechostr(0,"_CRmyoDevice\n");
#endif
        // Declare local variables
        int k = 0;

        int pUserd = MMpull(m);
  int pCbkd  = MMpull(m);
  int pUserc = MMpull(m);
  int pCbkc  = MMpull(m);

  // Get the channel without pulling it (first element in the stack)
  int channel = MMget(m, 0);

        // Test the channel
  if (channel == NIL)
  { 
                MMechostr(MSKDEBUG, "_CRmyoDevice : Channel NIL\n");
                return 0;
        }

  SMyo* myoInstance = SMyoManager::GetInstance()->AddMyoInstance();
        if (myoInstance == NULL)
  {
                MMechostr(MSKDEBUG, "_CRmyoDevice : myoInstance failed\n");
    MMset(m, 0, NIL);
    return 0;
        }

        // Allocate a space in the stack for a table of joypad object
  if ((MMpushPointer(m, myoInstance) != 0))
  {
    SMyoManager::GetInstance()->RemoveMyoInstance(myoInstance);
    MMset(m, 0, NIL);
    return MERRMEM;
  }

        // Create a new scol myo object
        k = OBJcreate(m, OBJ_MYO_SCOL, SCOL_PTR myoInstance, NULL, NULL);

  if ((k = MMpush(m, pCbkc)))     return k; /* reading callback */
  if ((k = MMpush(m, pUserc)))    return k; /* user param       */
  if ((k = OBJaddreflex(m, OBJ_MYO_SCOL, SCOL_MYO_CONNECTED_CB))) return k;

  if ((k = MMpush(m, pCbkd)))     return k; /* reading callback */
  if ((k = MMpush(m, pUserd)))    return k; /* user param       */
  if ((k = OBJaddreflex(m, OBJ_MYO_SCOL, SCOL_MYO_DISCONNECTED_CB))) return k;

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

  return k;
}

int _DSmyoDevice(mmachine m)
{
#ifdef  _SCOL_DEBUG_
        MMechostr(MSKDEBUG,"_DSmyoDevice\n");
#endif
        
        int myoTab = MMget(m, 0);
        if (myoTab == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
        
        OBJdelTM(m, OBJ_MYO_SCOL, myoTab);
        MMset(m, 0, ITOM(0));

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

int _VibrateMyoDevice(mmachine m)
{
#ifdef  _SCOL_DEBUG_
        MMechostr(MSKDEBUG,"_VibrateMyoDevice\n");
#endif
        
  int period = MTOI(MMpull(m));
        int myoTab = MMget(m, 0);
        if (myoTab == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
        
  SMyo* myoInstance = MMgetPointer<SMyo*>(m, MTOP(myoTab));
  myo::Myo::VibrationType vtype = myo::Myo::vibrationShort;

  if (period == 1)
    vtype = myo::Myo::vibrationMedium;
  else if (period == 2)
    vtype = myo::Myo::vibrationLong;

  myoInstance->Vibrate(vtype);
        
        MMset(m, 0, ITOM(0));

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

int _IsMyoDeviceTrained(mmachine m)
{
#ifdef  _SCOL_DEBUG_
        MMechostr(MSKDEBUG,"_IsMyoDeviceTrained\n");
#endif
        
        int myoTab = MMget(m, 0);
        if (myoTab == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
        
  //SMyo* myoInstance = MMgetPointer<SMyo*>(m, MTOP(myoTab));
        MMset(m, 0, ITOM(1));

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

int _GetMyoArmDirection(mmachine m)
{
#ifdef  _SCOL_DEBUG_
        MMechostr(MSKDEBUG,"_GetMyoArmDirection\n");
#endif
        
        int myoTab = MMget(m, 0);
        if (myoTab == NIL)
  {
    MMset(m, 0, NIL);
    return 0;
  }
        
  SMyo* myoInstance = MMgetPointer<SMyo*>(m, MTOP(myoTab));
  if (!myoInstance)
  {
    MMset(m, 0, NIL);
    return 0;
  }

  int dir = (myoInstance->GetDirection() == xDirectionTowardWrist) ? 1 : -1;

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

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

/* Callbacks */

int _CBMyoOrientation(mmachine m)
{
        return OBJaddreflex(m, OBJ_MYO_SCOL, SCOL_MYO_ORIENTATION_CB);
}

int getMyoOrientationCb(mmachine m, SCOL_PTR_TYPE id, LONG param)
{
        int k = 0;
  SMyo* myoInstance = (SMyo*)id;

  SCbData* data = (SCbData*)param;

  if (!myoInstance)
  {
    delete data;
    return 0;
  }

        // OBJbeginreflex(mmachine, objecttype, objectptr, cbtype)
  if (OBJbeginreflex(m, OBJ_MYO_SCOL, SCOL_PTR myoInstance, SCOL_MYO_ORIENTATION_CB))
  {
    delete data;
    return 0;
  }

  int tuple = MMmalloc(m, 4, TYPETAB);
        if(tuple==NIL)  
  {             
    return MERRMEM;     
  }
        MMstore(m, tuple, 0, FTOM(data->quat.x()));
        MMstore(m, tuple, 1, FTOM(data->quat.y()));
        MMstore(m, tuple, 2, FTOM(data->quat.z()));
        MMstore(m, tuple, 3, FTOM(data->quat.w()));
  MMpush(m, PTOM(tuple));

        k=OBJcallreflex(m, 1);

  delete data;
  return k;
}

int _CBMyoAccel(mmachine m)
{
        return OBJaddreflex(m, OBJ_MYO_SCOL, SCOL_MYO_ACCEL_CB);
}

int getMyoAccelCb(mmachine m, SCOL_PTR_TYPE id, LONG param)
{
        int k = 0;
  SMyo* myoInstance = (SMyo*)id;

  SCbData* data = (SCbData*)param;

  if (!myoInstance)
  {
    delete data;
    return 0;
  }

        // OBJbeginreflex(mmachine, objecttype, objectptr, cbtype)
  if (OBJbeginreflex(m, OBJ_MYO_SCOL, SCOL_PTR myoInstance, SCOL_MYO_ACCEL_CB))
  {
    delete data;
    return 0;
  }

  int ftuple = MMmalloc(m, 3, TYPETAB);
        if(ftuple==NIL) 
  {     
    return MERRMEM;     
  }
        MMstore(m, ftuple, 0, FTOM(data->svec.x()));
        MMstore(m, ftuple, 1, FTOM(data->svec.y()));
  MMstore(m, ftuple, 2, FTOM(data->svec.z()));
  MMpush(m, PTOM(ftuple));

        k=OBJcallreflex(m, 1);

  delete data;
  return k;
}

int _CBMyoPose(mmachine m)
{
        return OBJaddreflex(m, OBJ_MYO_SCOL, SCOL_MYO_POSE_CB);
}

int getMyoPoseCb(mmachine m, SCOL_PTR_TYPE id, LONG param)
{
        int k = 0;
  SMyo* myoInstance = (SMyo*)id;

  SCbData* data = (SCbData*)param;

  if (!myoInstance)
  {
    delete data;
    return 0;
  }

        // OBJbeginreflex(mmachine, objecttype, objectptr, cbtype)
  if (OBJbeginreflex(m, OBJ_MYO_SCOL, SCOL_PTR myoInstance, SCOL_MYO_POSE_CB))
  {
    delete data;
    return 0;
  }

  int pose = (int)data->pose.type();
  //hack for Alpha compatibilty replace twist gesture by pinky
  if (pose == 6)
    pose = 5;

  MMpush(m, ITOM(pose));  

        k=OBJcallreflex(m, 1);

  delete data;
  return k;
}

int getMyoConnectedCb(mmachine m, SCOL_PTR_TYPE id, LONG param)
{
        int k = 0;
  SMyo* myoInstance = (SMyo*)id;

  if (!myoInstance)
    return 0;

        // OBJbeginreflex(mmachine, objecttype, objectptr, cbtype)
  if (OBJbeginreflex(m, OBJ_MYO_SCOL, SCOL_PTR myoInstance, SCOL_MYO_CONNECTED_CB))
    return 0;

        k=OBJcallreflex(m, 0);
        return k;
}

int getMyoDisconnectedCb(mmachine m, SCOL_PTR_TYPE id, LONG param)
{
        int k = 0;
  SMyo* myoInstance = (SMyo*)id;

  if (!myoInstance)
    return 0;

        // OBJbeginreflex(mmachine, objecttype, objectptr, cbtype)
  if (OBJbeginreflex(m, OBJ_MYO_SCOL, SCOL_PTR myoInstance, SCOL_MYO_DISCONNECTED_CB))
    return 0;

        k=OBJcallreflex(m, 0);
        return k;
}

int _CBMyoArmRecognized(mmachine m)
{
        return OBJaddreflex(m, OBJ_MYO_SCOL, SCOL_MYO_ARMRECOGNIZED_CB);
}

int getMyoArmRecognizedCb(mmachine m, SCOL_PTR_TYPE id, LONG param)
{
        int k = 0;
  SMyo* myoInstance = (SMyo*)id;

  if (!myoInstance)
    return 0;

        // OBJbeginreflex(mmachine, objecttype, objectptr, cbtype)
  if (OBJbeginreflex(m, OBJ_MYO_SCOL, SCOL_PTR myoInstance, SCOL_MYO_ARMRECOGNIZED_CB))
    return 0;

  MMpush(m, ITOM(param));

        k=OBJcallreflex(m, 1);
        return k;
}

int _CBMyoArmLost(mmachine m)
{
        return OBJaddreflex(m, OBJ_MYO_SCOL, SCOL_MYO_ARMLOST_CB);
}

int getMyoArmLostCb(mmachine m, SCOL_PTR_TYPE id, LONG param)
{
        int k = 0;
  SMyo* myoInstance = (SMyo*)id;

  if (!myoInstance)
    return 0;

        // OBJbeginreflex(mmachine, objecttype, objectptr, cbtype)
  if (OBJbeginreflex(m, OBJ_MYO_SCOL, SCOL_PTR myoInstance, SCOL_MYO_ARMLOST_CB))
    return 0;

        k=OBJcallreflex(m, 0);
        return k;
}

char* mMYOname[]=
{
  "ObjMyo",

  "_CRmyoDevice",
  "_DSmyoDevice",
  "_VibrateMyoDevice",
  "_GetMyoArmDirection",
  "_IsMyoDeviceTrained",
  "_CBMyoOrientation",
  "_CBMyoAccel",
  "_CBMyoPose",
  "_CBMyoArmRecognized",
  "_CBMyoArmLost"
};

#define NMYOPKG (sizeof(mMYOname)/sizeof(char*))

int (*mMYOfun[NMYOPKG])(mmachine m)=
{
  NULL,             //ObjMyo

  _CRmyoDevice,
  _DSmyoDevice,
  _VibrateMyoDevice,
  _GetMyoArmDirection,
  _IsMyoDeviceTrained,
  _CBMyoOrientation,
  _CBMyoAccel,
  _CBMyoPose,
  _CBMyoArmRecognized,
  _CBMyoArmLost
};

int mMYOnarg[NMYOPKG]=
{
  TYPTYPE,          //ObjMyo

  5,                //_CRmyoDevice
  1,                //_DSmyoDevice
  2,                //_VibrateMyoDevice
  1,                //_GetMyoArmDirection
  1,                //_IsMyoDeviceTrained
  3,                //_CBMyoOrientation
  3,                //_CBMyoAccel
  3,                //_CBMyoPose
  3,                //_CBMyoArmRecognized
  3                 //_CBMyoArmLost
};

char* mMYOtype[NMYOPKG]=
{
  NULL,                                                            //ObjMyo

  "fun [Chn fun [ObjMyo u0] u1 u0 fun [ObjMyo u0] u1 u0] ObjMyo",  //_CRmyoDevice
  "fun [ObjMyo] I",                                                //_DSmyoDevice
  "fun [ObjMyo I] I",                                              //_VibrateMyoDevice
  "fun [ObjMyo] I",                                                //_GetMyoArmDirection
  "fun [ObjMyo] I",                                                //_IsMyoDeviceTrained
  "fun [ObjMyo fun [ObjMyo u0 [F F F F]] u1 u0] ObjMyo",           //_CBMyoOrientation
  "fun [ObjMyo fun [ObjMyo u0 [F F F]] u1 u0] ObjMyo",             //_CBMyoAccel
  "fun [ObjMyo fun [ObjMyo u0 I] u1 u0] ObjMyo",                   //_CBMyoPose
  "fun [ObjMyo fun [ObjMyo u0 I] u1 u0] ObjMyo",                   //_CBMyoArmRecognized
  "fun [ObjMyo fun [ObjMyo u0] u1 u0] ObjMyo"                      //_CBMyoArmLost
};


// Everything inside _cond and _endcond is ignored by doxygen

int LoadMyo(mmachine m)
{
        int k = 0;
  
        MMechostr(MSKDEBUG,"\n" );
  MMechostr(MSKDEBUG," > Loading MYO Support\n");

  // initialize myo manager singleton
        SMyoManager::GetInstance();

  // Declare a new type of object ("ObjMyo")
        OBJ_MYO_SCOL = OBJregister(7 /*nb of callback*/, 0, destroyMyoObj, "OBJ_MYO_SCOL");
        k = PKhardpak(m, "mMYO.pkg-1.0", NMYOPKG, mMYOname, mMYOfun, mMYOnarg, mMYOtype);

  MYO_CONNECTED_CB = OBJgetUserEvent();
  OBJdefEvent(MYO_CONNECTED_CB, (int (__cdecl *)(struct Mmachine*, SCOL_PTR_TYPE, LONG))getMyoConnectedCb);

  MYO_DISCONNECTED_CB = OBJgetUserEvent();
  OBJdefEvent(MYO_DISCONNECTED_CB, (int (__cdecl *)(struct Mmachine*, SCOL_PTR_TYPE, LONG))getMyoDisconnectedCb);

  MYO_ORIENTATION_CB = OBJgetUserEvent();
  OBJdefEvent(MYO_ORIENTATION_CB, (int (__cdecl *)(struct Mmachine*, SCOL_PTR_TYPE, LONG))getMyoOrientationCb);

  MYO_ACCEL_CB = OBJgetUserEvent();
  OBJdefEvent(MYO_ACCEL_CB, (int (__cdecl *)(struct Mmachine*, SCOL_PTR_TYPE, LONG))getMyoAccelCb);

  MYO_POSE_CB = OBJgetUserEvent();
  OBJdefEvent(MYO_POSE_CB, (int (__cdecl *)(struct Mmachine*, SCOL_PTR_TYPE, LONG))getMyoPoseCb);

  MYO_ARMRECOGNIZED_CB = OBJgetUserEvent();
  OBJdefEvent(MYO_ARMRECOGNIZED_CB, (int (__cdecl *)(struct Mmachine*, SCOL_PTR_TYPE, LONG))getMyoArmRecognizedCb);

  MYO_ARMLOST_CB = OBJgetUserEvent();
  OBJdefEvent(MYO_ARMLOST_CB, (int (__cdecl *)(struct Mmachine*, SCOL_PTR_TYPE, LONG))getMyoArmLostCb);

  MMechostr(MSKDEBUG," > Successfully Loaded\n\n");
        return k;
}

int CloseMyo()
{
  MMechostr(MSKDEBUG,"\n" );
  MMechostr(MSKDEBUG," > Unloading MYO Support\n");
  
        // kill myo manager singleton
        SMyoManager::GetInstance()->Kill();

  MMechostr(MSKDEBUG," > Successfully unloaded\n\n");
  return 0;
}


#ifndef SCOL_STATIC

extern "C" SCOL_EXPORT int ScolLoadPlugin(mmachine m, cbmachine w)
{
  SCOLinitplugin(w);
  mm = m;

  return LoadMyo(m);
}

extern "C" SCOL_EXPORT int ScolUnloadPlugin()
{
  return CloseMyo();
}
#else

extern "C" SCOL_EXPORT int ScolMyoLoadPlugin(mmachine m, cbmachine w)
{
  SCOLinitplugin(w);
  mm = m;

  return LoadMyo(m);
}

extern "C" SCOL_EXPORT int ScolMyoUnloadPlugin()
{
  return CloseMyo();
}
#endif