movesense_exercise/MovesenseExerciseService.cpp

#include "MovesenseExerciseService.h"

#include "app-resources/resources.h"
#include "common/core/debug.h"
#include "meas_acc/resources.h"
#include "whiteboard/builtinTypes/UnknownStructure.h"
#include "ui_ind/resources.h"

#include <float.h>
#include <math.h>

const char* const MovesenseExerciseService::LAUNCHABLE_NAME = "MExercise";
#define SAMPLE_RATE 13
const size_t BLINK_PERIOD_MS = 1000;

static const whiteboard::ExecutionContextId sExecutionContextId =
    WB_RES::LOCAL::EXERCISE_SUMVECTOR_DENES::EXECUTION_CONTEXT;

static const whiteboard::LocalResourceId sProviderResources[] = {
    WB_RES::LOCAL::EXERCISE_SUMVECTOR_DENES::LID,
};

namespace {
    whiteboard::FloatVector3D SumVectorAverage(const std::vector<whiteboard::FloatVector3D>& v)
    {
        whiteboard::FloatVector3D sum;
        for (size_t i = 0; i < v.size(); i++)
        {
            whiteboard::FloatVector3D fv = v[i];
            sum += fv;
        }
        return sum / (float)v.size();
    }
}

MovesenseExerciseService::MovesenseExerciseService()
    : ResourceClient(WBDEBUG_NAME(__FUNCTION__), sExecutionContextId),
      ResourceProvider(WBDEBUG_NAME(__FUNCTION__), sExecutionContextId),
      LaunchableModule(LAUNCHABLE_NAME, sExecutionContextId),
      isRunning(false),
      mLastTimeStamp(0),
      mTimer(whiteboard::ID_INVALID_TIMER),
      mMaxAccelerationData(SAMPLE_RATE)
{
}

MovesenseExerciseService::~MovesenseExerciseService()
{
}

bool MovesenseExerciseService::initModule()
{
    if (registerProviderResources(sProviderResources) != whiteboard::HTTP_CODE_OK)
    {
        return false;
    }

    mModuleState = WB_RES::ModuleStateValues::INITIALIZED;
    return true;
}

void MovesenseExerciseService::deinitModule()
{
    unregisterProviderResources(sProviderResources);
    mModuleState = WB_RES::ModuleStateValues::UNINITIALIZED;
}

/** @see whiteboard::ILaunchableModule::startModule */
bool MovesenseExerciseService::startModule()
{
    mModuleState = WB_RES::ModuleStateValues::STARTED;

    // Start LED timer. true = trigger repeatedly
    mTimer = ResourceClient::startTimer(BLINK_PERIOD_MS, true);

    return true;
}

void MovesenseExerciseService::stopModule()
{
    mModuleState = WB_RES::ModuleStateValues::STOPPED;

    // Stop LED timer
    ResourceClient::stopTimer(mTimer);
    mTimer = whiteboard::ID_INVALID_TIMER;
}

void MovesenseExerciseService::onUnsubscribeResult(whiteboard::RequestId requestId,
                                                     whiteboard::ResourceId resourceId,
                                                     whiteboard::Result resultCode,
                                                     const whiteboard::Value& rResultData)
{
    DEBUGLOG("MovesenseExerciseService::onUnsubscribeResult() called.");
}

void MovesenseExerciseService::onSubscribeResult(whiteboard::RequestId requestId,
                                                   whiteboard::ResourceId resourceId,
                                                   whiteboard::Result resultCode,
                                                   const whiteboard::Value& rResultData)
{
    DEBUGLOG("MovesenseExerciseService::onSubscribeResult() called. resourceId: %u, result: %d", resourceId.localResourceId, (uint32_t)resultCode);

    whiteboard::Request relatedIncomingRequest;
    bool relatedRequestFound = mOngoingRequests.get(requestId, relatedIncomingRequest);

    if (relatedRequestFound)
    {
        returnResult(relatedIncomingRequest, wb::HTTP_CODE_OK);
    }
}

whiteboard::Result MovesenseExerciseService::startRunning(whiteboard::RequestId& remoteRequestId)
{
    if (isRunning)
    {
        return whiteboard::HTTP_CODE_OK;
    }

    DEBUGLOG("MovesenseExerciseService::startRunning()");

    wb::Result result = getResource("Meas/Acc/13", mMeasAccResourceId);
    if (!wb::RETURN_OKC(result))
    {
        return result;
    }

    result = asyncSubscribe(mMeasAccResourceId, AsyncRequestOptions(&remoteRequestId, 0, true));

    if (!wb::RETURN_OKC(result))
    {
        DEBUGLOG("asyncSubscribe threw error: %u", result);
        return whiteboard::HTTP_CODE_BAD_REQUEST;
    }
    isRunning = true;

    return whiteboard::HTTP_CODE_OK;
}

