/*
 $License:
    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
 $
 */
/*******************************************************************************
 *
 * $Id: mlstates.c 5629 2011-06-11 03:13:08Z mcaramello $
 *
 *******************************************************************************/

/** 
 *  @defgroup MLSTATES
 *  @brief  Basic state machine definition and support for the Motion Library.
 *
 *  @{
 *      @file mlstates.c
 *      @brief The Motion Library state machine definition.
 */

#define ML_C

/* ------------------ */
/* - Include Files. - */
/* ------------------ */

#include <stdio.h>
#include <string.h>

#include "mlstates.h"
#include "mltypes.h"
#include "mlinclude.h"
#include "ml.h"
#include "mlos.h"
#include "mlFIFO.h"
#include <log.h>
#undef MPL_LOG_TAG
#define MPL_LOG_TAG "MPL-mlstates"

#define _stateDebug(x)          //{x}

#define MAX_STATE_CHANGE_PROCESSES (8)
struct state_callback_obj {
    int_fast8_t numStateChangeCallbacks;
    HANDLE mutex;
    state_change_callback_t stateChangeCallbacks[MAX_STATE_CHANGE_PROCESSES];
};

static struct state_callback_obj sStateChangeCallbacks = { 0 };
/* --------------- */
/* -  Functions. - */
/* --------------- */

static inv_error_t inv_init_state_callbacks(void)
{
    memset(&sStateChangeCallbacks, 0, sizeof(sStateChangeCallbacks));
    return inv_create_mutex(&sStateChangeCallbacks.mutex);
}

static inv_error_t MLStateCloseCallbacks(void)
{
    inv_error_t result = INV_SUCCESS;
    result = inv_destroy_mutex(sStateChangeCallbacks.mutex);
    memset(&sStateChangeCallbacks, 0, sizeof(sStateChangeCallbacks));
    return result;
}

/**
 *  @internal
 *  @brief  return a string containing the label assigned to the given state.
 *  @param  state   The state of which the label has to be returned.
 *  @return A string containing the state label.
**/
char *inv_state_name(unsigned char state)
{
    switch (state) {
    case INV_STATE_SERIAL_CLOSED:
        return INV_STATE_NAME(INV_STATE_SERIAL_CLOSED);
        break;
    case INV_STATE_SERIAL_OPENED:
        return INV_STATE_NAME(INV_STATE_SERIAL_OPENED);
        break;
    case INV_STATE_DMP_OPENED:
        return INV_STATE_NAME(INV_STATE_DMP_OPENED);
        break;
    case INV_STATE_DMP_STARTED:
        return INV_STATE_NAME(INV_STATE_DMP_STARTED);
        break;
    default:
        return NULL;
    }
}

/**
 *  @internal
 *  @brief  Perform a transition from the current state to newState.
 *          Check for the correctness of the transition.
 *          Print out an error message if the transition is illegal .
 *          This routine is also called if a certain normally constant parameters
 *          are changed such as the FIFO Rate.
 *  @param  newState    state we are transitioning to.
 *  @return  
**/
inv_error_t inv_state_transition(unsigned char newState)
{
    inv_error_t result = INV_SUCCESS;

    if (newState == INV_STATE_SERIAL_CLOSED) {
        // Always allow transition to closed
    } else if (newState == INV_STATE_SERIAL_OPENED) {
        inv_init_state_callbacks(); // Always allow first transition to start over
    } else if (((newState == INV_STATE_DMP_OPENED) &&
                ((inv_params_obj.state == INV_STATE_SERIAL_OPENED) ||
                 (inv_params_obj.state == INV_STATE_DMP_STARTED)))
               ||
               ((newState == INV_STATE_DMP_STARTED) &&
                (inv_params_obj.state == INV_STATE_DMP_OPENED))) {
        // Valid transitions but no special action required
    } else {
        // All other combinations are illegal
        MPL_LOGE("Error : illegal state transition from %s to %s\n",
                 inv_state_name(inv_params_obj.state),
                 inv_state_name(newState));
        result = INV_ERROR_SM_TRANSITION;
    }

    if (result == INV_SUCCESS) {
        _stateDebug(MPL_LOGV
                    ("ML State transition from %s to %s\n",
                     inv_state_name(inv_params_obj.state),
                     inv_state_name(newState)));
        result = inv_run_state_callbacks(newState);
        if (INV_SUCCESS == result && newState == INV_STATE_SERIAL_CLOSED) {
            MLStateCloseCallbacks();
        }
        inv_params_obj.state = newState;
    }
    return result;
}

