/*
 * SHMCyclicDataSyncTasks.c
 *
 *  Created on: Jan 5, 2025
 *      Author: tummalanr
 */





#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <stdarg.h>


#include <industrial_comms/ethercat_slave/icss_fwhal/tiescbsp.h>

#include <inttypes.h>
#include <kernel/dpl/DebugP.h>
#include <kernel/dpl/SemaphoreP.h>
#include <drivers/spinlock.h>
#include <drivers/ipc_notify.h>
#include <kernel/dpl/EventP.h>
#include <kernel/dpl/ClockP.h>
#include <kernel/dpl/TaskP.h>

#include "FreeRTOS.h"
#include "task.h"


#include    "IPCDataSyncGlobal.h"
#include    "AppSharedMemory.h"


#include    "SystemStsConfig.h"
#include    "SharedMemoryExterns.h"
#include    "IPCDataSyncExterns.h"




extern uint16_t    Emcymsgcnt;
extern uint16_t      dop_frameindx;

#define             ACyclic_DIModule_RDEvnt_BIT             (0x000001u)
#define             ACyclic_IOModule_UpdateEvnt_BIT         (0x000100u)
#define             ACyclic_PZxModule_UpdateEvnt_BIT        (0x000080u)
#define             ACyclic_DIModule_WREvnt_BIT             (0x000001u)
#define             ACyclic_ISEMBase_WREvnt_BIT             (0x000008u)

uint8_t     tempemcycheck = 0u;
uint8_t vavbemcycheck = 0u;
//Shared Memory Declarations for IO Modules*******************************************************************************************

/*!
 *  <!-- Description: -->
 *
 *  \brief
 *
 *   IO Modules Acquire Shared Memory Lock for Processing IPC Cyclic Messages/Data
 *
 *  <!-- Parameters and return values: -->
 *  \param[in] :None
 *  \return    :
 *              AppRET_T  : AppRET_OK on Success or Error
 */
static AppRET_T io_sharemem_cycliclock(void)
{
    int32_t  status;
    int32_t sempstatus;
    uint32_t    timeout = SPINLOCK_GET_TIMEOUT;

    /* Take local mutex to protect against multi-thread from same core */
    sempstatus = SemaphoreP_pend(&Cyclic_IOModules_mutexObj, 10u);       //SystemP_WAIT_FOREVER   10000
    if(sempstatus != SystemP_SUCCESS) {
           return AppGetIPCSHMLockErr;
    }

    // Spin till lock is acquired or timeout
    while(1u)
    {
      status = Spinlock_lock(spinlockBaseAddr, SPINLOCKNO_CYCLIC_IOMODULES);
      if(status == SPINLOCK_LOCK_STATUS_FREE) {
          break;  /* Free and taken */
      }

      timeout--;
      if(timeout == 0) {
              break;
      }

      //vTaskDelay( 1u );
      __asm__ volatile ("nop"); /* Prevent compiler from optimizing away the loop.*/
      //OSAL_SCHED_yield();
    }

    if(timeout == 0) {
      return AppGetIPCSHMLockTimeOutErr;
    }

    return AppRET_OK;
}

/*!
 *  <!-- Description: -->
 *
 *  \brief
 *
 *   IO Modules Release Cyclic Shared Memory Lock
 *
 *  <!-- Parameters and return values: -->
 *  \param[in] : None
 *  \return    : None
 */
static void io_sharemem_cyclicunlock(void)
{
    // Free the aquired lock and release local mutex
    Spinlock_unlock(spinlockBaseAddr, SPINLOCKNO_CYCLIC_IOMODULES);
    SemaphoreP_post(&Cyclic_IOModules_mutexObj);

    return;
}


/*!
 *  <!-- Description: -->
 *
 *  \brief
 *
 *   PZx Acquire Shared Memory Lock for Processing IPC Cyclic Messages/Data
 *
 *  <!-- Parameters and return values: -->
 *  \param[in] :None
 *  \return    :
 *              AppRET_T  : AppRET_OK on Success or Error
 */
