/*
FILENAME... EL734Driver.cpp
USAGE...    Motor driver support for the PSI EL734 controller.

Mark Koennecke
February 2013

Updated to have an MsgTxt field through SINQAxis, error handling

Mark Koennecke, May, August 2017

*/


#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <ctype.h>
#include <time.h>

#include <iocsh.h>
#include <epicsThread.h>
#include <errlog.h>

#include <asynOctetSyncIO.h>

#include "EL734Driver.h"
#include <epicsExport.h>

#define IDLEPOLL 60


/** Creates a new EL734Controller object.
  * \param[in] portName          The name of the asyn port that will be created for this driver
  * \param[in] EL734PortName     The name of the drvAsynSerialPort that was created previously to connect to the EL734 controller 
  * \param[in] numAxes           The number of axes that this controller supports 
  */
EL734Controller::EL734Controller(const char *portName, const char *EL734PortName, int numAxes)
  :  SINQController(portName, EL734PortName, numAxes) 
{
  int axis;
  asynStatus status;
  EL734Axis *pAxis;
  static const char *functionName = "EL734Controller::EL734Controller";


  /* Connect to EL734 controller */
  status = pasynOctetSyncIO->connect(EL734PortName, 0, &pasynUserController_, NULL);
  if (status) {
    asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, 
      "%s: cannot connect to EL734 controller\n",
      functionName);
  }
  pasynOctetSyncIO->setOutputEos(pasynUserController_,"\r",strlen("\r"));
  pasynOctetSyncIO->setInputEos(pasynUserController_,"\r",strlen("\r"));

  switchRemote();

  for (axis=0; axis<numAxes; axis++) {
    pAxis = new EL734Axis(this, axis+1);
  }

  startPoller(1000./1000., IDLEPOLL, 2);
}


/** Creates a new EL734Controller object.
  * Configuration command, called directly or from iocsh
  * \param[in] portName          The name of the asyn port that will be created for this driver
  * \param[in] EL734PortName       The name of the drvAsynIPPPort that was created previously to connect to the EL734 controller 
  * \param[in] numAxes           The number of axes that this controller supports 
  */
extern "C" int EL734CreateController(const char *portName, const char *EL734PortName, int numAxes)
{
  EL734Controller *pEL734Controller
    = new EL734Controller(portName, EL734PortName, numAxes);
  pEL734Controller = NULL;
  return(asynSuccess);
}

/** Reports on status of the driver
  * \param[in] fp The file pointer on which report information will be written
  * \param[in] level The level of report detail desired
  *
  * If details > 0 then information is printed about each axis.
  * After printing controller-specific information it calls asynMotorController::report()
  */
void EL734Controller::report(FILE *fp, int level)
{
  fprintf(fp, "EL734 motor driver %s, numAxes=%d\n", 
    this->portName, numAxes_);

  // Call the base class method
  asynMotorController::report(fp, level);
}

/** Returns a pointer to an EL734Axis object.
  * Returns NULL if the axis number encoded in pasynUser is invalid.
  * \param[in] pasynUser asynUser structure that encodes the axis index number. */
EL734Axis* EL734Controller::getAxis(asynUser *pasynUser)
{
  return static_cast<EL734Axis*>(asynMotorController::getAxis(pasynUser));
}

/** Returns a pointer to an EL734Axis object.
  * Returns NULL if the axis number encoded in pasynUser is invalid.
  * \param[in] axisNo Axis index number. */
EL734Axis* EL734Controller::getAxis(int axisNo)
{
  return static_cast<EL734Axis*>(asynMotorController::getAxis(axisNo));
}

void EL734Controller::switchRemote()
{
  char command[COMLEN], reply[COMLEN];
  int status;
  size_t in, out;
  int reason;

  strcpy(command,"RMT 1");
  status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
				       reply,COMLEN, 1.,&out,&in,&reason); 
  strcpy(command,"ECHO 0");
  status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
				       reply,COMLEN, 1.,&out,&in,&reason); 
  strcpy(command,"RMT 1");
  status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
				       reply,COMLEN, 1.,&out,&in,&reason); 
  strcpy(command,"ECHO 0");
  status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
				       reply,COMLEN, 1.,&out,&in,&reason); 
}
/**
 * send a command to the EL734 and read the reply. Do some error and controller 
 * issue fixing on the way
 * \param[in] command The command to send
 * \param[out] reply The controllers reply
 */

