/***************************************************************************
                          amcmotor.cpp  -  description
                             -------------------
    begin                : Sun Nov 24 2002
    copyright            : (C) 2002 by Martin Merck
    email                : merck@astro.uni-wuerzburg.de
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   This program is part of the MAGIC control software for the AMC.       *
 *   The software is only intended for use in the MAGIC telescope project  *
 *   All rights remain with the author.					   *
 ***************************************************************************/

#include <qcstring.h>
#include <qdatetime.h>
#include <stdio.h>
#include <unistd.h>
#include <math.h>
#include "amcmotor.h"
#include "amcerror.h"
#include "amcserialport.h"
#include "amcerrors.h"
#include "amccommands.h"

AMCMotor::AMCMotor( AMCSerialPort* p_pPort ) :
	m_pPort( p_pPort ), m_iCurrDriver(0), m_iCurrFreq(0)
{
}

AMCMotor::~AMCMotor(){
}

/** No descriptions */
void AMCMotor::sendCommand(const QCString& p_sCommand, QCString& p_sResponse, int p_iExpRes ) const
{
	char	buffer[256];	

  // This is because of test with optical fiber
	int iNumEcho;
  iNumEcho = NUM_ECHO;
//  if( m_iPort == 3 )
//		iNumEcho = 1;

  int rrlen;
	int len = p_sCommand.length();
	for( int i=0; i<len; i++)
	{
		int wres = m_pPort->transmitChar( p_sCommand[i] );
		// if we send the last charachter in the command we exprect
		// the full response from the AMC master, otherwise we expect
		// two echo characters, one from the line, the second from the
		// AMC master. If no box is selected we expect only the echo
		// from the line.
		int max;
		if( i == len-1 )
		{
      // sleep 30 ms to be sure to get the whole response
//	    usleep(30000);
//			max = p_iExpRes + iNumEcho;
			max = 255;
		}
		else
			max = iNumEcho;
//			max = NUM_ECHO;
		usleep(10000);
		int rlen = m_pPort->readBlock( buffer, max );
		// set the end of the response string to a 0 char to
		// convert the buffer into a cstring.
		buffer[rlen] = 0;
    rrlen = rlen;
	}

/*	
	if( p_sCommand.contains("B0") != 0 )
  {
		QString sResult, qsTemp;
		for( int i=0; i< rrlen; i++)
			sResult += qsTemp.sprintf(" %02X",(unsigned char) buffer[i]);		
		qDebug( sResult );
	}	
*/

	// Reset and unselect commands will not return any status
	if( (p_sCommand == IDS_COMMAND_UNSELECT_BOX)
		||
		(p_sCommand == IDS_COMMAND_RESET) )
	return;

	if( (p_sCommand == IDS_COMMAND_INFO)
      &&
			(buffer[rrlen-1] != '>') )
	{
//    usleep(20000);
		int rlen = m_pPort->readBlock( buffer+rrlen, IDS_CMD_RES_INFO );
		qDebug("Second iteration for info command. %d bytes", rlen);
		buffer[rrlen+rlen] = 0;
	}

	QCString sResp = buffer;
	// We expect at least two echos of the last character of the command
	// (the carriage return) and one AMC master status byte.
//	if( sResp.length() < NUM_ECHO + 1)
	if( sResp.length() < iNumEcho + 1)
	{
		// This is a hack for an ununderstood problem of the
		// AMC masters. It seems that we somtimes don't
		// get a response when sending very often the query
		// command.
		if( p_sCommand == IDS_COMMAND_QUERY)
			sResp = "  @";
		else
		{
			AMCError aErr( IDS_ERROR_NO_RESPONSE );
			aErr.setCommand( p_sCommand );
			aErr.setResponse( sResp );
			throw aErr;
		}
	}
	
	// Remove the trailling echos of the carriage return of the command.
//	p_sResponse = sResp.right( sResp.length() - NUM_ECHO );
	p_sResponse = sResp.right( sResp.length() - iNumEcho );
	
	// We now get the last character from the response send bz the AMC
	// master and check for controller errors.
	char ch = sResp[ sResp.length()-1 ];
	// Is it a valid AMC status byte ?
	if( ( ch != '>' )
		&&
		( ch != '@' )
		&&
		( ch != '#' ) )
	{
		AMCError aErr( IDS_ERROR_NO_AMC_STATUS );
		aErr.setCommand( p_sCommand );
		aErr.setResponse( p_sResponse );
		qDebug("No AMC status: " + aErr.formatError(0));
		qDebug("Response length %d",sResp.length() );
//		if( p_sCommand != IDS_COMMAND_QUERY)
			throw aErr;
	}
	
	// It seems that the AMC master software was changed. Test revealed
	// that some AMC boxes return '?>' in the case they don't understand
	// the command.
	if( ch =='?' )
	{
		AMCError aErr( IDS_ERROR_BAD_ARGUMENT );
		aErr.setCommand( p_sCommand );
		aErr.setResponse( p_sResponse );
		throw aErr;
	}
	
	// If AMC status indicades error Check which type of error code.
	// Here we recursivly call this method sendCommand.
	if( ch =='#' )
	{
		QCString sComm, sResp2;
		
		// send the query command
		sComm.sprintf( IDS_COMMAND_QUERY );
		sendCommand( sComm, sResp2, IDS_CMD_RES_QUERY );
		
		// The response should contain the error code in HEX plus '\r>'
		int iErrCode;
		sscanf( (const char*) sResp2, "%X\r", &iErrCode );
		qDebug("ErrorCode %X  %X %X", iErrCode, iErrCode & 0x07, iErrCode & 0xFF0);
		
		AMCError aErr;
		aErr.setCommand( p_sCommand + " - " + sComm );
		aErr.setResponse( p_sResponse + " - " + sResp2 );
		aErr.setDriver( iErrCode & 0x07 );
		
		switch( iErrCode & 0xFF0 )
		{
		case (0x10):
			aErr.setErrorText( IDS_ERROR_SPI_OVERFLOW );
			break;
		case (0x20):
			aErr.setErrorText( IDS_ERROR_SPI_WTIMEOUT );
			break;
		case (0x40):
			aErr.setErrorText( IDS_ERROR_SPI_RTIMEOUT );
			break;
		case (0x60):
			aErr.setErrorText( IDS_ERROR_BAD_DRIVER );
			break;
		case (0x80):
			aErr.setErrorText( IDS_ERROR_BAD_ARGUMENT );
			break;
		case (0x100):
			aErr.setErrorText( IDS_ERROR_NO_DRIVER );
			break;
		case (0x200):
			{
				// we detected a verges error. Some information parameters
				// of the box are outside the range. Lets find out which.
				sComm.sprintf( IDS_COMMAND_INFO );
				sendCommand( sComm, sResp2, IDS_CMD_RES_INFO );
				
				aErr.setCommand( p_sCommand + " - " + sComm );
				aErr.setResponse( p_sResponse + " - " + sResp2 );
				// The response should be: 'E%2X\r>'
				int iParErr;
				sscanf( (const char*) sResp, "E%X\r", &iParErr );
				switch( iParErr )
				{
				case (0x04):						// Temeperature
					aErr.setErrorText( IDS_ERROR_TEMP_RANGE );
					break;
				case (0x08):						// Humidity
					aErr.setErrorText( IDS_ERROR_HUMIDITY_RANGE );
					break;
				case (0x10):						// Power current
					aErr.setErrorText( IDS_ERROR_I_POWER_RANGE );
					break;
				case (0x20):						// Power voltage
					aErr.setErrorText( IDS_ERROR_V_POWER_RANGE );
					break;
				case (0x40):						// Secondary voltage
					aErr.setErrorText( IDS_ERROR_V_SECONDARY_RANGE );
					break;
				case (0x80):						// Logical voltage
					aErr.setErrorText( IDS_ERROR_V_LOGICAL_RANGE );
					break;
				default:
					aErr.setErrorText( IDS_ERROR_UNKNOWN );
					break;
				}
			}
		default:
			aErr.setErrorText( IDS_ERROR_UNKNOWN );
			break;
		}
		// throw the exception
		throw aErr;
	}
	
	return;
}


