/// @file matleap.cpp
/// @brief leap motion controller interface
/// @author Jeff Perry <jeffsp@gmail.com>
/// @version 1.0
/// @date 2013-09-12

// 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
// startup code has completed.  Also, a Leap::Controller must be
// deleted in the function specified by mexAtExit after all global
// destructors are called.  If the Leap::Controller is not allocated
// and freed in this way, the MEX function will crash and cause MATLAB
// to hang or close abruptly.  Linux and OS/X don't have these
// constraints, and you can just create a global Leap::Controller
// instance.

// Global instance pointer
matleap::frame_grabber *fg = 0;
int version = 4; // 1: orig, 2: with arm info, 3: with more hand info

// Exit function
void matleap_exit() {
    fg->close_connection();
    delete fg;
    fg = nullptr;
}

/// @brief process interface arguments
///
/// @param nlhs matlab mex output interface
/// @param plhs[] matlab mex output interface
/// @param nrhs matlab mex input interface
/// @param prhs[] matlab mex input interface
///
/// @return command number
int get_command(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
    int command;

    // check inputs - we take exactly one parameter
    switch (nrhs) {
        case 1:
            command = *mxGetPr(prhs[0]);
            break;
        case 0:
            mexErrMsgTxt("Not enough input arguments");
        default:
            mexErrMsgTxt("Too many input arguments");
    }

    // check that outputs agree with command
    switch (command) {
        case -1:
            // set debug command requires 0 outputs
            if (nlhs != 0) mexErrMsgTxt("Wrong number of outputs specified");
            break;
        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");
    }
    return command;
}

/// @brief helper function
///
/// @param v values to fill array with
///
/// @return created and filled array
mxArray *create_and_fill(const Leap::Vector &v) {
    mxArray *p = mxCreateNumericMatrix(1, 3, mxDOUBLE_CLASS, mxREAL);
    *(mxGetPr(p) + 0) = v.x;
    *(mxGetPr(p) + 1) = v.y;
    *(mxGetPr(p) + 2) = v.z;
    return p;
}

