
/***** Includes *****/
#include <xdc/std.h>
#include <xdc/runtime/System.h>
#include <string.h>
#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Task.h>
#include <ti/sysbios/knl/Semaphore.h>
#include <ti/sysbios/knl/Event.h>
#include <ti/sysbios/knl/Clock.h>

#include <ti/drivers/Watchdog.h>
#include <ti/drivers/PIN.h>
#include <ti/drivers/pin/PINCC26XX.h>

#include "Board.h"
#include "RadioProtocol.h"
#include "MOB.h"

#include <DmConcentratorRadioTask.h>
#include <DmConcentratorTask.h>


/***** Defines *****/
#define CONCENTRATOR_TASK_STACK_SIZE 1024
#define CONCENTRATOR_TASK_PRIORITY   3

#define CONCENTRATOR_EVENT_ALL              0xFFFFFFFF
#define CONCENTRATOR_EVENT_MOB_TAG_CONTACT  (uint32_t)(1 << 0)
#define CONCENTRATOR_EVENT_BROADCAST  (uint32_t)(1 << 1)
#define CONCENTRATOR_EVENT_BROADCAST_TIMEOUT (uint32_t)(1 << 2)

#define CONCENTRATOR_MAX_NODES 16

#define MOB_NODE_TIMEOUT_1S      20
#define BROADCAST_TIMEOUT_1S     3
#define LED_BLINK_TIME_100MS     3

#define ALLS_WELL_TIMEOUT_60S               15 //value given in minutes
#define ALLS_WELL_ACTIVATION_TIMEOUT_1S     5 //value given in seconds

#define CONCENTRATOR_GREEN_ACTIVITY_LED Board_PIN_GLED
#define CONCENTRATOR_RED_ACTIVITY_LED Board_PIN_RLED
#define CONCENTRATOR_BUZZER 5

/***** Type declarations *****/
struct MOBtagNode {
    uint8_t address;
    int8_t  latestRssi;
    uint8_t state;
};

struct MOBtagNodeMonitor {
    struct MOBtagNode   node;
    uint8_t             active;
    uint8_t             lost;
    uint8_t             deactivateAlarming;
    uint32_t            timeout;
};

/*
 * Application button pin configuration table:
 *   - Buttons interrupts are configured to trigger on falling edge.
 */
PIN_Config buttonPinTable[] = {
    Board_PIN_BUTTON0  | PIN_INPUT_EN | PIN_PULLUP | PIN_IRQ_NEGEDGE,
    Board_PIN_BUTTON1  | PIN_INPUT_EN | PIN_PULLUP | PIN_IRQ_NEGEDGE,
    Board_DIO12  | PIN_INPUT_EN | PIN_PULLUP,
    PIN_TERMINATE
};

/***** Variable declarations *****/
static Task_Params concentratorTaskParams;
Task_Struct concentratorTask;    /* not static so you can see in ROV */
static uint8_t concentratorTaskStack[CONCENTRATOR_TASK_STACK_SIZE];
Event_Struct concentratorEvent;  /* not static so you can see in ROV */
static Event_Handle concentratorEventHandle;
static struct MOBtagNode latestActiveMOBtagNode;
struct MOBtagNodeMonitor knownMOBtagNodes[CONCENTRATOR_MAX_NODES];
static uint8_t selectedNode = 0;
static PIN_Handle buttonPinHandle;
static PIN_State buttonPinState;

/* Clock for the report timeout */
Clock_Struct mobTimerClock;     /* not static so you can see in ROV */
static Clock_Handle mobTimerClockHandle;

/***** Prototypes *****/
static void concentratorTaskFunction(UArg arg0, UArg arg1);
static void packetReceivedCallback(struct MOBtagPacket* packet, int8_t rssi);
static void broadcastCallback();
static void addNewNode(struct MOBtagNode* node);
static void updateNode(struct MOBtagNode* node);
static uint8_t isKnownNodeAddress(uint8_t address);
void buttonCallback(PIN_Handle handle, PIN_Id pinId);
void mobTimerCallback(UArg arg0);

/* Pin driver handle */
static PIN_Handle gpioPinHandle;
static PIN_State gpioPinState;