/** Select a AMC box. */
void AMCMotor::selectBox( int p_iBox ) const
{
	QCString sComm, sResp;
	
	m_pPort->flush();
	sComm.sprintf( IDS_COMMAND_BOX, p_iBox );
	try	
	{
		sendCommand( sComm, sResp, IDS_CMD_RES_BOX );
	}
	catch( AMCError& e)
	{
	}
	sComm.sprintf( IDS_COMMAND_QUERY, p_iBox );
	sendCommand( sComm, sResp, IDS_CMD_RES_QUERY );	
}


/** Select one of the four drivers */
void AMCMotor::selectDriver( int p_iDriver ) const
{
	QCString sComm, sResp;
	
	sComm.sprintf( IDS_COMMAND_DRIVER, p_iDriver-1 );
	sendCommand( sComm, sResp, IDS_CMD_RES_DRIVER );
	m_iCurrDriver = p_iDriver;
	return;
}

/** Reset a driver. Kill an active movment and turn laser and motor
 *  power off. */
void AMCMotor::Reset() const
{
	killMovement();
	switchLaser( false );
	powerMotors( false );
	return;
}

/** Kill the last movement command. We should send this command
 *  when the motor movement doesnt finish in the expected time.
 *  In this case the motor may be stuck. We want to kill the
 *  movement command to not destroy anything. */
