/* Function to allocate memory from the SyslinkMemMgr */
Ptr SyslinkMemMgr_SharedMemory_alloc(SyslinkMemMgr_SharedMemory_Object *obj,
        SyslinkMemMgr_AllocParams *allocParams)
{
    Ptr   buf = NULL;
    Error_Block eb;

    Error_init (&eb);

    if (obj != NULL) {
        buf = Memory_alloc(obj->heapHandle, allocParams->size,
                allocParams->align, &eb);
    }

    return (buf);
}

void Event::begin()
{
    Error_Block eb;
    Error_init(&eb);
    
    Event_Params params;
    Event_Params_init(&params);
    
    EventHandle = Event_create(&params, &eb);
    
    if (EventHandle == NULL)
    {
        System_abort("Event create failed");
    }
}

/*!
 *  ======== InterruptArp32_intRegister ========
 */
Void InterruptArp32_intRegister(UInt16 remoteProcId,
                                 IInterrupt_IntInfo *intInfo,
                                 Fxn func, UArg arg)
{
    UInt        key;
    Hwi_Params  hwiAttrs;
    Error_Block eb;
    InterruptArp32_FxnTable *table;

    Assert_isTrue(remoteProcId < ti_sdo_utils_MultiProc_numProcessors, 
            ti_sdo_ipc_Ipc_A_internal);

    /* Assert that our MultiProc id is set correctly */
    Assert_isTrue((InterruptArp32_arp32ProcId == MultiProc_self()), 
                   ti_sdo_ipc_Ipc_A_internal);

    /* init error block */
    Error_init(&eb);
    
    /* Disable global interrupts */
    key = Hwi_disable();

    table = &(InterruptArp32_module->fxnTable);
    table->func = func;
    table->arg  = arg;

    InterruptArp32_intClear(remoteProcId, intInfo);
    
    /* Make sure the interrupt only gets plugged once */
    InterruptArp32_module->numPlugged++;
    if (InterruptArp32_module->numPlugged == 1) { 
        /* Register interrupt to remote processor */
        Hwi_Params_init(&hwiAttrs);
        hwiAttrs.arg = arg;
        hwiAttrs.vectorNum = intInfo->intVectorId;

        Hwi_create(ARP32INT,
                  (Hwi_FuncPtr)InterruptArp32_intShmStub,
                   &hwiAttrs,
                   &eb);
    }

    /* enable the mailbox and Hwi */
    InterruptArp32_intEnable(remoteProcId, intInfo);
    
    /* Restore global interrupts */
    Hwi_restore(key);
}

/*!
 *  ======== NotifySetup_attach ========
 *  Initialize interrupt
 */     
Int NotifySetup_attach(UInt16 remoteProcId, Ptr sharedAddr)
{
    NotifyDriverShm_Params notifyShmParams;
    NotifyDriverShm_Handle shmDrvHandle;
    ti_sdo_ipc_Notify_Handle notifyHandle;
    Int status = Notify_S_SUCCESS;
    Error_Block eb;
    
    Error_init(&eb);
    
    NotifyDriverShm_Params_init(&notifyShmParams);
    notifyShmParams.sharedAddr = sharedAddr;
    notifyShmParams.remoteProcId  = remoteProcId;
    
    /* Set the intVectorId if on the DSP */
    if ((MultiProc_self() == NotifySetup_dsp0ProcId) ||
        (MultiProc_self() == NotifySetup_dsp1ProcId)) {
            notifyShmParams.intVectorId = NotifySetup_dspIntVectId;
    }

    /* Set the intVectorId if on the EVE */
    if ((MultiProc_self() == NotifySetup_eve0ProcId) ||
        (MultiProc_self() == NotifySetup_eve1ProcId) ||
        (MultiProc_self() == NotifySetup_eve2ProcId) ||
        (MultiProc_self() == NotifySetup_eve3ProcId)) {
        if (PROCID(remoteProcId) < 4) {
            notifyShmParams.intVectorId = NotifySetup_eveIntVectId_INTC1;
        }
        else {
            notifyShmParams.intVectorId = NotifySetup_eveIntVectId_INTC0;
        }
    }
    
    shmDrvHandle = NotifyDriverShm_create(&notifyShmParams, &eb);
    if (shmDrvHandle == NULL) {
        return (Notify_E_FAIL);
    }
    
    notifyHandle = ti_sdo_ipc_Notify_create(
            NotifyDriverShm_Handle_upCast(shmDrvHandle), remoteProcId, 0, 
            NULL, &eb);
    if (notifyHandle == NULL) {
        NotifyDriverShm_delete(&shmDrvHandle);
        status = Notify_E_FAIL;
    }

    return (status);
}

