/*
 * Ethercat CIA402 node execution code
 *
 * */

#include "ecrt.h"

#include "beremiz.h"
#include "iec_types_all.h"

#include "accessor.h"
#include "POUS.h"

IEC_INT beremiz__IW%(location)s_0;
IEC_INT *__IW%(location)s_0 = &beremiz__IW%(location)s_0;

%(MCL_headers)s

static IEC_UINT __InactiveMask = 0x4f;
static IEC_UINT __ActiveMask = 0x6f;
static IEC_UINT __PowerMask = 0x10;
static IEC_BOOL __FirstTick = 1;

typedef enum {
	__Unknown,
	__NotReadyToSwitchOn,
	__SwitchOnDisabled,
	__ReadyToSwitchOn,
	__SwitchedOn,
	__OperationEnabled,
	__QuickStopActive,
    __FaultReactionActive,
    __Fault,
} __CIA402NodeState;

typedef struct {
%(entry_variables)s
    __CIA402NodeState state;
    axis_s* axis;
} __CIA402Node;

#define AXIS_UNIT_TO_USER_UNIT(param, type, name)\
(IEC_##type)(param) * __CIA402Node_%(location)s.axis->name##RatioDenominator / __CIA402Node_%(location)s.axis->name##RatioNumerator
#define USER_UNIT_TO_AXIS_UNIT(param, type, name)\
(IEC_##type)(param * __CIA402Node_%(location)s.axis->name##RatioNumerator / __CIA402Node_%(location)s.axis->name##RatioDenominator)

#define DEFAULT_AXIS_UNIT_TO_USER_UNIT(param) AXIS_UNIT_TO_USER_UNIT(param, LREAL,)
#define DEFAULT_USER_UNIT_TO_AXIS_UNIT(param) USER_UNIT_TO_AXIS_UNIT(param, DINT,)
#define TORQUE_AXIS_UNIT_TO_USER_UNIT(param) AXIS_UNIT_TO_USER_UNIT(param, LREAL, Torque)
#define TORQUE_USER_UNIT_TO_AXIS_UNIT(param) USER_UNIT_TO_AXIS_UNIT(param, INT, Torque)

static __CIA402Node __CIA402Node_%(location)s;

%(extern_located_variables_declaration)s

%(fieldbus_interface_declaration)s

int __init_%(location)s()
{
    __FirstTick = 1;
%(init_entry_variables)s
	*(__CIA402Node_%(location)s.ModesOfOperation) = 0x08;
    return 0;
}

void __cleanup_%(location)s()
{
}

void __retrieve_%(location)s()
{
	IEC_UINT statusword_inactive = *(__CIA402Node_%(location)s.StatusWord) & __InactiveMask;
	IEC_UINT statusword_active = *(__CIA402Node_%(location)s.StatusWord) & __ActiveMask;

	if (__FirstTick) {
		*__IW%(location)s_0 = __MK_Alloc_AXIS_REF();
		__CIA402Node_%(location)s.axis = __MK_GetPublic_AXIS_REF(*__IW%(location)s_0);
		__CIA402Node_%(location)s.axis->NetworkPosition = %(slave_pos)d;
%(init_axis_params)s
%(fieldbus_interface_definition)s
		__FirstTick = 0;
	}

	// CIA402 node state computation
	__CIA402Node_%(location)s.state = __Unknown;
	switch (statusword_inactive) {
		case 0x00:
			__CIA402Node_%(location)s.state = __NotReadyToSwitchOn;
			break;
		case 0x40:
			__CIA402Node_%(location)s.state = __SwitchOnDisabled;
			break;
		case 0x0f:
			__CIA402Node_%(location)s.state = __FaultReactionActive;
			break;
		case 0x08:
			__CIA402Node_%(location)s.state = __Fault;
			break;
		default:
			break;
	}
	switch (statusword_active) {
		case 0x21:
			__CIA402Node_%(location)s.state = __ReadyToSwitchOn;
			break;
		case 0x23:
			__CIA402Node_%(location)s.state = __SwitchedOn;
			break;
		case 0x27:
			__CIA402Node_%(location)s.state = __OperationEnabled;
			break;
		case 0x07:
			__CIA402Node_%(location)s.state = __QuickStopActive;
			break;
		default:
			break;
	}
	if (__CIA402Node_%(location)s.state == __Unknown) {
		return;
	}

	// Default variables retrieve
	__CIA402Node_%(location)s.axis->CommunicationReady = *(__CIA402Node_%(location)s.StatusWord) != 0;
	__CIA402Node_%(location)s.axis->ReadyForPowerOn = __CIA402Node_%(location)s.state == __SwitchedOn || __OperationEnabled;
	__CIA402Node_%(location)s.axis->PowerFeedback = __CIA402Node_%(location)s.state == __OperationEnabled;
	__CIA402Node_%(location)s.axis->ActualPosition = DEFAULT_AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualPosition));
	__CIA402Node_%(location)s.axis->ActualVelocity = DEFAULT_AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualVelocity));
	__CIA402Node_%(location)s.axis->ActualTorque = TORQUE_AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualTorque));

	// Extra variables retrieve
%(extra_variables_retrieve)s
}

void __publish_%(location)s()
{
	IEC_BOOL power = ((*(__CIA402Node_%(location)s.StatusWord) & __PowerMask) > 0) && __CIA402Node_%(location)s.axis->Power;

	// CIA402 node state transition computation
	switch (__CIA402Node_%(location)s.state) {
	    case __SwitchOnDisabled:
	    	*(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x87) | 0x06;
	    	break;
	    case __ReadyToSwitchOn:
	    case __OperationEnabled:
	    	if (!power) {
	    		*(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x8f) | 0x07;
	    		break;
	    	}
	    case __SwitchedOn:
	    	if (power) {
	    	    *(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x8f) | 0x0f;
	    	}
	    	break;
	    case __Fault:
	    	*(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x8f) | 0x80;
	    	break;
	    default:
	    	break;
	}

	// CIA402 node modes of operation computation according to axis motion mode
	switch (__CIA402Node_%(location)s.axis->AxisMotionMode) {
		case mc_mode_cst:
			*(__CIA402Node_%(location)s.ModesOfOperation) = 0x0a;
			break;
		case mc_mode_csv:
			*(__CIA402Node_%(location)s.ModesOfOperation) = 0x09;
			break;
		default:
			*(__CIA402Node_%(location)s.ModesOfOperation) = 0x08;
			break;
	}

	// Default variables publish
	*(__CIA402Node_%(location)s.TargetPosition) = DEFAULT_USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->PositionSetPoint);
	*(__CIA402Node_%(location)s.TargetVelocity) = DEFAULT_USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->VelocitySetPoint);
	*(__CIA402Node_%(location)s.TargetTorque) = TORQUE_USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->TorqueSetPoint);

	// Extra variables publish
%(extra_variables_publish)s
}