/* Standard Include Files. */
#include <stdint.h>
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <stdio.h>
/* BIOS/XDC Include Files. */
#include <xdc/std.h>
#include <xdc/cfg/global.h>
#include <xdc/runtime/IHeap.h>
#include <xdc/runtime/System.h>
#include <xdc/runtime/Error.h>
#include <xdc/runtime/Memory.h>
#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Task.h>
#include <ti/sysbios/knl/Event.h>
#include <ti/sysbios/knl/Semaphore.h>
#include <ti/sysbios/knl/Clock.h>
#include <ti/sysbios/heaps/HeapBuf.h>
#include <ti/sysbios/heaps/HeapMem.h>
#include <ti/sysbios/knl/Event.h>
#include <ti/sysbios/family/c64p/Cache.h>
#include <ti/sysbios/family/c64p/Hwi.h>
#include <ti/sysbios/family/c64p/EventCombiner.h>
/* Mailbox Driver: */
#include <ti/drivers/mailbox/mailbox.h>
#include <ti/drivers/mailbox/include/mailbox_internal.h>
#include <ti/drivers/soc/soc.h>
#include "ti/utils/testlogger/logger.h"
SOC_Handle  socHandle;

Mbox_Handle peerMailbox;

#define BIG_MSG_SIZE 3
#define BUFFER_SIZE 128

uint8_t buffer[BUFFER_SIZE];

volatile uint8_t mboxProcToken = 0;

void MmwDemo_sleep(void)
{
    /* issue WFI (Wait For Interrupt) instruction */
    asm(" IDLE ");
}


static int32_t mboxWrite_ch0(uint8_t message, int32_t len)
{
    int32_t retVal = -1;
    retVal = Mailbox_write(peerMailbox, &message, len);
    if (retVal == len)
    {
        retVal = 0;
    }
    return retVal;
}

static void mboxReadProc_ch0()
{
    uint8_t   message[BIG_MSG_SIZE];
    uint8_t id;
    int32_t           retVal = 0;
    int32_t           ret = 0;

    /* Read the message from the peer mailbox: We are not trying to protect the read
     * from the peer mailbox because this is only being invoked from a single thread */
    retVal = Mailbox_read(peerMailbox, message, sizeof(message)/sizeof(uint8_t));
    if (retVal < 0)
    {
        /* Error: Unable to read the message. Setup the error code and return values */
        System_printf("Error read dss\n");
        return;
    }
    else
    {
        Mailbox_readFlush (peerMailbox);
        id = message[1];
        if(message[0] == 1) {
            ret = mboxWrite_ch0(buffer[id], 1);
        } else if(message[0] == 0) {
            buffer[id] = message[2];
        }

        if (ret != 0)
        {
            System_printf ("Error: Mailbox send message failed \n");
        }
        /* We are done: There are no messages available from the peer execution domain. */
        return;
    }

}

void mboxCallback_ch0 (Mbox_Handle handle, Mailbox_Type peer)
{
    /* Message has been received from the peer endpoint. */
    mboxProcToken = 1;
}

void mBox(UArg a0, UArg a1)
{
    while(true){
        if (mboxProcToken == 1)
        {
            mboxProcToken = 0;
            /* If the mailbox has a message and the frame processing task has finished. */
            mboxReadProc_ch0();
        }
    }
}

void InitTask(UArg arg0, UArg arg1)
{
    int32_t      errCode;
    Mailbox_Config  cfg;
    Task_Params taskParams;

    Mailbox_init(MAILBOX_TYPE_DSS);
    if(Mailbox_Config_init(&cfg) < 0)
    {
        System_printf ("Error: Mailbox init failed\n");
        return;
    }

    /* Setup the configuration: */
    cfg.chType     = MAILBOX_CHTYPE_MULTI;
    cfg.chId         = MAILBOX_CH_ID_0;
    cfg.writeMode   = MAILBOX_MODE_BLOCKING;
    cfg.readMode     = MAILBOX_MODE_CALLBACK;
    cfg.readCallback = &mboxCallback_ch0;

    peerMailbox = Mailbox_open(MAILBOX_TYPE_MSS, &cfg, &errCode);
    if (peerMailbox == NULL)
    {
        System_printf ("Error: Mailbox open failed 0\n");
        return;
    }
    Task_Params_init(&taskParams);
    taskParams.priority = 2;
    taskParams.stackSize = 3 * 1024;
    Task_create(mBox, &taskParams, NULL);
    return;
}

int main (void)
{
    Task_Params taskParams;

    SOC_Cfg          socCfg;
    int32_t          errCode;

    /* Initialize the SOC confiugration: */
    memset ((void *)&socCfg, 0, sizeof(SOC_Cfg));

    /* Populate the SOC configuration: We are bypassing the clock initialization
     * in the unit test here since the MSS unit test is doing this. */
    socCfg.clockCfg = SOC_SysClock_BYPASS_INIT;

    /* Initialize the SOC Module: */
    socHandle = SOC_init (&socCfg, &errCode);
    if (socHandle == NULL)
    {
        System_printf ("Error: SOC Module Initialization failed [Error code %d]\n", errCode);
        return -1;
    }

    /* Initialize the Task Parameters. */
    Task_Params_init(&taskParams);
    taskParams.stackSize = 4*1024;
    taskParams.priority = 4;
    Task_create(InitTask, &taskParams, NULL);

    /* Start BIOS */
    BIOS_start();
    return 0;
}