/*
 *  ======== Power_registerConstraint ========
 *  Register an operational constraint with Power.
 *
 */
Power_Status Power_registerConstraint(Power_Constraint type, UArg value,
    Power_ConstraintHandle *handle)
{
    Power_Status status = Power_SOK;
    Power_ConstraintObj * pConstraint;
    Error_Block eb;

    /* check for NULL handle pointer */
    if (handle == NULL) {
        status = Power_EINVALIDPOINTER;
    }

    /* else, check for invalid constraint type */
    else if ((type < Power_DISALLOWED_CPU_SETPOINT_MASK) ||
             (type > Power_DISALLOWEDSLEEPSTATE_MASK)) {
        status = Power_EINVALIDVALUE;
    }

    /* else, allocate a constraint object */
    else {
        Error_init(&eb);
        pConstraint = Memory_calloc(Power_Object_heap(), 
            sizeof(Power_ConstraintObj), 0, &eb);
        if ((pConstraint == NULL) || Error_check(&eb)) {
            status = Power_EFAIL;
        }

        /* fill in and enqueue the constraint, update aggregate constraints */
        else {

            /* fill in constraint object elements */
            pConstraint->type = type;
            pConstraint->value = value;

            /* place constraint object on the constraints queue */
            Queue_put(Power_Module_State_constraintsQ(), 
                (Queue_Elem*) pConstraint);

            /* update aggregated constraints with the new constraint */
            Power_updateConstraints(type, value);

            /* set out parameters */
            *handle = (UArg *) pConstraint;
        }
    }

    return (status);
}

void Create_uartHandler() {
	// Create UART task (RS-485)
	Task_Handle UART_taskHandle;
	Task_Params UART_taskParams;
	Error_Block UART_eb;
	Task_Params_init(&UART_taskParams);
	Error_init(&UART_eb);
	UART_taskParams.stackSize = 2048;
	UART_taskParams.priority = 1;
	UART_taskParams.arg0 = 1000;
	UART_taskHandle = Task_create((Task_FuncPtr) uartHandler, &UART_taskParams,
			&UART_eb);
	if (UART_taskHandle == NULL) {
		System_printf("main: Failed to create uartHandler Task\n");
	}
}

/*
 *  ======== Power_registerNotify ========
 *  Register a function to be called on a specific power event.
 *
 */
Power_Status Power_registerNotify(Power_Event eventType, UInt eventMask,
    Fxn notifyFxn, UArg clientArg, Power_NotifyHandle * notifyHandle,
    Fxn *delayedCompletionFxn)
{
    Power_NotifyObj * pNotify;
    Queue_Handle notifyQ;
    Error_Block eb;

    /* check for out of range event type */
    if ((eventType < 0) || (eventType >= Power_INVALIDEVENT)) {
        return (Power_EINVALIDEVENT);
    }

    /* check for NULL pointers for notifyFxn and out params */
    if ((notifyFxn == NULL) || (notifyHandle == NULL) ||
       (delayedCompletionFxn == NULL)) {
        return (Power_EINVALIDPOINTER);
    }

    /* allocate a notification object */
    Error_init(&eb);
    pNotify = Memory_calloc(Power_Object_heap(), sizeof(Power_NotifyObj), 0, 
        &eb);
    if ((pNotify == NULL) || Error_check(&eb)) {
        return (Power_EFAIL);
    }

    /* fill in notify object elements */
    pNotify->eventType = eventType;
    pNotify->notifyFxn = notifyFxn;
    pNotify->clientArg = clientArg;
    pNotify->eventMask = eventMask;

    /* determine the appropriate notification queue */
    notifyQ = (Queue_Handle)((UInt8 *)(Power_module->notifyQ) +
                (UInt)(eventType * (2 * sizeof(Ptr))));

    /* place notify object on appropriate event queue */
    Queue_put(notifyQ, (Queue_Elem*) pNotify);

    /* set out parameters */
    *notifyHandle = pNotify;
    *delayedCompletionFxn = 
        (Fxn) ti_sysbios_family_c674_Power_delayCompletionFxns[eventType];

    return(Power_SOK);
}

