/*************************************************************************\
* Copyright (c) 2023 Paul Scherrer Institute
* ecmc is distributed subject to a Software License Agreement found
* in file LICENSE that is included with this distribution. 
*
*  ecmcSS1SafetyGroup.cpp
*
*  Created on: July 10, 2023
*      Author: anderssandstrom
*      Credits to  https://github.com/sgreg/dynamic-loading 
*
\*************************************************************************/

// Needed to get headers in ecmc right...
#define ECMC_IS_PLUGIN

#define ECMC_PLUGIN_ASYN_SAFETY_STAT      "status"

#include <sstream>
#include "ecmcSS1SafetyGroup.h"
#include "ecmcPluginClient.h"
#include "ecmcAsynPortDriver.h"
#include "ecmcAsynPortDriverUtils.h"
#include "ecmcMotion.h"

#ifdef BASE_VERSION
#define EPICS_3_13
extern DBBASE *pdbbase;
#endif

/** ecmc Safety interface class
 * This object can throw: 
 *    - bad_alloc
 *    - invalid_argument
 *    - runtime_error
*/
ecmcSS1SafetyGroup::ecmcSS1SafetyGroup(const char *name,
                                 const char *ec_rampdown_cmd,
                                 const char *ec_standstill_status,
                                 int time_delay_ms,
                                 const char *cfg_string,
                                 char* portName) 
                : asynPortDriver(portName,
                   1, /* maxAddr */
                   asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask |
                   asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask |
                   asynOctetMask | asynInt8ArrayMask | asynInt16ArrayMask |
                   asynInt32ArrayMask | asynUInt32DigitalMask, /* Interface mask */
                   asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask |
                   asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask |
                   asynOctetMask | asynInt8ArrayMask | asynInt16ArrayMask |
                   asynInt32ArrayMask | asynUInt32DigitalMask, /* Interrupt mask */
                   ASYN_CANBLOCK , /*NOT ASYN_MULTI_DEVICE*/
                   1, /* Autoconnect */
                   0, /* Default priority */
                   0) /* Default stack size */
                   {
  sName_                = strdup(name);
  sEcRampDownCmdNameOrg_   = strdup(ec_rampdown_cmd);
  sEcAxesStandStillStatOrg_= strdup(ec_standstill_status);
  sEcRampDownCmdNameStrip_ = strdup(ec_rampdown_cmd);
  sEcAxesStandStillStatStrip_ = strdup(ec_standstill_status);
  sConfig_              = strdup(cfg_string);
  delayMs_              = time_delay_ms;
  memset(&status_,0,sizeof(status_));
  ptrStatus_            = (int*)&status_;
  asynStatusId_         = -1;
  axesCounter_          = 0;
  ecmcSampleRateHz_     = getEcmcSampleRate();
  dataSourcesLinked_    = 0;
  ecMaster_             = NULL;
  ecEntryRampDown_      = NULL;
  ecEntryStandstill_    = NULL;
  masterId_             = 0;
  slaveIdRampDown_      = 0;
  bitRampDown_          = 0;
  slaveIdStandStill_    = 0;
  bitStandStill_        = 0;
  aliasRampDown_[0]     = 0;
  aliasStandStill_[0]   = 0;
  rampDownCmd_          = 0;
  axesAreStandstill_    = 0;
  rampDownCmdOld_       = 0;
  axesAreStandstillOld_ = 0;
  printEnableStatus_    = 1;

  // Config defaults
  cfgDbgMode_           = 0;
  parseConfigStr(cfg_string);
  initAsyn();
  if(cfgDbgMode_) {
    printf("Safety %s: Safety group created:\n"
           "    Type:                                      SS1\n"
           "    Name:                                      %s\n"
           "    I/O for rampdown command from saftey PLC:  %s\n" 
           "    I/O for axes standstill status:            %s\n"
           "    STO delay [ms]:                            %d\n"
           "    Configuration string:                      %s\n",           
           sName_,sName_,sEcRampDownCmdNameOrg_,
           sEcAxesStandStillStatOrg_,delayMs_,sConfig_);
  }
}