asynStatus EL734Controller::transactController(int axisNo,char command[COMLEN], char reply[COMLEN])
{
  asynStatus status;
  size_t in, out, i;
  int reason;
  char myReply[COMLEN], errTxt[256];
  SINQAxis *axis = getAxis(axisNo);

  pasynOctetSyncIO->flush(pasynUserController_);
  
  status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
				       reply,COMLEN, 1.,&out,&in,&reason); 
  if(status != asynSuccess){
    if(axis!= NULL){
      axis->updateMsgTxtFromDriver("Lost connection to motor controller");
    }
    return status;
  }

  /* 
     check for the offline reply
  */
  if(strstr(reply,"?LOC") != NULL){
    switchRemote();
    return pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
				       reply,COMLEN, 1.,&out,&in,&reason); 
  }

  /*
    check for echos. This means that the thing is offline.
  */
  if(strstr(reply,"p ") != NULL || strstr(reply,"u ") != NULL || strstr(reply,"msr ") != NULL){
    switchRemote();
    return pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
				       reply,COMLEN, 1.,&out,&in,&reason); 
  }
  /*
    check for EL734 errors
  */
  strcpy(myReply, reply);
  for(i = 0; i < strlen(reply); i++){
    myReply[i] = (char)tolower((int)reply[i]);
  }
  if(strstr(myReply,"?cmd") != NULL){
    snprintf(errTxt,sizeof(errTxt), "Bad command %s at axis %d", command, axisNo);
    errlogSevPrintf(errlogMajor, errTxt);
    if(axis!= NULL){
      axis->updateMsgTxtFromDriver(errTxt);
    }
    return asynError;
  } else if(strstr(myReply,"?par") != NULL){
    snprintf(errTxt,sizeof(errTxt), "Bad parameter in command %s", command);
    errlogSevPrintf(errlogMajor, errTxt);
    if(axis!= NULL){
      axis->updateMsgTxtFromDriver(errTxt);
    }
    return asynError;
  } else if(strstr(myReply,"?rng") != NULL){
    snprintf(errTxt,sizeof(errTxt), "Parameter out of range in command %s", command);
    errlogSevPrintf(errlogMajor, errTxt);
    if(axis!= NULL){
      axis->updateMsgTxtFromDriver(errTxt);
    }
    return asynError;
  }

  return status;
}

// These are the EL734Axis methods

/** Creates a new EL734Axis object.
  * \param[in] pC Pointer to the EL734Controller to which this axis belongs. 
  * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
  * 
  * Initializes register numbers, etc.
  */
EL734Axis::EL734Axis(EL734Controller *pC, int axisNo)
  : SINQAxis(pC, axisNo), pC_(pC)
{  
  char command[COMLEN], reply[COMLEN];
  asynStatus status;
  int count = 0;
  float low, high;

  /*
    get the hardware limits from the controller
  */
  sprintf(command,"H %d",axisNo_);
  status = pC_->transactController(axisNo_,command,reply);
  if(status == asynSuccess){
    count = sscanf(reply,"%f %f",&low, &high);
    if(count >= 2){
          pC_->setDoubleParam(axisNo_,pC_->motorLowLimit_,low);
          pC_->setDoubleParam(axisNo_,pC_->motorHighLimit_,high);
    } else {
      errlogPrintf("Bad response - %s - requesting limits at axis %d", reply, axisNo_);
    }
  } else {
    errlogPrintf("Failed to read limits at axis %d", axisNo_);
  }
}



/** Reports on status of the axis
  * \param[in] fp The file pointer on which report information will be written
  * \param[in] level The level of report detail desired
  *
  * After printing device-specific information calls asynMotorAxis::report()
  */
void EL734Axis::report(FILE *fp, int level)
{
  if (level > 0) {
    fprintf(fp, "  axis %d\n",
            axisNo_);
  }

  // Call the base class method
  //asynMotorAxis::report(fp, level);
}


