/*************************************************************************\
* 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,
                                       const char *ec_limit_velo,
                                       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);
  sEcLimitVeloOrg_          = strdup(ec_limit_velo);
  limitMaxVeloEnable_       = 1;
  
  if(strcmp(sEcLimitVeloOrg_, "empty") == 0) {
    limitMaxVeloEnable_     = 0;
  }
  
  sConfig_                  = strdup(cfg_string);
  delayMs_                  = time_delay_ms;  
  ptrStatus_                = (int*)&status_;
  asynStatusId_             = -1;
  axesCounter_              = 0;
  ecmcSampleRateHz_         = getEcmcSampleRate();
  dataSourcesLinked_        = 0;
  ecMaster_                 = NULL;
  ecEntryRampDown_          = NULL;
  ecEntryStandstill_        = NULL;
  ecEntryLimitVelo_         = NULL;
  masterId_                 = 0;
  slaveIdRampDown_          = 0;
  bitRampDown_              = 0;
  slaveIdStandStill_        = 0;
  bitStandStill_            = 0;
  aliasRampDown_[0]         = 0;  
  aliasStandStill_[0]       = 0;
  aliasLimitVelo_[0]        = 0;
  rampDownCmd_              = 0;
  axesAreStandstill_        = 0;
  rampDownCmdOld_           = 0;
  axesAreStandstillOld_     = 0;
  printEnableStatus_        = 1;
  limitVeloCmdOld_          = 0;
  limitVeloCmd_             = 0;
  cfgDbgMode_               = 0;
  memset(&status_,0,sizeof(status_));

  parseConfigStr(cfg_string);
  initAsyn();
  if(cfgDbgMode_) {
    printf("Safety %s: Group created:\n"
           "    Type:                                        SS1-t\n"
           "    Name:                                        %s\n"
           "    I/O for rampdown command from saftey PLC:    %s\n" 
           "    I/O for axes standstill status:              %s\n"
           "    I/O for velo limit command from safety PLC:  %s (%s)\n"
           "    STO delay [ms]:                              %d\n"
           "    Configuration string:                        %s\n",           
           sName_,sName_,sEcRampDownCmdNameOrg_,
           sEcAxesStandStillStatOrg_,sEcLimitVeloOrg_,
           limitMaxVeloEnable_ ? "enabled" : "disabled",
           delayMs_,sConfig_);
  }
}

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

  free(sName_);
  free(sEcRampDownCmdNameOrg_);
  free(sEcAxesStandStillStatOrg_);
  free(sConfig_);
  free(sEcLimitVeloOrg_);
}

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.");
  }
  
  // init to masterIdRampDown if case !limitMaxVeloEnable_ 
  int masterIdLimitVelo = masterIdStandStill;
  if(limitMaxVeloEnable_) {
    if(parseEcPath(sEcLimitVeloOrg_,
                  &masterIdLimitVelo,
                  &slaveIdLimitVelo_,
                  aliasLimitVelo_,
                  &bitLimitVelo_)) {    
      throw std::runtime_error( "Safety: Parse error: Data source for limit velo.");
    }
  }

  if(masterIdStandStill != masterIdRampDown || masterIdLimitVelo != 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.");
  }
  
  if(bitLimitVelo_ < 0) {
    throw std::runtime_error( "Safety: Parse error: Limit velo, 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.");
  }

  // Check if max velo enabled in axis
  int axesMaxVeloEnabled = 0;
  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
    axesMaxVeloEnabled = (*saxis)->veloMaxLimitEnabled_ || axesMaxVeloEnabled;
  }

  if(limitMaxVeloEnable_ && !axesMaxVeloEnabled) {
    printf("Safety %s: Error, Limit max velo enabled but no axis configured with max velo\n",sName_);
    throw std::runtime_error( "Safety: Error, Limit max velo enabled but no axis configured with max velo");
  }

  if(!limitMaxVeloEnable_ && axesMaxVeloEnabled) {
    printf("Safety %s: Error, Limit max velo disabled but axis configured with a max velo\n",sName_);
    throw std::runtime_error( "Safety: Error, Limit max velo disabled but axis configured with a max velo");
  }
}

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.");
  }

  if(bitRampDown_ >= ecEntryRampDown_->getBits()) {
    throw std::runtime_error( "Safety: Bit out of range for rampdown ethercat entry.");
  }

  // 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 standstill I/O NULL.");
  };

  if(bitStandStill_ >= ecEntryStandstill_->getBits()) {
    throw std::runtime_error( "Safety: Bit out of range for standstill ethercat entry.");
  }

  // Limit velo
  if( limitMaxVeloEnable_ ) {
    slave = ecMaster_->findSlave(slaveIdLimitVelo_);
    if(!slave) {
      throw std::runtime_error( "Safety: EtherCAT slave limit velo I/O NULL.");
    }
  
    ecEntryLimitVelo_ = slave->findEntry(aliasLimitVelo_);
    if(!ecEntryLimitVelo_) {
      throw std::runtime_error( "Safety: EtherCAT entry for limit velo I/O NULL.");
    };

    if(bitLimitVelo_ >= ecEntryLimitVelo_->getBits()) {
      throw std::runtime_error( "Safety: Bit out of range for limit velo ethercat entry.");
    }
  }
  dataSourcesLinked_ = 1;

  return;
}

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();
}

// Ramp down and disable if safety interlock
void ecmcSS1SafetyGroup::exeRampDown() {
  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;
  }
}

// Limit velo if needed
void ecmcSS1SafetyGroup::exeLimitVelo() {

  if(!limitMaxVeloEnable_) {
    return;
  }

    uint64_t data = 0;
  // Read ramp down command from safety plc
  if(ecEntryLimitVelo_->readBit(bitLimitVelo_,
                               &data)) {
    // Disable all axes
    setAxesDisable();           // disable
    setAxesSafetyInterlocks(0); // stop
    setAxesStandstillStatus(0); // set output
    throw std::out_of_range("Safety: Read limit velo cmd failed");
  }

  limitVeloCmdOld_ = limitVeloCmd_;
  limitVeloCmd_    = data == 0;

  if(limitVeloCmdOld_ != limitVeloCmd_) { 
    // Update asyn status wd
    status_.limitVeloCmdActive = limitVeloCmd_;
    refreshAsyn();

    resetPrintoutStatus();
    if(cfgDbgMode_) {
      if(limitVeloCmd_) {
        printf("Safety %s: Limit velo cmd active\n",sName_);
      } else {
        printf("Safety %s: Limit velo cmd not active\n",sName_);
      }
    }
  }
  
  if(limitVeloCmd_) {    
    checkAxesMaxVeloAndDisableIfNeeded();
  }

  // Write velo limit and activation to ecmc axis object
  setAxesMaxVelo();
}

// Executed by ecmc rt thread.
void ecmcSS1SafetyGroup::execute() {
  exeRampDown();
  exeLimitVelo();
}

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 max velo violation
void ecmcSS1SafetyGroup::checkAxesMaxVeloAndDisableIfNeeded() {
  if(!limitVeloCmd_) {
    return;
  }

  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
    if(!(*saxis)->veloMaxLimitEnabled_) {
      break;
    }
    if(checkAxisMaxVelo((*saxis))) {
      printf("Safety %s: Axis %d, velo too high, disabling axis.\n", sName_, (*saxis)->axisId_);
      setAxisEnable((*saxis)->axisId_,0);
    }
  }
  return;
}

// Set max velo in axis object
void ecmcSS1SafetyGroup::setAxesMaxVelo() {
  for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
    if( (*saxis)->veloMaxLimitEnabled_ ) {
      setAxisExtMaxVelo((*saxis)->axisId_,0.95*(*saxis)->veloMaxLimit_,limitVeloCmd_);
    }
  }
}

bool ecmcSS1SafetyGroup::checkAxisMaxVelo(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->veloMaxLimit_ || std::abs(enc) >= axis->veloMaxLimit_;
}

// 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->veloStandstillLimit_ && std::abs(enc) <= axis->veloStandstillLimit_;
}

// 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 veloStandstillLimit,
                                 int standStillTimeMs,
                                 double veloMaxLimit) {

  if(!getAxisValid(axisId)) {  
    throw std::out_of_range("Safety: Invalid axis id");
  }
  
  axes_.push_back(new safetyAxis(axisId, veloStandstillLimit, 
                                 standStillTimeMs, veloMaxLimit));
  axesCounter_++;

  if(cfgDbgMode_) {
    printf("Safety %s: Added axis %d to safety group.\n"
           "    Velo stand still limit: %lf\n"
           "    velo max limit:         %lf (%s)\n"
           "    standstill filter time: %d\n"
    ,sName_,axisId,veloStandstillLimit,veloMaxLimit,
    veloMaxLimit>0 ? "enabled" : "disabled", standStillTimeMs);
  }

  return;
}

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