/* Configure LED Pin */
PIN_Config gpioPinTable[] = {
        CONCENTRATOR_GREEN_ACTIVITY_LED | PIN_GPIO_OUTPUT_EN | PIN_GPIO_LOW | PIN_PUSHPULL | PIN_DRVSTR_MAX,
        CONCENTRATOR_RED_ACTIVITY_LED | PIN_GPIO_OUTPUT_EN | PIN_GPIO_LOW | PIN_PUSHPULL | PIN_DRVSTR_MAX,
        CONCENTRATOR_BUZZER | PIN_GPIO_OUTPUT_EN | PIN_GPIO_LOW | PIN_PUSHPULL | PIN_DRVSTR_MAX,
#ifdef __CC1350_LAUNCHXL_BOARD_H__
        Board_DIO1_RFSW | PIN_GPIO_OUTPUT_EN | PIN_GPIO_LOW | PIN_PUSHPULL | PIN_DRVSTR_MAX,
        Board_DIO30_SWPWR | PIN_GPIO_OUTPUT_EN | PIN_GPIO_LOW | PIN_PUSHPULL | PIN_DRVSTR_MAX,
#endif //__CC1350_LAUNCHXL_BOARD_H__
    PIN_TERMINATE
};

unsigned tagContactLED=0;
unsigned broadcastLED=0;

unsigned allsWellLED=0;
int      allsWellActive=0;
int      allsWellActivationTimeout=ALLS_WELL_ACTIVATION_TIMEOUT_1S*10;
unsigned long allsWellTimeout=ALLS_WELL_TIMEOUT_60S*600;

uint8_t modeUSEU=0; //UE or US frequency operation

Watchdog_Handle watchdogHandle;
Watchdog_Params params;
uint32_t        reloadValue;
uint32_t        concentratorTaskCnt;
#define TIMEOUT_MS      15000 //15s
/*
 *  ======== watchdogCallback ========
 */
void watchdogCallback(uintptr_t watchdogHandle)
{
    /*
     * If the Watchdog Non-Maskable Interrupt (NMI) is called,
     * loop until the device resets. Some devices will invoke
     * this callback upon watchdog expiration while others will
     * reset. See the device specific watchdog driver documentation
     * for your device.
     */
    PIN_setOutputValue(gpioPinHandle, CONCENTRATOR_RED_ACTIVITY_LED,1);
    while (1) {}
}


/***** Function definitions *****/
void ConcentratorTask_init(void) {
    int i;

    /* Open LED pins */
    gpioPinHandle = PIN_open(&gpioPinState, gpioPinTable);
    if (!gpioPinHandle)
    {
        System_abort("Error initializing board 3.3V domain pins\n");
    }
    PIN_setOutputValue(gpioPinHandle, CONCENTRATOR_GREEN_ACTIVITY_LED,0);
    PIN_setOutputValue(gpioPinHandle, CONCENTRATOR_RED_ACTIVITY_LED,0);
    PIN_setOutputValue(gpioPinHandle, CONCENTRATOR_BUZZER,0);

#ifdef __CC1350_LAUNCHXL_BOARD_H__
    /* Enable power to RF switch to 2.4G antenna */
    PIN_setOutputValue(gpioPinHandle, Board_DIO30_SWPWR, 1);
#endif //__CC1350_LAUNCHXL_BOARD_H__

    //Initialize variables
    for(i=0; i<CONCENTRATOR_MAX_NODES; i++) {
        knownMOBtagNodes[i].node.address=0;
        knownMOBtagNodes[i].node.latestRssi=0;
        knownMOBtagNodes[i].node.state=0;
        knownMOBtagNodes[i].timeout=0;
        knownMOBtagNodes[i].active=0;
        knownMOBtagNodes[i].lost=0;
        knownMOBtagNodes[i].deactivateAlarming=0;
    }

    Clock_Params clkParams;
    clkParams.period = 100000 / Clock_tickPeriod; //100ms
    clkParams.startFlag = FALSE;
    Clock_construct(&mobTimerClock, mobTimerCallback, 1, &clkParams);
    mobTimerClockHandle = Clock_handle(&mobTimerClock);

    /* Create event used internally for state changes */
    Event_Params eventParam;
    Event_Params_init(&eventParam);
    Event_construct(&concentratorEvent, &eventParam);
    concentratorEventHandle = Event_handle(&concentratorEvent);

    /* Create the concentrator radio protocol task */
    Task_Params_init(&concentratorTaskParams);
    concentratorTaskParams.stackSize = CONCENTRATOR_TASK_STACK_SIZE;
    concentratorTaskParams.priority = CONCENTRATOR_TASK_PRIORITY;
    concentratorTaskParams.stack = &concentratorTaskStack;
    Task_construct(&concentratorTask, concentratorTaskFunction, &concentratorTaskParams, NULL);

    concentratorTaskCnt=0;
    Watchdog_init();

    /* Open a Watchdog driver instance */
    Watchdog_Params_init(&params);
    params.callbackFxn = (Watchdog_Callback) watchdogCallback;
    params.debugStallMode = Watchdog_DEBUG_STALL_ON;
    params.resetMode = Watchdog_RESET_ON;

    watchdogHandle = Watchdog_open(Board_WATCHDOG0, &params);
    if (watchdogHandle == NULL) {
        /* Error opening Watchdog */
        while (1) {}
    }

    /*
     * The watchdog reload value is initialized during the
     * Watchdog_open() call. The reload value can also be
     * set dynamically during runtime.
     *
     * Converts TIMEOUT_MS to watchdog clock ticks.
     * This API is not applicable for all devices.
     * See the device specific watchdog driver documentation
     * for your device.
     */
    reloadValue = Watchdog_convertMsToTicks(watchdogHandle, TIMEOUT_MS);

    /*
     * A value of zero (0) indicates the converted value exceeds 32 bits
     * OR that the API is not applicable for this specific device.
     */
    if (reloadValue != 0) {
        Watchdog_setReload(watchdogHandle, reloadValue);
    }

    /* start MOB timer */
    Clock_setPeriod(mobTimerClockHandle, (100000 / Clock_tickPeriod)); //100ms
    Clock_start(mobTimerClockHandle);
}


