/*
 * DXLComm.h
 *
 *  Created on: 25/02/2011
 *      Author: mmcgill
 */

#ifndef DXLCOMM_H_
#define DXLCOMM_H_

#include <string.h>
#include <unistd.h>
#include <termios.h>
#include <sys/time.h>
#include <stdio.h>

#include <pthread.h>

///////////// set/get packet methods //////////////////////////
#define MAXNUM_TXPARAM		(150)
#define MAXNUM_RXPARAM		(60)
#define BROADCAST_ID		(254)

#define INST_PING			(1)
#define INST_READ			(2)
#define INST_WRITE			(3)
#define INST_REG_WRITE		(4)
#define INST_ACTION			(5)
#define INST_RESET			(6)
#define INST_SYNC_WRITE		(131)

#define P_GOAL_POSITION_L	30
#define P_GOAL_POSITION_H	31
#define P_PRESENT_POSITION_L	36
#define P_PRESENT_POSITION_H	37
#define P_MOVING		46

#define ERRBIT_VOLTAGE		(1)
#define ERRBIT_ANGLE		(2)
#define ERRBIT_OVERHEAT		(4)
#define ERRBIT_RANGE		(8)
#define ERRBIT_CHECKSUM		(16)
#define ERRBIT_OVERLOAD		(32)
#define ERRBIT_INSTRUCTION	(64)

#define	COMM_TXSUCCESS		(0)
#define COMM_RXSUCCESS		(1)
#define COMM_TXFAIL		(2)
#define COMM_RXFAIL		(3)
#define COMM_TXERROR		(4)
#define COMM_RXWAITING		(5)
#define COMM_RXTIMEOUT		(6)
#define COMM_RXCORRUPT		(7)

#define ID				(2)
#define LENGTH				(3)
#define INSTRUCTION			(4)
#define ERRBIT				(4)
#define PARAMETER			(5)
#define DEFAULT_BAUDNUMBER		(1)

#define DX2RADFACTOR		(2.0*M_PI*5.0/6.0/1024.0)
#define DXZERO				512
#define DXL2RAD(DX)		(((DX) - DXZERO) * DX2RADFACTOR)
#define RAD2DXL(RAD)	((RAD) / DX2RADFACTOR + DXZERO)
// Input is in radians per second (always positive), output should be
// between 1 and 1023 for minimum to 114RPM (or 11.9 radians per second).
// Note that 0 refers to no speed control at all!

#define RAD2DXLSPEEDFACTOR	(1023/11.9)
#define DXLSPEED2RAD(DX)	((DX)/RAD2DXLSPEEDFACTOR)
#define RAD2DXLSPEED(RAD)	((RAD)<=0?1:(RAD)>=11.9?0:(RAD)*RAD2DXLSPEEDFACTOR)

class DXLComm {
protected:

	// HAL variables
	int socket;

	long	startTime;
	float	rcvWaitTime;
	float	byteTransTime;

	char	deviceName[100];

	// Values

	unsigned char instructionPacket[MAXNUM_TXPARAM+10];
	unsigned char statusPacket[MAXNUM_RXPARAM+10];
	unsigned char rxPacketLength;
	unsigned char rxGetLength;
	int commStatus;
    pthread_mutex_t busMutex;
	int busUsing;

	int hal_open(const char *deviceIndex, float baudrate);
	void hal_close();
	int hal_set_baud( float baudrate );

	inline void hal_clear(void)
	{
		tcflush(socket, TCIFLUSH);
	}

	inline int hal_tx( unsigned char *pPacket, int numPacket )
	{
		return write(socket, pPacket, numPacket);
	}

	inline int hal_rx( unsigned char *pPacket, int numPacket )
	{
		memset(pPacket, 0, numPacket);
		return read(socket, pPacket, numPacket);
	}

	static inline long myclock()
	{
		struct timeval tv;
		gettimeofday (&tv, NULL);
		return (tv.tv_sec * 1000 + tv.tv_usec / 1000);
	}

	inline void hal_set_timeout( int NumRcvByte )
	{
		startTime = myclock();
		rcvWaitTime = (float)(byteTransTime*(float)NumRcvByte + 5.0f);
	}

	inline int hal_timeout(void)
	{
		long time;

		time = myclock() - startTime;
        
        //printf("DXL: %s: (%ld - %ld) == %ld, %lf\n", __PRETTY_FUNCTION__, myclock(), startTime, time, rcvWaitTime);

		if(time > rcvWaitTime)
			return 1;
		else if(time < 0)
			startTime = myclock();

		return 0;
	}
    
    void tx_packet(void);
	void rx_packet(void);

	void txrx_packet(void)
	{
		tx_packet();

		if( commStatus != COMM_TXSUCCESS )
			return;

		do{
			rx_packet();
			if (commStatus == COMM_RXWAITING) usleep(1000);
		}while( commStatus == COMM_RXWAITING );
	}

	inline void set_txpacket_id( int id )
	{
		instructionPacket[ID] = (unsigned char)id;
	}

	inline void set_txpacket_instruction( int instruction )
	{
		instructionPacket[INSTRUCTION] = (unsigned char)instruction;
	}

	inline void set_txpacket_parameter( int index, int value )
	{
		instructionPacket[PARAMETER+index] = (unsigned char)value;
	}

	inline void set_txpacket_length( int length )
	{
		instructionPacket[LENGTH] = (unsigned char)length;
	}

