/*
 * PololuArrayjrk driver
 *
 * by Raymond Sheh, 2009-04-02
 * based on DynamixelArray by Raymond Sheh
 *
 * For the control of one or more daisy-chained Pololu SMC04A devices.
 *
 * Each Pololu appears as a position1D interface of this multi-interface
 * driver. The configuration file specifies the mapping between interface
 * number (consecutive starting from 0) and Pololu ID, they need
 * not match (in fact, often the Pololu IDs aren't consecutive).
 */

#include <ros/ros.h>

#include <stdlib.h>
#include <stdio.h>
//#include <sys/types.h>
//#include <sys/stat.h>
#include <fcntl.h>
//#include <unistd.h>
//#include <sys/time.h>
//#include <sys/socket.h>
//#include <netinet/in.h>
//#include <netinet/tcp.h>
//#include <sys/ioctl.h>
//#include <pthread.h>
//#include <math.h>
//#include <cmath>
#include <termios.h>
//#include <string.h>
#include <errno.h>

#include <polulu_jrk/polulu_jrk.hpp>

//#define POLULU_POSITION_ACCURACY	10
#define POLULU_POSITION_ACCURACY	6

namespace casualty {

PololuArrayJRK::PololuArrayJRK() :
    listener(*this),
    numServos(0),
    fd(0),
    debug(0),
    search(0)
{}

PololuArrayJRK::~PololuArrayJRK() {}

//void PololuArrayJRK::FromPololuToRad(unsigned short inPololu,  float *outRad, int index)
//{//{{{
//	float retval;
//  if (index < 0 || index >= numServos)
//  {
//    fprintf(stderr, "PololuArray: PololuToRad called for index %d! Should be between 0 and %d.\n", index, numServos);
//    *outRad = 0;
//		return;
//  }
//  retval = (servos[index].reverse ? -1 : 1)*(((int)inPololu - servos[index].tickZero)*servos[index].radPerTick);
//	*outRad = retval;
//}//}}}
//
//void PololuArrayJRK::FromRadToPololu(float inRad, unsigned short *outPololu, int index)
//{//{{{
//  if (index < 0 || index >= numServos)
//  {
//    fprintf(stderr, "PololuArray: RadToPololu called for index %d! Should be between 0 and %d.\n", index, numServos);
//    *outPololu = 0;
//  }
//	inRad = MAX(inRad, servos[index].minRad);
//	inRad = MIN(inRad, servos[index].maxRad);
//  *outPololu = (unsigned short)((servos[index].reverse? -1 : 1)*(inRad)/servos[index].radPerTick) + servos[index].tickZero;
//	if (debug) printf("PololuDriver: rad=%f, radPerTick=%f, tickZero=%d, pololu=%d\n", inRad, servos[index].radPerTick, servos[index].tickZero, *outPololu);
//}//}}}

////////////////////////////////////////////////////////////////////////////////
// Constructor.  Retrieve options from the configuration file and do any
// pre-Setup() setup.



void PololuArrayJRK::Initialize(crosbot::ConfigElementPtr cf)
{//{{{
	// Read an option from the configuration file
	this->port_name = cf->getParam("port", "/dev/ttyUSB0");
	if ((numServos = cf->getParamAsInt("numservos", -1)) < 1)
	{
		fprintf(stderr, "PololuArrayJRK: MUST define \"numservos\" as the number of servos in the chain in your config file!\n");
		return;
	}

	servos.resize(numServos);
	int i;
	for (i = 0; i < numServos; i ++)
	{
		char tempStr[128];
		sprintf(tempStr, "servo%d", i);
		ServoSettings& servo = servos[i];
		crosbot::ConfigElementPtr servoConfig = cf->getChild(tempStr);
		if (servoConfig != NULL) {
//			sprintf(tempStr, "servo%dID", i);
			servo.id = servoConfig->getParamAsInt("id", -1);
			if (servo.id == -1)
			{
				fprintf(stderr, "PololuArray: MUST define a Pololu ID (\"servo%dID\") for servo index %d!\n", i, i);
				return;
			}

			fprintf(stdout, "PololuArray: Read config file, player ID %d, Pololu ID %d, string %s\n", i, servo.id, tempStr);
			// Read the min and max params.
			servo.minRad = DEG2RAD(servoConfig->getParamAsDouble("min", -150.0f));
			servo.maxRad = DEG2RAD(servoConfig->getParamAsDouble("max", 150.0f));
			servo.tickZero = servoConfig->getParamAsInt("zero", 512);
			servo.reverse = servoConfig->getParamAsInt("reverse", 0);
			servo.reverse = servoConfig->getParamAsInt("invert", servo.reverse);
			servo.brakeAtDestination = servoConfig->getParamAsInt("brake", 0);
			servo.zeroOnStartup = servoConfig->getParamAsInt("zeroOnStart", 0);
//			sprintf(tempStr, "servo%ddegPerTick", i);
			servo.radPerTick = DEG2RAD(servoConfig->getParamAsDouble("degPerTick", 0.09));
			//fprintf(stdout, "PololuArray: Parameters for servo %d (Radians): [%f,%f], offset %f, reverse %d\n", i, minRad[i], maxRad[i], offsetRad[i], reverse[i]);
		} else {
			fprintf(stderr, "PololuArray: No provides entry for servo %d!\n", i);
			return;
		}
	}


	this->debug = cf->getParamAsInt("debug", 0);
	this->search = cf->getParamAsInt("search", 0);
	if (debug) printf("Config file loaded. Using port %s\n", this->port_name.c_str());

	StartThread();
}//}}}

////////////////////////////////////////////////////////////////////////////////
// Set up the device.  Return 0 if things go well, and -1 otherwise.
//int PololuArrayJRK::MainSetup()
//{//{{{
//  int retval;
//  if (debug) printf("PololuArrayjrk driver Setting up ...\n");
//
//  // Here you do whatever is necessary to setup the device, like open and
//  // configure a serial port.
//
//  retval = OpenPort();
//  if (debug) printf("Retval = %d\n", retval);
//  if (retval == -1)
//  {
//		return -1;
//  }
//  // Start the device thread; spawns a new thread and executes
//  // PololuArray::Main(), which contains the main loop for the driver.
//  StartThread();
//  return(0);
//}//}}}

////////////////////////////////////////////////////////////////////////////////
// Open the terminal
// Returns 0 on success
int PololuArrayJRK::OpenPort()
{//{{{

  // Open the port
  fd = open(port_name.c_str(), O_RDWR | O_SYNC | S_IRUSR | S_IWUSR | O_NONBLOCK);
//  fd = open(port_name, O_RDWR | O_SYNC | S_IRUSR | S_IWUSR);
  if (fd < 0)
  {
    fprintf(stderr, "Pololu: Unable to open serial port [%s]; [%s]",
                  port_name.c_str(), strerror(errno));
    return -1;
  }

  // Change port settings
  struct termios term;
  if (tcgetattr(fd, &term) < 0)
  {
    fprintf(stderr, "Pololu: Unable to get serial port attributes");
    return -1;
  }
  cfmakeraw(&term);
  cfsetispeed(&term, B9600);

  term.c_cflag |= CLOCAL | CREAD;
  term.c_cflag &= ~(CRTSCTS);

  if (tcsetattr(fd, TCSAFLUSH, &term) < 0 )
  {
    fprintf(stderr, "Pololu: Unable to set serial port attributes");
    return -1;
  }

  // Make sure queues are empty before we begin
  tcflush(fd, TCIOFLUSH);

  return 0;
}//}}}

////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
//void PololuArrayJRK::MainQuit()
//{//{{{
//  printf("PololuArray: Shutting PololuArrayjrk driver down\n");
//
//  // Stop and join the driver thread
//  StopThread();
//  // Here you would shut the device down by, for example, closing a
//  // serial port.
//	ClosePort();
//  printf("PololuArray: PololuArrayjrk driver has been shutdown\n");
//
//  return;
//}//}}}

int PololuArrayJRK::ClosePort()
{//{{{
  close(fd);
  return 0;
}//}}}

void PololuArrayJRK::SetPosition(float pos, int index) {
	ServoSettings& servo = servos[index];
	if (debug) fprintf(stdout, "Command from client: pos %f\n", pos);
	unsigned short rawAngleTicks = servo.Rad2Tick(pos);
	int brake = (rawAngleTicks == servo.currentAngleTicks);
	servo.brakedAtDestination = 0;
	servo.mode = 0;
	motor_send(index, rawAngleTicks, brake);
}

void PololuArrayJRK::SetVelocity(float vel, int index) {
	ServoSettings& servo = servos[index];
	if (debug) fprintf(stdout, "Command from client: vel %f\n", vel);
	int brake = (vel == 0);
	servo.brakedAtDestination = 0;
	servo.mode = 1;
	motor_send_vel(index, vel, brake);
}

void PololuArrayJRK::motor_send(int index, unsigned short position, int brake) {
	ServoSettings& servo = servos[index];
	unsigned char command[5];
	int res;

	// Byte is formed as follows:
	// 0: 0xAA
	// 1: <motor#>
	// 2: <command id>
	// 3: <data1>
	// 4: <data2>

	unsigned char cmdbyte = 0;
	unsigned char data1 = 0;
	unsigned char data2 = 0;
	int cmdsize;
	if (brake == 1) {
		cmdsize = 3;
		cmdbyte = 0x7F;
	} else {
		cmdsize = 4;
		// Calculation provided from Pololu JRK Documentation
		cmdbyte = 0x40 + (position & 0x1F);
		data1 = (position >> 5) & 0x7F;
	}

	command[0] = 0xAA;
	command[1] = servo.id;
	command[2] = cmdbyte;
	command[3] = data1;
	command[4] = data2;
	res = write(fd, command, cmdsize);
	if (res != cmdsize) printf("Pololu: WARNING: Could not write motor: %d\n", res);
	if (debug) printf("Pololu: Sent to motor %02x,%02x,%02x,%02x,%02x\n", command[0], command[1], command[2], command[3], command[4]);

	if (debug) printf("Pololu: Sent to motor brake: %d, position: %d\n", brake, position);
	servo.lastCommandedTicks = position;
}//}}}

void PololuArrayJRK::motor_send_vel(int index, float vel, int brake) {
	ServoSettings& servo = servos[index];
	unsigned char command[5];
	int res;

	// Byte is formed as follows:
	// 0: 0xAA
	// 1: <motor#>
	// 2: <command id>
	// 3: <data1>
	// 4: <data2>

	unsigned char cmdbyte = 0;
	unsigned char data1 = 0;
	unsigned char data2 = 0;
	int cmdsize;
	if (brake == 1) {
		cmdsize = 3;
		cmdbyte = 0x7F;
	} else {
		vel = ((servo.reverse != 0) ? -1 : 1) * vel;
		cmdsize = 4;
		if (vel > 0) {
			cmdbyte = 0x61;
		} else {
			cmdbyte = 0x60;
		}
		data1 = 127;
	}

	command[0] = 0xAA;
	command[1] = servo.id;
	command[2] = cmdbyte;
	command[3] = data1;
	command[4] = data2;
	res = write(fd, command, cmdsize);
	if (res != cmdsize) printf("Pololu: WARNING: Could not write motor: %d\n", res);
	if (debug) printf("Pololu: Sent to motor %02x,%02x,%02x,%02x,%02x\n", command[0], command[1], command[2], command[3], command[4]);
	servo.lastCommandedTicks = 0xFFFF;
}//}}}


void PololuArrayJRK::listenThread() {
	// open port
	if (OpenPort() != 0) return;

	LOG("Port open.\n");

	if (search) {
		ServoSettings testSettings;
		unsigned short rawAngleTicks;
		testSettings.lastCommandedTicks = 0;

		LOG("PololuJRK: Searching for IDs\n");
		for (int i = 0; i <= 255; ++i) {
			testSettings.id = i;
			if (!ReadPosition(testSettings, &rawAngleTicks)) {
//				LOG("PololuJRK: Search id: %d - miss\n", i);
			} else {
				LOG("PololuJRK: Search id: %d - FOUND\n", i);
			}
		}
	}


	// Zero servos on start
	for (int i = 0; i < numServos; i ++)
	{
		ServoSettings& servo = servos[i];
		if (servo.zeroOnStartup)
		{
			int brake;
			unsigned short rawAngleTicks = servo.Rad2Tick(0);
			LOG("PololuJRK: Zeroing on start %d, id %d\n", i, servo.id);
			brake = rawAngleTicks == servo.currentAngleTicks;
			motor_send(i, rawAngleTicks, brake);
		}
	}

	while (listener.operating) {
		for (int i = 0; i < numServos; i ++)
		{
			ServoSettings& servo = servos[i];
			if(debug) LOG("PololuJRK: Reading position %d, id %d\n", i, servo.id);
			unsigned short rawAngleTicks;
		  //if (debug) fprintf(stdout, "PololuJRK: Reading position %d, id %d\n", i, servo.id);
			if (!ReadPosition(servos[i], &rawAngleTicks))
			{
			    if (debug) fprintf(stdout, "PololuJRK: Unable to read position\n");
				rawAngleTicks = servo.currentAngleTicks;
			}
			float pos = servo.TickToRad(rawAngleTicks);
			//if(debug) fprintf(stdout, "PololuJRK: At position rawTicks: %d ,rads: %f, dest: %d, mode: %d\n",
			//		rawAngleTicks, pos, servo.lastCommandedTicks, servo.mode);

//			data.vel = 0;
//			data.stall = 0;
//			data.status = 0;
//			Publish(addr[i], PLAYER_MSGTYPE_DATA, PLAYER_POSITION1D_DATA_STATE, &data, sizeof(player_position1d_data_t), NULL);
			// If we brake when we reach the right position, do it here.
			if ((servo.mode == 0) && servo.brakeAtDestination && (abs(rawAngleTicks - servo.lastCommandedTicks) < POLULU_POSITION_ACCURACY) && !servo.brakedAtDestination)
//			if (brakeAtDestination[i] && (fabs(rawAngleTicks - lastCommandedTicks[i]) < 10))
			{
				if(debug) fprintf(stdout, "PololuJRK: brake!\n");
				motor_send(i, servo.lastCommandedTicks, 1);
				servo.brakedAtDestination=1;
			}
			servo.currentAngleTicks = rawAngleTicks;
		}

		// TODO: Publish servo positions
		PublishPosition();

		usleep(10000);
	}
	// Stop servos
	for (int i = 0; i < numServos; i ++)
	{
		ServoSettings& servo = servos[i];
		motor_send(i, servo.lastCommandedTicks, 1);
	}

	// Close port
	ClosePort();
}

////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
//void PololuArrayJRK::Main()
//{//{{{
//  // The main loop; interact with the device here
//  int i;
//	for (i = 0; i < numServos; i ++)
//	{
//		ServoSettings& servo = servos[i];
//		if (servo.zeroOnStartup)
//		{
//			int brake;
//			unsigned short rawAngleTicks = servo.Rad2Tick(0);
//			brake = rawAngleTicks == servo.currentAngleTicks;
//			motor_send(i, rawAngleTicks, brake);
//		}
//	}
//  for(;;)
//  {
//    if (debug) printf("In main loop ...\n");
//    // test if we are supposed to cancel
//    pthread_testcancel();
//
//    // Process incoming messages.  PololuArray::ProcessMessage() is
//    // called on each message.
////    ProcessMessages();
//		// We need to add a delay in here before we read otherwise for some reason we keep getting stale data.
////    usleep(1000);
//
//    // Interact with the device, and push out the resulting data, using
//    // Driver::Publish()
//
//		for (i = 0; i < numServos; i ++)
//		{
//			ServoSettings& servo = servos[i];
////			player_position1d_data_t data;
////			memset(&data, 0, sizeof(data));
//		  unsigned short rawAngleTicks;
//if (debug) fprintf(stdout, "Pololu: Reading position %d, id %d\n", i, servo.id);
//			if (!ReadPosition(i, &rawAngleTicks))
//			{
//if (debug) fprintf(stdout, "Pololu: Unable to read position\n");
////				continue;
//				rawAngleTicks = servo.currentAngleTicks;
//			}
//			float pos = servo.TickToRad(rawAngleTicks);
//			if(debug) fprintf(stdout, "Pololu: At position rawTicks: %d ,rads: %f, dest: %d, mode: %d\n",
//					rawAngleTicks, pos, servo.lastCommandedTicks, servo.mode);
//
////			data.vel = 0;
////			data.stall = 0;
////			data.status = 0;
////			Publish(addr[i], PLAYER_MSGTYPE_DATA, PLAYER_POSITION1D_DATA_STATE, &data, sizeof(player_position1d_data_t), NULL);
//			// If we brake when we reach the right position, do it here.
//			if ((servo.mode == 0) && servo.brakeAtDestination && (fabs(rawAngleTicks - servo.lastCommandedTicks) < 10) && !servo.brakedAtDestination)
////			if (brakeAtDestination[i] && (fabs(rawAngleTicks - lastCommandedTicks[i]) < 10))
//			{
//				if(debug) fprintf(stdout, "Pololu: brake!\n");
//				motor_send(i, servo.lastCommandedTicks, 1);
//				servo.brakedAtDestination=1;
//			}
//			servo.currentAngleTicks = rawAngleTicks;
//		}
//
//    // repeat frequency (default to 10 Hz)
//    usleep(10000);
//  }
//}//}}}

/**
 * Reads the current position of the Pololu in ticks. Returns 1 if
 * everything is OK, 0 otherwise.
 */
int PololuArrayJRK::ReadPosition(ServoSettings& servo, unsigned short * outTicks)
{
//	ServoSettings& servo = servos[index];
	//TODO:WHY???? if (servo.lastCommandedTicks == 0xFFFF) return 0;
	unsigned char command[5];
	int res;

	command[0] = 0xAA;
	command[1] = servo.id;
	command[2] = 0x25; // 0xA5
	res = write(fd, command, 3);
	if (res != 3) printf("Pololu: WARNING: Could not write motor: %d\n", res);


	unsigned char status[2];
	status[0] = 0;
	status[1] = 0;

	int retval = -1;

	tcflush(fd, TCIFLUSH);
	int waitLimit = 100;
	// wait for data

	while ((waitLimit--) > 0 && ((retval = read(fd, status, 1)) <= 0))
	{
//	fprintf(stderr, "%d %x\n", retval, status[0]);
		usleep(1000);
	}
//	fprintf(stderr, "%d %x\n", retval, status[0]);
	if (waitLimit <= 0)
	{
		fprintf(stderr, "PololuArray: Error, got no data!\n");
		return 0;
	}
	waitLimit = 100;
//	retval = read(fd, status+1, 1);
	while ((waitLimit--) > 0 && ((retval = read(fd, status+1, 1)) <= 0))
	{
		usleep(1000);
	}
	if (waitLimit <= 0)
	{
		fprintf(stderr, "PololuArray: Error reading second byte. The first one was %x\n", status[0]);
		return 0;
	}

	short rawPIDError;
	rawPIDError = (short)(status[0]) | ((short)(status[1] & 0xFF) << 8);
	int position = (int)(rawPIDError);
	*outTicks = (unsigned short)position;
	return 1;
}

} // namespace casualty