static void concentratorTaskFunction(UArg arg0, UArg arg1)
{

    buttonPinHandle = PIN_open(&buttonPinState, buttonPinTable);
    if(!buttonPinHandle)
    {
        System_abort("Error initializing button pins\n");
    }

    /* Setup callback for button pins */
    if (PIN_registerIntCb(buttonPinHandle, &buttonCallback) != 0)
    {
        System_abort("Error registering button callback function");
    }

    /* Register a packet received callback with the radio task */
    ConcentratorRadioTask_registerPacketReceivedCallback(packetReceivedCallback);

    ConcentratorRadioTask_registerBroadcastCallback(broadcastCallback);

    //set selected node to 0
    selectedNode = 0;

    //Select US/EU frequency
    if (PIN_getInputValue(Board_DIO12) == 0) {
        modeUSEU=1;
    } else {
        modeUSEU=2;
    }

    /* Enter main task loop */
    while (1)
    {
        /* Wait for event */
        uint32_t events = Event_pend(concentratorEventHandle, 0, CONCENTRATOR_EVENT_ALL, BIOS_WAIT_FOREVER);

        if (events & CONCENTRATOR_EVENT_MOB_TAG_CONTACT)
        {
            tagContactLED=LED_BLINK_TIME_100MS;
        }

        if (events & CONCENTRATOR_EVENT_BROADCAST_TIMEOUT) {
            RadioTask_Broadcast();
        }

        if(events & CONCENTRATOR_EVENT_BROADCAST) {
            switch(modeUSEU){
            case 1:
                broadcastLED=LED_BLINK_TIME_100MS*3;
                break;
            case 2:
                broadcastLED=LED_BLINK_TIME_100MS;
                break;
            }
            Clock_stop(mobTimerClockHandle);
            concentratorTaskCnt++;
            Clock_start(mobTimerClockHandle);
        }
    }
}

