
/***** Includes *****/
#include <xdc/std.h>
#include <xdc/runtime/System.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/Power.h>
#include <ti/drivers/power/PowerCC26XX.h>
#include <ti/drivers/pin/PINCC26XX.h>

/* Drivers */
#include <ti/drivers/rf/RF.h>
#include <ti/drivers/PIN.h>

/* Board Header files */
#include "Board.h"

#include "MOB.h"

#include <stdlib.h>
#ifdef DEVICE_FAMILY
    #undef DEVICE_FAMILY_PATH
    #define DEVICE_FAMILY_PATH(x) <ti/devices/DEVICE_FAMILY/x>
    #include DEVICE_FAMILY_PATH(driverlib/trng.h)
    #include DEVICE_FAMILY_PATH(driverlib/aon_batmon.h)
#else
    #error "You must define DEVICE_FAMILY at the project level as one of cc26x0, cc26x0r2, cc13x0, etc."
#endif
#include <DmNodeRadioTask.h>
#include "easylink/EasyLink.h"
#include "RadioProtocol.h"
#include "seb/SEB.h"


/***** Defines *****/
#define NODERADIO_TASK_STACK_SIZE 1024
#define NODERADIO_TASK_PRIORITY   3

#define RADIO_EVENT_ALL                 0xFFFFFFFF
#define RADIO_EVENT_SEND_DATA           (uint32_t)(1 << 0)
#define RADIO_EVENT_DATA_ACK_RECEIVED   (uint32_t)(1 << 1)
#define RADIO_EVENT_ACK_TIMEOUT         (uint32_t)(1 << 2)
#define RADIO_EVENT_SEND_FAIL           (uint32_t)(1 << 3)
#define RADIO_EVENT_BROADCAST_RECEIVED  (uint32_t)(1 << 4)

#define NODERADIO_MAX_RETRIES 3
#define NORERADIO_ACK_TIMEOUT_TIME_MS (1000)

#define NODE_0M_TXPOWER    -10

/***** Type declarations *****/
struct RadioOperation {
    EasyLink_TxPacket easyLinkTxPacket;
    uint8_t retriesDone;
    uint8_t maxNumberOfRetries;
    uint32_t ackTimeoutMs;
    enum NodeRadioOperationStatus result;
};


/***** Variable declarations *****/
static Task_Params nodeRadioTaskParams;
Task_Struct nodeRadioTask;        /* not static so you can see in ROV */
static uint8_t nodeRadioTaskStack[NODERADIO_TASK_STACK_SIZE];
Semaphore_Struct radioAccessSem;  /* not static so you can see in ROV */
static Semaphore_Handle radioAccessSemHandle;
Event_Struct radioOperationEvent; /* not static so you can see in ROV */
static Event_Handle radioOperationEventHandle;
Semaphore_Struct radioResultSem;  /* not static so you can see in ROV */
static Semaphore_Handle radioResultSemHandle;
static struct RadioOperation currentRadioOperation;
extern uint8_t nodeAddress;
static struct MOBtagPacket nodePacket;
static struct AckPacket* ackRxPacket;
static struct BroadcastPacket* broadcastRxPacket;

/* Pin driver handle */
extern PIN_Handle pinHandle;

uint8_t shutdownReported=0;
extern PIN_Config ButtonTableWakeUp[];

/***** Prototypes *****/
static void nodeRadioTaskFunction(UArg arg0, UArg arg1);
static void returnRadioOperationStatus(enum NodeRadioOperationStatus status);
static void sendPacket(struct MOBtagPacket *nodePacket, uint8_t maxNumberOfRetries, uint32_t ackTimeoutMs);
static void resendPacket();
static void rxDoneCallback(EasyLink_RxPacket * rxPacket, EasyLink_Status status);

/***** Function definitions *****/
void NodeRadioTask_init(void) {

    /* Create semaphore used for exclusive radio access */
    Semaphore_Params semParam;
    Semaphore_Params_init(&semParam);
    Semaphore_construct(&radioAccessSem, 1, &semParam);
    radioAccessSemHandle = Semaphore_handle(&radioAccessSem);

    /* Create semaphore used for callers to wait for result */
    Semaphore_construct(&radioResultSem, 0, &semParam);
    radioResultSemHandle = Semaphore_handle(&radioResultSem);

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

    /* Create the radio protocol task */
    Task_Params_init(&nodeRadioTaskParams);
    nodeRadioTaskParams.stackSize = NODERADIO_TASK_STACK_SIZE;
    nodeRadioTaskParams.priority = NODERADIO_TASK_PRIORITY;
    nodeRadioTaskParams.stack = &nodeRadioTaskStack;
    Task_construct(&nodeRadioTask, nodeRadioTaskFunction, &nodeRadioTaskParams, NULL);
}

