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

/* CANopen includes */
#include <gen_define.h>
#include <co_canopen.h>
#include "hal_system.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,           /* Sets new data for the next cycle */
    TEST_BENCH_WRITE,
    TEST_BENCH_WRITE_WAIT,
    TEST_BENCH_READ,
    TEST_BENCH_READ_WAIT,
    TEST_BENCH_VERIFY,
    TEST_BENCH_CYCLE_COMPLETE  /* End of cycle, reset for next iteration */
} SdlTestBenchState_en;

typedef enum
{
    TEST_BENCH_DATA_VERIF_DEFAULT,
	TEST_BENCH_DATA_NODE_SKIPPED,
    TEST_BENCH_DATA_VERIF_SUCCESS,
    TEST_BENCH_DATA_VERIF_FAILURE,
} TestBenchStatus_en;

typedef struct
{
    uint8 targetPositions_gau8[ENDURANCE_TEST_BENCH_LSS_NODE_COUNT];
    uint8 readPosition_gau8[ENDURANCE_TEST_BENCH_LSS_NODE_COUNT];
    TestBenchStatus_en status_en[ENDURANCE_TEST_BENCH_LSS_NODE_COUNT];
} TestBenchData_en;


/******************************************************************************
 * Global variable declarations
 ******************************************************************************/
static uint8 currentNode_gu8 	= 0u;
static uint32 readPosition_gu32 = 0u;

static TestBenchData_en testBenchData_en;
static SdlTestBenchState_en testBenchState_en = TEST_BENCH_IDLE;

static uint64 readExecutedTime_u64;
static uint64 writeTime_u64;
/******************************************************************************
 * Public Function Definitions
 ******************************************************************************/
