/********************************************
 *  pmacAxis.cpp
 * 
 *  PMAC Asyn motor based on the 
 *  asynMotorAxis class.
 * 
 *  Matthew Pearson
 *  23 May 2012
 * 
 * Substantially modified for use at SINQ at PSI. 
 * The thing with the PMACS is that they can be programmed.
 * This affects also the commands they understand. 
 * Our PMACS also do not seem to like to return multiple replies.....
 *
 * I use a starting flag to catch a peculiar feature of our PMAC implementation:
 * when the motor is hung, it wont start. I check for this and cause an alarm.
 *
 * Another mode where the motor is in trouble is if it stays too long in status 5 or 6.
 * I check and cause an alarm in this state too.
 *
 * Mark Koennecke, February 2013 
 ********************************************/

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

#include <epicsTime.h>
#include <epicsThread.h>
#include <epicsExport.h>
#include <epicsExit.h>
#include <epicsString.h>
#include <iocsh.h>
#include <errlog.h>

#include "pmacController.h"


#define MULT 1000.

extern "C" void shutdownCallback(void *pPvt)
{
  pmacController *pC = static_cast<pmacController *>(pPvt);

  pC->lock();
  pC->shuttingDown_ = 1;
  pC->unlock();
}

// These are the pmacAxis class methods
pmacAxis::pmacAxis(pmacController *pC, int axisNo)
  :   asynMotorAxis(pC, axisNo),
      pC_(pC)
{
  static const char *functionName = "pmacAxis::pmacAxis";

  pC_->debugFlow(functionName); 

  //Initialize non-static data members
  setpointPosition_ = 0.0;
  encoderPosition_ = 0.0;
  currentVelocity_ = 0.0;
  velocity_ = 0.0;
  accel_ = 0.0;
  highLimit_ = 0.0;
  lowLimit_ = 0.0;
  scale_ = 1;
  previous_position_ = 0.0;
  previous_direction_ = 0;
  axisErrorCount = 0;
  startTime = 0;
  status6Time = 0;
  starting = 0;
  homing = 0;

  /* Set an EPICS exit handler that will shut down polling before asyn kills the IP sockets */
  epicsAtExit(shutdownCallback, pC_);

  //Do an initial poll to get some values from the PMAC
  if (getAxisInitialStatus() != asynSuccess) {
    asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
	      "%s: getAxisInitialStatus failed to return asynSuccess. Controller: %s, Axis: %d.\n", functionName, pC_->portName, axisNo_);
  }
  

  callParamCallbacks();

  /* Wake up the poller task which will make it do a poll, 
   * updating values for this axis to use the new resolution (stepSize_) */   
  pC_->wakeupPoller();
 
}

asynStatus pmacAxis::getAxisInitialStatus(void)
{
  char command[pC_->PMAC_MAXBUF_];
  char response[pC_->PMAC_MAXBUF_];
  int cmdStatus = 0;
  double low_limit = 0.0;
  double high_limit = 0.0;
  int nvals = 0;
  int limVal;

  static const char *functionName = "pmacAxis::getAxisInitialStatus";

  sprintf(command, "Q%2.2d00", axisNo_);
  cmdStatus = pC_->lowLevelWriteRead(command, response);
  nvals = sscanf(response, "%lf", &scale_);
  if (cmdStatus || nvals != 1) {
    asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Error: failed to read scale_factor on axis %d.\n", functionName, axisNo_);
    return asynError;
  }  

  sprintf(command, "I%d13", axisNo_);
  cmdStatus = pC_->lowLevelWriteRead(command, response);
  nvals = sscanf(response, "%d", &limVal);
  if (cmdStatus || nvals != 1) {
    asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Error: failed to read high limit on axis %d.\n", functionName, axisNo_);
    return asynError;
  }  
  high_limit = ((double)limVal)*scale_;

  sprintf(command, "I%d14", axisNo_);
  cmdStatus = pC_->lowLevelWriteRead(command, response);
  nvals = sscanf(response, "%d", &limVal);
  if (cmdStatus || nvals != 1) {
    asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Error: failed to read low limit on axis %d.\n", functionName, axisNo_);
    return asynError;
  }  
  low_limit = ((double)limVal)*scale_;

  char message[132];
  sprintf(message,"scale_factor = %lf, high = %lf, low = %lf", scale_,high_limit,  low_limit);
  pC_->debugFlow(message);

  setDoubleParam(pC_->motorLowLimit_,  low_limit);
  setDoubleParam(pC_->motorHighLimit_, high_limit);
  setIntegerParam(pC_->motorStatusHasEncoder_, 1);
 
  // Enable the axis. After startup, the axis are disabled on the controller...
  sprintf(command, "M%2.2d14=1", axisNo_);
  cmdStatus = pC_->lowLevelWriteRead(command, response);
  if (cmdStatus ) {
    asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: Error: enaabling axis %d failed.\n", functionName, axisNo_);
    return asynError;
  }  

  callParamCallbacks();

  return asynSuccess;
}