uint8_t nodeRadioTask_getNodeAddr(void) {
    return nodeAddress;
}

uint8_t checksum(void * data, int size) {
    int i;
    uint8_t checksum=0;
    for (i=0; i<size-1; i++) {
        checksum+=(*((uint8_t*)data));
        data++;
    }
    return checksum;
}

uint8_t modeUSEU=0, preselectedModeUSEU=1;

static void nodeRadioTaskFunction(UArg arg0, UArg arg1)
{
    /* Set mulitclient mode for EasyLink */
    EasyLink_setCtrl(EasyLink_Ctrl_MultiClient_Mode, 1);

    /* Initialize EasyLink */
    if (EasyLink_init(RADIO_EASYLINK_MODULATION) != EasyLink_Status_Success)
    {
        System_abort("EasyLink_init failed");
    }

    /* Set the filter to the "new tag" address */
    if (EasyLink_enableRxAddrFilter(&nodeAddress, 1, 1) != EasyLink_Status_Success)
    {
        System_abort("EasyLink_enableRxAddrFilter failed");
    }

    //For preselectedModeUSEU=1;
    EasyLink_setFrequency(868000000);

    /* Setup node packet */
    nodePacket.header.sourceAddress = nodeAddress;
    nodePacket.header.packetType = RADIO_PACKET_TYPE_MOB_TAG_PACKET;
    nodePacket.header.id[0]='M';
    nodePacket.header.id[1]='O';
    nodePacket.header.id[2]='B';
    nodePacket.state=0;
    nodePacket.checksum=0;

    nodeStatus=0;

    /* Enter RX */
    if (EasyLink_receiveAsync(rxDoneCallback, 0) != EasyLink_Status_Success)
    {
        System_abort("EasyLink_receiveAsync failed");
    }

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

        if(modeUSEU==0) { //Frequency not selected yet
            nodeStatus&=(~((uint8_t)NODE_STATUS_FREQ_SELECTED));
            if (events & RADIO_EVENT_BROADCAST_RECEIVED)
            {
                //Use actual frequency when broadcast received
                modeUSEU=preselectedModeUSEU;
            } else if (events & RADIO_EVENT_SEND_DATA) {
                //Use periodic SEND_DATA event to switch between frequencies
                EasyLink_abort();
                switch(preselectedModeUSEU) {
                case 1:
                    preselectedModeUSEU=2;
                    EasyLink_setFrequency(915000000);
                    break;
                case 2:
                    preselectedModeUSEU=1;
                    EasyLink_setFrequency(868000000);
                    break;
                }
                if (EasyLink_receiveAsync(rxDoneCallback, 0) != EasyLink_Status_Success)
                {
                    System_abort("EasyLink_receiveAsync failed");
                }
            }
            returnRadioOperationStatus(NodeRadioStatus_Success);
        } else {
            nodeStatus|=NODE_STATUS_FREQ_SELECTED;

            /* If we should send data */
            if (events & RADIO_EVENT_SEND_DATA)
            {
                nodePacket.header.sourceAddress = nodeAddress;
                /* Use the True Random Number Generator to generate sensor node identification randomly */;
                Power_setDependency(PowerCC26XX_PERIPH_TRNG);
                TRNGEnable();
                    while (!(TRNGStatusGet() & TRNG_NUMBER_READY))
                    {
                        //wiat for randum number generator
                    }
                nodePacket.randValueH = (uint8_t)TRNGNumberGet(TRNG_LOW_WORD);
                nodePacket.randValueL = (uint8_t)TRNGNumberGet(TRNG_HI_WORD);
                TRNGDisable();
                Power_releaseDependency(PowerCC26XX_PERIPH_TRNG);
                nodePacket.batt = AONBatMonBatteryVoltageGet();
                nodePacket.state = nodeStatus;
                nodePacket.checksum = checksum((void *)&nodePacket, sizeof(struct MOBtagPacket));

                sendPacket(&nodePacket, NODERADIO_MAX_RETRIES, NORERADIO_ACK_TIMEOUT_TIME_MS + nodePacket.randValueH + nodePacket.randValueL);

                shutdownReported=0;
                if((nodeStatus&NODE_STATUS_SHUTDOWN)!=0) {
                    shutdownReported=1;
                }
            }

            /* If we get an ACK from the concentrator */
            if (events & RADIO_EVENT_DATA_ACK_RECEIVED)
            {
                    //Goto shutdown
                    if(shutdownReported!=0) {
                        //Wait while button is pressed
                        while(PIN_getInputValue(Board_PIN_BUTTON0) == 0) {
                            Task_sleep(100);
                        }
                        Task_sleep(100); //debounce

                        PIN_setOutputValue(pinHandle, NODE_MOB_ACTIVITY_LED,0);

                        /* Configure DIO for wake up from shutdown */
                        PINCC26XX_setWakeup(ButtonTableWakeUp);
                        /* Go to shutdown */
                        Power_shutdown(0, 0);
                    }


                nodeStatus&=(~((uint8_t)NODE_STATUS_MAIN_STATION_LOST));

                if(nodeAddress!=0xFF && nodeAddress!=0) { //node has unique address and is being monitored
                    nodeStatus|=NODE_STATUS_MONITORED;
                } else {
                    nodeStatus&=(~((uint8_t)NODE_STATUS_MONITORED));
                }

                if(ackRxPacket->status!=0) {
                    nodeStatus|=NODE_STATUS_MAIN_STATION_ALARM;
                } else {
                    nodeStatus&=(~((uint8_t)NODE_STATUS_MAIN_STATION_ALARM));
                }

                returnRadioOperationStatus(NodeRadioStatus_Success);
            }

            /* If we get an ACK timeout */
            if (events & RADIO_EVENT_ACK_TIMEOUT)
            {

                /* If we haven't resent it the maximum number of times yet, then resend packet */
                if (currentRadioOperation.retriesDone < currentRadioOperation.maxNumberOfRetries)
                {
                    resendPacket();
                }
                else
                {
                    /* Else return send fail */
                    Event_post(radioOperationEventHandle, RADIO_EVENT_SEND_FAIL);

                    nodeStatus|=NODE_STATUS_MAIN_STATION_LOST;
                }
            }

            /* If send fail */
            if (events & RADIO_EVENT_SEND_FAIL)
            {
                returnRadioOperationStatus(NodeRadioStatus_Failed);
            }

        }
    }
}

