##############################################################################
## Example: Demo of ecmc grbl g-code plugin
## https://github.com/anderssandstrom/ecmc_plugin_grbl
## https://github.com/anderssandstrom/e3-ecmc_plugin_grbl
##
## The plugin exposes:
##

## Initiation:
epicsEnvSet("IOC" ,"$(IOC="IOC_TEST")")
epicsEnvSet("ECMCCFG_INIT" ,"")  #Only run startup once (auto at PSI, need call at ESS), variable set to "#" in startup.cmd
epicsEnvSet("SCRIPTEXEC" ,"$(SCRIPTEXEC="iocshLoad")")

require ecmccfg ruckig

# Epics Motor record driver that will be used:
epicsEnvShow(ECMC_MR_MODULE)

# run module startup.cmd (only needed at ESS  PSI auto call at require)
$(ECMCCFG_INIT)$(SCRIPTEXEC) ${ecmccfg_DIR}startup.cmd, "IOC=$(IOC),ECMC_VER=ruckig"


##############################################################################
## Configure hardware:

$(SCRIPTEXEC) $(ecmccfg_DIR)ecmcMCU1021_coupler.cmd

# ADDITIONAL SETUP
# Set all outputs to feed switches
ecmcConfigOrDie "Cfg.WriteEcEntryIDString(${ECMC_EC_SLAVE_NUM_DIG_OUT},binaryOutput01,1)"
ecmcConfigOrDie "Cfg.WriteEcEntryIDString(${ECMC_EC_SLAVE_NUM_DIG_OUT},binaryOutput02,1)"
ecmcConfigOrDie "Cfg.WriteEcEntryIDString(${ECMC_EC_SLAVE_NUM_DIG_OUT},binaryOutput03,1)"
ecmcConfigOrDie "Cfg.WriteEcEntryIDString(${ECMC_EC_SLAVE_NUM_DIG_OUT},binaryOutput04,1)"
ecmcConfigOrDie "Cfg.WriteEcEntryIDString(${ECMC_EC_SLAVE_NUM_DIG_OUT},binaryOutput05,1)"
ecmcConfigOrDie "Cfg.WriteEcEntryIDString(${ECMC_EC_SLAVE_NUM_DIG_OUT},binaryOutput06,1)"
ecmcConfigOrDie "Cfg.WriteEcEntryIDString(${ECMC_EC_SLAVE_NUM_DIG_OUT},binaryOutput07,1)"
ecmcConfigOrDie "Cfg.WriteEcEntryIDString(${ECMC_EC_SLAVE_NUM_DIG_OUT},binaryOutput08,1)"
# END of ADDITIONAL SETUP

##############################################################################
## AXIS 1
#
epicsEnvSet("DEV",      "$(IOC)")
$(SCRIPTEXEC) ($(ecmccfg_DIR)configureAxis.cmd, CONFIG=./cfg/linear_1.ax)
# Set external setpoints
$(SCRIPTEXEC) ($(ecmccfg_DIR)applyAxisSynchronization.cmd, CONFIG=./cfg/linear_1.sax)

##############################################################################
## AXIS 2
#
#epicsEnvSet("DEV",      "$(IOC)")
$(SCRIPTEXEC) ($(ecmccfg_DIR)configureAxis.cmd, CONFIG=./cfg/linear_2.ax)
# Set external setpoints
$(SCRIPTEXEC) ($(ecmccfg_DIR)applyAxisSynchronization.cmd, CONFIG=./cfg/linear_2.sax)

##############################################################################
## Load plugin:
epicsEnvSet("PLUGIN_VER" ,"develop")
require ecmc_plugin_grbl $(PLUGIN_VER)

epicsEnvSet(ECMC_PLUGIN_FILNAME,"/home/pi/epics/base-7.0.5/require/${E3_REQUIRE_VERSION}/siteMods/ecmc_plugin_grbl/$(PLUGIN_VER)/lib/${EPICS_HOST_ARCH=linux-x86_64}/libecmc_plugin_grbl.so")
epicsEnvSet(ECMC_PLUGIN_CONFIG,"DBG_PRINT=1;X_AXIS=1;Y_AXIS=2;AUTO_ENABLE=1;AUTO_START=1;") # Only one option implemented in this plugin
${SCRIPTEXEC} ${ecmccfg_DIR}loadPlugin.cmd, "PLUGIN_ID=0,FILE=${ECMC_PLUGIN_FILNAME},CONFIG='${ECMC_PLUGIN_CONFIG}', REPORT=1"
epicsEnvUnset(ECMC_PLUGIN_FILNAME)
epicsEnvUnset(ECMC_PLUGIN_CONFIG)

ecmcGrblAddCommand("G1X20Y20F360");
ecmcGrblAddCommand("G4P2");
ecmcGrblAddCommand("G2X0Y0R20");
ecmcGrblAddCommand("G4P2");
ecmcGrblAddCommand("G0X10Y10");
ecmcGrblAddCommand("G4P2");
ecmcGrblAddCommand("G1X0Y0");

#ecmcGrblAddCommand("G2X0adadsdY0R20");
#ecmcGrblAddCommand("G1X0Y20");
#ecmcGrblAddCommand("G1X10Y0F360");
#ecmcGrblAddCommand("G4P1");
#ecmcGrblAddCommand("G1X50Y50F180");
#ecmcGrblAddCommand("G1X0Y0");

##############################################################################
## PLC 0
$(SCRIPTEXEC) $(ecmccfg_DIR)loadPLCFile.cmd, "PLC_ID=0, SAMPLE_RATE_MS=1000,FILE=./plc/grbl.plc")

##############################################################################
############# Configure diagnostics:

ecmcConfigOrDie "Cfg.EcSetDiagnostics(1)"
ecmcConfigOrDie "Cfg.EcEnablePrintouts(0)"
ecmcConfigOrDie "Cfg.EcSetDomainFailedCyclesLimit(100)"
ecmcConfigOrDie "Cfg.SetDiagAxisIndex(1)"
ecmcConfigOrDie "Cfg.SetDiagAxisFreq(2)"
ecmcConfigOrDie "Cfg.SetDiagAxisEnable(0)"

$(SCRIPTEXEC) ($(ecmccfg_DIR)setAppMode.cmd)

#asynSetTraceMask(MC_CPU1, -1, 0x41)
# Note need to test 0x21 later..
#asynSetTraceMask(MC_CPU1, -1, 0x21)
asynSetTraceMask(MC_CPU1, -1, 0x41)
iocInit()

dbpf $(IOC):Axis1.SPAM 0
dbpf $(IOC):Axis2.SPAM 0