/*************************************************************************\
* 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;
}

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

  // create new ecmcSS1SafetyGroup object
  ecmcSS1SafetyGroup* 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 "_%d", safetyGroupsCounter);
  try {
    safetyGroup = new ecmcSS1SafetyGroup(name,
                                      ec_rampdown_cmd,
                                      ec_standstill_status,
                                      time_delay_ms,
                                      configString,
                                      portNameBuffer);
  }
  catch(std::exception& e) {
    if(safetyGroup) {
      delete safetyGroup;
    }
    return ECMC_PLUGIN_SAFETY_ERROR_CODE;
  }
  
  safetyGroups.push_back(safetyGroup);
  safetyGroupsCounter++;

  return 0;
}

int addAxisToSafetyGroup(const char *groupName, 
                         int axisId,
                         double veloLimit,
                         int standStillTimeMs) {
  // Find group by name
  for(std::vector<ecmcSS1SafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
    bool found = strcmp(groupName, (*psafetyGroup)->getName().c_str()) == 0;   
    if(found) {
      (*psafetyGroup)->addAxis(axisId,veloLimit,standStillTimeMs);
      return asynSuccess;
    }
  }

  return asynError;
}

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() {
  for(std::vector<ecmcSS1SafetyGroup*>::iterator psafetyGroup = safetyGroups.begin(); psafetyGroup != safetyGroups.end(); ++psafetyGroup) {
    if(*psafetyGroup) {
      try {
        (*psafetyGroup)->validate();
      }
      catch(std::exception& e) {
        printf("Exception: %s. Plugin will unload.\n",e.what());
        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("Exception: %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>,<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("          <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,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(time_delay_ms <= 0) {
    printf("Error: time_delay invalid.\n");
    exit (1);
  }

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

  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 =
{ "STO delay [ms]", iocshArgInt };

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

static const iocshFuncDef    initFuncDef_1 = { "ecmcAddSS1SafetyGroup", 4, initArgs_1 };
static void initCallFunc_1(const iocshArgBuf *args) {
  ecmcAddSS1SafetyGroup(args[0].sval, args[1].sval, args[2].sval, args[3].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("          <velo limit>   : Axis standstill velo limit [unit of axis].\n");
  printf("          <time>         : Time for axis to be below velo limit [ms].\n");
  printf("\n");
}

int ecmcAddAxisToSafetyGroup(const char* name, int axis_id, double velo_lim, int stand_still_time) {
  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 (1);
  }

  if(velo_lim < 0) {
    printf("Error: Invalid velocity limit.\n");
    exit (1);
  }

  if(stand_still_time < 0) {
    printf("Error: Invalid stand still filter time.\n");
    exit (1);
  }
  try {
    return addAxisToSafetyGroup(name,axis_id, velo_lim, stand_still_time);
  }
  catch(std::exception& e) {
    printf("Exception: %s. Add axis to safety group failed.\n",e.what());
    exit (1);
  }

  return asynSuccess;
}

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

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

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

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

epicsExportRegistrar(ecmcSafetyPlgRegister);