ecmcSS1SafetyGroup::~ecmcSS1SafetyGroup() {
  if(cfgDbgMode_) {
    printf("Safety %s: Cleanup\n",sName_);
  }

  free(sName_);
  free(sEcRampDownCmdNameOrg_);
  free(sEcAxesStandStillStatOrg_);
  free(sEcRampDownCmdNameStrip_);
  free(sEcAxesStandStillStatStrip_);
  free(sConfig_);
}

void ecmcSS1SafetyGroup::parseConfigStr(const char *configStr) {

  // check config parameters
  if (configStr && configStr[0]) {    
    char *pOptions = strdup(configStr);
    char *pThisOption = pOptions;
    char *pNextOption = pOptions;
    
    while (pNextOption && pNextOption[0]) {
      pNextOption = strchr(pNextOption, ';');
      if (pNextOption) {
        *pNextOption = '\0'; /* Terminate */
        pNextOption++;       /* Jump to (possible) next */
      }
      
      // ECMC_PLUGIN_DBG_PRINT_OPTION_CMD (1/0)
      if (!strncmp(pThisOption, ECMC_PLUGIN_DBG_PRINT_OPTION_CMD, strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD))) {
        pThisOption += strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD);
        cfgDbgMode_ = atoi(pThisOption);
      } 

      pThisOption = pNextOption;
    }
    free(pOptions);
  }
}

void ecmcSS1SafetyGroup::validate() { 
  validateCfgs();
  connectToDataSources();
  validateAxes();
}

void ecmcSS1SafetyGroup::validateCfgs() {

  // Validate data sources
  int masterIdRampDown=-1;
  if(parseEcPath(sEcRampDownCmdNameOrg_,
                &masterIdRampDown,
                &slaveIdRampDown_,
                aliasRampDown_,
                &bitRampDown_)) {
    throw std::runtime_error( "Safety: Parse error: Data source for rampdown cmd.");
  }

  int masterIdStandStill=-1;
  if(parseEcPath(sEcAxesStandStillStatOrg_,
                &masterIdStandStill,
                &slaveIdStandStill_,
                aliasStandStill_,
                &bitStandStill_)) {    
    throw std::runtime_error( "Safety: Parse error: Data source for standstill status.");
  }

  if(masterIdStandStill != masterIdRampDown ) {
    throw std::runtime_error( "Safety: Parse error: Master id for datasources different.");
  }
  masterId_ = masterIdStandStill;

  if(bitRampDown_ < 0) {
    throw std::runtime_error( "Safety: Parse error: Rampdown cmd, bit invalid.");
  }

  if(bitStandStill_ < 0) {
    throw std::runtime_error( "Safety: Parse error: Standstill status, bit invalid.");
  }
}

void ecmcSS1SafetyGroup::validateAxes() {  
  // Ensure that axis is not linked twice to group 
  for(std::vector<safetyAxis*>::iterator iaxis = axes_.begin(); iaxis != axes_.end(); ++iaxis) {
    int axisLinkedCounter = 0;
    
    if(!(*iaxis)) {
      throw std::runtime_error( "Safety: Axis object NULL.");
    }
    int axisCheck_1 = (*iaxis)->axisId_;
    int axisCheck_2 = -1;
    for(std::vector<safetyAxis*>::iterator jaxis = axes_.begin(); jaxis != axes_.end(); ++jaxis) {
      if(!(*jaxis)) {
        throw std::runtime_error( "Safety: Axis object NULL.");
      }

      axisCheck_2 =(*jaxis)->axisId_;
      if( axisCheck_1 == axisCheck_2) {
        axisLinkedCounter++;
      }
    }

    if(axisLinkedCounter > 1) {
      if(cfgDbgMode_) {
        printf("Safety %s: Axis %d linked %d times to group\n",
              sName_,axisCheck_2,axisLinkedCounter);
      }
      throw std::runtime_error( "Safety: Axis linked multiple times");
    }    
  }

  int axisCounter = 0;
  // Check axis object exists and valid
  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
    if(!getAxisValid((*saxis)->axisId_)) {
      throw std::runtime_error( "Safety: Ecmc Axis object not valid.");
    }
    axisCounter++;
  }

  // do not allow empty group
  if(axisCounter == 0) {
    printf("Safety %s: Error, group empty (axis count zero)\n",sName_);
    throw std::runtime_error( "Safety: Error, empty group not allowed.");
  }
}