void AMCMotor::killMovement() const
{
	QCString sComm, sResp;

  qDebug("Sending kill command");	
	sComm.sprintf( IDS_COMMAND_KILL );
	sendCommand( sComm, sResp, IDS_CMD_RES_KILL );
	return;
}

/** Get the environmental information from the AMC box */
void AMCMotor::getInfo( int* p_iTemp,
						int* p_iHum,
						int* p_iPowerI,
						int* p_iPowerV,
						int* p_iSecondV,
						int* p_iLogicV ) const
{
	QCString sComm, sResp;
	
	sComm.sprintf( IDS_COMMAND_INFO );
	sendCommand( sComm, sResp, IDS_CMD_RES_INFO );
	sscanf( (const char*) sResp,
			"T=%d\rH=%d\rI=%d\rV=%d\rS=%d\rL=%d\r",
			p_iTemp,
			p_iHum,
			p_iPowerI,
			p_iPowerV,
			p_iSecondV,
			p_iLogicV );
			
	return;
}

/** Return the frequency with which the motors are driven */
int AMCMotor::getFrequency() const
{
	QCString sComm, sResp;
	
	sComm.sprintf( IDS_COMMAND_FREQUENCY_READ );
	sendCommand( sComm, sResp, IDS_CMD_RES_FREQUENCY_READ );
	
	int iFrequency;
	sscanf( (const char*) sResp, "%d\r", &iFrequency);
	m_iCurrFreq = iFrequency;
	return( iFrequency );
}

/** Set the frequency with which the current driver will drive the motors.  */
void AMCMotor::setFrequency( int p_iFreq ) const
{
	QCString sComm, sResp;
	
	sComm.sprintf( IDS_COMMAND_FREQUENCY_WRITE, p_iFreq );
	sendCommand( sComm, sResp, IDS_CMD_RES_FREQUENCY_WRITE );
	m_iCurrFreq = p_iFreq;

	return;	
}

/** Return the working current set for the selected driver. */
int AMCMotor::getWorkingCurrent() const
{
	QCString sComm, sResp;
	
	sComm.sprintf( IDS_COMMAND_WORKING_READ );
	sendCommand( sComm, sResp, IDS_CMD_RES_WORKING_READ );
	
	int iCurrent;
	sscanf( (const char*) sResp, "%d\r", &iCurrent);
	return( iCurrent );
}

/** Set the working current with which the current driver will drive the motors. */
void AMCMotor::setWorkingCurrent( int p_iCurrent ) const
{
	QCString sComm, sResp;
	
	sComm.sprintf( IDS_COMMAND_WORKING_WRITE, p_iCurrent );
	sendCommand( sComm, sResp, IDS_CMD_RES_WORKING_WRITE );

	return;	
}

/** Return the holding current set for the selected driver. */
int AMCMotor::getHoldingCurrent() const
{
	QCString sComm, sResp;
	
	sComm.sprintf( IDS_COMMAND_HOLDING_READ );
	sendCommand( sComm, sResp, IDS_CMD_RES_HOLDING_READ );
	
	int iCurrent;
	sscanf( (const char*) sResp, "%d\r", &iCurrent);
	return( iCurrent );
}

/** Set the holding current with which the current driver will drive the motors. */
void AMCMotor::setHoldingCurrent( int p_iCurrent ) const
{
	QCString sComm, sResp;
	
	sComm.sprintf( IDS_COMMAND_HOLDING_WRITE, p_iCurrent );
	sendCommand( sComm, sResp, IDS_CMD_RES_HOLDING_READ );

	return;	
}

/** Switch the laser on or off. */
void AMCMotor::switchLaser( bool zOn ) const
{
	QCString sComm, sResp;
	
	if( zOn	)
		sComm.sprintf( IDS_COMMAND_LASER_ON );
	else
		sComm.sprintf( IDS_COMMAND_LASER_OFF );
	sendCommand( sComm, sResp, IDS_CMD_RES_LASER_ON );

	return;	
}
/** Turn the power to the motors on or off. By default the motors are always powered on.
  * This is necesary to send the holding current through the motors. We can turn
  * the power off in case of a broken motor. However the power will remain turned off
  * only if we deselect the driver in the next command. Selecting the driverv again
  * or sending a movement command, will automatically turn the motor power on again.
  */
