#include "fsm.h"
#include "api.h"
#include "time.h"

//=========================== typedef =========================================

typedef struct
{
    fsm_timer_cbt event;
    uint16_t      delay;
} fsm_event_t;

typedef struct
{
    // FSM
    uint8_t       state;

    // event queue for API c-library
    fsm_event_t   eventQueue[FSM_QUEUE_LEN];
    uint8_t       queueWrIdx;
    uint8_t       queueRdIdx;

} fsm_vars_t;

//=========================== variables =====================================

fsm_vars_t fsm_vars;

//=========================== prototypes ======================================

void fsm_event(void); // this function is called from RTC interrupt

//=========================== public ========================================

//===== enterState

/**
 Transition FSM to new state and schedule any default entry events.
 */
void fsm_enterState(uint16_t specificDelay, uint8_t newState)
{
    uint16_t delay = CMD_PERIOD_MS;

    // Use default delay if none given
    if (specificDelay > 0)
        delay = specificDelay;

    // Schedule default events for transition into states
    switch (newState)
    {
    case FSM_STATE_START:
        DEBUG_MSG("FSM START\n");

        // reset local variables
        memset(&fsm_vars, 0, sizeof(fsm_vars));

        // mote port and measurements are initialized here
        api_init();

        // load MAC address from the mote's flash an init wired interface
        fsm_scheduleEvent(delay, api_getMacAddress);
        break;

    case FSM_STATE_PRE_JOIN:
        DEBUG_MSG("FSM PREJOIN\n");
        // PRE_JOIN and JOINING have the same enterig point, but PRE_JOIN doen't send join command
        fsm_scheduleEvent(delay, api_setHartDeviceInfo);
        break;

    case FSM_STATE_JOINING:
        DEBUG_MSG("FSM JOINING\n");
        // PRE_JOIN and JOINING have the same enterig point, but PRE_JOIN doen't send join command
        fsm_scheduleEvent(delay, api_setHartDeviceInfo);
        break;
    /*
        case FSM_STATE_REQ_SERVICE:
            DEBUG_MSG("FSM REQ SERVICE\n");
            fsm_scheduleEvent(delay, api_setService);
            break;
    */
    case FSM_STATE_RESETTING:
        DEBUG_MSG("FSM RESETTING\n");
        if (MOTE_DISCONNECT_BEFORE_RESET)
            fsm_scheduleEvent(delay, api_disconnect); // More graceful
        else
            fsm_scheduleEvent(delay, api_reset); // Faster
        break;

    case FSM_STATE_PUBLISHING:
        DEBUG_MSG("FSM PUBLISHING\n");
        
		//fsm_scheduleEvent(delay, api_send);
        break;

    case FSM_STATE_DISCONNECTED: // this happens when manager issues disconnect cmd
        DEBUG_MSG("FSM DISCONNECTED\n");
        break;

    case FSM_STATE_SUSPENDED:
        DEBUG_MSG("FSM SUSPENDED\n");

        // pause all bursts
        time_configurePublishEvent(0, NULL);

        break;

    case FSM_STATE_SEND_FAILED:
        DEBUG_MSG("FSM SEND FAILED\n");
        break;

    case FSM_STATE_CONNECTED:
        // Load service table from mote
        DEBUG_MSG("FSM CONNECTED\n");
        fsm_scheduleEvent(delay, api_getService);
        break;
    default:
        //increment error counter
        FSM_INVALID_STATE_ERR();
        ERR_MSG("FSM ERR\n");
        fsm_enterState(0, FSM_STATE_RESETTING);
        return;
    }

    // store new state
    fsm_vars.state = newState;
}
//===== scheduleEvent

/**
 Schedule function to be called after a given delay.
 */
int16_t fsm_scheduleEvent(uint16_t delay_ms, fsm_timer_cbt cb)
{
    fsm_event_t *thisEvent = &fsm_vars.eventQueue[fsm_vars.queueWrIdx++];
    bool         runTimer  = false;

    fsm_vars.queueWrIdx   %= FSM_QUEUE_LEN; // index rollover

    // check if there is event waiting for execution
    runTimer = (fsm_vars.eventQueue[fsm_vars.queueRdIdx].event == NULL);

    // remember what function to call
    thisEvent->event = cb;
    thisEvent->delay = delay_ms;

    // Run timer, if it is not already running
    // otherwise, it will be run after comletition of ongoing event (in fsm_cancelEvent)
    if (runTimer)
    {
        //fsm_runTimer(thisEvent->delay);
        time_configureFsmEvent(thisEvent->delay, fsm_event);
        return (0); // event scheduled
    }
    else
    {
        DEBUG_MSG("Event put to queue\n");
        FSM_EVENT_PUT_TO_QUEUE();
        return (1); // event put to queue
    }
}


//==== ready

// returns true if fsm is ready to answer HART commands
bool fsm_ready(void)
{
    bool ready = true;

    if((fsm_vars.state == FSM_STATE_RESETTING) || (fsm_vars.state == FSM_STATE_START))
        ready = false;

    return ready;
}

//==== getState

uint8_t fsm_getState(void)
{
    return fsm_vars.state;
}


//=============================================================================
// Execution of event, called from time.c
//=============================================================================

void fsm_event(void)
{
    fsm_event_t *thisEvent, *nextEvent;

    // init event pointer
    thisEvent            = &fsm_vars.eventQueue[fsm_vars.queueRdIdx++];
    fsm_vars.queueRdIdx %= FSM_QUEUE_LEN;                              // index rollover
    nextEvent            = &fsm_vars.eventQueue[fsm_vars.queueRdIdx];

    if (thisEvent->event != NULL)
    {
        // Issue Callback
        thisEvent->event();

        // Callback is empty
        thisEvent->event = NULL;
    }

    // schedule next event in queue, if armed
    if (nextEvent->event != NULL)
    {
        time_configureFsmEvent(nextEvent->delay, fsm_event);
    }
}