asynStatus EL734Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
{
  asynStatus status;
  //static const char *functionName = "EL734Axis::move";
  char command[COMLEN], reply[COMLEN];

  // status = sendAccelAndVelocity(acceleration, maxVelocity);
  
  if (relative) {
    position += this->position;
  }
  oredMSR = 0;
  homing = 0;
  sprintf(command, "p %d %.3f", axisNo_, position/1000.);
  status = pC_->transactController(axisNo_,command,reply);
  setIntegerParam(pC_->motorStatusProblem_, false);
  updateMsgTxtFromDriver("");
  next_poll = -1;
  return status;
}

asynStatus EL734Axis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
{
  asynStatus status;
  //static const char *functionName = "EL734Axis::home";
  char command[COMLEN], reply[COMLEN];

  // status = sendAccelAndVelocity(acceleration, maxVelocity);

  setIntegerParam(pC_->motorStatusProblem_, false);
  updateMsgTxtFromDriver("");

  sprintf(command, "R %d", axisNo_);
  homing = 1;
  next_poll= -1;
  status = pC_->transactController(axisNo_,command,reply);
  return status;
}

asynStatus EL734Axis::moveVelocity(double minVelocity, double maxVelocity, double acceleration)
{
  asynStatus status;
  //static const char *functionName = "EL734Axis::moveVelocity";
  char command[COMLEN], reply[COMLEN];

  // asynPrint(pasynUser_, ASYN_TRACE_FLOW,
  //   "%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n",
  //   functionName, minVelocity, maxVelocity, acceleration);
    

  if (maxVelocity > 0.) {
    /* This is a positive move */
    sprintf(command, "FF %d", axisNo_);
  } else {
      /* This is a negative move  */
      sprintf(command, "FB %d", axisNo_);
  }
  status = pC_->transactController(axisNo_,command,reply);
  setIntegerParam(pC_->motorStatusProblem_, false);
  updateMsgTxtFromDriver("");
  next_poll = -1;
  return status;
}

asynStatus EL734Axis::stop(double acceleration )
{
  asynStatus status;
  //static const char *functionName = "EL734Axis::stop";
  char command[COMLEN], reply[COMLEN];

  sprintf(command, "S %d", axisNo_);
  status = pC_->transactController(axisNo_,command,reply);
  errlogPrintf("Sent STOP on Axis %d\n", axisNo_);
  updateMsgTxtFromDriver("Axis interrupted");

  return status;
}

asynStatus EL734Axis::setPosition(double position)
{
  asynStatus status;
  //static const char *functionName = "EL734Axis::setPosition";
  char command[COMLEN], reply[COMLEN];

  sprintf(command, "U %d %f", axisNo_, position/1000.);
  status = pC_->transactController(axisNo_,command,reply);
  next_poll = -1;

  return status;
}

asynStatus EL734Axis::setClosedLoop(bool closedLoop)
{
  //static const char *functionName = "EL734Axis::setClosedLoop";
 
  /*
    This belongs into the Kingdom of Electronics.
    We do not do this.
   */

  return asynError;
}

/** Polls the axis.
  * This function reads the motor position, the limit status, the home status, the moving status, 
  * and the drive power-on status. 
  * It calls setIntegerParam() and setDoubleParam() for each item that it polls,
  * and then calls callParamCallbacks() at the end.
  * \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). */