static AppRET_T pzx_sharemem_cycliclock(void)
{
    int32_t  status;
    int32_t sempstatus;
    uint32_t    timeout = SPINLOCK_GET_TIMEOUT;

    /* Take local mutex to protect against multi-thread from same core */
    sempstatus = SemaphoreP_pend(&Cyclic_PZx_mutexObj, 10u);       //SystemP_WAIT_FOREVER   10000
    if(sempstatus != SystemP_SUCCESS) {
               return AppGetSHMLockErr;
    }

    // Spin till lock is acquired or timeout
    while(1u)
    {
      status = Spinlock_lock(spinlockBaseAddr, SPINLOCKNO_CYCLIC_PZx);
      if(status == SPINLOCK_LOCK_STATUS_FREE) {
          break;  /* Free and taken */
      }

      timeout--;
      if(timeout == 0) {
              break;
      }

      //vTaskDelay( 1u );
      __asm__ volatile ("nop"); /* Prevent compiler from optimizing away the loop.*/
    }
    if(timeout == 0) {
      return AppGetSHMLockTimeOutErr;
    }

    return AppRET_OK;
}

/*!
 *  <!-- Description: -->
 *
 *  \brief
 *
 *   PZ1 Release Shared Memory Lock
 *
 *  <!-- Parameters and return values: -->
 *  \param[in] : None
 *  \return    : None
 */
static void pzx_sharemem_cyclicunlock(void)
{
    // Free the aquired lock and release local mutex
    Spinlock_unlock(spinlockBaseAddr, SPINLOCKNO_CYCLIC_PZx);
    SemaphoreP_post(&Cyclic_PZx_mutexObj);

    return;
}



/*!
 *  <!-- Description: -->
 *
 *  \brief
 *
 *   IO Modules Acquire Shared Memory Lock for Processing IPC ACyclic Messages/Data
 *
 *  <!-- Parameters and return values: -->
 *  \param[in] :None
 *  \return    :
 *              AppRET_T  : AppRET_OK on Success or Error
 */
static AppRET_T io_sharemem_acycliclock(void)
{
    int32_t  status;
    int32_t sempstatus;
    uint32_t    timeout = SPINLOCK_GET_TIMEOUT;   //30u;

    /* Take local mutex to protect against multi-thread from same core */
    sempstatus = SemaphoreP_pend(&ACyclic_IOModules_mutexObj, 10u);
    if(sempstatus != SystemP_SUCCESS) {
           return AppGetIPCSHMLockErr;
    }

    // Spin till lock is acquired or timeout
    for(;;)
    {
      status = Spinlock_lock(spinlockBaseAddr, ACYCLIC_SPINLOCKNO_IOMODULES);
      if(status == SPINLOCK_LOCK_STATUS_FREE) {
          break;  /* Free and taken */
      }

      timeout--;
      if(timeout == 0) {
              break;
      }

      //vTaskDelay( 1u );
      __asm__ volatile ("nop"); /* Prevent compiler from optimizing away the loop.*/
    }
    if(timeout == 0) {
      return AppGetIPCSHMLockTimeOutErr;
    }

    return AppRET_OK;
}

/*!
 *  <!-- Description: -->
 *
 *  \brief
 *
 *   IO Modules Release Cyclic Shared Memory Lock
 *
 *  <!-- Parameters and return values: -->
 *  \param[in] : None
 *  \return    : None
 */
static void io_sharemem_acyclicunlock(void)
{
    // Free the aquired lock and release local mutex
    Spinlock_unlock(spinlockBaseAddr, ACYCLIC_SPINLOCKNO_IOMODULES);
    SemaphoreP_post(&ACyclic_IOModules_mutexObj);

    return;
}

