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

#include "dynamixel/DXLComm.h"

#include <stdio.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/serial.h>

void DXLComm::hal_close()
{
	if(socket != -1)
		close(socket);
	socket = -1;
}

int DXLComm::hal_open(const char *deviceIndex, float baudrate)
{
	struct termios newtio;
	struct serial_struct serinfo;
//	char dev_name[100] = {0, };

//	sprintf(dev_name, "/dev/ttyUSB%d", deviceIndex);

	strcpy(deviceName, deviceIndex);
	memset(&newtio, 0, sizeof(newtio));
	hal_close();

	if((socket = open(deviceName, O_RDWR|O_NOCTTY|O_NONBLOCK)) < 0) {
		fprintf(stderr, "device open error: %s\n", deviceIndex);
		goto DXL_HAL_OPEN_ERROR;
	}

	newtio.c_cflag		= B38400|CS8|CLOCAL|CREAD;
	newtio.c_iflag		= IGNPAR;
	newtio.c_oflag		= 0;
	newtio.c_lflag		= 0;
	newtio.c_cc[VTIME]	= 0;	// time-out 값 (TIME * 0.1초) 0 : disable
	newtio.c_cc[VMIN]	= 0;	// MIN 은 read 가 return 되기 위한 최소 문자 개수

	tcflush(socket, TCIFLUSH);
	tcsetattr(socket, TCSANOW, &newtio);

	if(socket == -1)
		return 0;

	if(ioctl(socket, TIOCGSERIAL, &serinfo) < 0) {
		fprintf(stderr, "Cannot get serial info\n");
		return 0;
	}

	serinfo.flags &= ~ASYNC_SPD_MASK;
	serinfo.flags |= ASYNC_SPD_CUST;
	serinfo.custom_divisor = serinfo.baud_base / baudrate;

	if(ioctl(socket, TIOCSSERIAL, &serinfo) < 0) {
		fprintf(stderr, "Cannot set serial info\n");
		return 0;
	}

	hal_close();

	byteTransTime = (float)((1000.0f / baudrate) * 12.0f);

	strcpy(deviceName, deviceIndex);
	memset(&newtio, 0, sizeof(newtio));
	hal_close();

	if((socket = open(deviceName, O_RDWR|O_NOCTTY|O_NONBLOCK)) < 0) {
		fprintf(stderr, "device open error: %s\n", deviceIndex);
		goto DXL_HAL_OPEN_ERROR;
	}

	newtio.c_cflag		= B38400|CS8|CLOCAL|CREAD;
	newtio.c_iflag		= IGNPAR;
	newtio.c_oflag		= 0;
	newtio.c_lflag		= 0;
	newtio.c_cc[VTIME]	= 0;	// time-out 값 (TIME * 0.1초) 0 : disable
	newtio.c_cc[VMIN]	= 0;	// MIN 은 read 가 return 되기 위한 최소 문자 개수

	tcflush(socket, TCIFLUSH);
	tcsetattr(socket, TCSANOW, &newtio);

	return 1;

DXL_HAL_OPEN_ERROR:
	hal_close();
	return 0;
}

int DXLComm::hal_set_baud( float baudrate )
{
	struct serial_struct serinfo;

	if(socket == -1)
		return 0;

	if(ioctl(socket, TIOCGSERIAL, &serinfo) < 0) {
		fprintf(stderr, "Cannot get serial info\n");
		return 0;
	}

	serinfo.flags &= ~ASYNC_SPD_MASK;
	serinfo.flags |= ASYNC_SPD_CUST;
	serinfo.custom_divisor = serinfo.baud_base / baudrate;

	if(ioctl(socket, TIOCSSERIAL, &serinfo) < 0) {
		fprintf(stderr, "Cannot set serial info\n");
		return 0;
	}

	//hal_close();
	//hal_open(deviceName, baudrate);

	byteTransTime = (float)((1000.0f / baudrate) * 12.0f);
	return 1;
}

