/**
 * @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"
#include "nms_can.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		0x2002
#define ENDURANCE_TEST_BENCH_POSITION_SETPOINT_SUB_INDEX	0x0
#define ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_INDEX		0x2004
#define ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_SUB_INDEX	0x0

#define TEST_PATTERN_COUNT 3
#define VALVE_COUNT 20

static const uint8 testPatterns[TEST_PATTERN_COUNT][VALVE_COUNT] = {
    /* Pattern 0: Columns alternately closed or open */
    {
        0, 0, 0, 0, 255,     		// Row 0
        255, 252, 252, 252, 252,   	// Row 1
        0, 0, 0, 0, 252,     		// Row 2
        252, 252, 252, 252, 252     // Row 3
    },
    /* Pattern 1: Random placeholder (to be filled dynamically at runtime) */
    {
        0, 0, 0, 0, 0,       // Row 0
        0, 0, 0, 0, 0,       // Row 1
        0, 0, 0, 0, 0,       // Row 2
        0, 0, 0, 0, 0        // Row 3
    },
    /* Pattern 2: Inverted version of Pattern 0 */
    {
        252, 252, 252, 252, 252,   // Row 0
        0, 0, 0, 0, 252,           // Row 1
        252, 252, 252, 252, 252,   // Row 2
        0, 0, 0, 0, 252            // Row 3
    }
};

/******************************************************************************
 * Type Declarations
 ******************************************************************************/
typedef enum
{
    TEST_BENCH_STARTUP,
    TEST_BENCH_IDLE,
    TEST_BENCH_WRITE,
    TEST_BENCH_WRITE_WAIT,
    TEST_BENCH_CYCLE_COMPLETE
} 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 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 patternIndex_u8 = alternate_u8 % TEST_PATTERN_COUNT;

	    if (patternIndex_u8 == 1u)
	    {
	        /* Fill with random values */
	        for (uint8 i_u8 = 0u; i_u8 < VALVE_COUNT; i_u8++)
	        {
	            testBenchData_en.targetPositions_gau8[i_u8] = (uint8)(rand() % 256);
	        }
	    }
	    else
	    {
	        /* Copy from predefined pattern */
	        for (uint8 i_u8 = 0u; i_u8 < VALVE_COUNT; i_u8++)
	        {
	            testBenchData_en.targetPositions_gau8[i_u8] = testPatterns[patternIndex_u8][i_u8];
	        }
	    }

	    alternate_u8++;
	    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)
			{
				NmsCanPutObj_u8(0x2004, currentNode_gu8 + 1, testBenchData_en.targetPositions_gau8[currentNode_gu8]);
				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_CYCLE_COMPLETE;
        }
        break;
    }

    case TEST_BENCH_WRITE_WAIT:
    {
        /* Wait 100ms between writes */
        if ((currentTime_u64 - writeTime_u64) >= 200)
        {
            /* Move to next node write */
            currentNode_gu8++;
            testBenchState_en = TEST_BENCH_WRITE;
        }
        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);
}