void EnduranceTestBenchRun(bool *testBenchStarted_pb)
{
    static uint64 startTime_u64 	= 0uLL;
    static uint8 alternate_u8 		= 0u;
    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 */
		    testBenchState_en 	= TEST_BENCH_IDLE;
		}

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

	case TEST_BENCH_IDLE:
	{
		*testBenchStarted_pb = true;
		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
				{
					if (i_u8 % 5u == 4u) /* Last node of the group */
					{
						testBenchData_en.targetPositions_gau8[i_u8] = max_u8; /* 255 for the last node */
					}
					else
					{
						testBenchData_en.targetPositions_gau8[i_u8] = min_u8; /* 0 for the other nodes */
					}
				}
			}
		}
		alternate_u8++;  /* Switch to the next cycle pattern */
		currentNode_gu8 = 0u;
		testBenchState_en = TEST_BENCH_WRITE;
	}
	break;

	/* -------------------- Write -------------------- */
    case TEST_BENCH_WRITE:
    {
        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,
				&testBenchData_en.targetPositions_gau8[currentNode_gu8],
				sizeof(uint8),
				CO_FALSE,
				ENDURANCE_TEST_BENCH_TIMEOUT
			);

			if (retVal_en == RET_OK)
			{
				HalSystemGetRunTimeMs(&writeTime_u64);
				retries_u8 = 0u;
				testBenchState_en = TEST_BENCH_WRITE_WAIT;
			}
			else
			{
				retries_u8++;
				if (retries_u8 >= ENDURANCE_TEST_BENCH_MAX_RETRY_CNT)
				{
					testBenchData_en.status_en[currentNode_gu8] = TEST_BENCH_DATA_NODE_SKIPPED;

					/* Mark node as skipped and move on */
					currentNode_gu8++;
					retries_u8 = 0u;
					testBenchState_en = TEST_BENCH_WRITE;
				}
			}
		}

        else
        {
            /* Finished writing to all nodes */
            currentNode_gu8 = 0u;   /* Reset index for read phase */
            testBenchState_en = TEST_BENCH_READ;
        }
        break;
    }

    case TEST_BENCH_WRITE_WAIT:
    {
        /* Wait 100ms between writes */
        if ((currentTime_u64 - writeTime_u64) >= 500)
        {
            /* Move to next node write */
            currentNode_gu8++;
            testBenchState_en = TEST_BENCH_WRITE;
        }
        break;
    }

    /* -------------------- Read -------------------- */
    case TEST_BENCH_READ:
    {
        if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
        {
            RET_T retVal_en = coSdoRead(
                (currentNode_gu8 + 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)
            {
                retries_u8 = 0u;
                /* The read indication callback is assumed to update
                 * testBenchData_en.readPosition_gau8[currentNode_gu8] and
                 * then we simply continue to next node after a small delay.
                 */
                HalSystemGetRunTimeMs(&readExecutedTime_u64);
                testBenchData_en.readPosition_gau8[currentNode_gu8] = (uint8)readPosition_gu32;
                testBenchState_en = TEST_BENCH_READ_WAIT;
            }
            else if (retVal_en == RET_SERVICE_BUSY && retries_u8 < ENDURANCE_TEST_BENCH_MAX_RETRY_CNT)
            {
                retries_u8++;
            }
            else
            {
                /* If read fails, mark as skipped and move on */
                testBenchData_en.status_en[currentNode_gu8] = TEST_BENCH_DATA_NODE_SKIPPED;
                currentNode_gu8++;
                retries_u8 = 0u;
            }
        }
        else
        {
            /* Finished reading all nodes */
            currentNode_gu8 = 0u;  /* Reset for verification phase */
            testBenchState_en = TEST_BENCH_VERIFY;
        }
        break;
    }

    case TEST_BENCH_READ_WAIT:
    {
        /* Wait 100ms between node reads */
        if ((currentTime_u64 - readExecutedTime_u64) >= 10)
        {
            currentNode_gu8++;
            testBenchState_en = TEST_BENCH_READ;
        }
        break;
    }

    /* -------------------- Batch Verify -------------------- */
    case TEST_BENCH_VERIFY:
    {
        if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
        {
            /* Process only if the node was not already skipped */
            if (testBenchData_en.status_en[currentNode_gu8] != TEST_BENCH_DATA_NODE_SKIPPED)
            {
                if (abs(testBenchData_en.targetPositions_gau8[currentNode_gu8] -
                        testBenchData_en.readPosition_gau8[currentNode_gu8]) <= 2u)     /* Accepted difference between read and write for successful operation */
                {
                    testBenchData_en.status_en[currentNode_gu8] = TEST_BENCH_DATA_VERIF_SUCCESS;
                }
                else
                {
                    testBenchData_en.status_en[currentNode_gu8] = TEST_BENCH_DATA_VERIF_FAILURE;
                }
            }
            /* Move to the next node in any case */
            currentNode_gu8++;
            testBenchState_en = TEST_BENCH_VERIFY;
        }
        else
        {
            /* All nodes verified; complete cycle */
            testBenchState_en = TEST_BENCH_CYCLE_COMPLETE;
        }
        break;
    }

    /* -------------------- Cycle Complete -------------------- */
    case TEST_BENCH_CYCLE_COMPLETE:
    {
        /* Optionally, can log or process the batch results here */
        /* Reset all node data for the next cycle */
        for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
        {
            testBenchData_en.targetPositions_gau8[i_u8] = 0u;
            testBenchData_en.readPosition_gau8[i_u8]    = 0u;
            testBenchData_en.status_en[i_u8]            = TEST_BENCH_DATA_VERIF_DEFAULT;
        }

        /* Reset everything and Start next cycle */
        currentNode_gu8 	= 0u;
        retries_u8			= 0u;
        testBenchState_en 	= TEST_BENCH_IDLE;
        break;
    }

    default:
        break;
    }
}

/******************************************************************************
 * Callback Functions
 ******************************************************************************/
void EnduranceTestBenchWriteInd(uint8 sdoNr_u8, uint16 index_u16, uint8 subIndex_u8, uint32 errorVal_u32)
{
	HalSystemGetRunTimeMs(&writeTime_u64);
}

void EnduranceTestBenchReadInd(uint8 sdoNr_u8, uint16 index_u16, uint8 subIndex_u8, uint32 size, uint32 errorVal_u32)
{
	HalSystemGetRunTimeMs(&readExecutedTime_u64);
}
