/***** Includes *****/
#include <xdc/std.h>
#include <xdc/runtime/System.h>

#include <DmConcentratorRadioTask.h>
#include <seb/SEB.h>

#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Task.h>
#include <ti/sysbios/knl/Semaphore.h>
#include <ti/sysbios/knl/Event.h>

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

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

#include "easylink/EasyLink.h"
#include "RadioProtocol.h"

#include "MOB.h"


/***** Defines *****/
#define CONCENTRATORRADIO_TASK_STACK_SIZE 1024
#define CONCENTRATORRADIO_TASK_PRIORITY   3

#define RADIO_EVENT_ALL                  0xFFFFFFFF
#define RADIO_EVENT_VALID_PACKET_RECEIVED      (uint32_t)(1 << 0)
#define RADIO_EVENT_INVALID_PACKET_RECEIVED (uint32_t)(1 << 1)
#define RADIO_EVENT_BROADCAST (uint32_t)(1 << 2)

#define CONCENTRATORRADIO_MAX_RETRIES 2
#define NORERADIO_ACK_TIMEOUT_TIME_MS (1000)

#define CONCENTRATOR_0M_TXPOWER    -10

/***** Variable declarations *****/
static Task_Params concentratorRadioTaskParams;
Task_Struct concentratorRadioTask; /* not static so you can see in ROV */
static uint8_t concentratorRadioTaskStack[CONCENTRATORRADIO_TASK_STACK_SIZE];
Event_Struct radioOperationEvent;  /* not static so you can see in ROV */
static Event_Handle radioOperationEventHandle;

static ConcentratorRadio_PacketReceivedCallback packetReceivedCallback;
static ConcentratorRadio_BroadcastCallback broadcastCallback;
static struct MOBtagPacket latestRxPacket;
static struct PacketHeader* rxPacketHeader;
static EasyLink_TxPacket txPacket;
static struct AckPacket ackPacket;
static struct BroadcastPacket broadcastPacket;
static uint8_t concentratorAddress;
static int8_t latestRssi;

/***** Prototypes *****/
static void concentratorRadioTaskFunction(UArg arg0, UArg arg1);
static void rxDoneCallback(EasyLink_RxPacket * rxPacket, EasyLink_Status status);
static void notifyPacketReceived(struct MOBtagPacket* latestRxPacket);
static void notifyBroadcast();
static void sendAck(struct MOBtagPacket* latestRxPacket);


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

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

    /* Create the concentrator radio protocol task */
    Task_Params_init(&concentratorRadioTaskParams);
    concentratorRadioTaskParams.stackSize = CONCENTRATORRADIO_TASK_STACK_SIZE;
    concentratorRadioTaskParams.priority = CONCENTRATORRADIO_TASK_PRIORITY;
    concentratorRadioTaskParams.stack = &concentratorRadioTaskStack;
    Task_construct(&concentratorRadioTask, concentratorRadioTaskFunction, &concentratorRadioTaskParams, NULL);
}

void ConcentratorRadioTask_registerPacketReceivedCallback(ConcentratorRadio_PacketReceivedCallback callback) {
    packetReceivedCallback = callback;
}

void ConcentratorRadioTask_registerBroadcastCallback(ConcentratorRadio_BroadcastCallback callback) {
    broadcastCallback = callback;
}

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;
}