Int RcmClient_create(String server, const RcmClient_Params *params,
        RcmClient_Handle *handle)
{
    RcmClient_Object *obj;
    Error_Block eb;
    Int status = RcmClient_S_SUCCESS;


    Log_print1(Diags_ENTRY, "--> %s: ()", (IArg)FXNN);

    /* initialize the error block */
    Error_init(&eb);
    *handle = (RcmClient_Handle)NULL;

    /* TODO: add check that params was initialized correctly */

    /* allocate the object */
    obj = (RcmClient_Handle)xdc_runtime_Memory_calloc(RcmClient_Module_heap(),
        sizeof(RcmClient_Object), sizeof(Int), &eb);

    if (NULL == obj) {
        Log_error2(FXNN": out of memory: heap=0x%x, size=%u",
            (IArg)RcmClient_Module_heap(), sizeof(RcmClient_Object));
        status = RcmClient_E_NOMEMORY;
        goto leave;
    }

    Log_print1(Diags_LIFECYCLE, FXNN": instance create: 0x%x", (IArg)obj);

    /* object-specific initialization */
    status = RcmClient_Instance_init(obj, server, params);

    if (status < 0) {
        RcmClient_Instance_finalize(obj);
        xdc_runtime_Memory_free(RcmClient_Module_heap(),
            (Ptr)obj, sizeof(RcmClient_Object));
        goto leave;
    }

    /* success, return opaque pointer */
    *handle = (RcmClient_Handle)obj;


leave:
    Log_print2(Diags_EXIT, "<-- %s: %d", (IArg)FXNN, (IArg)status);
    return(status);
}

//void SWItrigger::begin(uint8_t trigger, void (*functionSWItrigger)(uint8_t))
void SWItrigger::begin(uint8_t trigger, void (*functionSWItrigger)())
{
    Error_Block eb;
    Error_init(&eb);
    
    Swi_Params swiParams;
    Swi_Params_init(&swiParams);
    swiParams.trigger = trigger;
    
//    SWItriggerHandle = Swi_create((Swi_FuncPtr)functionSWItrigger(Swi_getTrigger()), &swiParams, &eb);
    SWItriggerHandle = Swi_create((Swi_FuncPtr)functionSWItrigger, &swiParams, &eb);

    if (SWItriggerHandle == NULL)
    {
        System_abort("SWItrigger create failed");
    }
}

/*
 *  ======== main ========
 */
Int main()
{ 
    Task_Handle task;
    Error_Block eb;

    System_printf("enter main()\n");

    Error_init(&eb);
    task = Task_create(taskMstr, NULL, &eb);
    if (task == NULL) {
        System_printf("Task_create() failed!\n");
        BIOS_exit(0);
    }

    BIOS_start();    /* does not return */
    return(0);
}

/**
 * \brief create semaphore object
 *
 * Creates a counting semaphore for inter-thread synchronization.
 * The initial semaphore count is specified as an input parameter.
 *
 * \param[in] p_sem_handle      Handle to semaphore control block
 * \param[in] p_name            Name of the semaphore - UNUSED in SYS/BIOS
 * \param[in] count             Initial count of the semaphore
 *
 * \return   e_SUCCESS on success, e_FAILURE on error
 *
 * \sa OS_semaphore_delete, OS_semaphore_put, OS_semaphore_get
 * \note
 *
 * \warning
 */
e_ret_status OS_semaphore_create(handle p_handle,
                                 const void *const p_name,     //UNUSED
                                 uInt16 count)
{
    Semaphore_Handle sem_handle = NULL;
    Error_Block eb;

    Error_init(&eb);

    sem_handle = Semaphore_create(count, NULL, &eb);
    if (NULL == sem_handle) {
        return e_FAILURE;
    }

    *(Semaphore_Handle *)p_handle = sem_handle;
    return e_SUCCESS;
}