asynStatus EL734Axis::poll(bool *moving)
{ 
  int msr, count;
  asynStatus comStatus;
  char command[COMLEN], reply[COMLEN], errTxt[256];
  int driverError = 0;


  // protect against excessive polling
  if(time(NULL) < next_poll){
    *moving = false;
    return asynSuccess;
  }

  // Read the current motor position
  sprintf(command,"u %d", axisNo_);
  comStatus = pC_->transactController(axisNo_,command,reply);
  if(comStatus == asynError){
     driverError = 1;
  }
  if(comStatus) goto skip;
  if(strstr(reply,"*ES") != NULL){
    *moving = false;
    setIntegerParam(pC_->motorStatusDone_, true);
    setIntegerParam(pC_->motorStatusProblem_, true);
    updateMsgTxtFromDriver("Emergency Stop Engaged");
    driverError = 1;
    goto skip;
  } else if(strstr(reply,"?BSY") != NULL){
    *moving = true;
    setIntegerParam(pC_->motorStatusDone_, false);
    updateMsgTxtFromDriver("");
    goto skip;
  }
  count = sscanf(reply,"%lf", &position);
  if(count != 1) {
    snprintf(errTxt,sizeof(errTxt),"Bad reply %s when reading position for %d", reply, axisNo_);
    updateMsgTxtFromDriver(errTxt);
    comStatus = asynError;
    driverError =1;
    goto skip;
  }
  //errlogPrintf("Axis %d, reply %s, position %lf\n", axisNo_, reply, position);
  setDoubleParam(pC_->motorPosition_, position*1000);
  //setDoubleParam(pC_->motorEncoderPosition_, position);
  

  // Read the moving status of this motor
  sprintf(command,"msr %d",axisNo_);
  comStatus = pC_->transactController(axisNo_,command,reply);
  if(comStatus) goto skip;
  sscanf(reply,"%x",&msr);
  // errlogPrintf("Axis %d, reply %s, msr %d, position = %lf\n", 
  // 	       axisNo_, reply, msr, position);
  oredMSR |= msr;
  if( (msr & 0x1) == 0){
    // done: check for trouble
    //errlogPrintf("Axis %d finished\n", axisNo_);
    next_poll = time(NULL)+IDLEPOLL;
    if(oredMSR & 0x10){
      setIntegerParam(pC_->motorStatusLowLimit_, true);
      updateMsgTxtFromDriver("Lower Limit Hit");
      driverError = 1;
    }  else {
      setIntegerParam(pC_->motorStatusLowLimit_, false);
    }
    if(oredMSR & 0x20){
      setIntegerParam(pC_->motorStatusHighLimit_, true);
      updateMsgTxtFromDriver("Upper Limit Hit");
      driverError = 1;
    }  else {
      setIntegerParam(pC_->motorStatusHighLimit_, false);
    }
    if(homing){
      setIntegerParam(pC_->motorStatusAtHome_, true);
    }
    setIntegerParam(pC_->motorStatusProblem_, false);
    if(oredMSR &0x1000){
      setIntegerParam(pC_->motorStatusProblem_, true);
      errlogSevPrintf(errlogMajor, "Air cushion problem on %d", axisNo_);
      updateMsgTxtFromDriver("Air cushion error");
      driverError = 1;
    }
    if(oredMSR &0x80){
      setIntegerParam(pC_->motorStatusProblem_, true);
      errlogSevPrintf(errlogMajor, "Positioning fault at %d", axisNo_);
      updateMsgTxtFromDriver("Positioning fault");
      driverError = 1;
    }
    *moving = false;
    setIntegerParam(pC_->motorStatusDone_, true);
    } else {
    *moving = true;
    next_poll = -1;
    setIntegerParam(pC_->motorStatusDone_, false);
    //updateMsgTxtFromDriver("Creeping");
  }

  skip:
  if(driverError == 0){
    updateMsgTxtFromDriver("");
  }
  setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1:0);
  callParamCallbacks();
  return comStatus ? asynError : asynSuccess;
}

/** Code for iocsh registration */
static const iocshArg EL734CreateControllerArg0 = {"Port name", iocshArgString};
static const iocshArg EL734CreateControllerArg1 = {"EL734 port name", iocshArgString};
static const iocshArg EL734CreateControllerArg2 = {"Number of axes", iocshArgInt};
static const iocshArg * const EL734CreateControllerArgs[] = {&EL734CreateControllerArg0,
                                                             &EL734CreateControllerArg1,
                                                             &EL734CreateControllerArg2};
static const iocshFuncDef EL734CreateControllerDef = {"EL734CreateController", 3, EL734CreateControllerArgs};
static void EL734CreateContollerCallFunc(const iocshArgBuf *args)
{
  EL734CreateController(args[0].sval, args[1].sval, args[2].ival);
}

static void EL734Register(void)
{
  iocshRegister(&EL734CreateControllerDef, EL734CreateContollerCallFunc);
}

extern "C" {
epicsExportRegistrar(EL734Register);
}