/// @brief get a frame from the leap controller
///
/// @param nlhs matlab mex output interface
/// @param plhs[] matlab mex output interface
void get_frame(int nlhs, mxArray *plhs[]) {
    // get the frame
    const matleap::frame &f = fg->get_frame();
    // create matlab frame struct
    const char *frame_field_names[] =
            {
                    "id",
                    "timestamp",
                    "gesture",
                    "hands",
                    "version"
            };
    int frame_fields = sizeof(frame_field_names) / sizeof(*frame_field_names);
    plhs[0] = mxCreateStructMatrix(1, 1, frame_fields, frame_field_names);
    // fill the frame struct
    mxSetFieldByNumber(plhs[0], 0, 0, mxCreateDoubleScalar(f.id));
    mxSetFieldByNumber(plhs[0], 0, 1, mxCreateDoubleScalar(f.timestamp));
    mxSetFieldByNumber(plhs[0], 0, 2, mxCreateDoubleScalar(f.has_gesture ? 1 : 0));

    if (f.detectedHands > 0) {
        const char *hand_field_names[] =
                {
                        "id", // 0
                        "confidence", // 2
                        "visible_time",
                        "pinch_strength",
                        "grab_strength",
                        "palm",
                        "digits"
                };

        int hand_fields = sizeof(hand_field_names) / sizeof(*hand_field_names);
        mxArray *p = mxCreateStructMatrix(1, f.detectedHands, hand_fields, hand_field_names);
        mxSetFieldByNumber(plhs[0], 0, 3, p);
        // 3 because hands is the third (fourth) field name in
        // the overall struct we are creating.

        for (int i = 0; i < f.detectedHands; ++i) {
            // one by one, get the fields for the hand
            mxSetFieldByNumber(p, i, 0, mxCreateDoubleScalar(f.hands[i].id()));
            mxSetFieldByNumber(p, i, 1, mxCreateDoubleScalar(f.hands[i].confidence()));
            mxSetFieldByNumber(p, i, 2, mxCreateDoubleScalar(f.hands[i].timeVisible()));
            mxSetFieldByNumber(p, i, 3, mxCreateDoubleScalar(f.hands[i].pinchStrength()));
            mxSetFieldByNumber(p, i, 4, mxCreateDoubleScalar(f.hands[i].grabStrength()));

            // palm
            const char *palm_field_names[] =
                    {
                            "position",
                            "stabilized_position",
                            "velocity",
                            "normal",
                            "width",
                            "direction",
                    };
            int palm_fields = sizeof(palm_field_names) / sizeof(*palm_field_names);
            mxArray *palm = mxCreateStructMatrix(1, 1, palm_fields, palm_field_names);
            mxSetFieldByNumber(p, i, 5, palm);
            mxSetFieldByNumber(palm, 0, 0, create_and_fill(f.hands[i].palmPosition()));
            mxSetFieldByNumber(palm, 0, 1, create_and_fill(f.hands[i].stabilizedPalmPosition()));
            mxSetFieldByNumber(palm, 0, 2, create_and_fill(f.hands[i].palmVelocity()));
            mxSetFieldByNumber(palm, 0, 3, create_and_fill(f.hands[i].palmNormal()));
            mxSetFieldByNumber(palm, 0, 4, mxCreateDoubleScalar(f.hands[i].palmWidth()));
            mxSetFieldByNumber(palm, 0, 5, create_and_fill(f.hands[i].direction()));
            // get bones for all fingers
            const char *digit_field_names[] =
                    {
                            "finger_id", // 0
                            "is_extended",
                            "bones",
                    };
            int digit_fields = sizeof(digit_field_names) / sizeof(*digit_field_names);
            Leap::Finger digits[5];
            std::copy(f.hands[i].fingers().begin(), f.hands[i].fingers().end(), std::begin(digits));
            mxArray *d = mxCreateStructMatrix(1, 5, digit_fields, digit_field_names);
            mxSetFieldByNumber(p, i, 6, d);

            for (int d_it = 0; d_it < 5; d_it++) {
                mxSetFieldByNumber(d, d_it, 0, mxCreateDoubleScalar(digits[d_it].id()));
                mxSetFieldByNumber(d, d_it, 1, mxCreateDoubleScalar(digits[d_it].isExtended()));
                const char *bone_field_names[] =
                        {
                                "prev_joint",    // 0
                                "next_joint",   // 1
                                "width",// 2
                                "rotation"
                        };
                int bone_fields = sizeof(bone_field_names) / sizeof(*bone_field_names);
                mxArray *bones = mxCreateStructMatrix(1, 4, bone_fields, bone_field_names);
                mxSetFieldByNumber(d, d_it, 2, bones);

                auto const boneParts = {Leap::Bone::TYPE_METACARPAL, Leap::Bone::TYPE_PROXIMAL,
                                        Leap::Bone::TYPE_INTERMEDIATE, Leap::Bone::TYPE_DISTAL};

                Leap::Bone bone;
                for (auto bi : boneParts) {
                    bone = digits[d_it].bone(bi);

                    mxSetFieldByNumber(bones, bi, 0, create_and_fill(bone.prevJoint())); // 0
                    mxSetFieldByNumber(bones, bi, 1, create_and_fill(bone.nextJoint())); // 1
                    mxSetFieldByNumber(bones, bi, 2, mxCreateDoubleScalar(bone.width())); // 2
                    mxSetFieldByNumber(bones, bi, 3, create_and_fill(bone.direction()));

                }
            }
        } // re: for f.hands.count()
    } // re: if f.hands.count() > 0

    mxSetFieldByNumber(plhs[0], 0, 5, mxCreateDoubleScalar(version));
}

/// @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 == 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)) {
        case -1: // turn on debug
            fg->set_debug(true);
            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;
            break;
        case 1: // get frame
            get_frame(nlhs, plhs);
            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.\n");
}

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) {
}