void APP_SHM_IOACyclicWRTask(void* pArgs_p)
{
    //AppRET_T sts = 0;
    int32_t     status;
    uint32_t    eventBits;



        status = EventP_clearBits(&gIOModuleAcyclicWriteEvent, (ACyclic_DIModule_WREvnt_BIT));//| EVENT_BIT_PONG
        DebugP_assert(SystemP_SUCCESS == status);


        for(;;)
        {

            status = EventP_waitBits(&gIOModuleAcyclicWriteEvent, ( ACyclic_DIModule_WREvnt_BIT), 0u, 0u, SystemP_WAIT_FOREVER, &eventBits);
            if(status == SystemP_SUCCESS )
            {

                #ifdef Debug_VerboseSHM2
                    DebugP_log("[R5F0_0] %s Event Triggered  eventBits[%#x] %" PRId64 " ms\r\n",__func__,eventBits,ClockP_getTimeUsec()/1000);
                    started = ClockP_getTimeUsec()/1000;
                #endif

                //uint8_t iomodulecnt = app_sys_discvrystsinfo.DiscoveredIOModuleCnt;

                //sts = io_sharemem_acycliclock();
                //if(sts == AppRET_OK)
                //{
                    uint32_t msgValue = 0U;
                    if( eventBits & ACyclic_DIModule_WREvnt_BIT)
                    {
                        status = EventP_clearBits(&gIOModuleAcyclicWriteEvent, ACyclic_DIModule_WREvnt_BIT);

                        msgValue = 0x2000u;
                    }

                    //io_sharemem_acyclicunlock();

                    if(msgValue) {
                        IpcNotify_sendMsg(CSL_CORE_ID_R5FSS1_0, IOModules_IPC_ClientId, msgValue, 1);  //notify the CIP core response is ready
                    }

                //}// if(sts == AppRET_OK)

            }// if(sempsts == SystemP_SUCCESS)

            #ifdef Debug_VerboseSHM2
                DebugP_log("[CIP] %s  started : eventBits[%#x] [%" PRId64 " ms] [%" PRId64 " ms]\r\n",__func__,eventBits,started,ClockP_getTimeUsec()/1000);
            #endif
        }
}


void APP_SHM_IOACyclicUpdateTask(void* pArgs_p)
{
    AppRET_T sts = 0;
    int32_t     status;
    uint32_t    eventBits;



    /* Clearing the ACyclic_IOModule_UpdateEvnt_BIT bits */
    status = EventP_clearBits(&gACyclic_IOModule_SynchEvent,  ACyclic_IOModule_UpdateEvnt_BIT );
    DebugP_assert(SystemP_SUCCESS == status);

    for(;;)
    {
        status = EventP_waitBits(&gACyclic_IOModule_SynchEvent,  ACyclic_IOModule_UpdateEvnt_BIT, 0u, 0u, SystemP_WAIT_FOREVER , &eventBits);

        //DebugP_log(" %s status[%#x] eventBits[%#x]riggered [%" PRId64 " ms]\r\n",__func__,status,eventBits,ClockP_getTimeUsec()/1000);
        if(status == SystemP_SUCCESS )
        {
            if( eventBits & ACyclic_IOModule_UpdateEvnt_BIT)
             {
                 #ifdef Debug_VerboseSHM2
                     DebugP_log("[R5F0_0] %s ACyclic_IOModule_UpdateEvnt_BIT Triggered  %" PRId64 " ms\r\n",__func__,ClockP_getTimeUsec()/1000);
                     //started = ClockP_getTimeUsec()/1000;
                 #endif

                 uint32_t    ioacyclicmsgval = 0u;
                 uint8_t i = 0;
                 for( i = 0; i < SHM_Max_ReqIds; i++)
                 {
                     ioacyclicmsgval = IOAcyclicReqIDs[i];
                     if(ioacyclicmsgval)  {
                         IOAcyclicReqIDs[i] = 0u;
                         break;
                     }
                 }
                 //Some more pending
                 for( i = 0; i < SHM_Max_ReqIds; i++) {
                     if(IOAcyclicReqIDs[i] != 0u) {
                         break;
                     }
                 }

                 #ifdef Debug_VerboseSHM
                     DebugP_log("[R5F0_0] %s SHM_Max_ReqIds[ i:%d /%d] ioacyclicmsgval[%#x]\r\n",__func__,i,SHM_Max_ReqIds,ioacyclicmsgval);
                 #endif

                 if(i == SHM_Max_ReqIds) {
                     status = EventP_clearBits(&gACyclic_IOModule_SynchEvent, (ACyclic_IOModule_UpdateEvnt_BIT ));
                     DebugP_assert(SystemP_SUCCESS == status);
                     //continue;
                 }

             }//if( eventBits & ACyclic_IOModule_UpdateEvnt_BIT)

        }//if(status == SystemP_SUCCESS )

    }// for(;;)
}