void ecmcSS1SafetyGroup::connectToDataSources() {
  
  ecMaster_ = (ecmcEc*)getEcMaster();

  if(!ecMaster_) {
    throw std::runtime_error( "Safety: EtherCAT master object NULL.");
  }

  // rampdown
  ecmcEcSlave *slave = ecMaster_->findSlave(slaveIdRampDown_);
  if(!slave) {
    throw std::runtime_error( "Safety: EtherCAT slave for rampdown I/O NULL.");
  }

  ecEntryRampDown_ = slave->findEntry(aliasRampDown_);
  if(!ecEntryRampDown_) {
    throw std::runtime_error( "Safety: EtherCAT entry for rampdown I/O NULL.");
  }

  // standstill
  slave = ecMaster_->findSlave(slaveIdStandStill_);
  if(!slave) {
    throw std::runtime_error( "Safety: EtherCAT slave for standstill I/O NULL.");
  }

  ecEntryStandstill_ = slave->findEntry(aliasStandStill_);
  if(!ecEntryStandstill_) {
    throw std::runtime_error( "Safety: EtherCAT entry for rampdown I/O NULL.");
  };
  
  dataSourcesLinked_ = 1;

  return;
  
  //// Get dataItem for rampdown command
  //dataItemRampDownCmd_ = (ecmcDataItem*) getEcmcDataItem(sEcRampDownCmdNameStrip_);
  //if(!dataItemRampDownCmd_) {
  //  throw std::runtime_error( "Safety: Data item for ramp down command NULL.");
  //}
  //
  //// Get dataItem for axes standstill status
  //dataItemStandStillStat_ = (ecmcDataItem*) getEcmcDataItem(sEcAxesStandStillStatStrip_);
  //if(!dataItemStandStillStat_) {
  //  throw std::runtime_error( "Safety: Data item for axes standstill status NULL.");
  //}
  //
  //if(cfgDbgMode_) {
  //  printf("Safety: Safety group \"%s\"\": Data sources linked.\n",sName_);
  //}
  //
  //dataSourcesLinked_ = 1;
}

void ecmcSS1SafetyGroup::refreshAsyn() { 
  setIntegerParam(asynStatusId_, (epicsInt32)*ptrStatus_);
  callParamCallbacks();
}

asynStatus ecmcSS1SafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) {
  int function = pasynUser->reason;
  if( function == asynStatusId_ ) {
    *value = (epicsInt32)*ptrStatus_;
    return asynSuccess;
  }

  return asynError;
}

void ecmcSS1SafetyGroup::initAsyn() {

  // Add motion "plugin.safety.ss1.<grp_name>.status"
  std::string paramName = ECMC_PLUGIN_ASYN_PREFIX "." ECMC_PLUGIN_ASYN_SS1 "." + std::string(sName_) +
             "." + ECMC_PLUGIN_ASYN_SAFETY_STAT;
  int *paramId = &asynStatusId_;
  if( createParam(0, paramName.c_str(), asynParamInt32, paramId ) != asynSuccess ) {
    throw std::runtime_error("Safety: Failed create asyn parameter mode");
  }
  setIntegerParam(asynStatusId_, (epicsInt32)*ptrStatus_);

  // Update integers
  callParamCallbacks();
}

// Avoid issues with std:to_string()
std::string ecmcSS1SafetyGroup::to_string(int value) {
  std::ostringstream os;
  os << value;
  return os.str();
}

// Executed by ecmc rt thread.
void ecmcSS1SafetyGroup::execute() {
  
  uint64_t data = 0;
  // Read ramp down command from safety plc
  if(ecEntryRampDown_->readBit(bitRampDown_,
                               &data)) {
    // Disable all axes
    setAxesDisable();           // disable
    setAxesSafetyInterlocks(0); // stop
    setAxesStandstillStatus(0); // set output
    throw std::out_of_range("Safety: Read rampdown cmd failed");
  }

  rampDownCmdOld_ = rampDownCmd_;
  rampDownCmd_ = data == 0;

  if(rampDownCmdOld_ != rampDownCmd_) {
    
    // Update asyn status wd
    status_.rampDownCmdActive = rampDownCmd_;
    refreshAsyn();

    resetPrintoutStatus();
    if(cfgDbgMode_) {
      if(rampDownCmd_) {
        printf("Safety %s: Ramp down cmd active\n",sName_);
      } else {
        printf("Safety %s: Ramp down cmd not active\n",sName_);
      }
    }
  }

  // set safety interlock in ecmc
  setAxesSafetyInterlocks(rampDownCmd_);
  // check if axes are standstill to safety PLC
  axesAreStandstill_ = checkAxesStandstillAndDisableIfNeeded();
  setAxesStandstillStatus(axesAreStandstill_);
 
  // Disable
  if(axesAreStandstill_ && rampDownCmd_) {    
    setAxesDisable();
  } else {
    printEnableStatus_ = true;
  }
}