void mobTimerCallback(UArg arg0)
{
    static int broadcastInterval=0;
    int i=0;

    if(broadcastInterval==0) {
        Event_post(concentratorEventHandle, CONCENTRATOR_EVENT_BROADCAST_TIMEOUT);
        broadcastInterval = BROADCAST_TIMEOUT_1S*10;
    } else {
        broadcastInterval--;
    }

    if(tagContactLED!=0) {
        tagContactLED--;
        PIN_setOutputValue(gpioPinHandle, CONCENTRATOR_GREEN_ACTIVITY_LED,1);
    } else {
        PIN_setOutputValue(gpioPinHandle, CONCENTRATOR_GREEN_ACTIVITY_LED,0);

    }

    if(getMOBstatus()!=0) {
        PIN_setOutputValue(gpioPinHandle, CONCENTRATOR_RED_ACTIVITY_LED,1);
        PIN_setOutputValue(gpioPinHandle, CONCENTRATOR_BUZZER,1);
        broadcastLED=0;
    } else {
        PIN_setOutputValue(gpioPinHandle, CONCENTRATOR_BUZZER,0);
        if(broadcastLED!=0) {
            if(broadcastLED<LED_BLINK_TIME_100MS || broadcastLED>2*LED_BLINK_TIME_100MS) {
                PIN_setOutputValue(gpioPinHandle, CONCENTRATOR_RED_ACTIVITY_LED,1);
            } else {
                PIN_setOutputValue(gpioPinHandle, CONCENTRATOR_RED_ACTIVITY_LED,0);
            }
            broadcastLED--;
            if(broadcastLED==0) {
                //Add longer blick if the All's Well function is active
                if(allsWellActive!=0){
                    allsWellLED=6*LED_BLINK_TIME_100MS;
                }
            }
        } else {
            if(allsWellLED>0) {
                if(allsWellLED<3*LED_BLINK_TIME_100MS) {
                    PIN_setOutputValue(gpioPinHandle, CONCENTRATOR_RED_ACTIVITY_LED,1);
                } else {
                    PIN_setOutputValue(gpioPinHandle, CONCENTRATOR_RED_ACTIVITY_LED,0);
                }
                allsWellLED--;
            } else {
                PIN_setOutputValue(gpioPinHandle, CONCENTRATOR_RED_ACTIVITY_LED,0);
            }
        }
    }

    //Count the all's well timeout
    if(allsWellTimeout>0)
        allsWellTimeout--;

    //Toggle All's Well active/inactive when the button is active for the given time:
    if(PIN_getInputValue(Board_PIN_BUTTON0) == 0) {
        if(allsWellActivationTimeout>0) {
            allsWellActivationTimeout--;
            if(allsWellActivationTimeout==0) {
                if(allsWellActive==0){
                    allsWellActive=1;
                } else {
                    allsWellActive=0;
                }
            }
        }
    } else {
        allsWellActivationTimeout=ALLS_WELL_ACTIVATION_TIMEOUT_1S*10;
    }

    //Search for the lost tags:
    for (i = 0; i < CONCENTRATOR_MAX_NODES; i++) {
        if (knownMOBtagNodes[i].active!=0)
        {
            //Switch off tags going to shutdown:
            if((knownMOBtagNodes[i].node.state&NODE_STATUS_SHUTDOWN)!=0) {
                knownMOBtagNodes[i].active=0;
                knownMOBtagNodes[i].lost=0;
            }

            if(knownMOBtagNodes[i].timeout==0) {
                knownMOBtagNodes[i].lost=1;
            } else {
                knownMOBtagNodes[i].timeout--;
            }
        }
    }

    static int wdCheckPeriod=0;
    if(wdCheckPeriod++ > 100){ //10s
        if (concentratorTaskCnt>=3) {
            wdCheckPeriod=0;
            concentratorTaskCnt=0;
            Watchdog_clear(watchdogHandle);
        }
    }
}

static void packetReceivedCallback(struct MOBtagPacket* packet, int8_t rssi)
{
    /* If we recived a tag packet, for backward compatibility */
    if (packet->header.packetType == RADIO_PACKET_TYPE_MOB_TAG_PACKET)
    {
        /* Save the values */
        latestActiveMOBtagNode.address = packet->header.sourceAddress;
        latestActiveMOBtagNode.state = packet->state;
        latestActiveMOBtagNode.latestRssi = rssi;

        /* If we knew this node from before, update the value */
        if (isKnownNodeAddress(latestActiveMOBtagNode.address))
        {
            updateNode(&latestActiveMOBtagNode);
        }
        else
        {
            /* Else add it */
            addNewNode(&latestActiveMOBtagNode);
        }

        Event_post(concentratorEventHandle, CONCENTRATOR_EVENT_MOB_TAG_CONTACT);
    }
}

static void broadcastCallback(void) {
    Event_post(concentratorEventHandle, CONCENTRATOR_EVENT_BROADCAST);
}

static uint8_t isKnownNodeAddress(uint8_t address) {
    uint8_t found = 0;
    uint8_t i;
    for (i = 0; i < CONCENTRATOR_MAX_NODES; i++)
    {
        if (knownMOBtagNodes[i].active!=0 && knownMOBtagNodes[i].node.address == address)
        {
            found = 1;
            break;
        }
    }
    return found;
}