	inline int get_rxpacket_error( int errbit )
	{
		if( statusPacket[ERRBIT] & (unsigned char)errbit )
			return 1;

		return 0;
	}

	inline int get_rxpacket_length(void)
	{
		return (int)statusPacket[LENGTH];
	}

	inline int get_rxpacket_parameter( int index )
	{
		return (int)statusPacket[PARAMETER+index];
	}

	inline int makeword( int lowbyte, int highbyte )
	{
		unsigned short word;

		word = highbyte;
		word = word << 8;
		word = word + lowbyte;
		return (int)word;
	}

	inline int get_lowbyte( int word )
	{
		unsigned short temp;

		temp = word & 0xff;
		return (int)temp;
	}

	inline int get_highbyte( int word )
	{
		unsigned short temp;

		temp = word & 0xff00;
		temp = temp >> 8;
		return (int)temp;
	}

public:
	DXLComm() : socket(-1), startTime(0), rcvWaitTime(0.0f), byteTransTime(0.0f),
			rxPacketLength(0), rxGetLength(0), commStatus(COMM_RXSUCCESS), busUsing(0)
	{
		memset(instructionPacket, 0, sizeof(instructionPacket));
		memset(statusPacket, 0, sizeof(statusPacket));
        pthread_mutex_init(&busMutex, NULL);
	}

	int initialize(const char *deviceIndex, float baudnum )
	{
	//	float baudrate;
	//	baudrate = 2000000.0f / (float)(baudnum + 1);

		if( hal_open(deviceIndex, baudnum) == 0 )
			return 0;

		commStatus = COMM_RXSUCCESS;
		busUsing = 0;
		return 1;
	}

	inline void terminate(void)
	{
		hal_close();
	}

	inline bool connected()
	{
		return socket != -1;
	}

	inline int get_result(void)
	{
		return commStatus;
	}

	inline void ping( int id )
	{
		pthread_mutex_lock(&busMutex);
//		while(busUsing);

		instructionPacket[ID] = (unsigned char)id;
		instructionPacket[INSTRUCTION] = INST_PING;
		instructionPacket[LENGTH] = 2;

		txrx_packet();
		pthread_mutex_unlock(&busMutex);
	}

	inline int read_byte( int id, int address )
	{
		pthread_mutex_lock(&busMutex);
//		while(busUsing);

		instructionPacket[ID] = (unsigned char)id;
		instructionPacket[INSTRUCTION] = INST_READ;
		instructionPacket[PARAMETER] = (unsigned char)address;
		instructionPacket[PARAMETER+1] = 1;
		instructionPacket[LENGTH] = 4;

		txrx_packet();

		pthread_mutex_unlock(&busMutex);
		return (int)statusPacket[PARAMETER];
	}

	inline void write_byte( int id, int address, int value )
	{
		pthread_mutex_lock(&busMutex);
//		while(busUsing);

		instructionPacket[ID] = (unsigned char)id;
		instructionPacket[INSTRUCTION] = INST_WRITE;
		instructionPacket[PARAMETER] = (unsigned char)address;
		instructionPacket[PARAMETER+1] = (unsigned char)value;
		instructionPacket[LENGTH] = 4;

		txrx_packet();
		pthread_mutex_unlock(&busMutex);
	}

	inline int read_word( int id, int address )
	{
		pthread_mutex_lock(&busMutex);
//		while(busUsing);

		instructionPacket[ID] = (unsigned char)id;
		instructionPacket[INSTRUCTION] = INST_READ;
		instructionPacket[PARAMETER] = (unsigned char)address;
		instructionPacket[PARAMETER+1] = 2;
		instructionPacket[LENGTH] = 4;

		txrx_packet();

		pthread_mutex_unlock(&busMutex);
		return makeword((int)statusPacket[PARAMETER], (int)statusPacket[PARAMETER+1]);
	}

	inline void write_word( int id, int address, int value )
	{
		pthread_mutex_lock(&busMutex);
//		while(busUsing);

		instructionPacket[ID] = (unsigned char)id;
		instructionPacket[INSTRUCTION] = INST_WRITE;
		instructionPacket[PARAMETER] = (unsigned char)address;
		instructionPacket[PARAMETER+1] = (unsigned char)get_lowbyte(value);
		instructionPacket[PARAMETER+2] = (unsigned char)get_highbyte(value);
		instructionPacket[LENGTH] = 5;

		txrx_packet();
		pthread_mutex_unlock(&busMutex);
	}

	inline int read_pos(int id) {
		return read_word(id, P_PRESENT_POSITION_L);
	}

	inline void write_pos( int id, int pos, int speed )
	{
		pthread_mutex_lock(&busMutex);
//        while(busUsing);

		instructionPacket[ID] = (unsigned char)id;
		instructionPacket[INSTRUCTION] = INST_WRITE;
		instructionPacket[PARAMETER] = (unsigned char)P_GOAL_POSITION_L;
		instructionPacket[PARAMETER+1] = (unsigned char)get_lowbyte(pos);
		instructionPacket[PARAMETER+2] = (unsigned char)get_highbyte(pos);
		instructionPacket[PARAMETER+3] = (unsigned char)get_lowbyte(speed);
		instructionPacket[PARAMETER+4] = (unsigned char)get_highbyte(speed);
		instructionPacket[LENGTH] = 7;

		txrx_packet();
		pthread_mutex_unlock(&busMutex);
	}
};


#endif /* DXLCOMM_H_ */
