/*
 * For a detailed description see header file.
 */

#include <string.h>

#include "LossyCom.h"
#include "VMS_Implementations/VMS_impl/vmalloc.h"

/*
 * Initializes the central exchange structure and sets all trigger counter to 0
 * Allocates memory to fit the number of endpoints.
 * Returns NULL if an error occurs.
 */
lossyCom__exchange_t* lossyCom__initialize(uint16_t numEndpoints)
{
    lossyCom__exchange_t* centralExchange;
    
    centralExchange = VMS_WL__malloc(sizeof(lossyCom__exchange_t));
    if(centralExchange == NULL)
        return NULL;
    
    centralExchange->broadcastTriggerCounter = 0;
    centralExchange->numEndpoints = numEndpoints;
    centralExchange->outboxArray = VMS_WL__malloc(sizeof(lossyCom__msg_t)*numEndpoints);
    if(centralExchange->outboxArray == NULL){
        VMS_WL__free(centralExchange);
        return NULL;
    }
    
    centralExchange->p2pTriggerCounters = VMS_WL__malloc(sizeof(uint16_t)*numEndpoints);
    if(centralExchange->p2pTriggerCounters == NULL){
        VMS_WL__free(centralExchange->outboxArray);
        VMS_WL__free(centralExchange);
        return NULL;
    }
    
    //reset all point 2 point trigger counter
    memset((void*)centralExchange->p2pTriggerCounters, 0, sizeof(uint16_t)*numEndpoints);
    
    return centralExchange;    
}

/*
 * Connects the local endpoint to the central exchange structure.
 * This registers a message handler that handles all incoming messages.
 * Also an endpointID is set this ID has to be between 0 and total number of
 * endpoints-1 and unique.
 */
void lossyCom__initialize_endpoint(lossyCom__endpoint_t* localEndpoint,
                                   lossyCom__exchange_t* centralExchange,
                                   lossyCom__endpointID_t endpointID,
                                   lossyCom__msgHandler msgHandler,
                                   void* msgHandlerData)
{
    localEndpoint->lastReceivedBroadcastTrigger = 0;
    localEndpoint->endpointID = endpointID;
    localEndpoint->centralExchange = centralExchange;
    localEndpoint->msgHandler = msgHandler;
    localEndpoint->msgHandlerData = msgHandlerData;
}

/*
 * Prepare a lossyCom__msg_t in the correct format that contains the trigger 
 * value, the receiver endpoint ID and the message body
 */
inline lossyCom__msg_t prepareMsg(uint16_t triggerValue,
                                  lossyCom__endpointID_t receiverEndpointID,
                                  lossyCom__msgBody_t msgBody)
{
    lossyCom__msg_t msgDraft;
    
    msgDraft = (0 | msgBody);
    msgDraft |= ((lossyCom__msg_t)receiverEndpointID << ENDPOINT_ID_SHIFT);
    msgDraft |= ((lossyCom__msg_t)triggerValue << TRIGGER_SHIFT);
    
    return msgDraft;
}

/*
 * This broadcasts a message to all connected receivers
 */
void lossyCom__broadcastMsg(lossyCom__endpoint_t* localEndpoint,
                                   lossyCom__msgBody_t msgBody)
{
    lossyCom__exchange_t* centralExchange = localEndpoint->centralExchange;
    uint16_t increasedTrigger;
    lossyCom__msg_t msg;
    
    increasedTrigger = centralExchange->broadcastTriggerCounter +1;
    
    //build message
    msg = prepareMsg(increasedTrigger, BROADCAST_ID, msgBody);
    
    //write msg to central exchange
    centralExchange->outboxArray[localEndpoint->endpointID] = msg;
    
    //update broadcast trigger counter
    centralExchange->broadcastTriggerCounter = increasedTrigger;
}

/*
 * This sends a message another endpoint. Again it is not guaranteed that the 
 * message is received. But in most cases it will.
 */