static void updateNode(struct MOBtagNode* node) {
    uint8_t i;
    for (i = 0; i < CONCENTRATOR_MAX_NODES; i++) {
        if (knownMOBtagNodes[i].active!=0 && knownMOBtagNodes[i].node.address == node->address)
        {
            knownMOBtagNodes[i].node.latestRssi = node->latestRssi;
            knownMOBtagNodes[i].node.state = node->state;
            knownMOBtagNodes[i].timeout=MOB_NODE_TIMEOUT_1S*10;
            knownMOBtagNodes[i].lost=0;
            knownMOBtagNodes[i].active=1;
            break;
        }
    }
}

static void addNewNode(struct MOBtagNode* node) {
    int i;

    //Don't add nodes going to shutdown:
    if(((node->state)&NODE_STATUS_SHUTDOWN)!=0)
        return;

    for (i = 0; i < CONCENTRATOR_MAX_NODES; i++) {
        if (knownMOBtagNodes[i].active==0)
        {
            knownMOBtagNodes[i].node.address = node->address;
            knownMOBtagNodes[i].node.latestRssi = node->latestRssi;
            knownMOBtagNodes[i].node.state = node->state;
            knownMOBtagNodes[i].timeout=MOB_NODE_TIMEOUT_1S*10;
            knownMOBtagNodes[i].lost=0;
            knownMOBtagNodes[i].deactivateAlarming=0;
            knownMOBtagNodes[i].active=1;
            break;
        }
    }
}

uint8_t findFreeAddr(void) {
    int i;
    uint8_t addr, addrUsed;

    addr=1;
    do
    {
        addrUsed=0;
        for (i = 0; i < CONCENTRATOR_MAX_NODES; i++) {
            if (knownMOBtagNodes[i].active!=0 && knownMOBtagNodes[i].node.address==addr)
            {
                addrUsed=1;
                addr++;
                break;
            }
        }
    }
    while (addrUsed!=0);

    if(addr>CONCENTRATOR_MAX_NODES)
        addr=0xFF; //no free address found. Tag must wait to be monitored (when other tag will be removed).

    return addr;
}

uint8_t getMOBstatus(void) {
    int i;
    uint8_t status=0;

    for (i = 0; i < CONCENTRATOR_MAX_NODES; i++) {
        if (knownMOBtagNodes[i].active!=0 &&
           (knownMOBtagNodes[i].lost!=0 || (knownMOBtagNodes[i].node.state & NODE_STATUS_NODE_ALARM)!=0) )
        {
            if(knownMOBtagNodes[i].deactivateAlarming!=0 && (knownMOBtagNodes[i].node.state & NODE_STATUS_NODE_ALARM)!=0) {
                //Just remove lost tag whos alarming was already deactivated:
                if(knownMOBtagNodes[i].lost!=0) {
                    knownMOBtagNodes[i].lost=0;
                    knownMOBtagNodes[i].active=0;
                }
                break; //alarming for this tag was deactivated
            }
            status=1;
            break;
        }

        //when tag alarm was cleared, also clear alarm deactivation:
        if (knownMOBtagNodes[i].active!=0 && knownMOBtagNodes[i].lost!=0 &&
           (knownMOBtagNodes[i].node.state & NODE_STATUS_NODE_ALARM)==0)
        {
            knownMOBtagNodes[i].deactivateAlarming=0;
        }
    }

    if(allsWellActive!=0) {
        if(allsWellTimeout==0)
            status=1;
    } else {
        allsWellTimeout=ALLS_WELL_TIMEOUT_60S*600;
    }

    return status;
}


void buttonCallback(PIN_Handle handle, PIN_Id pinId)
{
    int i;

    /* Debounce logic, only toggle if the button is still pushed (low) */
    CPUdelay(8000*50);

    //Reset active lost MOB tags alarms:
    if (PIN_getInputValue(Board_PIN_BUTTON0) == 0)
    {
        for (i = 0; i < CONCENTRATOR_MAX_NODES; i++) {
            if (knownMOBtagNodes[i].active==1 && knownMOBtagNodes[i].lost==1)
            {
                knownMOBtagNodes[i].lost=0;
                knownMOBtagNodes[i].active=0;
            }
            if (knownMOBtagNodes[i].active==1 && ((knownMOBtagNodes[i].node.state)&NODE_STATUS_NODE_ALARM)!=0) {
                knownMOBtagNodes[i].deactivateAlarming=1;
            }
        }
        allsWellTimeout=ALLS_WELL_TIMEOUT_60S*600;
    }
}