extern uint32_t radioTaskCnt;
static void concentratorRadioTaskFunction(UArg arg0, UArg arg1)
{
    /* Set mulitclient mode for Prop Sub1G */
    EasyLink_setCtrl(EasyLink_Ctrl_MultiClient_Mode, 1);

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


    //Wait for mode selection
    while(modeUSEU==0){
        Task_sleep(100);
    }
    switch(modeUSEU) {
    case 1:
        EasyLink_setFrequency(868000000);
        break;
    case 2:
        EasyLink_setFrequency(915000000);
        break;
    }

    /* Set concentrator address */;
    concentratorAddress = RADIO_CONCENTRATOR_ADDRESS;
    EasyLink_enableRxAddrFilter(&concentratorAddress, 1, 1);

    /* Set up Ack packet */
    ackPacket.header.sourceAddress = concentratorAddress;
    ackPacket.header.packetType = RADIO_PACKET_TYPE_ACK_PACKET;
    ackPacket.header.id[0]='M';
    ackPacket.header.id[1]='O';
    ackPacket.header.id[2]='B';
    ackPacket.status=0;
    ackPacket.newAddress=0;
    ackPacket.checksum=0;

    //Set up broadcast packet
    broadcastPacket.header.sourceAddress = concentratorAddress;
    broadcastPacket.header.packetType = RADIO_PACKET_TYPE_BROADCAST_PACKET;
    broadcastPacket.header.id[0]='M';
    broadcastPacket.header.id[1]='O';
    broadcastPacket.header.id[2]='B';
    broadcastPacket.checksum = checksum((void *)&broadcastPacket, sizeof(struct BroadcastPacket));

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

    while (1)
    {
        uint32_t events = Event_pend(radioOperationEventHandle, 0, RADIO_EVENT_ALL, BIOS_WAIT_FOREVER);

        /* If we should send broadcast */
        if (events & RADIO_EVENT_BROADCAST)
        {
            /* RX timed out abort */
            EasyLink_abort();

            txPacket.dstAddr[0] = 0xFF; //new tags

            memcpy(txPacket.payload, &broadcastPacket, sizeof(struct BroadcastPacket));
            txPacket.len = sizeof(struct BroadcastPacket);

            if (EasyLink_transmit(&txPacket) != EasyLink_Status_Success)
            {
                System_abort("EasyLink_transmit failed");
            }

            notifyBroadcast();

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

            __disable_irq();
            radioTaskCnt++;
            __enable_irq();
        }

        /* If valid packet received */
        if (events & RADIO_EVENT_VALID_PACKET_RECEIVED)
        {
            /* Send ack packet */
            sendAck(&latestRxPacket);

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

        /* If invalid packet received */
        if (events & RADIO_EVENT_INVALID_PACKET_RECEIVED)
        {
            /* Go back to RX */
            if (EasyLink_receiveAsync(rxDoneCallback, 0) != EasyLink_Status_Success)
            {
                System_abort("EasyLink_receiveAsync failed");
            }
        }
    }
}

static void sendAck(struct MOBtagPacket* latestRxPacket) {

    /* Set destinationAdress */
    //This is 0xFF in answer to the new tag (new address will be configured with the ACK packet)
    txPacket.dstAddr[0] = latestRxPacket->header.sourceAddress;

    //Give new address if the new tag was found:
    if(latestRxPacket->header.sourceAddress==0xFF) {
        latestRxPacket->header.sourceAddress=findFreeAddr();
    }

    /* Call packet received callback */
    // This must be called after the new addres of the tag is set in the latestRxPacket (next time tag will have this address)
    // This updates status, so should be called before sending ACK packet (containing the status).
    notifyPacketReceived(latestRxPacket);

    //Send MOB system status with the ACK packet:
    ackPacket.status = getMOBstatus();
    //Return random values to the tag (to avoid giving the same address for two tags)
    ackPacket.randValueH=latestRxPacket->randValueH;
    ackPacket.randValueL=latestRxPacket->randValueL;
    //Send new address to the tag:
    ackPacket.newAddress=latestRxPacket->header.sourceAddress;
    //Calculate checksum for the ack packet:
    ackPacket.checksum = checksum((void *)&ackPacket, sizeof(struct AckPacket));

    /* Copy ACK packet to payload, skipping the destination adress byte.
     * Note that the EasyLink API will implcitily both add the length byte and the destination address byte. */
    memcpy(txPacket.payload, &ackPacket, sizeof(struct AckPacket));
    txPacket.len = sizeof(struct AckPacket);

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

static void notifyPacketReceived(struct MOBtagPacket* latestRxPacket)
{
    if (packetReceivedCallback)
    {
        packetReceivedCallback(latestRxPacket, latestRssi);
    }
}

static void notifyBroadcast(void)
{
    if (broadcastCallback)
    {
        broadcastCallback();
    }
}

static void rxDoneCallback(EasyLink_RxPacket * rxPacket, EasyLink_Status status)
{
    struct MOBtagPacket* mobRxPacket;

    /* If we received a packet successfully */
    if (status == EasyLink_Status_Success)
    {
        /* Save the latest RSSI, which is later sent to the receive callback */
        latestRssi = (int8_t)rxPacket->rssi;

        /* Check that this is a valid packet */
        rxPacketHeader = (struct PacketHeader*)(rxPacket->payload);

        /* If this is a known packet */
        if ( rxPacketHeader->packetType == RADIO_PACKET_TYPE_MOB_TAG_PACKET )
        {
            mobRxPacket = (struct MOBtagPacket*)(rxPacket->payload);
            if( mobRxPacket->header.id[0] == 'M' &&
                mobRxPacket->header.id[1] == 'O' &&
                mobRxPacket->header.id[2] == 'B' &&
                mobRxPacket->checksum == checksum((void*)mobRxPacket, sizeof(struct MOBtagPacket)))
            {
                /* Save packet */
                latestRxPacket=*mobRxPacket;

                /* Signal packet received */
                Event_post(radioOperationEventHandle, RADIO_EVENT_VALID_PACKET_RECEIVED);
            }
            else
            {
                /* Signal invalid packet received */
                Event_post(radioOperationEventHandle, RADIO_EVENT_INVALID_PACKET_RECEIVED);
            }
        }
        else
        {
            /* Signal invalid packet received */
            Event_post(radioOperationEventHandle, RADIO_EVENT_INVALID_PACKET_RECEIVED);
        }
    }
    else if (status != EasyLink_Status_Aborted)
    {
        /* Signal invalid packet received */
        Event_post(radioOperationEventHandle, RADIO_EVENT_INVALID_PACKET_RECEIVED);
    }
}

void RadioTask_Broadcast(void)
{
   Event_post(radioOperationEventHandle, RADIO_EVENT_BROADCAST);
}