void lossyCom__sendMsg(lossyCom__endpoint_t* localEndpoint,
                       lossyCom__endpointID_t receiverEndpointID,
                       lossyCom__msgBody_t msgBody)
{
    lossyCom__exchange_t* centralExchange = localEndpoint->centralExchange;
    lossyCom__msg_t msg;
    uint16_t increasedTrigger;
    
    increasedTrigger =
            centralExchange->p2pTriggerCounters[receiverEndpointID] +1;
    
        //build message
    msg = prepareMsg(increasedTrigger, receiverEndpointID, msgBody);
    
    //write msg to central exchange
    centralExchange->outboxArray[localEndpoint->endpointID] = msg;
    
    //write back increased trigger counter
    centralExchange->p2pTriggerCounters[receiverEndpointID] = increasedTrigger;
}

int inline isUnreceivedMsg(uint16_t msgTrigger, 
                           uint16_t lastReceivedTrigger,
                           uint16_t triggerSnapshot)
{
    if(msgTrigger > lastReceivedTrigger ||
                            msgTrigger <=  triggerSnapshot)
    {
        // check if the message is new (msg trigger > archived trigger)
        // and already valid (msgTrigger <= currentTriggerCopy)
        if((msgTrigger > lastReceivedTrigger &&
                msgTrigger <=  triggerSnapshot) ||
                ((int64_t) triggerSnapshot- // check for triggerCounterOverflow
                    (int64_t)lastReceivedTrigger < -MAX_TRIGGER/2))
        {
            return TRUE;
        }
    }
    return FALSE;
}

void lossyCom__receiveMsg(lossyCom__endpoint_t* localEndpoint)
{
    uint16_t  broadcastTriggerSnapshot, p2pTriggerSnapshot;
    lossyCom__exchange_t*  centralExchange;
    lossyCom__endpointID_t senderEndpointID;
    lossyCom__endpointID_t receiverEndpointID;
    lossyCom__msg_t msgCopy;
    uint16_t msgTrigger;
    lossyCom__msgBody_t msgBody;
    
    centralExchange = localEndpoint->centralExchange;
    //save trigger counter to know find valid messages
    broadcastTriggerSnapshot = centralExchange->broadcastTriggerCounter;
    p2pTriggerSnapshot = 
            centralExchange->p2pTriggerCounters[localEndpoint->endpointID];    
    
    //new message arrived if trigger counter is higher than the last time read
    if( broadcastTriggerSnapshot > localEndpoint->lastReceivedBroadcastTrigger ||
            p2pTriggerSnapshot > localEndpoint->lastReceivedp2pTrigger)
    {
        senderEndpointID = 0;
        //search outboxes for new messages
        while(senderEndpointID < centralExchange->numEndpoints)
        {
            //ignore own outbox
            if(senderEndpointID != localEndpoint->endpointID)
            {
                msgCopy = centralExchange->outboxArray[senderEndpointID];
                msgTrigger = 0xFFFF & (msgCopy >> TRIGGER_SHIFT);
                receiverEndpointID = 0xFFFF & (msgCopy >> ENDPOINT_ID_SHIFT);
                
                if(receiverEndpointID == BROADCAST_ID){//receive broadcast message
                    if(isUnreceivedMsg(msgTrigger,
                                       localEndpoint->lastReceivedBroadcastTrigger,
                                       broadcastTriggerSnapshot))
                    {
                        //let the message handler parse the message
                        msgBody = 0xFFFFFFFF & msgCopy;        
                        //only receive broadcast and p2p for own receiverID

                        (*(localEndpoint->msgHandler))(senderEndpointID,
                                                       msgBody,
                                                       localEndpoint->msgHandlerData);
                    }
                }else{//point 2 point message
                    if(receiverEndpointID == localEndpoint->endpointID &&
                       isUnreceivedMsg(msgTrigger,
                                       localEndpoint->lastReceivedp2pTrigger,
                                       p2pTriggerSnapshot))
                    {
                        //let the message handler parse the message
                        msgBody = 0xFFFFFFFF & msgCopy;        
                        //only receive broadcast and p2p for own receiverID

                        (*(localEndpoint->msgHandler))(senderEndpointID,
                                                       msgBody,
                                                       localEndpoint->msgHandlerData);
                    }
                }
            }
            senderEndpointID++;
        }//search outbox loop
    }
    //save last TriggerCounter of last parsed Msg
    localEndpoint->lastReceivedBroadcastTrigger =  broadcastTriggerSnapshot;
    localEndpoint->lastReceivedp2pTrigger = p2pTriggerSnapshot;
}