enum NodeRadioOperationStatus NodeRadioTask_sendData(void)
{
    enum NodeRadioOperationStatus status;

    /* Get radio access sempahore */
    Semaphore_pend(radioAccessSemHandle, BIOS_WAIT_FOREVER);

    /* Raise RADIO_EVENT_SEND_DATA event */
    Event_post(radioOperationEventHandle, RADIO_EVENT_SEND_DATA);

    /* Wait for result */
    Semaphore_pend(radioResultSemHandle, BIOS_WAIT_FOREVER);

    /* Get result */
    status = currentRadioOperation.result;

    /* Return radio access semaphore */
    Semaphore_post(radioAccessSemHandle);

    return status;
}

static void returnRadioOperationStatus(enum NodeRadioOperationStatus result)
{
    /* Save result */
    currentRadioOperation.result = result;

    /* Post result semaphore */
    Semaphore_post(radioResultSemHandle);
}

static void sendPacket(struct MOBtagPacket * nodePacket, uint8_t maxNumberOfRetries, uint32_t ackTimeoutMs)
{
    /* Set destination address in EasyLink API */
    currentRadioOperation.easyLinkTxPacket.dstAddr[0] = RADIO_CONCENTRATOR_ADDRESS;

    /* Copy packet to payload
     * Note that the EasyLink API will implcitily both add the length byte and the destination address byte. */
    memcpy(currentRadioOperation.easyLinkTxPacket.payload, nodePacket, sizeof(struct MOBtagPacket));
    currentRadioOperation.easyLinkTxPacket.len = sizeof(struct MOBtagPacket);

    /* Setup retries */
    currentRadioOperation.maxNumberOfRetries = maxNumberOfRetries;
    currentRadioOperation.ackTimeoutMs = ackTimeoutMs;
    currentRadioOperation.retriesDone = 0;
    EasyLink_setCtrl(EasyLink_Ctrl_AsyncRx_TimeOut, EasyLink_ms_To_RadioTime(ackTimeoutMs));

    /* Send packet  */
    if (EasyLink_transmit(&currentRadioOperation.easyLinkTxPacket) != EasyLink_Status_Success)
    {
        System_abort("EasyLink_transmit failed");
    }

    /* Enter RX */
    if (EasyLink_receiveAsync(rxDoneCallback, 0) != EasyLink_Status_Success)
    {
        System_abort("EasyLink_receiveAsync failed");
    }
}