/**
 *  @internal
 *  @brief  To be moved in mlstates.c
**/
unsigned char inv_get_state(void)
{
    return (inv_params_obj.state);
}

/**
 * @internal
 * @brief   This registers a function to be called each time the state 
 *          changes. It may also be called when the FIFO Rate is changed.
 *          It will be called at the start of a state change before the
 *          state change has taken place. See Also inv_unregister_state_callback()
 *          The FIFO does not have to be on for this callback.
 * @param func Function to be called when a DMP interrupt occurs.
 * @return INV_SUCCESS or non-zero error code.
 */
inv_error_t inv_register_state_callback(state_change_callback_t callback)
{
    INVENSENSE_FUNC_START;
    int kk;
    inv_error_t result;

    result = inv_lock_mutex(sStateChangeCallbacks.mutex);
    if (INV_SUCCESS != result) {
        return result;
    }
    // Make sure we have not filled up our number of allowable callbacks
    if (sStateChangeCallbacks.numStateChangeCallbacks <
        MAX_STATE_CHANGE_PROCESSES) {
        // Make sure we haven't registered this function already
        for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) {
            if (sStateChangeCallbacks.stateChangeCallbacks[kk] == callback) {
                result = INV_ERROR_INVALID_PARAMETER;
                break;
            }
        }

        if (INV_SUCCESS == result) {
            // Add new callback
            sStateChangeCallbacks.stateChangeCallbacks[sStateChangeCallbacks.
                                                       numStateChangeCallbacks]
                = callback;
            sStateChangeCallbacks.numStateChangeCallbacks++;
        }
    } else {
        result = INV_ERROR_MEMORY_EXAUSTED;
    }

    inv_unlock_mutex(sStateChangeCallbacks.mutex);
    return result;
}

/**
 * @internal
 * @brief   This unregisters a function to be called each time the state 
 *          changes. See Also inv_register_state_callback()
 *          The FIFO does not have to be on for this callback.
 * @return INV_SUCCESS or non-zero error code.
 */
inv_error_t inv_unregister_state_callback(state_change_callback_t callback)
{
    INVENSENSE_FUNC_START;
    int kk, jj;
    inv_error_t result;

    result = inv_lock_mutex(sStateChangeCallbacks.mutex);
    if (INV_SUCCESS != result) {
        return result;
    }
    // Make sure we haven't registered this function already
    result = INV_ERROR_INVALID_PARAMETER;
    for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) {
        if (sStateChangeCallbacks.stateChangeCallbacks[kk] == callback) {
            for (jj = kk + 1;
                 jj < sStateChangeCallbacks.numStateChangeCallbacks; ++jj) {
                sStateChangeCallbacks.stateChangeCallbacks[jj - 1] =
                    sStateChangeCallbacks.stateChangeCallbacks[jj];
            }
            sStateChangeCallbacks.numStateChangeCallbacks--;
            result = INV_SUCCESS;
            break;
        }
    }

    inv_unlock_mutex(sStateChangeCallbacks.mutex);
    return result;
}

inv_error_t inv_run_state_callbacks(unsigned char newState)
{
    int kk;
    inv_error_t result;

    result = inv_lock_mutex(sStateChangeCallbacks.mutex);
    if (INV_SUCCESS != result) {
        MPL_LOGE("MLOsLockMutex returned %d\n", result);
        return result;
    }

    for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) {
        if (sStateChangeCallbacks.stateChangeCallbacks[kk]) {
            result = sStateChangeCallbacks.stateChangeCallbacks[kk] (newState);
            if (INV_SUCCESS != result) {
                break;          // Can't return, must release mutex
            }
        }
    }

    inv_unlock_mutex(sStateChangeCallbacks.mutex);
    return result;
}