sMyo.cpp

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

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

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

#include "SMyo.h"

extern int MYO_CONNECTED_CB;
extern int MYO_DISCONNECTED_CB;
extern int MYO_ORIENTATION_CB;
extern int MYO_ACCEL_CB;
extern int MYO_POSE_CB;
extern int MYO_ARMRECOGNIZED_CB;
extern int MYO_ARMLOST_CB;

// Initialize singleton
SMyoManager* SMyoManager::_singleton = 0;

SMyo::SMyo()
{
  mMyoInstance = 0;
  mDirection = xDirectionUnknown;
  mConnected = false;
  mPrevQuat = myo::Quaternion<float>(0.0f, 0.0f, 0.0f, 1.0f);
}

SMyo::~SMyo()
{
  Disconnect();
  // mMyoAvailable
}

myo::Myo* SMyo::GetMyoInstance()
{
  return mMyoInstance;
}

void SMyo::SetMyoInstance(myo::Myo* myoInstance)
{
  mMyoInstance = myoInstance;
}

void SMyo::SetDirection(XDirection direction)
{
  mDirection = direction;
}

XDirection SMyo::GetDirection()
{
  return mDirection;
}

void SMyo::Connect()
{
  mConnected = true;
  OBJpostEvent(MYO_CONNECTED_CB, SCOL_PTR this, 0);
}

void SMyo::Disconnect()
{
  mConnected = false;
  OBJpostEvent(MYO_DISCONNECTED_CB, SCOL_PTR this, 0);
}

void SMyo::SendPose(myo::Pose pose)
{
  SCbData* data = new SCbData();
  data->pose = pose;
  OBJpostEvent(MYO_POSE_CB, SCOL_PTR this, (int)data);
}

void SMyo::SendOrientation(myo::Quaternion<float> quat)
{
  SCbData* data = new SCbData();
  quat = quat.normalized();
  myo::Quaternion<float> dir = Quaternion<float>(0.0f, 1.0f, 0.0f, 0.0f);
  quat = (mDirection != xDirectionTowardWrist) ? Quaternion<float>(quat.y(), quat.z(), quat.x(), quat.w()) : Quaternion<float>(quat.y(), quat.z(), quat.x(), quat.w());
  data->quat = quat * dir;
  mPrevQuat = quat;
  OBJpostEvent(MYO_ORIENTATION_CB, SCOL_PTR this, (int)data);
}

void SMyo::SendAccel(myo::Vector3<float> vec)
{
  SCbData* data = new SCbData();
  data->svec = vec;

  OBJpostEvent(MYO_ACCEL_CB, SCOL_PTR this, (int)data);
}

void SMyo::Vibrate(myo::Myo::VibrationType type)
{
  if (mMyoInstance)
    mMyoInstance->vibrate(type);
}

SMyoManager::SMyoManager()
{
  mMyoHub = 0;
  mTerminate = false;
}

SMyoManager::~SMyoManager()
{
  mTerminate = true;
  mThread.join();

  MyoList::iterator ismyo;
        for (ismyo = mMyoList.begin(); ismyo != mMyoList.end(); ismyo++)
        {
                SMyo* myoInstance = (*ismyo);
                SAFE_DELETE(myoInstance);
                ismyo = mMyoList.erase(ismyo);
        }

  SAFE_DELETE(mMyoHub);
}

SMyoManager* SMyoManager::GetInstance()
{
  if (0 == _singleton)
  {
    _singleton = new SMyoManager();
  }

  return _singleton;
}

void SMyoManager::Kill()
{
  if (0 != _singleton)
  {
    delete _singleton;
    _singleton = 0;
  }
}

