/*************************************************************************\
* 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. 
*
*  ecmcSafetyPlgWrap.cpp
*
*  Created on: jan 29, 2024
*      Author: anderssandstrom
*      Credits to  https://github.com/sgreg/dynamic-loading 
*
\*************************************************************************/

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

#include <vector>
#include <stdexcept>
#include <string>
#include <epicsExport.h>

#include <iocsh.h>
#include "ecmcSafetyPlgWrap.h"
#include "ecmcSS1SafetyGroup.h"

#define ECMC_PLUGIN_MAX_PORTNAME_CHARS 64
#define ECMC_PLUGIN_PORTNAME_PREFIX "PLUGIN.SAFETY"

static std::vector<ecmcSS1SafetyGroup*>  safetyGroups;
static int safetyGroupsCounter = 0;
static char portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS];

static const char *configString = NULL;

int setCfgString(const char* cfgString) {
  configString = cfgString;
  return 0;
}

ecmcSS1SafetyGroup* getGroupFromName(const char *name) { 
  // Find group by name
  for(std::vector<ecmcSS1SafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
    bool found = strcmp(name, (*psafetyGroup)->getName().c_str()) == 0;   
    if(found) {
      return (*psafetyGroup);
    }
  }
  return NULL;
}

int createSafetyGroup(const char *name,
                      const char *ec_rampdown_cmd,
                      const char *ec_standstill_status,
                      const char *ec_max_velo_cmd,
                      int time_delay_ms) {

  // ensure group does not already exist
  ecmcSS1SafetyGroup* safetyGroup = getGroupFromName(name);
  if( safetyGroup ) {
    printf("Safety: Error, group %s already defined. Plugin will unload.\n",name);
    throw std::runtime_error( "Safety: Error, group already defined.");
  }

  // create new ecmcSS1SafetyGroup object
  safetyGroup = NULL;

  // create asynport name for new object ()
  memset(portNameBuffer, 0, ECMC_PLUGIN_MAX_PORTNAME_CHARS);
  snprintf (portNameBuffer, ECMC_PLUGIN_MAX_PORTNAME_CHARS,
            ECMC_PLUGIN_PORTNAME_PREFIX ".GROUP.%s",name );
  try {
    safetyGroup = new ecmcSS1SafetyGroup(name,
                                      ec_rampdown_cmd,
                                      ec_standstill_status,
                                      ec_max_velo_cmd,
                                      time_delay_ms,
                                      configString,
                                      portNameBuffer);
  }
  catch(std::exception& e) {
    if(safetyGroup) {
      delete safetyGroup;
    }
  }
  
  safetyGroups.push_back(safetyGroup);
  safetyGroupsCounter++;

  return 0;
}

int addAxisToSafetyGroup(const char *groupName, 
                         int axisId,
                         double veloLimit,
                         int standStillTimeMs,
                         double maxVeloLimit) {
  
  ecmcSS1SafetyGroup* grp = getGroupFromName(groupName);

  if( !grp ) {
    printf("Safety: Error, group %s not found. Plugin will unload.\n",groupName);
    throw std::runtime_error( "Safety: Error, group not found.");
  }

  grp->addAxis(axisId,veloLimit,standStillTimeMs,maxVeloLimit);
  
  return asynSuccess;
}

void deleteAllSafetyGroups() {
  
  return;  // The delete process results in seg fault.. need to investigate..

  for(std::vector<ecmcSS1SafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
    if(*psafetyGroup) {
      delete (*psafetyGroup);
    }
  }
}

int validate() {
  int groupCount = 0;
  for(std::vector<ecmcSS1SafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
    if(*psafetyGroup) {
      try {
        (*psafetyGroup)->validate();
        groupCount++;
      }
      catch(std::exception& e) {
        printf("Safety: %s. Plugin will unload.\n",e.what());
        return ECMC_PLUGIN_SAFETY_ERROR_CODE;
      }
    }
  }
  
  // Do not allow loaded plugin with no group configured
  if(groupCount == 0) {
    printf("Saftey: Error, group count 0. Plugin will unload.\n");
    return ECMC_PLUGIN_SAFETY_ERROR_CODE;
  }

  return 0;
}

int executeSafetyGroups() {

  for(std::vector<ecmcSS1SafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
    if(*psafetyGroup) {
      try {
        (*psafetyGroup)->execute();
      }
      catch(std::exception& e) {
        printf("Safety: %s. Plugin will unload.\n",e.what());
        return ECMC_PLUGIN_SAFETY_ERROR_CODE;
      }
    }
  }
  return 0;
}

/** 
 * EPICS iocsh shell command: ecmcAddSS1SafetyGroup
*/
void ecmcAddSS1SafetyGroupPrintHelp() {
  printf("\n");
  printf("       Use ecmcAddSS1SafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<ec_max_velo_cmd>,<time_delay_ms>)\n");
  printf("          <name>                     : Name of group.\n");
  printf("          <ec_rampdown_cmd>          : Ethercat entry input for rampdown cmd.\n");
  printf("          <ec_standstill_status>     : Ethercat entry output for group standstill status.\n");
  printf("          <ec_max_velo_cmd>          : Ethercat entry input for activation of maximum velo limitation (set to \"empty\" to disable).\n");
  printf("          <time_delay_ms>            : Time delay of STO [ms].\n");
  printf("\n");
}

