/**
 * @file	   monitoring.c
 *
 * @copyright  Nehemis SARL reserves all rights even in the event of industrial
 *             property rights. We reserve all rights of disposal such as
 *             copying and passing on to third parties.
 *
 * @brief
 *
 */

/******************************************************************************
 * Include Header Files
 ******************************************************************************/
#include "enduranceTestBench.h"
#include "hal_system.h"
#include "stdlib.h"

/* CANopen includes */
#include <gen_define.h>
#include <co_canopen.h>

/******************************************************************************
 * Macro constant declarations
 ******************************************************************************/
#define ENDURANCE_TEST_BENCH_MAX_RETRY_CNT					3u
#define ENDURANCE_TEST_BENCH_TIMEOUT						1000u
#define ENDURANCE_TEST_BENCH_LSS_NODE_COUNT					20u
#define ENDURANCE_TEST_BENCH_POSITION_SETPOINT_INDEX		0x6002
#define ENDURANCE_TEST_BENCH_POSITION_SETPOINT_SUB_INDEX	0x0
#define ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_INDEX		0x6004
#define ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_SUB_INDEX	0x0
/******************************************************************************
 * Type Declarations
 ******************************************************************************/
typedef enum
{
    TEST_BENCH_STARTUP,
	TEST_BENCH_IDLE,
	TEST_BENCH_WRITE_REQUEST,
	TEST_BENCH_READ_FEEDBACK,
	TEST_BENCH_VERIFY_RESULTS,
	TEST_BENCH_DELAY_BEFORE_NEXT
} SdlTestBenchState_en;

typedef struct
{
    uint8 targetPositions_gau8[ENDURANCE_TEST_BENCH_LSS_NODE_COUNT];
    uint8 readPosition_gau8[ENDURANCE_TEST_BENCH_LSS_NODE_COUNT];
    uint8 status[ENDURANCE_TEST_BENCH_LSS_NODE_COUNT];
} TestBenchData_en;
/******************************************************************************
 * Global variable declarations
 ******************************************************************************/
static TestBenchData_en testBenchData_en;
static uint8 currentNode_gu8 					= 0u;
static SdlTestBenchState_en testBenchState_en 	= TEST_BENCH_IDLE;
static uint32 readPosition_gu32;
/******************************************************************************
 * Public Function Definitions
 ******************************************************************************/

