/*
 * GluexXPSAxis.cpp
 *
 *  Created on: Apr 5, 2016
 *      Author: Hovanes Egiyan
 *
 * Modified on: Apr 12, 2016
 *       Vanik Kakoyan
*/

#include "GluexXPSAxis.hh"
#include "GluexXPSController.hh"
#include "GluexXPSGroup.hh"

using namespace std;
using namespace boost::assign; // bring 'map_list_of()' into scope

// This overwrites the values in XPSController.cpp, so we defined it to be twice as big.
#define MAX_MESSAGE_LEN   1024
#define MAX_GROUPNAME_LEN  128


static const char *driverName = "GluexXPSAxis";

GluexXPSAxis::GluexXPSAxis( GluexXPSController *pController, int axisNo, const char *positionerName,
		double stepSize ) :
		XPSAxis( pController, axisNo, positionerName, stepSize ) {
	/* Clean PCO message boxes */
	char message[10];
	
	strcpy( message, " " );
	setStringParam( getController()->pcoSetMessage_, message );
	setStringParam( getController()->pcoEnableMessage_, message );
	setStringParam( getController()->pcoDisableMessage_, message );
	setStringParam( getController()->pcoMessage_, message );

 	/* Clean message box for Referencing, groupMessage*/
 	setStringParam( getController()->positionerReferencingMessage_, message);

 	return;
}

GluexXPSAxis::~GluexXPSAxis() {
	// TODO Auto-generated destructor stub
}

// Override XPSAxis setStringParam method
inline asynStatus GluexXPSAxis::setStringParam( int index, const char *value ) {
	return pC_->setStringParam( axisNo_, index, value );
}

// Override XPSAxis setIntegerParam
inline asynStatus GluexXPSAxis::setIntegerParam( int index, int value ) {
	asynStatus status = asynError;
	status = XPSAxis::setIntegerParam( index, value );
	return status;
}

// Override XPSAxis setDoubleParam method
inline asynStatus GluexXPSAxis::setDoubleParam( int index, double value ) {
	asynStatus status = asynError;
	status = XPSAxis::setDoubleParam( index, value );
	return status;
}


asynStatus GluexXPSAxis::home( double min_velocity, double max_velocity, double acceleration,
		int forwards ) {
	int status;
	int groupStatus;
	char errorBuffer[100];
	static const char *functionName = "home";

	/* Find out if any axes are in the same group, and set referencing mode for them all.*/
	GluexXPSAxis *pTempAxis = NULL;
	for ( int axis = 0; axis < getController()->getNumberOfAxes(); axis++ ) {
		pTempAxis = dynamic_cast<GluexXPSAxis*> ( getController()->getAxis( axis ) );
		if ( strcmp( groupName_, pTempAxis->groupName_ ) == 0 ) {
			pTempAxis->referencingMode_ = 0;
		}
	}

	status = GroupStatusGet( pollSocket_, groupName_, &groupStatus );
	/* The XPS won't allow a home command if the group is in the Ready state
	 * If the group is Ready, then make it not Ready  */
	if ( groupStatus >= 10 && groupStatus <= 18 ) {
		status = GroupKill( pollSocket_, groupName_ );
		if ( status ) {
			asynPrint( pasynUser_, ASYN_TRACE_ERROR,
					"%s:%s: [%s,%d]: error calling GroupKill error=%s\n", driverName, functionName, pC_->portName, axisNo_, getXPSError(status, errorBuffer) );
			return asynError;
		}
	}
	status = GroupStatusGet( pollSocket_, groupName_, &groupStatus );
	/* If axis not initialized, then initialize it */
	if ( (groupStatus >= 0 && groupStatus <= 9) || (groupStatus == 50) || (groupStatus == 63) ) {
		status = GroupInitialize( pollSocket_, groupName_ );
		if ( status ) {
			asynPrint( pasynUser_, ASYN_TRACE_ERROR,
					"%s:%s: [%s,%d]: error calling GroupInitialize error=%s\n", driverName, functionName, pC_->portName, axisNo_, getXPSError(status, errorBuffer) );
			return asynError;
		}
	}
	status = GroupHomeSearch( moveSocket_, groupName_ );
	if ( status ) {
		asynPrint( pasynUser_, ASYN_TRACE_ERROR,
				"%s:%s: [%s,%d]: error calling GroupHomeSearch error=%s\n", driverName, functionName, pC_->portName, axisNo_, getXPSError(status, errorBuffer) );
		return asynError;
	}
	moving_ = true;

	return asynSuccess;
}