SMyo* SMyoManager::AddMyoInstance()
{
  boost::upgrade_lock< boost::shared_mutex > lock(mMutex);
  if (mMyoHub == 0)
  {
    try
    {
      mMyoHub = new myo::Hub("com.scol.myo");
      if (mMyoHub)
      {
        mMyoHub->setLockingPolicy(Hub::lockingPolicyNone);
        mMyoHub->addListener(this);

        mTerminate = false;
        mThread = boost::thread(boost::bind(&SMyoManager::UpdateThread, this));
      }
    }
    catch(std::exception &e)
    {
      //dongle not connected or already used
      MMechostr(MSKRUNTIME, "Myo error : %s", e.what());
    }
  }

  SMyo* myoInstance = 0;

  if (mMyoHub)
  {
    //take the lock exclusive
    boost::upgrade_to_unique_lock< boost::shared_mutex > uniqueLock(lock);

    myoInstance = new SMyo();
    mMyoList.insert(myoInstance);
  }

        return myoInstance;
}

void SMyoManager::RemoveMyoInstance(SMyo* myoInstance)
{
  {
    boost::upgrade_lock< boost::shared_mutex > lock(mMutex);

    if (myoInstance->GetMyoInstance())
    {
      //take the lock exclusive
      boost::upgrade_to_unique_lock< boost::shared_mutex > uniqueLock(lock);
      mMyoAvailable.insert(myoInstance->GetMyoInstance());
    }

          MyoList::iterator ismyo = mMyoList.find(myoInstance);
          if (ismyo != mMyoList.end())
          {
      //take the lock exclusive
      boost::upgrade_to_unique_lock< boost::shared_mutex > uniqueLock(lock);

                  mMyoList.erase(ismyo);
                  SAFE_DELETE(myoInstance);
    }
  }

  if (mMyoList.empty() && mMyoHub != 0)
  {
    //Remove thread
    mTerminate = true;
    mThread.join();

    mMyoHub->removeListener(this);
    mMyoHub->runOnce(1);
    SAFE_DELETE(mMyoHub);
    mMyoAvailable.clear();
  }
}

void SMyoManager::UpdateThread()
{
  while (!mTerminate)
  {
    if (!mTerminate)
      boost::shared_lock< boost::shared_mutex > lock(mMutex);

    // get number of paired myo
    size_t nbPaired = 0;
    MyoList::iterator ismyo;

          for (ismyo = mMyoList.begin(); ismyo != mMyoList.end(); ismyo++)
          {
                  SMyo* myoInstance = (*ismyo);
      if (myoInstance->GetMyoInstance() == 0)
      {
        if (mMyoAvailable.size() > 0)
        {
          myo::Myo* myo = (*mMyoAvailable.begin());
          myoInstance->SetMyoInstance(myo);
          myoInstance->Connect();
          mMyoAvailable.erase(mMyoAvailable.begin());
          nbPaired++;
        }
      }
      else
      {
        nbPaired++;
      }
          }

    // need to pair myos
    if (nbPaired < mMyoList.size())
    {
      myo::Myo* myo = 0;      
      try
      {
        myo = mMyoHub->waitForMyo(1);
      }
      catch (std::exception &e)
      {
        std::string mess = e.what();
        MMechostr(MSKDEBUG, "Myo Exception : %s", e.what());
        //error
      }
    }

    mMyoHub->run(33);
  }
}

void SMyoManager::onPair(Myo *myo, uint64_t timestamp, FirmwareVersion firmwareVersion)
{
  //all myo listener callbacks are in a foreign thread
  //boost::mutex::scoped_lock l(mMutex);
}

void SMyoManager::onConnect(Myo *myo, uint64_t timestamp, FirmwareVersion firmwareVersion)
{
  //all myo listener callbacks are in a foreign thread
  //boost::mutex::scoped_lock l(mMutex);
  mMyoAvailable.insert(myo);

  MyoList::iterator ismyo;
        for (ismyo = mMyoList.begin(); ismyo != mMyoList.end(); ismyo++)
        {
                SMyo* myoInstance = (*ismyo);
    if (myoInstance->GetMyoInstance() == 0)
    {
      myoInstance->SetMyoInstance(myo);
      myoInstance->Connect();

      MyoAvailables::iterator availableIt = mMyoAvailable.find(myo);
      if (availableIt != mMyoAvailable.end())
        mMyoAvailable.erase(availableIt);
      break;
    }
        }
}

