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

#include "ecrt.h"

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

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

/* From CiA402, page 27

        Table 30 - State coding
    Statusword      |      PDS FSA state
xxxx xxxx x0xx 0000 | Not ready to switch on
xxxx xxxx x1xx 0000 | Switch on disabled
xxxx xxxx x01x 0001 | Ready to switch on
xxxx xxxx x01x 0011 | Switched on
xxxx xxxx x01x 0111 | Operation enabled
xxxx xxxx x00x 0111 | Quick stop active
xxxx xxxx x0xx 1111 | Fault reaction active
xxxx xxxx x0xx 1000 | Fault
*/
#define FSAFromStatusWord(SW) (SW & 0x006f)
#define NotReadyToSwitchOn  0b00000000 FSA_sep 0b00100000
#define SwitchOnDisabled    0b01000000 FSA_sep 0b01100000
#define ReadyToSwitchOn     0b00100001
#define SwitchedOn          0b00100011
#define OperationEnabled    0b00100111
#define QuickStopActive     0b00000111
#define FaultReactionActive 0b00001111 FSA_sep 0b00101111
#define Fault               0b00001000 FSA_sep 0b00101000

// SatusWord bits :
#define SW_ReadyToSwitchOn     0x0001
#define SW_SwitchedOn          0x0002
#define SW_OperationEnabled    0x0004
#define SW_Fault               0x0008
#define SW_VoltageEnabled      0x0010
#define SW_QuickStop           0x0020
#define SW_SwitchOnDisabled    0x0040
#define SW_Warning             0x0080
#define SW_Remote              0x0200
#define SW_TargetReached       0x0400
#define SW_InternalLimitActive 0x0800

// ControlWord bits :
#define SwitchOn        0x0001
#define EnableVoltage   0x0002
#define QuickStop       0x0004
#define EnableOperation 0x0008
#define FaultReset      0x0080
#define Halt            0x0100


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

%(MCL_headers)s

static IEC_BOOL __FirstTick = 1;

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

#define AxsPub __CIA402Node_%(location)s

static __CIA402Node AxsPub;

%(extern_located_variables_declaration)s

%(fieldbus_interface_declaration)s

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

void __cleanup_%(location)s()
{
}

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

	// Default variables retrieve
	AxsPub.axis->CommunicationReady = 
        *(AxsPub.StatusWord) != 0;
#define FSA_sep || FSA ==
    {
        uint16_t FSA = FSAFromStatusWord(*(AxsPub.StatusWord));
        AxsPub.axis->ReadyForPowerOn = FSA == ReadyToSwitchOn;
        AxsPub.axis->PowerFeedback = FSA == OperationEnabled;
    }
#undef FSA_sep 
	AxsPub.axis->ActualRawPosition = *(AxsPub.ActualPosition);
	AxsPub.axis->ActualRawVelocity = *(AxsPub.ActualVelocity);
	AxsPub.axis->ActualRawTorque = *(AxsPub.ActualTorque);

	// Extra variables retrieve
%(extra_variables_retrieve)s
}

void __publish_%(location)s()
{
	IEC_BOOL power = 
        ((*(AxsPub.StatusWord) & SW_VoltageEnabled) != 0) 
        && AxsPub.axis->Power;
    uint16_t CW = *(AxsPub.ControlWord);

#define FSA_sep : case
	// CIA402 node state transition computation
	switch (FSAFromStatusWord(*(AxsPub.StatusWord))) {
	    case SwitchOnDisabled :
            CW &= ~(SwitchOn | FaultReset);
            CW |= EnableVoltage | QuickStop;
	    	break;
	    case ReadyToSwitchOn :
	    case OperationEnabled :
	    	if (!power) {
                CW &= ~(FaultReset | EnableOperation);
                CW |= SwitchOn | EnableVoltage | QuickStop;
	    		break;
	    	}
	    case SwitchedOn :
	    	if (power) {
                CW &= ~(FaultReset);
                CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation;
	    	}
	    	break;
	    case Fault :
            /* TODO reset fault only when MC_Reset */
            CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation);
            CW |= FaultReset;
	    	break;
	    default:
	    	break;
	}
#undef FSA_sep 
    *(AxsPub.ControlWord) = CW;

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

	// Default variables publish
	*(AxsPub.TargetPosition) = 
            AxsPub.axis->RawPositionSetPoint;
	*(AxsPub.TargetVelocity) = 
            AxsPub.axis->RawVelocitySetPoint;
	*(AxsPub.TargetTorque) = 
            AxsPub.axis->RawTorqueSetPoint;

	// Extra variables publish
%(extra_variables_publish)s
}