inline asynStatus GluexXPSAxis::poll( bool *moving ) {
	/* run PositionerPositionCompareGet() function*/
	getController()->getPco( axisNo_ );
 	getController()->getPositionerErrorAndHardwareStatus( axisNo_ );
 	getController()->getPositionerStageParameters( axisNo_ );
	
	return XPSAxis::poll( moving );
}

inline GluexXPSController* GluexXPSAxis::getController() {
	return dynamic_cast<GluexXPSController*>(pC_);
}

// Perform referencing action for this particular axis. The parameters for
// the ExecuteAction function is takem from the controllers asyn parameter
// lists.
asynStatus GluexXPSAxis::referencingActionExecute() {
	bool positionerReferencingActionExecuteOK = true;
	int status = 0;
	char message[MAX_MESSAGE_LEN];

	int referenceAction;
	getController()->getIntegerParam( axisNo_, getController()->positionerReferencingAction_, &referenceAction );
//	cout << "Action is " << referenceAction << endl;

	int referenceSensor;
	getController()->getIntegerParam( axisNo_, getController()->positionerReferencingSensor_, &referenceSensor );
//	cout << "Sensor is " << referenceSensor << endl;

	double referenceParameter;
	getController()->getDoubleParam( axisNo_, getController()->positionerReferencingParameter_,	&referenceParameter );

	//    printf("**************** positionerReferencingActionExecute***   functionName =%s  \n",functionName);

	//   printf("** ReferencingActionExecute: Group = %s ; Positioner = %s    Action = %s;   Sensor = %s;   Parameter = %f \n",  groupName,positionerName,referencingAction,referencingSensor,referencingParameter);

	string referenceSensorString;
	map<int, string> referenceSensorStringMap = map_list_of
			( 0, "MinusEndOfRun" )	( 1, "MechanicalZero" )
			( 2, "PlusEndOfRun" )	( 3, "None" );
	if ( referenceSensorStringMap.count( referenceSensor ) > 0 ) {
		referenceSensorString = referenceSensorStringMap[referenceSensor];
	}

	string referenceActionString;
	map<epicsInt32, string> referenceActionStringMap = map_list_of
			( 0, "LatchOnLowToHighTransition" )		( 1, "LatchOnHighToLowTransition" )
			( 2, "LatchOnIndex" )				( 3, "LatchOnIndexAfterSensorHighToLowTransition" )
			( 4, "SetPosition" )				( 5, "SetPositionToHomePreset" )
			( 6, "MoveToPreviouslyLatchedPosition" )( 7, "MoveRelative" );
	if ( referenceActionStringMap.count( referenceAction ) > 0 ) {
		referenceActionString = referenceActionStringMap[referenceAction] ;
	}
/*
	cout << " Positioner: " <<  positionerName_  << " , Action will be: " <<  referenceActionString << " , Sensor will be: " <<
	referenceSensorString  << ", Parameter will be: "<< referenceParameter << endl;
*/
getController()->lock();
	status = GroupReferencingActionExecute( moveSocket_, positionerName_,
			const_cast<char*>( referenceActionString.c_str() ),
			const_cast<char*>( referenceSensorString.c_str() ), referenceParameter );
getController()->unlock();

	if ( status ) positionerReferencingActionExecuteOK = false;

	switch ( -status ) {
	case 0:
		strcpy( message, " " );
		break;
	case 1:
		strcpy( message,
				"GroupReferencingActionExecute: Busy socket: previous command not yet finished(-1)" );
		break;
	case 7:
		strcpy( message,
				"GroupReferencingActionExecute: Valid command format: ERR_WRONG_FORMAT(-7)" );
		break;
	case 8:
		strcpy( message,
				"GroupReferencingActionExecute: Valid Action name and Sensor name: ERR_WRONG_OBEKT_TYPE(-8)" );
		break;
	case 9:
		strcpy( message,
				"GroupReferencingActionExecute: Number of command parameters: ERR_WRONG_PARAMEER_NUMBER(-9)" );
		break;
	case 14:
		strcpy( message,
				"GroupReferencingActionExecute: Valid Input parameter type: ERR_WRONG_TYPE_DOUBLE(-14)" );
		break;
	case 17:
		strcpy( message,
				"GroupReferencingActionExecute: Input parameter coherence: ERR_PARAMETER_OUT_OF_RANGE(-17)" );
		break;
	case 18:
		strcpy( message,
				"GroupReferencingActionExecute: Valid Positioner name: ERR_POSITIONER_NAME(-18)" );
		break;
	case 20:
		strcpy( message,
				"GroupReferencingActionExecute: Configuration files reading: ERR_FATAL_INIT(-20)" );
		break;
	case 21:
		strcpy( message,
				"GroupReferencingActionExecute: XPS initialization in progress: ERR_IN_INITIALIZATION(-21)" );
		break;
	case 22:
		strcpy( message,
				"GroupReferencingActionExecute: Group state must be 'NOT REFERENCED': ERR_NOT_ALLOWED_ACTION(-22)" );
		break;
	case 25:
		strcpy( message,
				"GroupReferencingActionExecute:  ERR_FOLLOWING_ERROR(-25)" );
		break;
	case 28:
		strcpy( message,
				"GroupReferencingActionExecute:  ERR_GROUP_HOME_SEARCH_TIMEOUT(-28)" );
		break;
	default:
		sprintf( message, "GroupReferencingActionExecute: Unknown error=%d", status );
		break;
	}

//	cout << " Positioner: " <<  positionerName_  << " , status: " << status << " , messag: " << message<< endl;

	setStringParam( getController()->positionerReferencingMessage_, message );
	int groupExecuteStatus =
			(positionerReferencingActionExecuteOK) ?
					GROUP_REFERENCING_STATUS_SUCCESS : GROUP_REFERENCING_STATUS_FAILURE;
	setIntegerParam( getController()->positionerReferencingActionExecuteStatus_, groupExecuteStatus );

	setIntegerParam( getController()->positionerReferencingActionExecute_, 0 );

	callParamCallbacks();
	return status ? asynError : asynSuccess;
}

string GluexXPSAxis::getGroupName( const std::string positionerName ) {
	// Make sure that there is one and only one '.' character in the positioner name.
	if( ( positionerName.find(".") == string::npos ) || ( positionerName.find(".") !=  positionerName.rfind(".") ) ) {
		return "";
	}
	// find position of "." substring
	std::size_t groupNameEndPosition = positionerName.find(".");
	// pick the groupname from the subtrstring from zero to the position of "."
	string groupName = positionerName.substr( 0, groupNameEndPosition );

	return groupName;
}

// Actually check if this axis is in the group called groupName.
// It may not be for instance if such a group does not exist.

bool GluexXPSAxis::isInGroup( const std::string groupName ) {
	map<string,GluexXPSGroup*> groupMap = this->getController()->getGroupMap();
	if( groupMap.count( groupName ) > 0 ) {
		if( groupMap[groupName]->contains( string(positionerName_) ) > 0 ) {
			return true;
		} else {
			return false;
		}
	} else {
		return false;
	}
}