void EnduranceTestBenchRun(void)
{
    static uint64 startTime_u64 	= 0uLL;
    static uint8 alternate_u8 		= 0u;
    static uint8 batchCompleted_u8 	= 1u;  /* Indicates if we need to populate a new batch */
    static uint8 retries_u8 		= 0u;
    uint64 currentTime_u64;

    HalSystemGetRunTimeMs(&currentTime_u64);

    if (startTime_u64 == 0uLL)
    {
        HalSystemGetRunTimeMs(&startTime_u64);
        testBenchState_en = TEST_BENCH_STARTUP;
    }

    switch (testBenchState_en)
    {
	case TEST_BENCH_STARTUP:
	{
		uint8 max_u8 = NMS_UINT8_MAX; /* Fully open (255) */
		uint8 min_u8 = 0u;            /* Fully closed (0) */

		if ((currentTime_u64 - startTime_u64) < 5000uLL)
		{
		    /* First 5 seconds: First 10 open, rest closed */
		    for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
		    {
		    	testBenchData_en.targetPositions_gau8[i_u8] = (i_u8 < 10u) ? max_u8 : min_u8;
		    }
		}
		else if ((currentTime_u64 - startTime_u64) < 10000uLL)
		{
		    /* Next 5 seconds: First 10 closed, rest open */
		    for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
		    {
		    	testBenchData_en.targetPositions_gau8[i_u8] = (i_u8 < 10) ? min_u8 : max_u8;
		    }
		}
		else
		{
		    /* After 10 seconds, move to endurance test */
		    batchCompleted_u8 	= 1u;
		    testBenchState_en 	= TEST_BENCH_IDLE;
		}

		currentNode_gu8 		= 0u;
		testBenchState_en 		= TEST_BENCH_WRITE_REQUEST;
	}
	break;

	case TEST_BENCH_IDLE:
	{
	    if (batchCompleted_u8)
	    {
	        batchCompleted_u8 = 0u;  					/* Lock batch update until all nodes confirm their positions */

	        uint8 max_u8 = NMS_UINT8_MAX;        		/* Fully open (255) */
	        uint8 min_u8 = 0u;                   		/* Fully closed (0) */
	        uint8 currentGroup_u8 = alternate_u8 % 2;  	/* Alternates between two patterns */

	        /* Check if it's a random cycle */
	        if ((alternate_u8 % 2) == 1)
	        {
	            /* Randomized cycle */
	            for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
	            {
	            	testBenchData_en.targetPositions_gau8[i_u8] = (uint8)(rand() % 256);  /* Assign random value between 0-255 */
	            }
	        }
	        else
	        {
	            /* Normal alternating open-close cycle */
	            for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
	            {
	                if (((i_u8 / 5) % 2) == currentGroup_u8)
	                {
	                	testBenchData_en.targetPositions_gau8[i_u8] = max_u8;  /* Fully open */
	                }
	                else
	                {
	                	testBenchData_en.targetPositions_gau8[i_u8] = min_u8;  /* Fully closed */
	                }
	            }
	        }

	        alternate_u8++;  /* Switch to the next cycle pattern */
	        currentNode_gu8 = 0u;
	        testBenchState_en = TEST_BENCH_WRITE_REQUEST;
	    }
	}
	break;


	case TEST_BENCH_WRITE_REQUEST:
	{
		if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
		{
			testBenchData_en.targetPositions_gau8[currentNode_gu8] = (uint8)(rand() % 256);

			RET_T retVal_en = coSdoWrite((1), ENDURANCE_TEST_BENCH_POSITION_SETPOINT_INDEX, ENDURANCE_TEST_BENCH_POSITION_SETPOINT_SUB_INDEX,
										 &testBenchData_en.targetPositions_gau8[currentNode_gu8], sizeof(uint8),
										 CO_FALSE, ENDURANCE_TEST_BENCH_TIMEOUT);

			if (retVal_en == RET_OK)
			{
				retries_u8 = 0u;
				currentNode_gu8++;
				HalSystemGetRunTimeMs(&startTime_u64);
			}
			else
			{
				retries_u8++;
				if (retries_u8 >= ENDURANCE_TEST_BENCH_MAX_RETRY_CNT)
				{
					currentNode_gu8++;  /* Skip this node */
				}
			}
		}
	}
	break;

	case TEST_BENCH_READ_FEEDBACK:
	{
		RET_T retVal_en = coSdoRead((1),ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_INDEX, ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_SUB_INDEX,
									(uint8*)&readPosition_gu32, sizeof(uint32), CO_FALSE, ENDURANCE_TEST_BENCH_TIMEOUT);

		if (retVal_en == RET_OK)
		{
			/* Wait for indication */
		}

		else if (retVal_en == RET_SERVICE_BUSY && retries_u8 < ENDURANCE_TEST_BENCH_MAX_RETRY_CNT)
		{
			retries_u8++;
		}
		else
		{
			currentNode_gu8++;
			retries_u8 = 0;
		}
	}
	break;

	case TEST_BENCH_VERIFY_RESULTS:
	{
		for (uint8 index_u8 = 0u; index_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; index_u8++)
		{
			if (testBenchData_en.targetPositions_gau8[index_u8] != testBenchData_en.readPosition_gau8[index_u8])
			{
				testBenchData_en.status[index_u8] = 1u;
			}
			else
			{
				testBenchData_en.status[index_u8] = 0u;
			}
		}
		testBenchState_en = TEST_BENCH_DELAY_BEFORE_NEXT;
	}
	break;


	case TEST_BENCH_DELAY_BEFORE_NEXT:
	{
		HalSystemGetRunTimeMs(&currentTime_u64);

		if ((currentTime_u64 - startTime_u64) >= ENDURANCE_TEST_BENCH_TIMEOUT)
		{
			currentNode_gu8 = 0u;
			testBenchState_en = TEST_BENCH_WRITE_REQUEST;
		}
	}
	break;

    }
}


/******************************************************************************
 * Callback Function
 ******************************************************************************/
void EnduranceTestBenchWriteInd(uint8 sdoNr_u8, uint16 index_u16, uint8 subIndex_u8, uint32 errorVal_u32)
{
    if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
    {
        testBenchState_en = TEST_BENCH_WRITE_REQUEST;
    }
    else
    {
        currentNode_gu8 = 0u; /* Reset before reading */
        testBenchState_en = TEST_BENCH_READ_FEEDBACK;
    }
}


void EnduranceTestBenchReadInd(uint8 sdoNr_u8, uint16 index_u16, uint8 subIndex_u8, uint32 size, uint32 errorVal_u32)
{
    if (sdoNr_u8 <= ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
    {
        testBenchData_en.readPosition_gau8[currentNode_gu8] = readPosition_gu32;

        if (currentNode_gu8 + 1 == ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
        {
            testBenchState_en = TEST_BENCH_VERIFY_RESULTS;
        }
        else
        {
            testBenchState_en = TEST_BENCH_READ_FEEDBACK;
        }
    }
}