/*!
 *  <!-- Description: -->
 *
 *  \brief
 *
 *   PZx Acquire Shared Memory Lock for Processing IPC ACyclic Messages/Data
 *
 *  <!-- Parameters and return values: -->
 *  \param[in] :None
 *  \return    :
 *              AppRET_T  : AppRET_OK on Success or Error
 */
AppRET_T pzx_sharemem_acycliclock(void)
{
    int32_t  status;
    int32_t sempstatus;
    uint32_t    timeout = SPINLOCK_GET_TIMEOUT;

    /* Take local mutex to protect against multi-thread from same core */
    sempstatus = SemaphoreP_pend(&ACyclic_PZx_mutexObj, 10u);       //SystemP_WAIT_FOREVER   10000
    if(sempstatus != SystemP_SUCCESS) {
               return AppGetSHMLockErr;
    }

    // Spin till lock is acquired or timeout
    while(1u)
    {
      status = Spinlock_lock(spinlockBaseAddr, SPINLOCKNO_ACYCLIC_PZx);
      if(status == SPINLOCK_LOCK_STATUS_FREE) {
          break;  /* Free and taken */
      }

      timeout--;
      if(timeout == 0) {
              break;
      }

      //vTaskDelay( 1u );
      __asm__ volatile ("nop"); /* Prevent compiler from optimizing away the loop.*/
    }
    if(timeout == 0) {
      return AppGetSHMLockTimeOutErr;
    }

    return AppRET_OK;
}

/*!
 *  <!-- Description: -->
 *
 *  \brief
 *
 *   PZx Release Shared Memory Lock
 *
 *  <!-- Parameters and return values: -->
 *  \param[in] : None
 *  \return    : None
 */
void pzx_sharemem_acyclicunlock(void)
{
    // Free the aquired lock and release local mutex
    Spinlock_unlock(spinlockBaseAddr, SPINLOCKNO_ACYCLIC_PZx);
    SemaphoreP_post(&ACyclic_PZx_mutexObj);

    return;
}



