///**
// * @file	   nms_can.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       Source code for CANOpen files abstracted for furthur use in the project.
// *
// */
//
///******************************************************************************
// * Include Header Files
// ******************************************************************************/
#include "nms_can.h"
#include "enduranceTestBench.h"

/* CANopen includes */
#include <gen_define.h>
#include <co_canopen.h>
#include "co_sdo.h"
#include "co_pdo.h"
#include "co_odaccess.h"
//
///******************************************************************************
// * Global Variable Declaration
// ******************************************************************************/
//
///******************************************************************************
// * Macro constant declarations
// ******************************************************************************/
#define NMS_CAN_CANOPEN_SLAVE_LINE				0u
#define NMS_CAN_CANOPEN_MASTER_LINE				1u

#define NMS_CAN_CANOPEN_BITRATE					250u

#define NMS_CAN_PU_MASTER_NODE_ID				1u
#define NMS_CAN_PU_SLAVE_NODE_ID				3u

#define NMS_CAN_LSS_TIMEOUT						20u		/* Defined in stack examples */
#define NMS_CAN_LSS_CONFIG_TIMEOUT				1000u
#define NMS_CAN_LSS_CONFIG_INTERVAL				4u
#define NMS_CAN_APPL_TIMER_TIME					250000uL

#define NMS_CAN_LSS_NODE_COUNT					10u
#define NMS_CAN_LSS_NODE_HB_MS					1000uL
#define NMS_CAN_LSS_NODE_STATE_RESET_INTERVAL 	3u