void AMCMotor::powerMotors( bool zOn ) const
{
	QCString sComm, sResp;
	
	if( zOn	)
		sComm.sprintf( IDS_COMMAND_POWER_ON );
	else
		sComm.sprintf( IDS_COMMAND_POWER_OFF );
	sendCommand( sComm, sResp, IDS_CMD_RES_POWER_ON );

	return;	
}

/** Move the motors to the end switches and then find the central poosition. */
void AMCMotor::centerMotors() const
{
	QCString sComm, sResp;
	
	sComm.sprintf( IDS_COMMAND_CENTER );
	sendCommand( sComm, sResp, IDS_CMD_RES_CENTER );

	return;	
}

/** Move the motors by the given number of steps. If the number is positive we move
 * upwards, if negative downwards. Be aware that the directions can be changed with
 * the tuning commands.
 */
void AMCMotor::moveMotors( int p_iX, int p_iY ) const
{
	QCString sComm, sResp;
	
	sComm.sprintf( IDS_COMMAND_MOVE, p_iX, p_iY );
	sendCommand( sComm, sResp, IDS_CMD_RES_MOVE );

	return;	
}

/** Calculate the time needed by the motors to complete the number of steps.
 * We add 200 ms for the communications overhead. The returned value
 * is the next high integer number of seconds.
 */
int AMCMotor::calcTimeout( int p_iSteps ) const
{
	// timeout for negative number of steps
	if( p_iSteps < 0)
		return(0);
		
	if( m_iCurrFreq == 0 )
		getFrequency();

	// The timeout is calculated by deviding the number of steps by the frequency.
	// We add 200ms to account for the communications overhead.
	// We convert the result to the next higher integer and return the timeout
	// in seconds.		
	int iTimeout = (int) ceil( ((double) p_iSteps / (double) m_iCurrFreq) + 0.2 );
	return(iTimeout);
	
}

/** Wait for the motor movement to complete. */
void AMCMotor::waitForMotors( int p_iTimeout ) const
{
	QTime qtStop, qtStart;
	QCString sComm, sResp;
	
	qtStart.start();
	qtStop = qtStart.addSecs( p_iTimeout );
	// near midnight the time may wrap. In this case we sleep
	// timeout seconds to be sure to be in the next day.
	if (qtStop < qtStart)
		sleep( p_iTimeout );
	
	sComm.sprintf( IDS_COMMAND_QUERY );
	for(;;)
	{
		// Check for a timeout during centering
		if( qtStart.elapsed() > p_iTimeout*1000 )
		{
			// Kill the current movemnet
			qDebug("Killing movement after %d msecs when %ds where expected", qtStart.elapsed(), p_iTimeout);
			killMovement();
			resetBox();
			// throw the exception
			AMCError aErr( IDS_ERROR_MOVEMENT_TIMEOUT, m_iCurrDriver );
			throw aErr;
		}
		sendCommand( sComm, sResp, IDS_CMD_RES_QUERY );
		if( sResp[0] != '@' )
		{
			// During centering when we hit the end switches
			// the AMC box will issue the status "xx\r>" and
			// then change direction. xx will give the number
			// of steps needed to come out of the end switch.
			// In such a case we still are in the movement so
			// we continue checking, otherwise we quit.
			if( ( sResp.length() > 1 )
				&&
				( sResp[sResp.length()-1] == '>' ) )
				continue;
//			qDebug("Waiting for motors %d %s", sResp.length(), sResp );
//			break;
			if( (sResp.length() == 1) && (sResp[0] == '>') )
			{
        qDebug("panel movement stopped after %dms when waiting %ds", qtStart.elapsed(), p_iTimeout );
				break;
			}
		}
    usleep(30000);
	}
		
	return;	
	
}

/** Unselect the currently selected box. */
void AMCMotor::unselectBox() const
{
	QCString sComm, sResp;
	
	sComm.sprintf( IDS_COMMAND_UNSELECT_BOX );
	sendCommand( sComm, sResp, IDS_CMD_RES_UNSELECT_BOX );

	return;	
}

/** Reset the currently selected box. */
void AMCMotor::resetBox() const
{
	QCString sComm, sResp;
	
	sComm.sprintf( IDS_COMMAND_RESET );
	sendCommand( sComm, sResp, IDS_CMD_RES_RESET );

	return;	
}

/** Reset the currently selected box. */
void AMCMotor::resetDriver() const
{
	QCString sComm, sResp;
	
	sComm.sprintf( IDS_COMMAND_DRIVER_RESET );
	sendCommand( sComm, sResp, IDS_CMD_RES_DRIVER_RESET );

	return;	
}