void DXLComm::tx_packet(void)
{
	unsigned char i;
	unsigned char TxNumByte, RealTxNumByte;
	unsigned char checksum = 0;

	if( busUsing == 1 )
		return;

	busUsing = 1;

	if( instructionPacket[LENGTH] > (MAXNUM_TXPARAM+2) )
	{
		commStatus = COMM_TXERROR;
		busUsing = 0;
		return;
	}

	if( instructionPacket[INSTRUCTION] != INST_PING
		&& instructionPacket[INSTRUCTION] != INST_READ
		&& instructionPacket[INSTRUCTION] != INST_WRITE
		&& instructionPacket[INSTRUCTION] != INST_REG_WRITE
		&& instructionPacket[INSTRUCTION] != INST_ACTION
		&& instructionPacket[INSTRUCTION] != INST_RESET
		&& instructionPacket[INSTRUCTION] != INST_SYNC_WRITE )
	{
		commStatus = COMM_TXERROR;
		busUsing = 0;
		return;
	}

	instructionPacket[0] = 0xff;
	instructionPacket[1] = 0xff;
	for( i=0; i<(instructionPacket[LENGTH]+1); i++ )
		checksum += instructionPacket[i+2];
	instructionPacket[instructionPacket[LENGTH]+3] = ~checksum;

	if( commStatus == COMM_RXTIMEOUT || commStatus == COMM_RXCORRUPT )
		hal_clear();

	TxNumByte = instructionPacket[LENGTH] + 4;
	RealTxNumByte = hal_tx( (unsigned char*)instructionPacket, TxNumByte );

	if( TxNumByte != RealTxNumByte )
	{
		commStatus = COMM_TXFAIL;
		busUsing = 0;
		return;
	}

	if( instructionPacket[INSTRUCTION] == INST_READ )
		hal_set_timeout( instructionPacket[PARAMETER+1] + 6 );
	else
		hal_set_timeout( 6 );

	commStatus = COMM_TXSUCCESS;
}

void DXLComm::rx_packet(void)
{
	unsigned char i, nRead;
	unsigned char checksum = 0;

	if( busUsing == 0 )
		return;

	if( instructionPacket[ID] == BROADCAST_ID )
	{
		commStatus = COMM_RXSUCCESS;
		busUsing = 0;
		return;
	}

	if( commStatus == COMM_TXSUCCESS )
	{
		rxGetLength = 0;
		rxPacketLength = 6;
		if( instructionPacket[INSTRUCTION] == INST_READ )
		{
			rxPacketLength += instructionPacket[PARAMETER+1];
		}
	}

	nRead = hal_rx( (unsigned char*)&statusPacket[rxGetLength], rxPacketLength );
	rxGetLength += nRead;
	rxPacketLength -= nRead;
	if( rxPacketLength > 0 )
	{
		if( hal_timeout() == 1 )
		{
			commStatus = COMM_RXTIMEOUT;
			busUsing = 0;
		}
		else
		{
			commStatus = COMM_RXWAITING;
		}

		return;
	}

	// Find packet header
	for( i=0; i<(rxGetLength-1); i++ )
	{
		if( statusPacket[i] == 0xff && statusPacket[i+1] == 0xff )
		{
			rxPacketLength = i;
			break;
		}
		else if( i == rxGetLength-2 && statusPacket[rxGetLength-1] == 0xff )
		{
			rxPacketLength = rxGetLength-1;
			break;
		}
	}

	if( rxPacketLength > 0 )
	{
		for( i=0; i<(rxGetLength-rxPacketLength); i++ )
			statusPacket[i] = statusPacket[i+rxPacketLength];

		rxGetLength -= rxPacketLength;
		commStatus = COMM_RXWAITING;
		return;
	}

	// Check id pairing
	if( instructionPacket[ID] != statusPacket[ID])
	{
		commStatus = COMM_RXCORRUPT;
		busUsing = 0;
		return;
	}

	// Check packet length
	if( statusPacket[LENGTH] != (rxGetLength-4))
	{
		commStatus = COMM_RXCORRUPT;
		busUsing = 0;
		return;
	}

	// Check checksum
	for( i=0; i<(statusPacket[LENGTH]+1); i++ )
		checksum += statusPacket[i+2];
	checksum = ~checksum;

	if( statusPacket[statusPacket[LENGTH]+3] != checksum )
	{
		commStatus = COMM_RXCORRUPT;
		busUsing = 0;
		return;
	}

	commStatus = COMM_RXSUCCESS;
	busUsing = 0;
}