void ecmcSS1SafetyGroup::resetPrintoutStatus() {
  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
    (*saxis)->printEnableStat_ = 1;    
  }
}

void ecmcSS1SafetyGroup::setAxesStandstillStatus(int standstill) {

  if(axesAreStandstillOld_ != standstill) {
    if(standstill) {
      printf("Safety %s: Axes are not moving\n",sName_);
    } else {
      printf("Safety %s: Axes are moving\n",sName_);
    }

    // Update asyn status wd
    status_.axesAtStandstill = standstill;
    refreshAsyn();
  }

  if(ecEntryStandstill_->writeBit(bitStandStill_,
                                  standstill > 0)) {
    throw std::out_of_range("Safety: Read rampdown cmd failed");
  }
  axesAreStandstillOld_ = standstill;
}

// Check standstill axes
bool ecmcSS1SafetyGroup::checkAxesStandstill() {
  bool standstill = 1;
  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
    standstill= standstill && checkAxisStandstill((*saxis));    
  }
  return standstill;
}

// Check standstill axis
bool ecmcSS1SafetyGroup::checkAxisStandstill(safetyAxis* axis) {
  double traj = 1;

  int err = getAxisTrajVelo(axis->axisId_, &traj);
  if(err) {
    return 0;
  }

  double enc = 1;
  err = getAxisEncVelo(axis->axisId_, &enc);
  if(err) {
    return 0;
  }

  return std::abs(traj) <= axis->veloLimit_ && std::abs(enc) <= axis->veloLimit_;
}

// Check standstill and disable
bool ecmcSS1SafetyGroup::checkAxesStandstillAndDisableIfNeeded() {
  bool standstill = 1;
  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
    standstill = standstill && checkAxisStandstillAndDisableIfNeeded((*saxis));
  }
  return standstill;
}

// Check standstill and disable
bool ecmcSS1SafetyGroup::checkAxisStandstillAndDisableIfNeeded(safetyAxis* axis) {
  bool standstill = checkAxisStandstill(axis);
  if( standstill && rampDownCmd_) {
    setAxisEnable(axis->axisId_,0);
    if(cfgDbgMode_ && axis->printEnableStat_) {
      printf("Safety %s: Disabling axis %d\n",sName_,axis->axisId_);
      axis->printEnableStat_ = 0;
    }
  }
  return standstill;
}

// Disable all axes
void ecmcSS1SafetyGroup::setAxesDisable() {
  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
    if(!*saxis) {
      throw std::runtime_error("Safety: Axis object NULL.");
    }
    setAxisEnable((*saxis)->axisId_,0);    
  }

  if(printEnableStatus_ && cfgDbgMode_) {
    printf("Safety %s: All axes in group are beeing disabled\n",sName_);
  }
  printEnableStatus_ = 0;
}

// Set safety interlock
void ecmcSS1SafetyGroup::setAxesSafetyInterlocks(int stop) {
  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
    if(!*saxis) {
      throw std::runtime_error( "Safety: Axis object NULL.");
    }
    setAxisEmergencyStopInterlock((*saxis)->axisId_,stop);    
  }
}

void ecmcSS1SafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs) {

  if(!getAxisValid(axisId)) {  
    throw std::out_of_range("Safety: Invalid axis id");
  }
  
  axes_.push_back(new safetyAxis(axisId, veloLimit, standStillTimeMs));  
  axesCounter_++;
  
  
  if(cfgDbgMode_) {
    printf("Safety %s: Added axis %d to safety group (velo limit = %lf, standstill filter : %d)\n"
    ,sName_,axisId,veloLimit,standStillTimeMs);
  }

  return;
}

std::string ecmcSS1SafetyGroup::getName() {
  return sName_;
}