/*****************************************************************************
** Function name:  Task_Init1     
**
** Descriptions: 
**     			   Create a task in a non-running state    
** parameters:     Task *pThis, Task name, Task function, Task Fn Arg, Stack Size, 
				 Task Priority.
** Returned value: None   
**
** Dependencies/Limitations/Side Effects/Design Notes:
**		Task is just created, and it will not run until Task_Start
**                
******************************************************************************/
BOOL Task_Init1(Task *pThis, String sName, pfnTask pfn, VOID *pArg, TaskStack eStacksize, TaskPri ePriority  )
{
	Error_Block eb;

	//pThis->tskattrs = TSK_ATTRS;
	 Task_Params_init(&(pThis->tskattrs));
	 Error_init(&eb);

	//pThis->tskattrs.stacksize = eStacksize;

	pThis->tskattrs.stackSize= eStacksize;
	//Setting of Task name
	if (sName != NULL)
	{
		memcpy(pThis->Name, sName, TASK_NAME_SIZE);
		//pThis->tskattrs.name = (String )&pThis->Name[0];
		pThis->tskattrs.instance->name = (String )&pThis->Name[0];
	}
	else
	{
		//pThis->tskattrs.name = "DUMMY";
		pThis->tskattrs.instance->name ="DUMMY";
	}

	//Setting of Task priority
	pThis->tskattrs.priority = STE_TASK_SUSPEND; // Suspend the priority.
	pThis->hPriority = ePriority;
	//Setting argument
	//pThis->tskattrs.arg0 =*(xdc_UArg*)pArg;
	pThis->tskattrs.arg0 =(xdc_UArg)pArg;
	
	//pThis->Handle = TSK_create((Fxn)pfn, &pThis->tskattrs, pArg);
	pThis->Handle = Task_create((Task_FuncPtr)pfn, &pThis->tskattrs,&eb);

    if (pThis->Handle == NULL)
    {
			printf("TASK: Failed create thread");
			return FALSE;
            ///SYS_abort("Failed create thread");
			//Raise an exception.
    }

	return TRUE;

}

/*!
 *  ======== InterruptDsp_intRegister ======== 
 */
Void InterruptDsp_intRegister(UInt16 remoteProcId, IInterrupt_IntInfo *intInfo,
                              Fxn func, UArg arg)
{
    UInt        key;
    Hwi_Params  hwiAttrs;
    Error_Block eb;
    InterruptDsp_FxnTable *table;    
   
    Assert_isTrue(intInfo->intVectorId <= 15, ti_sdo_ipc_Ipc_A_internal);

    /* init error block */
    Error_init(&eb);

    /* Disable global interrupts */
    key = Hwi_disable();

    table = &(InterruptDsp_module->fxnTable);
    table->func = func;
    table->arg  = arg;
    
    InterruptDsp_intClear(remoteProcId, intInfo);

    /* Make sure the interrupt only gets plugged once */
    InterruptDsp_module->numPlugged++;
    if (InterruptDsp_module->numPlugged == 1) { 
        /* Register interrupt to remote processor */
        Hwi_Params_init(&hwiAttrs);
        hwiAttrs.arg = arg;
        hwiAttrs.eventId = 90;

        Hwi_create(intInfo->intVectorId,
                   (Hwi_FuncPtr)InterruptDsp_intShmStub,
                   &hwiAttrs,
                   &eb);
        
        /* enable the interrupt vector */
        Hwi_enableInterrupt(intInfo->intVectorId);
    }
    
    /* Enable the mailbox interrupt to the DSP */
    InterruptDsp_intEnable(remoteProcId, intInfo);
    
    /* Restore global interrupts */
    Hwi_restore(key);
}

    int wc_InitMutex(wolfSSL_Mutex* m)
    {
        Semaphore_Params params;
        Error_Block eb;

        Error_init(&eb);
        Semaphore_Params_init(&params);
        params.mode = Semaphore_Mode_BINARY;

        *m = Semaphore_create(1, &params, &eb);
        if (Error_check(&eb)) {
            Error_raise(&eb, Error_E_generic, "Failed to Create the semaphore.",
                NULL);
            return BAD_MUTEX_E;
        }
        else
            return 0;
    }