whiteboard::Result MovesenseExerciseService::stopRunning()
{
    if (!isRunning)
    {
        return whiteboard::HTTP_CODE_OK;
    }

    if (isResourceSubscribed(WB_RES::LOCAL::EXERCISE_SUMVECTOR_DENES::ID) == wb::HTTP_CODE_OK)
    {
        DEBUGLOG("MovesenseExerciseService::stopRunning() skipping. Subscribers still exist.");
        return whiteboard::HTTP_CODE_OK;
    }

    DEBUGLOG("MovesenseExerciseService::stopRunning()");

    // Unsubscribe the LinearAcceleration resource, when unsubscribe is done, we get callback
    wb::Result result = asyncUnsubscribe(mMeasAccResourceId, NULL);
    if (!wb::RETURN_OKC(result))
    {
        DEBUGLOG("asyncUnsubscribe threw error: %u", result);
    }
    isRunning = false;
    releaseResource(mMeasAccResourceId);

    return whiteboard::HTTP_CODE_OK;
}

// This callback is called when the resource we have subscribed notifies us
void MovesenseExerciseService::onNotify(whiteboard::ResourceId resourceId, const whiteboard::Value& value,
                                          const whiteboard::ParameterList& parameters)
{
    DEBUGLOG("onNotify() called.");

    // Confirm that it is the correct resource
    switch (resourceId.localResourceId)
    {
    case WB_RES::LOCAL::MEAS_ACC_SAMPLERATE::LID:
    {
        const WB_RES::AccData& linearAccelerationValue =
            value.convertTo<const WB_RES::AccData&>();

        if (linearAccelerationValue.arrayAcc.size() <= 0)
        {
            // No value, do nothing...
            return;
        }

        const whiteboard::Array<whiteboard::FloatVector3D>& arrayData = linearAccelerationValue.arrayAcc;

        mLastTimeStamp = linearAccelerationValue.timestamp;

        for (size_t i = 0; i < arrayData.size(); i++)
        {
            mMaxAccelerationData.push_back(arrayData[i]);
        }
    }
    break;
    }
}

void MovesenseExerciseService::onSubscribe(const whiteboard::Request& request,
                                             const whiteboard::ParameterList& parameters)
{
    switch (request.getResourceConstId())
    {
    case WB_RES::LOCAL::EXERCISE_SUMVECTOR_DENES::ID:
    {
        // Someone subscribed to our service. Start collecting data and notifying when our service changes state (every 10 seconds)
        whiteboard::RequestId remoteRequestId;
        whiteboard::Result result = startRunning(remoteRequestId);

        if (isRunning)
        {
            return returnResult(request, whiteboard::HTTP_CODE_OK);
        }

        if (!whiteboard::RETURN_OK(result))
        {
            return returnResult(request, result);
        }
        bool queueResult = mOngoingRequests.put(remoteRequestId, request);
        (void)queueResult;
        WB_ASSERT(queueResult);
        break;
    }
    default:
        ASSERT(0); // Should not happen
    }
}

void MovesenseExerciseService::onUnsubscribe(const whiteboard::Request& request,
                                               const whiteboard::ParameterList& parameters)
{
    switch (request.getResourceConstId())
    {
    case WB_RES::LOCAL::EXERCISE_SUMVECTOR_DENES::ID:
        stopRunning();
        returnResult(request, wb::HTTP_CODE_OK);
        break;

    default:
        returnResult(request, wb::HTTP_CODE_BAD_REQUEST);
        ASSERT(0); // Should not happen
    }
}

void MovesenseExerciseService::onRemoteWhiteboardDisconnected(whiteboard::WhiteboardId whiteboardId)
{
    DEBUGLOG("MovesenseExerciseService::onRemoteWhiteboardDisconnected()");
    stopRunning();
}

void MovesenseExerciseService::onClientUnavailable(whiteboard::ClientId clientId)
{
    DEBUGLOG("MovesenseExerciseService::onClientUnavailable()");
    stopRunning();
}

void MovesenseExerciseService::onTimer(whiteboard::TimerId timerId)
{
    if (timerId != mTimer)
    {
        return;
    }

    uint16_t indicationType = 2; // SHORT_VISUAL_INDICATION, defined in ui/ind.yaml

    // Make PUT request to trigger led blink
    asyncPut(WB_RES::LOCAL::UI_IND_VISUAL::ID, AsyncRequestOptions::Empty, indicationType);

    // Notify our subscribers
    /// @todo The timestamp is the last measurement's timestamp and not the current time.
    WB_RES::SampleDataValue sampleDataValue;
    sampleDataValue.relativeTime = mLastTimeStamp;
    sampleDataValue.value = SumVectorAverage(mMaxAccelerationData);

    // Reset members
    mLastTimeStamp = 0;
    mMaxAccelerationData.clear();

    // and update our WB resource. This causes notification to be fired to our subscribers
    updateResource(WB_RES::LOCAL::EXERCISE_SUMVECTOR_DENES(),
                   ResponseOptions::Empty, sampleDataValue);
}