static void resendPacket()
{
    /* Send packet  */
    if (EasyLink_transmit(&currentRadioOperation.easyLinkTxPacket) != EasyLink_Status_Success)
    {
        System_abort("EasyLink_transmit failed");
    }

    /* Enter RX and wait for ACK with timeout */
    if (EasyLink_receiveAsync(rxDoneCallback, 0) != EasyLink_Status_Success)
    {
        System_abort("EasyLink_receiveAsync failed");
    }

    /* Increase retries by one */
    currentRadioOperation.retriesDone++;
}

static void rxDoneCallback(EasyLink_RxPacket * rxPacket, EasyLink_Status status)
{
    struct PacketHeader* packetHeader;

    /* If this callback is called because of a packet received */
    if (status == EasyLink_Status_Success)
    {
        /* Check the payload header */
        packetHeader = (struct PacketHeader*)rxPacket->payload;

        if (packetHeader->packetType == RADIO_PACKET_TYPE_BROADCAST_PACKET)
        {
            /* Signal ACK packet received */
            broadcastRxPacket = (struct BroadcastPacket*)(rxPacket->payload);
            if( broadcastRxPacket->header.id[0] == 'M' &&
                broadcastRxPacket->header.id[1] == 'O' &&
                broadcastRxPacket->header.id[2] == 'B' &&
                broadcastRxPacket->checksum == checksum((void*)broadcastRxPacket, sizeof(struct BroadcastPacket)))
            {
                Event_post(radioOperationEventHandle, RADIO_EVENT_BROADCAST_RECEIVED);
            }
        } else if (packetHeader->packetType == RADIO_PACKET_TYPE_ACK_PACKET) {
            /* Signal ACK packet received */
            ackRxPacket = (struct AckPacket*)(rxPacket->payload);
            if( ackRxPacket->header.id[0] == 'M' &&
                ackRxPacket->header.id[1] == 'O' &&
                ackRxPacket->header.id[2] == 'B' &&
                ackRxPacket->checksum == checksum((void*)ackRxPacket, sizeof(struct AckPacket)))
            {
                if(ackRxPacket->randValueH==nodePacket.randValueH && ackRxPacket->randValueL==nodePacket.randValueL) {
                    //Assign new address to the node
                    nodeAddress=ackRxPacket->newAddress;
                } else {
                    //If rand values do not match, reset node address to initial address
                    nodeAddress=0xFF;
                }
                if (EasyLink_enableRxAddrFilter(&nodeAddress, 1, 1) != EasyLink_Status_Success)
                {
                    System_abort("EasyLink_enableRxAddrFilter failed");
                }
                Event_post(radioOperationEventHandle, RADIO_EVENT_DATA_ACK_RECEIVED);
            } else {
                /* Packet Error, treat as a Timeout and Post a RADIO_EVENT_ACK_TIMEOUT
                   event */
                Event_post(radioOperationEventHandle, RADIO_EVENT_ACK_TIMEOUT);
            }
        }
        else
        {
            /* Packet Error, treat as a Timeout and Post a RADIO_EVENT_ACK_TIMEOUT
               event */
            Event_post(radioOperationEventHandle, RADIO_EVENT_ACK_TIMEOUT);
        }
    }
    /* did the Rx timeout */
    else if(status == EasyLink_Status_Rx_Timeout)
    {
        /* Post a RADIO_EVENT_ACK_TIMEOUT event */
        Event_post(radioOperationEventHandle, RADIO_EVENT_ACK_TIMEOUT);
    }
    else if (status != EasyLink_Status_Aborted)
    {
        /* Rx Error, treat as a Timeout and Post a RADIO_EVENT_ACK_TIMEOUT
           event */
        Event_post(radioOperationEventHandle, RADIO_EVENT_ACK_TIMEOUT);
    }

}
