/**
 * @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_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_CAN_LSS_NODE_COUNT					10u
#define NMS_CAN_MIN_OPERATIONAL_NODES			8u
/******************************************************************************
 * Type Declarations
 ******************************************************************************/
typedef struct
{
    uint32 vendorId_u32;
    uint32 productId_u32;
    uint32 versionNbr_u32;
    uint32 serialNbr_u32;
    uint8 nodeId_u8;
} NmsCanLssNodeInfo_t;
/******************************************************************************
 * Global Declarations
 ******************************************************************************/
NmsCanLssNodeInfo_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 NmsCanLssNodeInfo_t nodeLookupTable_gast[NMS_CAN_LSS_NODE_COUNT] =
{
	    {0x59A, 0x184,   0x1,   0x65, 0x5} ,
	    {0x59A, 0x184,   0x1,   0x66, 0x6} ,
	    {0x59A, 0x184,   0x1,   0x67, 0x7} ,
	    {0x59A, 0x184,   0x1,   0x68, 0x8} ,
	    {0x59A, 0x184,   0x1,   0x69, 0x9} ,
	    {0x59A, 0x184,   0x1,   0x6A, 0xA} ,
	    {0x59A, 0x184,   0x1,   0x6B, 0xB} ,
	    {0x59A, 0x184,   0x1,   0x6C, 0xC} ,
	    {0x59A, 0x184,   0x1,   0x6D, 0xD} ,
	    {0x59A, 0x184,   0x1,   0x6E, 0xE} ,
};
static CO_NMT_STATE_T nodeNMTState_gaen[NMS_CAN_LSS_NODE_COUNT];
static CO_TIMER_T 	nodeResetTimer_gst;
/******************************************************************************
 * 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()
{
	for (uint8 i_u8 = 0u; i_u8 < NMS_CAN_LSS_NODE_COUNT; i_u8++)
	{
		nodeNMTState_gaen[i_u8] = CO_NMT_STATE_UNKNOWN;
	}

	codrvHardwareInit();
	codrvCanInit(NMS_CAN_CANOPEN_BITRATE);

	/* CANopen Initialization */
	coCanOpenStackInit(NmsCanOverwriteLoadIndication);
	codrvTimerSetup(CO_TIMER_INTERVAL);
	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);
	//coTimerStart(&nodeResetTimer_gst, (NMS_CAN_LSS_NODE_STATE_RESET_INTERVAL * 1000000uLL), NmsCanLssResetNodesCallback, 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(0, 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_u16(uint16 index_u16, uint8 subIndex_u8, uint16 newVal_u16)
{
	uint32 error_u32 = NMS_ERR_DEFAULT;

	RET_T retVal_en = coOdPutObj_u16(index_u16, subIndex_u8, newVal_u16);
	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;
}


bool NmsCanAreAllNodesOperational(void)
{
	uint8 operationalNodeNb_u8 = 0u;

    for (uint8 i_u8 = 0u; i_u8 < NMS_CAN_LSS_NODE_COUNT ; i_u8++)
    {
        if (nodeNMTState_gaen[i_u8] == CO_NMT_STATE_OPERATIONAL)
        {
        	operationalNodeNb_u8++;
        }
    }

    return (operationalNodeNb_u8 >= NMS_CAN_MIN_OPERATIONAL_NODES) ? CO_TRUE : CO_FALSE;
}
/******************************************************************************
 * 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 || nodeNMTState_gaen[i_u8] == CO_NMT_STATE_UNKNOWN)
			{
				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 < NMS_CAN_LSS_NODE_COUNT; 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;
}