void APP_SHM_PZxACyclicUpdateTask(void* pArgs_p)
{
    AppRET_T sts = 0;
    int32_t     status;
    uint32_t    eventBits;
    uint32_t    diagpos = 0u;
    uint8_t dvpos = 0u;
    uint8_t diagcnt = 0u;
    uint8_t dvfinish =0u;


    //Wait for Discovery complete in CAN

    /* Clearing the ACyclic_IOModule_UpdateEvnt_BIT bits */
    status = EventP_clearBits(&gACyclic_PZxModule_SynchEvent, (ACyclic_PZxModule_UpdateEvnt_BIT ) );
    DebugP_assert(SystemP_SUCCESS == status);

    for(;;)
    {
        status = EventP_waitBits(&gACyclic_PZxModule_SynchEvent, (ACyclic_PZxModule_UpdateEvnt_BIT ), 0u, 0u, SystemP_WAIT_FOREVER, &eventBits);
        if(status != SystemP_SUCCESS ) {
            continue;
        }

            if( eventBits & ACyclic_PZxModule_UpdateEvnt_BIT)
            {
                #ifdef Debug_VerboseSHM2
                    DebugP_log("[R5F0_0] %s ACyclic_PZxModule_UpdateEvnt_BIT Triggered  %" PRId64 " ms\r\n",__func__,ClockP_getTimeUsec()/1000);
                    //started = ClockP_getTimeUsec()/1000;
                #endif
                uint32_t    pzxacyclicmsgval = 0u;
                uint8_t i = 0;
                for( i = 0; i < SHM_Max_ReqIds; i++)
                {
                    pzxacyclicmsgval = PZxAcyclicReqIDs[i];
                    if(pzxacyclicmsgval)
                    {
                        PZxAcyclicReqIDs[i] = 0u;
                        break;
                    }
                }
                #ifdef Debug_VerboseSHM
                    DebugP_log("[R5F0_0] %s SHM_Max_ReqIds[ i:%d /%d] pzno[%d]  %" PRId64 " ms\r\n",__func__,i,SHM_Max_ReqIds,(uint8_t)pzxacyclicmsgval,ClockP_getTimeUsec()/1000);
                    //started = ClockP_getTimeUsec()/1000;
                #endif

                if(i == SHM_Max_ReqIds)
                {
                    status = EventP_clearBits(&gACyclic_PZxModule_SynchEvent, (ACyclic_PZxModule_UpdateEvnt_BIT ));
                    DebugP_assert(SystemP_SUCCESS == status);
                    continue;
                }


            }//if( eventBits & ACyclic_IOModule_UpdateEvnt_BIT)

    }// for(;;)
}


void APP_SHM_PZxACyclicWRTask(void* pArgs_p)
{
    AppRET_T sts = 0;
    //uint32_t   msgValue = 0;
    int32_t     status;
    uint32_t    eventBits;



      status = EventP_clearBits(&gPZxModuleAcyclicWriteEvent, ACyclic_ISEMBase_WREvnt_BIT);
      DebugP_assert(SystemP_SUCCESS == status);

    for(;;)
    {

        status = EventP_waitBits(&gPZxModuleAcyclicWriteEvent, (ACyclic_ISEMBase_WREvnt_BIT), 0u, 0u, SystemP_WAIT_FOREVER, &eventBits);
        if(status == SystemP_SUCCESS )
        {

            #ifdef Debug_VerboseSHM2
                DebugP_log("[R5F0_0] %s Event Triggered  eventBits[%#x] %" PRId64 " ms\r\n",__func__,eventBits,ClockP_getTimeUsec()/1000);
                started = ClockP_getTimeUsec()/1000;
            #endif

            //sts = pzx_sharemem_acycliclock();
            //if(sts == AppRET_OK)
            //{
                uint32_t msgValue = 0U;
                if( eventBits & ACyclic_ISEMBase_WREvnt_BIT)
                {
                    status = EventP_clearBits(&gPZxModuleAcyclicWriteEvent, ACyclic_ISEMBase_WREvnt_BIT);

                    msgValue = 0x1000u;
                }
               // pzx_sharemem_acyclicunlock();


                if(msgValue)
                  IpcNotify_sendMsg(CSL_CORE_ID_R5FSS1_0, PZxModules_IPC_ClientId, msgValue, 1);  //notify the CIP core response is ready

            //}// if(sts == AppRET_OK)

        }// if(sempsts == SystemP_SUCCESS)

        #ifdef Debug_VerboseSHM2
            DebugP_log("[CIP] %s  started : eventBits[%#x] [%" PRId64 " ms] [%" PRId64 " ms]\r\n",__func__,eventBits,started,ClockP_getTimeUsec()/1000);
        #endif
    }
}