MBX_Handle MBX_create(Uns size, Uns length, MBX_Attrs *attrs)
{
    Mailbox_Params params;
    Error_Block eb;

    if (attrs == NULL) {
        attrs = &MBX_ATTRS;
    }

    Mailbox_Params_init(&params);

    params.instance->name = attrs->name;
    params.heap = MEM_getHandle(attrs->segid);

    Error_init(&eb);

    return ((MBX_Handle)Mailbox_create(size, length, &params, &eb));
}

/*
 *  ======== ListMP_openByAddr ========
 */
Int ListMP_openByAddr(Ptr sharedAddr, ListMP_Handle *handlePtr)
{
    ti_sdo_ipc_ListMP_Params params;
    ti_sdo_ipc_ListMP_Attrs *attrs;
    Error_Block eb;
    Int status;
    UInt16 id;
    
    Error_init(&eb);
    
    ti_sdo_ipc_ListMP_Params_init(&params);
    
    /* Tell Instance_init() that we're opening */
    params.openFlag = TRUE;
    
    attrs = (ti_sdo_ipc_ListMP_Attrs *)sharedAddr;
    params.sharedAddr = sharedAddr;
    id = SharedRegion_getId(sharedAddr);
    
    if (SharedRegion_isCacheEnabled(id)) {
        Cache_inv(attrs, sizeof(ti_sdo_ipc_ListMP_Attrs), Cache_Type_ALL, TRUE);
    }
    
    if (attrs->status != ti_sdo_ipc_ListMP_CREATED) {
        *handlePtr = NULL;
        status = ListMP_E_NOTFOUND;
    }
    else {
        /* Create the object */
        *handlePtr = (ListMP_Handle)ti_sdo_ipc_ListMP_create(&params, &eb);
        if (*handlePtr == NULL) {
            status = ListMP_E_FAIL;
        }
        else {
            status = ListMP_S_SUCCESS;
        }
    }

    if (SharedRegion_isCacheEnabled(id)) {
        Cache_inv(attrs, sizeof(ti_sdo_ipc_ListMP_Attrs), Cache_Type_ALL, TRUE);
    }

    return (status);
}

/* Function to create a SyslinkMemMgr instance */
SyslinkMemMgr_TilerMemory_Handle SyslinkMemMgr_TilerMemory_create(Ptr params)
{
    Error_Block eb;

    Error_init (&eb);

    SyslinkMemMgr_TilerMemory_Object * obj = NULL;
    SyslinkMemMgr_TilerMemory_Params *instParams =
            (SyslinkMemMgr_TilerMemory_Params *)params;

    /* Allocate Memory for the object */
    obj = (SyslinkMemMgr_TilerMemory_Object *)Memory_alloc(NULL,
            sizeof(SyslinkMemMgr_TilerMemory_Object), 0, &eb);
    if (obj != NULL) {
        obj->heapHandle = instParams->heapHandle;
    }

    return (obj);
}

/**
 * /fn InitializeLedUserSwitch
 * /brief Initialize led D1 und USR SW1.
 * /return void.
 */