int ecmcAddSS1SafetyGroup(const char* name,
                          const char* ec_rampdown_cmd,
                          const char* ec_standstill_status,
                          const char* ec_max_velo_cmd,
                          int         time_delay_ms) {
  if(!name) {
    ecmcAddSS1SafetyGroupPrintHelp();
    return asynError;
  }

  if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
    ecmcAddSS1SafetyGroupPrintHelp();
    return asynSuccess;
  }

  if(!ec_rampdown_cmd) {
    printf("Error: ec_rampdown_cmd ethercat entry not defined.\n");
    ecmcAddSS1SafetyGroupPrintHelp();
    return asynError;
  }

  if(!ec_standstill_status) {
    printf("Error: ec_standstill_status ethercat entry not defined.\n");
    ecmcAddSS1SafetyGroupPrintHelp();
    return asynError;
  }

  if(!ec_max_velo_cmd) {
    printf("Error: ec_max_velo_cmd ethercat entry not defined.\n");
    ecmcAddSS1SafetyGroupPrintHelp();
    return asynError;
  }
  
  if(time_delay_ms <= 0) {
    printf("Error: time_delay invalid.\n");
    exit (EXIT_FAILURE);
  }

  try {
    createSafetyGroup(name,ec_rampdown_cmd,ec_standstill_status,ec_max_velo_cmd,time_delay_ms);
  }
  catch(std::exception& e) {
    printf("Exception: %s. Add safety group failed.\n",e.what());
    exit (EXIT_FAILURE);    
  }

  return asynSuccess;
}

static const iocshArg initArg0_1 =
{ "Name", iocshArgString };
static const iocshArg initArg1_1 =
{ "ec entry input ramp down cmd", iocshArgString };
static const iocshArg initArg2_1 =
{ "ec entry output axes standstill status", iocshArgString };
static const iocshArg initArg3_1 =
{ "ec entry input activate max velo", iocshArgString };
static const iocshArg initArg4_1 =
{ "STO delay [ms]", iocshArgInt };

static const iocshArg *const initArgs_1[]  = { &initArg0_1, 
                                               &initArg1_1,
                                               &initArg2_1,
                                               &initArg3_1,
                                               &initArg4_1,};

static const iocshFuncDef    initFuncDef_1 = { "ecmcAddSS1SafetyGroup", 5, initArgs_1 };
static void initCallFunc_1(const iocshArgBuf *args) {
  ecmcAddSS1SafetyGroup(args[0].sval, args[1].sval, args[2].sval, args[3].sval, args[4].ival);
}

/** 
 * EPICS iocsh shell command: ecmcAddAxisToSafetyGroup
*/
void ecmcAddAxisToSafetyGroupPrintHelp() {
  printf("\n");
  printf("       Use ecmcAddAxisToSafetyGroup(<group_name>, <axis_index>)\n");
  printf("          <name>                    : Name of safety group.\n");
  printf("          <Axis id>                 : Axis index to add.\n");
  printf("          <standstill_velo_limit>   : Axis standstill velo limit [unit of axis].\n");
  printf("          <standstill_time>         : Time for axis to be below standstill_velo_limit [ms].\n");
  printf("          <max_velo_limit>          : Axis max velo limit [unit of axis] (0 to disable).\n");
  printf("\n");
}

int ecmcAddAxisToSafetyGroup(const char* name, 
                             int axis_id,
                             double standstill_velo_limit,
                             int stand_still_time,
                             double max_velo_limit) {
  if(!name) {    
    ecmcAddAxisToSafetyGroupPrintHelp();
    return asynError;
  }

  if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
    ecmcAddAxisToSafetyGroupPrintHelp();
    return asynSuccess;
  }

  if(axis_id <= 0) {
    printf("Error: Invalid axis id.\n");
    exit(EXIT_FAILURE);
  }

  if(standstill_velo_limit < 0) {
    printf("Error: Invalid standstill velocity limit.\n");
    exit(EXIT_FAILURE);
  }

  if(stand_still_time < 0) {
    printf("Error: Invalid standstill filter time.\n");
    exit(EXIT_FAILURE);
  }

  if(max_velo_limit < 0) {
    printf("Error: Invalid maximum velocity limit.\n");
    exit(EXIT_FAILURE);
  }
  
  try {
    return addAxisToSafetyGroup(name,axis_id,
                                standstill_velo_limit,
                                stand_still_time,
                                max_velo_limit);
  }
  catch(std::exception& e) {
    printf("Exception: %s. Add axis to safety group failed.\n",e.what());
    exit(EXIT_FAILURE);
  }

  return asynSuccess;
}

static const iocshArg initArg0_2 =
{ "Group name", iocshArgString };
static const iocshArg initArg1_2 =
{ "Axis id []", iocshArgInt };
static const iocshArg initArg2_2 =
{ "Velo stand still limit [unit same as axis cfg]", iocshArgDouble };
static const iocshArg initArg3_2 =
{ "Velo stand still filter time [ms]", iocshArgInt };
static const iocshArg initArg4_2 =
{ "Velo max limit [unit same as axis cfg]", iocshArgDouble };

static const iocshArg *const initArgs_2[]  = { &initArg0_2, 
                                               &initArg1_2,
                                               &initArg2_2,
                                               &initArg3_2,
                                               &initArg4_2};

static const iocshFuncDef    initFuncDef_2 = { "ecmcAddAxisToSafetyGroup", 5, initArgs_2};
static void initCallFunc_2(const iocshArgBuf *args) {
  ecmcAddAxisToSafetyGroup(args[0].sval, args[1].ival, args[2].dval, args[3].ival, args[4].dval);
}

void ecmcSafetyPlgRegister(void) {
  iocshRegister(&initFuncDef_1,  initCallFunc_1);
  iocshRegister(&initFuncDef_2,  initCallFunc_2);
}

epicsExportRegistrar(ecmcSafetyPlgRegister);