pmacAxis::~pmacAxis() 
{
  //Destructor
}


asynStatus pmacAxis::move(double position, int relative, double min_velocity, double max_velocity, double acceleration)
{
  asynStatus status = asynError;
  static const char *functionName = "pmacAxis::move";
  double realPosition;

  pC_->debugFlow(functionName);  

  char command[128] = {0};
  char response[32] = {0};

  pC_->debugFlow(functionName); 

  if(relative){
    realPosition = previous_position_ + position/MULT;
  } else {
    realPosition = position/MULT;
  }
  startTime = time(NULL);
  status6Time = 0;
  starting = 1;

  sprintf( command, "P%2.2d23=0 Q%2.2d01=%12.4f M%2.2d=1", axisNo_, axisNo_, realPosition, axisNo_);
        
  status = pC_->lowLevelWriteRead(command, response);
  
  return status;
}


 
asynStatus pmacAxis::home(double min_velocity, double max_velocity, double acceleration, int forwards)
{
  asynStatus status = asynError;
  char command[128] = {0};
  char response[128] = {0};
  static const char *functionName = "pmacAxis::home";

  pC_->debugFlow(functionName); 

  sprintf(command, "M%2.2d=9", axisNo_);
  
  status = pC_->lowLevelWriteRead(command, response);
  homing = 1;

  return status;
}

asynStatus pmacAxis::moveVelocity(double min_velocity, double max_velocity, double acceleration)
{
  asynStatus status = asynError;
  char command[128] = {0};
  char response[32] = {0};
  static const char *functionName = "pmacAxis::moveVelocity";

  pC_->debugFlow(functionName);  

  return asynError; // can we do this, actually?

  // status = pC_->lowLevelWriteRead(command, response);

  return status;
}


asynStatus pmacAxis::setPosition(double position)
{
  //int status = 0;
  static const char *functionName = "pmacAxis::setPosition";
  
  pC_->debugFlow(functionName);  

  // Cannot do this. 

  return asynSuccess;
}

asynStatus pmacAxis::stop(double acceleration)
{
  asynStatus status = asynError;
  static const char *functionName = "pmacAxis::stopAxis";

  pC_->debugFlow(functionName); 

  char command[128] = {0};
  char response[32] = {0};

  sprintf( command, "M%2.2d=8",  axisNo_ );

  status = pC_->lowLevelWriteRead(command, response);
  return status;
}

asynStatus pmacAxis::poll(bool *moving)
{
  int status = 0;
  static const char *functionName = "pmacAxis::poll";
  char message[132];

  sprintf(message, "%s: Polling axis: %d", functionName, this->axisNo_);
  pC_->debugFlow(message); 

  //Now poll axis status
  if ((status = getAxisStatus(moving)) != asynSuccess) {
    asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
	      "%s: getAxisStatus failed to return asynSuccess. Controller: %s, Axis: %d.\n", functionName, pC_->portName, axisNo_);
  }
  
  callParamCallbacks();
  return status ? asynError : asynSuccess;
}