void SMyoManager::onDisconnect(Myo *myo, uint64_t timestamp)
{
  //all myo listener callbacks are in a foreign thread
  //boost::mutex::scoped_lock l(mMutex);
  MyoAvailables::iterator availableIt = mMyoAvailable.find(myo);
  if (availableIt != mMyoAvailable.end())
    mMyoAvailable.erase(availableIt);

  MyoList::iterator ismyo;
        for (ismyo = mMyoList.begin(); ismyo != mMyoList.end(); ismyo++)
        {
                SMyo* myoInstance = (*ismyo);
    if (myoInstance->GetMyoInstance() == myo)
    {
      myoInstance->SetMyoInstance(0);
      myoInstance->Disconnect();
      break;
    }
        }
}

void SMyoManager::onPose(Myo *myo, uint64_t timestamp, Pose pose)
{
  //all myo listener callbacks are in a foreign thread
  //boost::mutex::scoped_lock l(mMutex);

  MyoList::iterator ismyo;
        for (ismyo = mMyoList.begin(); ismyo != mMyoList.end(); ismyo++)
        {
                SMyo* myoInstance = (*ismyo);
    if (myoInstance->GetMyoInstance() == myo)
    {
      myoInstance->SendPose(pose);
      break;
    }
        }
}

void SMyoManager::onOrientationData(Myo *myo, uint64_t timestamp, const Quaternion< float > &rotation)
{
  //all myo listener callbacks are in a foreign thread
  //boost::mutex::scoped_lock l(mMutex);

  MyoList::iterator ismyo;
        for (ismyo = mMyoList.begin(); ismyo != mMyoList.end(); ismyo++)
        {
                SMyo* myoInstance = (*ismyo);
    if (myoInstance->GetMyoInstance() == myo)
    {
      myoInstance->SendOrientation(rotation);
      break;
    }
        }
}

void SMyoManager::onAccelerometerData(Myo *myo, uint64_t timestamp, const Vector3< float > &accel)
{
  //all myo listener callbacks are in a foreign thread
  //boost::mutex::scoped_lock l(mMutex);

  MyoList::iterator ismyo;
        for (ismyo = mMyoList.begin(); ismyo != mMyoList.end(); ismyo++)
        {
                SMyo* myoInstance = (*ismyo);
    if (myoInstance->GetMyoInstance() == myo)
    {
      myoInstance->SendAccel(accel);
      break;
    }
        }
}

void SMyoManager::onGyroscopeData(Myo *myo, uint64_t timestamp, const Vector3< float > &gyro)
{
  //all myo listener callbacks are in a foreign thread
  //boost::mutex::scoped_lock l(mMutex);
}

void SMyoManager::onRssi(Myo *myo, uint64_t timestamp, int8_t rssi)
{
  //all myo listener callbacks are in a foreign thread
  //boost::mutex::scoped_lock l(mMutex);
}

void SMyoManager::onArmSync(Myo* myo, uint64_t timestamp, Arm arm, XDirection xDirection)
{
  //all myo listener callbacks are in a foreign thread
  //boost::mutex::scoped_lock l(mMutex);

  MyoList::iterator ismyo;
        for (ismyo = mMyoList.begin(); ismyo != mMyoList.end(); ismyo++)
        {
                SMyo* myoInstance = (*ismyo);
    myoInstance->SetDirection(xDirection);
    if (myoInstance->GetMyoInstance() == myo)
    {
      OBJpostEvent(MYO_ARMRECOGNIZED_CB, (int)myoInstance, (int)arm);
      break;
    }
        }
}

void SMyoManager::onArmUnsync(Myo* myo, uint64_t timestamp)
{
  //all myo listener callbacks are in a foreign thread
  //boost::mutex::scoped_lock l(mMutex);

  MyoList::iterator ismyo;
        for (ismyo = mMyoList.begin(); ismyo != mMyoList.end(); ismyo++)
        {
                SMyo* myoInstance = (*ismyo);
    if (myoInstance->GetMyoInstance() == myo)
    {
      OBJpostEvent(MYO_ARMLOST_CB, (int)myoInstance, 0);
      break;
    }
        }
}

void SMyoManager::onUnlock(Myo* myo, uint64_t timestamp)
{
}

void SMyoManager::onLock(Myo* myo, uint64_t timestamp)
{
}