static void InitializeLedUserSwitch() {
	uint32_t strength;
	uint32_t pinType;
	Hwi_Params buttonHWIParams;
	Hwi_Handle buttonHwi;
	Error_Block eb;

	// enable port N
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPION);
	// LED2
	GPIOPinTypeGPIOOutput(GPIO_PORTN_BASE, GPIO_PIN_0);

	/* set pin gpio port to output */
	GPIOPinTypeGPIOOutput(GPIO_PORTN_BASE, GPIO_PIN_1);
	/*configure pad as standard pin with default output current*/
	GPIOPadConfigGet(GPIO_PORTN_BASE, GPIO_PIN_1, &strength, &pinType);
	GPIOPadConfigSet(GPIO_PORTN_BASE, GPIO_PIN_1, strength, GPIO_PIN_TYPE_STD);

	/* turn off led 1 */
	GPIOPinWrite(GPIO_PORTN_BASE, GPIO_PIN_1, 0);

	/* configure switch 1 with pull up as input on GPIO_PIN_0 as pull-up pin */
	GPIOPinTypeGPIOInput(GPIO_PORTJ_BASE, GPIO_PIN_0);
	GPIOPadConfigGet(GPIO_PORTJ_BASE, GPIO_PIN_0, &strength, &pinType);
	GPIOPadConfigSet(GPIO_PORTJ_BASE, GPIO_PIN_0, strength,
	GPIO_PIN_TYPE_STD_WPU);

	Error_init(&eb);
	Hwi_Params_init(&buttonHWIParams);
	buttonHWIParams.arg = 0;
	buttonHWIParams.enableInt = false;

	buttonHwi = Hwi_create(INT_GPIOJ_TM4C129, ButtonFunction, &buttonHWIParams,
			&eb);

	if (buttonHwi == NULL) {
		System_abort("Button Hardware interrupt create failed.");
	}
	Hwi_enableInterrupt(INT_GPIOJ_TM4C129);
	GPIOIntEnable(GPIO_PORTJ_BASE, GPIO_INT_PIN_0);

}

/**
 * \brief creates a mutex for inter-thread mutual exclusion for resource protection
 *
 *
 *
 *  \param[in] p_handle     Handle to a mutex control block
 *  \param[in] p_name       Name of mutex
 *
 * \return   e_SUCCESS on success, e_FAILURE on error
 *
 * \sa OS_mutex_delete, OS_mutex_lock, OS_mutex_unlock
 * \note
 *
 * \warning
 */
e_ret_status OS_mutex_create(handle p_handle, const void *const p_name)
{
    GateMutex_Params gateParams;
    GateMutex_Handle mtx_handle = {0};

    Error_Block eb;

    GateMutex_Params_init(&gateParams);
    Error_init(&eb);

    gateParams.instance->name = "Default_Name";

    mtx_handle = GateMutex_create(&gateParams, &eb);
    if (NULL == mtx_handle) {
        return e_FAILURE;
    }

    *(GateMutex_Handle *)p_handle = mtx_handle;
    return e_SUCCESS;
}

/*****************************************************************************
** Function name:  Task_Init      
**
** Descriptions: 
**     			   Create a task in a non-running state    
** parameters:     Task *pThis, Task name, Task function, Task Fn Arg, Stack Size, 
				 Task Priority.
** Returned value: None   
**
** Dependencies/Limitations/Side Effects/Design Notes:
**		Task is just created, and it will not run until Task_Start
**                
******************************************************************************/
BOOL Task_Init(Task *pThis, String sName, pfnTask pfn, VOID *pArg)
{

	//pThis->tskattrs = TSK_ATTRS;
	Error_Block eb;
	 Task_Params_init(&(pThis->tskattrs));
	 Error_init(&eb);

	//Setting of Task name
	if (sName != NULL)
	{
		memcpy(pThis->Name, sName, TASK_NAME_SIZE);
		//pThis->tskattrs.name = (String )&pThis->Name[0];
		pThis->tskattrs.instance->name = (String )&pThis->Name[0];
	}
	else
	{
		//pThis->tskattrs.name = "DUMMY";
		pThis->tskattrs.instance->name ="DUMMY";
	}

	//Setting of Task priority
	pThis->tskattrs.priority = STE_TASK_SUSPEND; // Suspend the priority.
	pThis->hPriority = STE_TASK_DEFAULT_PRI;
//	printf("task stacksize %d\n",pThis->tskattrs.stacksize);
	pThis->tskattrs.stackSize= 4*1024;
	//Setting argument
	//	pThis->tskattrs.arg0 =*(xdc_UArg*)pArg;
	pThis->tskattrs.arg0 =(xdc_UArg)pArg;
	
	//pThis->Handle = TSK_create((Fxn)pfn, &pThis->tskattrs, pArg);
	pThis->Handle = Task_create((Task_FuncPtr)pfn, &pThis->tskattrs,&eb);

    if (pThis->Handle == NULL)
    {
			printf("TASK: Failed create thread");
			return FALSE;
    }

	return TRUE;
}