1 files changed, 80 insertions, 33 deletions

diff --git a/matleap.cpp b/matleap.cpp
index 9a3cb25..4a24d3d 100755..100644
--- a/matleap.cpp
+++ b/matleap.cpp
@@ -4,7 +4,9 @@
 /// @version 1.0
 /// @date 2013-09-12
 
-#include "matleap.h"
+// original by Jeff Perry, updated and adjusted by Niklas Halle <niklas@niklashalle.net>
+
+#include "matleap.hpp"
 #include <array>
 
 // Under Windows, a Leap::Controller must be allocated after the MEX
@@ -24,7 +26,7 @@ int version = 4; // 1: orig, 2: with arm info, 3: with more hand info
 void matleap_exit() {
     fg->close_connection();
     delete fg;
-    fg = 0;
+    fg = nullptr;
 }
 
 /// @brief process interface arguments
@@ -37,7 +39,8 @@ void matleap_exit() {
 /// @return command number
 int get_command(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
     int command;
-    // check inputs
+
+    // check inputs - we take exactly one parameter
     switch (nrhs) {
         case 1:
             command = *mxGetPr(prhs[0]);
@@ -47,28 +50,17 @@ int get_command(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
         default:
             mexErrMsgTxt("Too many input arguments");
     }
-    // check that inputs agree with command
+
+    // check that outputs agree with command
     switch (command) {
-        case -1: {
+        case -1:
             // set debug command requires 0 outputs
-            if (nlhs != 0)
-                mexErrMsgTxt("Wrong number of outputs specified");
-        }
+            if (nlhs != 0) mexErrMsgTxt("Wrong number of outputs specified");
             break;
-        case 0: {
-            // get version command requires 1 outputs
-            if (nlhs != 0 && nlhs != 1)
-                mexErrMsgTxt("Wrong number of outputs specified");
-        }
-            break;
-        case 1: {
-            // frame grab command only requires one input
-            if (nrhs > 1)
-                mexErrMsgTxt("Too many inputs specified");
-            // frame grab command requires exactly one output
-            if (nlhs != 0 && nlhs != 1)
-                mexErrMsgTxt("Wrong number of outputs specified");
-        }
+        case 0:
+        case 1:
+            // get version and frame grab command can populate zero or one output
+            if (nlhs != 0 && nlhs != 1) mexErrMsgTxt("Wrong number of outputs specified");
             break;
         default:
             mexErrMsgTxt("An unknown command was specified");
@@ -204,31 +196,86 @@ void get_frame(int nlhs, mxArray *plhs[]) {
     mxSetFieldByNumber(plhs[0], 0, 5, mxCreateDoubleScalar(version));
 }
 
-void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
+/// @brief entry point for matlab
+///
+/// @param nlhs count of left hand side
+/// @param plhs pointer to left hand side array
+/// @param nrhs count of right hand side
+/// @param prhs pointer to right hand side array
+void mexFunction(int nlhs, mxArray *plhs[], int nrhs, mxArray const *prhs[]) {
+    // if there is no frame grabber yet, create one
     if (!fg) {
         fg = new matleap::frame_grabber;
-        if (fg == 0)
+        if (fg == nullptr) {
             mexErrMsgTxt("Cannot allocate a frame grabber");
+        }
         fg->open_connection();
+        // register exit handler
         mexAtExit(matleap_exit);
     }
+
+    // parse rhs and lhs to determine what is requested from us
     switch (get_command(nlhs, plhs, nrhs, prhs)) {
-        // turn on debug
-        case -1:
+        case -1: // turn on debug
             fg->set_debug(true);
-            return;
-            // get version
-        case 0:
+            break;
+        case 0: // get version
             plhs[0] = mxCreateNumericMatrix(1, 2, mxDOUBLE_CLASS, mxREAL);
             *(mxGetPr(plhs[0]) + 0) = MAJOR_REVISION;
             *(mxGetPr(plhs[0]) + 1) = MINOR_REVISION;
-            return;
-            // get frame
-        case 1:
+            break;
+        case 1: // get frame
             get_frame(nlhs, plhs);
-            return;
+            break;
         default:
             // this is a logic error
             mexErrMsgTxt("unknown error: please contact developer");
+            break;
     }
 }
+
+/* -------- frame grabber class -------- */
+
+void matleap::frame_grabber::open_connection() {
+    mexPrintf("Waiting for connection");
+    while (!controllerConnection.isConnected()) {
+        mexPrintf(".");
+        usleep(500000);
+    }
+    //controllerConnection.enableGesture(Leap::Gesture::TYPE_SWIPE);
+    controllerConnection.enableGesture(Leap::Gesture::TYPE_CIRCLE);
+    //controllerConnection.enableGesture(Leap::Gesture::TYPE_KEY_TAP);
+    //controllerConnection.enableGesture(Leap::Gesture::TYPE_SCREEN_TAP);
+    mexPrintf(" Connected!\n");
+    // TODO: check if needed
+    //LeapSetPolicyFlags(*controllerConnection, eLeapPolicyFlag_BackgroundFrames, 0);
+}
+
+void matleap::frame_grabber::close_connection() {
+    mexPrintf("Good bye.");
+}
+
+matleap::frame_grabber::~frame_grabber() {
+    if (debug) mexPrintf("Closing matleap frame grabber\n");
+}
+
+void matleap::frame_grabber::set_debug(bool flag) {
+    if (flag == debug) return;
+    if (flag) mexPrintf("Toggle debug to %i\n", flag);
+    debug = flag;
+}
+
+matleap::frame const &matleap::frame_grabber::get_frame() {
+    auto const &frame = controllerConnection.frame();
+    current_frame.id = frame.id();
+    if (debug) mexPrintf("Got frame with id %d\n", current_frame.id);
+    current_frame.timestamp = frame.timestamp();
+    current_frame.detectedHands = frame.hands().count();
+    current_frame.hands = frame.hands();
+    current_frame.has_gesture = !frame.gestures().isEmpty();
+    return current_frame;
+}
+
+matleap::frame_grabber::frame_grabber()
+        : debug(false) {
+}