/**
 * @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"

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

/******************************************************************************
 * Macro constant declarations
 ******************************************************************************/
#define ENDURANCE_TEST_BENCH_MAX_RETRY_CNT					5u
#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_WAIT_BEFORE_READ,
	TEST_BENCH_WAIT_FOR_FEEDBACK,
	TEST_BENCH_DELAY_BEFORE_NEXT
} SdlTestBenchState_en;
/******************************************************************************
 * Global variable declarations
 ******************************************************************************/
static uint8 currentNode_gu8 					= 0u;
static SdlTestBenchState_en testBenchState_en 	= TEST_BENCH_IDLE;
static uint8 targetPositions_gau8[ENDURANCE_TEST_BENCH_LSS_NODE_COUNT];

/******************************************************************************
 * 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++)
		    {
		        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++)
		    {
		        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++)
	            {
	                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)
	                {
	                    targetPositions_gau8[i_u8] = max_u8;  /* Fully open */
	                }
	                else
	                {
	                    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)
		{
			RET_T retVal_en = coSdoWrite((currentNode_gu8 + 1), ENDURANCE_TEST_BENCH_POSITION_SETPOINT_INDEX, ENDURANCE_TEST_BENCH_POSITION_SETPOINT_SUB_INDEX,
										 &targetPositions_gau8[currentNode_gu8], sizeof(targetPositions_gau8[currentNode_gu8]),
										 CO_FALSE, ENDURANCE_TEST_BENCH_TIMEOUT);

			if (retVal_en == RET_OK)
			{
				retries_u8 = 0u;
				HalSystemGetRunTimeMs(&startTime_u64);
				testBenchState_en = TEST_BENCH_WAIT_BEFORE_READ;
			}
			else if (retVal_en == RET_SERVICE_BUSY)
			{
				retries_u8++;
				if (retries_u8 < 5u)
				{
					printf("SDO Busy for node %d, retrying...\n", currentNode_gu8 + 1);
				}
				else
				{
					currentNode_gu8++;  /* Skip this node */
					testBenchState_en = TEST_BENCH_WRITE_REQUEST;
				}
			}
			else
			{
				currentNode_gu8++;  /* Skip this node */
				testBenchState_en = TEST_BENCH_WRITE_REQUEST;
			}
		}
		else
		{
			batchCompleted_u8 = 1u;
			testBenchState_en = TEST_BENCH_IDLE;
		}
	}
	break;

	case TEST_BENCH_WAIT_BEFORE_READ:
	{
	    HalSystemGetRunTimeMs(&currentTime_u64);

	    if ((currentTime_u64 - startTime_u64) >= 2000uLL) /* Wait for a predefined settling time */
	    {
	        testBenchState_en = TEST_BENCH_WAIT_FOR_FEEDBACK;
	    }
	}
	break;

	case TEST_BENCH_WAIT_FOR_FEEDBACK:
	{
		uint8 readPos_u8 	= 0u;
		RET_T retVal_en 	= coSdoRead((currentNode_gu8 + 1), ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_INDEX, ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_SUB_INDEX,
									&readPos_u8, sizeof(readPos_u8), CO_FALSE, ENDURANCE_TEST_BENCH_TIMEOUT);

		if (retVal_en == RET_OK)
		{
			if (readPos_u8 == targetPositions_gau8[currentNode_gu8])
			{
				printf("Node %d reached target position %d\n", currentNode_gu8 + 1, readPos_u8);
				currentNode_gu8++;
				retries_u8 = 0u; /* Reset retries */

				if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
				{
					HalSystemGetRunTimeMs(&startTime_u64);  /* Store time before sending the next command */
					testBenchState_en = TEST_BENCH_DELAY_BEFORE_NEXT;
				}
				else
				{
					batchCompleted_u8 = 1u;
					testBenchState_en = TEST_BENCH_IDLE;
				}
			}
		}
		else
		{
			retries_u8++;
			if (retries_u8 >= ENDURANCE_TEST_BENCH_MAX_RETRY_CNT)
			{
				currentNode_gu8++;

				if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
				{
					HalSystemGetRunTimeMs(&startTime_u64);
					testBenchState_en = TEST_BENCH_DELAY_BEFORE_NEXT;
				}
				else
				{
					batchCompleted_u8 = 1u;
					testBenchState_en = TEST_BENCH_IDLE;
				}
			}
		}
	}
	break;

	case TEST_BENCH_DELAY_BEFORE_NEXT:
	{
		HalSystemGetRunTimeMs(&currentTime_u64);

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

    }
}