#define NMS_CAN_SDO_TRANSMIT					0x580u
#define NMS_CAN_SDO_RECEIVE						0x600u
#define NMS_CAN_SDO_PARAM_INDEX					0x1280u
#define NMS_LSS_NODE_COUNT						20u
///******************************************************************************
// * Type Declarations
// ******************************************************************************/
//typedef struct
//{
//    uint32 vendorId_u32;
//    uint32 productId_u32;
//    uint32 versionNbr_u32;
//    uint32 serialNbr_u32;
//    uint8 nodeId_u8;
//} SdlLssNodeInfo_t;
///******************************************************************************
// * Global Declarations
// ******************************************************************************/
//SdlLssNodeInfo_t 	var_gst;
//static CO_TIMER_T 	monitorTimer_gst;							/**< application timer */
//static bool 		monitorSleep_gb = CO_FALSE;				/**< sleep flag */
//static bool   		masterStarted_gb;						/**< master started flag */
//static const SdlLssNodeInfo_t nodeLookupTable_gast[NMS_LSS_NODE_COUNT] =
//{
//	    {0x319, 0x4d2,   0x1,   0x01, 0x5} ,
//	    {0x319, 0x4d2,   0x1,   0x02, 0x6} ,
//	    {0x319, 0x4d2,   0x1,   0x03, 0x7} ,
//	    {0x319, 0x4d2,   0x1,   0x04, 0x8} ,
//	    {0x319, 0x4d2,   0x1,   0x05, 0x9} ,
//	    {0x319, 0x4d2,   0x1,   0x06, 0xA} ,
//	    {0x319, 0x4d2,   0x1,   0x07, 0xB} ,
//	    {0x319, 0x4d2,   0x1,   0x08, 0xC} ,
//	    {0x319, 0x4d2,   0x1,   0x09, 0xD} ,
//	    {0x319, 0x4d2,   0x1,   0x0A, 0xE} ,
//	    {0x319, 0x4d2,   0x1,   0x0B, 0xF} ,
//	    {0x319, 0x4d2,   0x1,   0x0C, 0x10},
//	    {0x319, 0x4d2,   0x1,   0x0D, 0x11},
//	    {0x319, 0x4d2,   0x1,   0x0E, 0x12},
//	    {0x319, 0x4d2,   0x1,   0x0F, 0x13},
//	    {0x319, 0x4d2,   0x1,   0x10, 0x14},
//	    {0x319, 0x4d2,   0x1,   0x11, 0x15},
//	    {0x319, 0x4d2,   0x1,   0x12, 0x16},
//	    {0x319, 0x4d2,   0x1,   0x13, 0x17},
//	    {0x319, 0x4d2,   0x1,   0x14, 0x18}
//};
//static CO_NMT_STATE_T nodeNMTState_gaen[NMS_LSS_NODE_COUNT];
///******************************************************************************
// * Private Function Definition
// ******************************************************************************/
//
//static RET_T NmsCanOverwriteLoadIndication(uint8 index_u8);
//static void NmsCanLssIndCallback( CO_LSS_MASTER_SERVICE_T service_en, uint16 errorCode_u16, uint8 errorSpec_u8, uint32 *pIdentity_pu32);
//static void NmsCanLssHbMonitorCallback(uint8 nodeID_u8, CO_ERRCTRL_T state_en, CO_NMT_STATE_T nmtState_en);
//static void NmsCanLssResetNodesCallback(void *pData_pv);									/**< callback of reset handler */
//static uint8 NmsCanLssGetNodeIndex(uint8 nodeId_u8);
//static void NmsCanLssToggleMonitorFlag(void *pData_pv);
//static void NmsCanLssNodeHandlerRun(void);
//static uint32 NmsCanLssConfigureNode(uint8 arrIndex_u8);
//static RET_T NmsCanLssSetSdoCobID(uint8 sdoNr_u8, uint8 nodeId_u8);
///* Supress warnings for implicit declaration.
// * Need this function for overwriting the manual COB ids
// * for the TPDO. */
//extern void userOverwriteCobIdSettings(void);
///******************************************************************************
// * Extern Function Declarations
// ******************************************************************************/
//void NmsCanInit()
//{
//	codrvHardwareInit();
//	codrvCanInit(NMS_CANOPEN_BITRATE);
//	codrvTimerSetup(CO_TIMER_INTERVAL);
//
//	/* CANopen Initialization */
//	coCanOpenStackInit(NmsCanOverwriteLoadIndication);
//	coEventRegister_LSS_MASTER(NmsCanLssIndCallback);
//	coEventRegister_ERRCTRL(NmsCanLssHbMonitorCallback);
//
//	/* Register SDO event handlers */
//	coEventRegister_SDO_CLIENT_READ(EnduranceTestBenchReadInd);
//    coEventRegister_SDO_CLIENT_WRITE(EnduranceTestBenchWriteInd);
//
//	/* enable CAN communication */
//	codrvCanEnable();
//
//	coTimerStart(&monitorTimer_gst, NMS_CAN_APPL_TIMER_TIME, NmsCanLssToggleMonitorFlag, NULL, CO_TIMER_ATTR_ROUNDUP_CYCLIC);
//	coLssIdentifyNonConfiguredSlaves(NMS_CAN_LSS_CONFIG_TIMEOUT, NMS_CAN_LSS_CONFIG_INTERVAL);
//
//	/* node == 0, the NMT request is sent to all nodes. */
//	coNmtStateReq(NMS_CAN_PU_MASTER_NODE_ID, CO_NMT_STATE_OPERATIONAL, CO_TRUE);
//}
//
//
//void NmsCanRun(void)
//{
//	coCommTask();
//
//	/* LSS main runner */
//	NmsCanLssNodeHandlerRun();
//}
//
//
uint32 NmsCanGetObj_u8(uint16 index_u16, uint8 subIndex_u8, uint8 *pObj_pu8)
{
	uint32 error_u32 = NMS_ERR_DEFAULT;

	RET_T retVal_en = coOdGetObj_u8(index_u16, subIndex_u8, pObj_pu8);
	if ( retVal_en == RET_OK)
	{
		error_u32 =  NMS_ERR_NONE;
	}

	return error_u32;
}


uint32 NmsCanGetObj_u32(uint16 index_u16, uint8 subIndex_u8, uint32 *pObj_pu32)
{
	uint32 error_u32 = NMS_ERR_DEFAULT;

	RET_T retVal_en = coOdGetObj_u32(index_u16, subIndex_u8, pObj_pu32);
	if ( retVal_en == RET_OK)
	{
		error_u32 =  NMS_ERR_NONE;
	}

	return error_u32;
}


uint32 NmsCanGetObj_f32(uint16 index_u16, uint8 subIndex_u8, float32 *pObj_pf32)
{
	uint32 error_u32 = NMS_ERR_DEFAULT;

	RET_T retVal_en = coOdGetObj_r32(index_u16, subIndex_u8, pObj_pf32);
	if ( retVal_en == RET_OK)
	{
		error_u32 =  NMS_ERR_NONE;
	}

	return error_u32;
}


uint32 NmsCanPutObj_u8(uint16 index_u16, uint8 subIndex_u8, uint8 newVal_u8)
{
	uint32 error_u32 = NMS_ERR_DEFAULT;

	RET_T retVal_en = coOdPutObj_u8(index_u16, subIndex_u8, newVal_u8);
	if ( retVal_en == RET_OK)
	{
		error_u32 =  NMS_ERR_NONE;
	}

	return error_u32;
}