static char *translateAxisError(int axErr)
{
  switch(axErr){
  case 0:
    return strdup("no error");
    break;
  case 1:
    return strdup("limit violation");
    break;
  case 2:
  case 3:
  case 4:
    return strdup("jog error");
    break;
  case 5:
    return strdup("command not allowed");
    break;
  case 6:
    return strdup("watchdog triggere");
    break;
  case 7:
    return strdup("current limit reached");
    break;
  case 8:
  case 9:
    return strdup("air cushion error");
    break;
  case 10:
    return strdup("MCU limit reached");
    break;
  case 11:
    return strdup("following error triggered");
    break;
  case 12:
    return strdup("EMERGENCY STOP activated");
    break;
  case 13:
    return strdup("Driver electronics error");
    break;
  case 15:
    return strdup("Motor blocked");
    break;
  default:
    return strdup("Unknown axis error");
    break;
  }
}

asynStatus pmacAxis::getAxisStatus(bool *moving)
{
    char command[pC_->PMAC_MAXBUF_];
    char response[pC_->PMAC_MAXBUF_];
    int cmdStatus = 0;; 
    int done = 0;
    double position = 0; 
    double enc_position = 0;
    int nvals = 0;
    int axisProblemFlag = 0;
    int limitsDisabledBit = 0;
    epicsUInt32 axErr = 0, axStat = 0, highLim = 0, lowLim= 0;
    int errorPrintMin = 10000;
    char message[132], *axMessage;
    
            
    /* read our status items one by one: our PMAC does not seem to give multiple responses ..*/
    sprintf(command,"P%2.2d01",axisNo_);
    cmdStatus = pC_->lowLevelWriteRead(command, response);
    nvals = sscanf( response, "%d", &axErr);
    if ( cmdStatus || nvals != 1) {
      asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
		"drvPmacAxisGetStatus: Failed to read axis Error  Status: %d\nCommand :%s\nResponse:%s\n",
		cmdStatus, command, response );
    }

    sprintf(command,"Q%2.2d10",axisNo_);
    cmdStatus = pC_->lowLevelWriteRead(command, response);
    nvals = sscanf( response, "%lf", &position);
    if ( cmdStatus || nvals != 1) {
      asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
		"drvPmacAxisGetStatus: Failed to read position  Status: %d\nCommand :%s\nResponse:%s\n",
		cmdStatus, command, response );
    }

    sprintf(command,"P%2.2d00",axisNo_);
    cmdStatus = pC_->lowLevelWriteRead(command, response);
    nvals = sscanf( response, "%d", &axStat);
    if ( cmdStatus || nvals != 1) {
      asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
		"drvPmacAxisGetStatus: Failed to read axis status,  Status: %d\nCommand :%s\nResponse:%s\n",
		cmdStatus, command, response );
    }

    sprintf(command,"M%2.2d21", axisNo_);
    cmdStatus = pC_->lowLevelWriteRead(command, response);
    nvals = sscanf( response, "%d", &highLim);
    if ( cmdStatus || nvals != 1) {
      asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
		"drvPmacAxisGetStatus: Failed to read high limit flag Status: %d\nCommand :%s\nResponse:%s\n",
		cmdStatus, command, response );
    }

    sprintf(command,"M%2.2d22", axisNo_);
    cmdStatus = pC_->lowLevelWriteRead(command, response);
    nvals = sscanf( response, "%d", &lowLim);
    if ( cmdStatus || nvals != 1) {
      asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
		"drvPmacAxisGetStatus: Failed to low limit flag Status: %d\nCommand :%s\nResponse:%s\n",
		cmdStatus, command, response );
    }

      
    int direction = 0;
            
    setDoubleParam(pC_->motorPosition_, position*MULT);
    setDoubleParam(pC_->motorEncoderPosition_, position*MULT);
      
    /* Use previous position and current position to calculate direction.*/
    if ((position - previous_position_) > 0) {
      direction = 1;
    } else if (position - previous_position_ == 0.0) {
      direction = previous_direction_;
    } else {
      direction = 0;
    }
    setIntegerParam(pC_->motorStatusDirection_, direction);

    /*Store position to calculate direction for next poll.*/
    previous_position_ = position;
    previous_direction_ = direction;

    /* are we done? */
    if((axStat == 0 || axStat == 14 || axStat < 0) && starting == 0 ){
      done = 1;
    } else {
      starting = 0;
      done = 0;
    }
    
    if(starting && time(NULL) > startTime + 10){
      /*
	did not start in 10 seconds: messed up: cause an alarm
      */
      done = 1;
      *moving = false;
      setIntegerParam(pC_->motorStatusMoving_, false);
      setIntegerParam(pC_->motorStatusDone_, true);
      setIntegerParam(pC_->motorStatusProblem_, true);
      errlogPrintf("Axis %d did not start within 10 seconds!! BROKEN\n", axisNo_);
      starting = 0;
      return asynSuccess;
    }

    /*
      code to check for checking against too long status 6
    */
    if(axStat == 5 || axStat == 6){
      if(status6Time == 0){
	status6Time = time(NULL);
      } else {
	if(time(NULL) > status6Time + 60){
	  done = 1;
	  *moving = false;
	  setIntegerParam(pC_->motorStatusMoving_, false);
	  setIntegerParam(pC_->motorStatusDone_, true);
	  setIntegerParam(pC_->motorStatusProblem_, true);
	  errlogPrintf("Axis %d stayed in 5,6 for more then 60 seconds BROKEN\n", axisNo_);
	  status6Time = 0;
	  return asynSuccess;
        }
      }
    } 

    if (!done) {
      *moving = true;
      setIntegerParam(pC_->motorStatusMoving_, true);
      setIntegerParam(pC_->motorStatusDone_, false);
    } else {
      *moving = false;
      setIntegerParam(pC_->motorStatusMoving_, false);
      setIntegerParam(pC_->motorStatusDone_, true);
      if(homing){
	setIntegerParam(pC_->motorStatusHomed_, done);
	homing = 0;
      }
   }

   sprintf(message,"poll results: axis %d, status %d, axErr %d, done = %d", 
	   axisNo_, axStat, axErr, done);
   pC_->debugFlow(message);

    /* search for trouble */
    if(highLim){
      setIntegerParam(pC_->motorStatusHighLimit_, true );
    } else {
      setIntegerParam(pC_->motorStatusHighLimit_, false );
    }
    if(lowLim){
      setIntegerParam(pC_->motorStatusLowLimit_, true );
    } else {
      setIntegerParam(pC_->motorStatusLowLimit_, false );
    }
    if(axErr == 11) {
      setIntegerParam(pC_->motorStatusFollowingError_,true );
    } else {
      setIntegerParam(pC_->motorStatusFollowingError_,false);
    }
    /* Set any axis specific general problem bits. */
    if (axStat < 0 || axErr != 0) {
      axisProblemFlag = 1;
      if(axisErrorCount < 10){
	axMessage = translateAxisError(axErr);
	asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
	 "drvPmacAxisGetStatus: Axis %d is in deep trouble: axis error code %d, translated: %s:, status code = %d\n", 
		  axisNo_, axErr, axMessage, axStat);
	if(axMessage != NULL){
	  free(axMessage);
	}
	axisErrorCount++;
      } else if (axisErrorCount == 10){
	asynPrint(pC_->pasynUserSelf,ASYN_TRACE_ERROR, "Suppressing further axis error messages\n");
	axisErrorCount++;
      }
    } else {
      axisProblemFlag = 0;
      axisErrorCount = 0;
    }
    setIntegerParam(pC_->motorStatusProblem_, axisProblemFlag);
      

    return asynSuccess;
}