uint32 NmsCanPutObj_u32(uint16 index_u16, uint8 subIndex_u8, uint32 newVal_u32)
{
	uint32 error_u32 = NMS_ERR_DEFAULT;

	RET_T retVal_en = coOdPutObj_u32(index_u16, subIndex_u8, newVal_u32);
	if ( retVal_en == RET_OK)
	{
		error_u32 =  NMS_ERR_NONE;
	}

	return error_u32;
}


uint32 NmsCanPutObj_f32(uint16 index_u16, uint8 subIndex_u8, uint32 newVal_u32)
{
	uint32 error_u32 = NMS_ERR_DEFAULT;

	RET_T retVal_en = coOdPutObj_r32(index_u16, subIndex_u8, newVal_u32);
	if ( retVal_en == RET_OK)
	{
		error_u32 =  NMS_ERR_NONE;
	}

	return error_u32;
}
///******************************************************************************
// * Private Function Definitions
// ******************************************************************************/
///**
// * @brief	Handler for the defined nodes in the network.
// * 			This function starts the configuration of a node if needed
// *
// * @return	void
// *
// */
//static void NmsCanLssNodeHandlerRun(void)
//{
//    if (monitorSleep_gb != CO_TRUE)
//    {
//		for (uint8 i_u8 = 0u; i_u8 < NMS_CAN_LSS_NODE_COUNT; i_u8++)
//		{
//			if (nodeNMTState_gaen[i_u8] == CO_NMT_STATE_PREOP)
//			{
//				NmsCanLssConfigureNode(i_u8);
//			}
//			if (nodeNMTState_gaen[i_u8] == CO_NMT_STATE_OPERATIONAL)
//			{
//				/* Do nothing */
//			}
//		}
//    }
//    monitorSleep_gb = CO_TRUE;
//}
//
//
///**
// * @brief	Load function for overwriting the TPDO COB id values.
// *
// * @param	index_u8	Subindex parameter to point parameter area(unused)
// * 			canLine_u8	The canline MASTER or SLAVE
// *
// * @return	Error Code
// */
//static RET_T NmsCanOverwriteLoadIndication(uint8 index_u8)
//{
//    userOverwriteCobIdSettings();
//
//    return RET_OK;
//}
//
//
///**
// * @brief	This function is called if a new NMT state of a node,
// * 			is recordnized by the CANopen stack.
// *
// * @param	nodeID_u8	Node id of the node that has registered a change.
// * 			state_en	Error control state
// * 			nmtState_en	NMT state of the particular node.
// *
// * @return	Array index of the node in the network.
// *
// */
//static uint8 NmsCanLssGetNodeIndex(uint8 nodeId_u8)
//{
//	uint8 arrIndex_u8;
//
//	/* find node ID array arrIndex_u8 */
//	for (arrIndex_u8 = 0u; arrIndex_u8 < NMS_CAN_LSS_NODE_COUNT; arrIndex_u8++)
//	{
//		if (nodeId_u8 == nodeLookupTable_gast[arrIndex_u8].nodeId_u8)
//		{
//			return arrIndex_u8;
//		}
//	}
//
//	return NMS_UINT8_MAX;
//}
//
//
///**
// * @brief	This function is called when a new NMT state of a node
// *          is recognized by the CANopen stack.
// *
// * @param	canLine_u8	Indicates whether the node is on the MASTER or SLAVE CAN line.
// * 			pData		Pointer to additional data (unused).
// */
//static void NmsCanLssToggleMonitorFlag(void *pData_pv)
//{
//	(void)pData_pv;
//
//	/* deactivate application sleep flag */
//	monitorSleep_gb = CO_FALSE;
//}
//
//
///**
//* @brief	Configure a specific node.
//* 			This function adds the defined node to heartbeat consumers,
//* 			and sets its heartbeat interval.
//*
//* @param	arrIndex_u8	Index of the node in the array which is to be configured.
//*
//* @return	Status of the operation.
//*
//*/
//static uint32 NmsCanLssConfigureNode(uint8 arrIndex_u8)
//{
//    uint32 error_u32 	= NMS_ERR_DEFAULT;
//    RET_T retVal_en		= RET_INTERNAL_ERROR;
//
//    /* Get the node ID from the lookup table using arrIndex_u8 */
//    uint8 nodeId_u8 	= nodeLookupTable_gast[arrIndex_u8].nodeId_u8;
//
//    /* Add node to hb consumers with +25% tolerance
//     * Rationale - Allows some flexibility in detecting timeouts due to minor clock drifts,
//     * bus delays, or transmission issues.*/
//    retVal_en = coHbConsumerSet(nodeId_u8,
//    		((NMS_CAN_LSS_NODE_HB_MS / 100 * 25) + NMS_CAN_LSS_NODE_HB_MS));
//    retVal_en = coHbConsumerStart(nodeId_u8);
//
//    /* setup SDO channel */
//    retVal_en = NmsCanLssSetSdoCobID((arrIndex_u8 + 1), nodeLookupTable_gast[arrIndex_u8].nodeId_u8);
//
//    //retVal_en = coSdoWrite(NmsCan_CANOPEN_MASTER_LINE, (arrIndex_u8 + 1), 0x1017u, 0u, (uint8*)(&nodeHBs[0]), 2u, CO_FALSE, 1000u);
//
//    /* start node */
//    retVal_en = coNmtStateReq(nodeId_u8, CO_NMT_STATE_OPERATIONAL, CO_FALSE);
//    if (retVal_en != RET_OK)
//    {
//    	error_u32 = NMS_LSS_NODE_CONFIG_ERROR;
//    }
//    /* set local state to operational, if not operational yet */
//    if (masterStarted_gb == CO_FALSE)
//    {
//        /* set local state */
//        coNmtLocalStateReq(CO_NMT_STATE_OPERATIONAL);
//
//        /* save started flag */
//        masterStarted_gb = CO_TRUE;
//    }
//
//    return error_u32;
//}
//
//
///******************************************************************************
// * Callback Function Definitions
// ******************************************************************************/
///**
// * @brief	LSS indication function for handling the LSS api calls for dynamic
// * 			setup of node ids.
// *
// * @param	canLine_u8		 The canline MASTER or SLAVE
// * 			service_en		 LSS master services for indication functions
// * 			errorCode_u16	 Error code in the module
// * 			errorSpec_u8	 Specific error case that has occured
// * 			pIdentity_pu32	 LSS slave identity.
// *
// */
//static void NmsCanLssIndCallback(CO_LSS_MASTER_SERVICE_T service_en, uint16 errorCode_u16, uint8 errorSpec_u8, uint32 *pIdentity_pu32)
//{
//    static uint8 matchedIndex_u8 = NMS_UINT8_MAX;
//
//    if (errorCode_u16 != 0u)
//    {
//        if (errorCode_u16 == NMS_UINT16_MAX)
//        {
//        	/* ERROR */
//        }
//        else
//        {
//        	 /* ERROR */
//        }
//
//        if (service_en == CO_LSS_MASTER_SERVICE_STORE)
//        {
//        	 /* DEBUG INFO */
//            coLssSwitchGlobal(CO_LSS_STATE_WAITING);
//        }
//        return;
//    }
//
//    switch (service_en)
//    {
//        case CO_LSS_MASTER_SERVICE_NON_CONFIG_SLAVE:
//        	/* DEBUG INFO */
//            coLssFastScan(NMS_CAN_LSS_TIMEOUT);
//            break;
//
//        case CO_LSS_MASTER_SERVICE_FASTSCAN:
//            /* Match detected node with lookup table */
//            for (uint8 i_u8 = 0; i_u8 < NMS_CAN_LSS_NODE_COUNT; i_u8++)
//            {
//                if (pIdentity_pu32[0] == nodeLookupTable_gast[i_u8].vendorId_u32 &&
//                    pIdentity_pu32[1] == nodeLookupTable_gast[i_u8].productId_u32 &&
//                    pIdentity_pu32[2] == nodeLookupTable_gast[i_u8].versionNbr_u32 &&
//                    pIdentity_pu32[3] == nodeLookupTable_gast[i_u8].serialNbr_u32)
//                {
//                    coLssSwitchSelective(pIdentity_pu32[0], pIdentity_pu32[1],
//                                         pIdentity_pu32[2], pIdentity_pu32[3], 20);
//                    matchedIndex_u8 = i_u8;
//                    break;
//                }
//            }
//            break;
//
//        case CO_LSS_MASTER_SERVICE_SWITCH_SELECTIVE:
//            if (matchedIndex_u8 < NMS_CAN_LSS_NODE_COUNT)
//            {
//                coLssSetNodeId(nodeLookupTable_gast[matchedIndex_u8].nodeId_u8, NMS_CAN_LSS_TIMEOUT);
//                matchedIndex_u8 = NMS_UINT8_MAX;
//            }
//            else
//            {
//            	/* ERROR */
//            }
//            break;
//
//        case CO_LSS_MASTER_SERVICE_SET_NODEID:
//        	/* DEBUG INFO */
//            coLssInquireNodeId(NMS_CAN_LSS_TIMEOUT);
//            break;
//
//        case CO_LSS_MASTER_SERVICE_INQUIRE_NODEID:
//        	/* DEBUG INFO */
//            coLssStoreConfig( 200);
//            break;
//
//        case CO_LSS_MASTER_SERVICE_STORE:
//        	/* DEBUG INFO */
//            coLssSwitchGlobal(CO_LSS_STATE_WAITING);
//            break;
//
//        default:
//        	/* ERROR */
//            break;
//    }
//}
//
//
///**
// * @brief	This function is called if a new NMT state of a node,
// * 			is recordnized by the CANopen stack.
// *
// * @param	nodeID_u8	Node id of the node that has registered a change.
// * 			state_en	Error control state
// * 			nmtState_en	NMT state of the particular node.
// *
// * @return	void
// *
// */
//static void NmsCanLssHbMonitorCallback(uint8 nodeID_u8, CO_ERRCTRL_T state_en, CO_NMT_STATE_T nmtState_en)
//{
//	uint8 arrIndex_u8;
//
//	/* look if node is monitored */
//	arrIndex_u8 = NmsCanLssGetNodeIndex(nodeID_u8);
//
//	/* handle monitored node */
//	if (arrIndex_u8 != NMS_UINT16_MAX)
//	{
//		/* save states */
//		nodeNMTState_gaen[arrIndex_u8] = nmtState_en;
//
//		/* indicate if monitored node lost heartbeat */
//		if (nmtState_en == CO_NMT_STATE_UNKNOWN)
//		{
//			/* To be transmitted via CAN */
//			/* ERROR */
//		}
//
//		/* indicate if monitored node sent a bootup message */
//		if (state_en == CO_ERRCTRL_BOOTUP)
//		{
//			/* INFO */
//		}
//
//	/* handle unmonitored node */
//	}
//	else
//	{
//		/* ERROR */
//	}
//}
//
//
///**
//* @brief	This function tries to reset nodes with unknown NMT state.
//*
//* @param	pData_pv	Data.
//*/
//static void NmsCanLssResetNodesCallback(void *pData_pv)
//{
//	uint8 arrIndex_u8;
//
//	(void)pData_pv;
//
//	/* reset defined nodes without known state */
//	for (arrIndex_u8 = 0u; arrIndex_u8 < 3; arrIndex_u8++)
//	{
//		if ((nodeNMTState_gaen[arrIndex_u8] != CO_NMT_STATE_PREOP) &&
//			(nodeNMTState_gaen[arrIndex_u8] != CO_NMT_STATE_OPERATIONAL))
//		{
//			/* reset node */
//			coNmtStateReq(nodeLookupTable_gast[arrIndex_u8].nodeId_u8, CO_NMT_STATE_RESET_NODE, CO_FALSE);
//		}
//	}
//}
//
//
///**
//* @brief 		This function configures a SDO client for the
//* 				given node ID and SDO number.
//*
//* @param		sdoNr_u8	Sdo number
//* 				nodeId_u8	Node id of the node.
//*
//* @return		Error state.
//*
//*/
//static RET_T NmsCanLssSetSdoCobID(uint8 sdoNr_u8, uint8 nodeId_u8)
//{
//	uint16 cobIndex_u16;
//	uint32 newCobId_u32;
//	RET_T retVal = RET_INTERNAL_ERROR;
//
//	/* get od index of sdoNr */
//	cobIndex_u16 = NMS_CAN_SDO_PARAM_INDEX + sdoNr_u8 - 1u;
//
//	/* set cobID for client to server direction (request) */
//	newCobId_u32 = NMS_CAN_SDO_RECEIVE + nodeId_u8;
//	retVal = coOdSetCobid(cobIndex_u16, 1u, newCobId_u32);
//
//	if (retVal == RET_OK)
//	{
//		/* set cobID for server to client direction (response) */
//		newCobId_u32 = NMS_CAN_SDO_TRANSMIT + nodeId_u8;
//		retVal = coOdSetCobid(cobIndex_u16, 2u, newCobId_u32);
//	}
//
//	/* print failed setup details */
//	if (retVal != RET_OK)
//	{
//		/* ERROR */
//	}
//
//	return retVal;
//}