/*
 * MovementControl.cpp
 *
 *  Created on: 06.12.2012
 *      Author: georg
 */

#include "MovementControl.h"


MovementControl::MovementControl() {
	this->comPortHandle = -1;
	this->position_x = 0;
	this->position_y = 0;
	this->position_z = 0;
	this->errorCount = 0;
}

MovementControl::~MovementControl() {
	close(this->comPortHandle);
}

/**
 * Returns the com port.
 * If no com port is open, the value is -1.
 */
int MovementControl::getComPortHandle(){
	return comPortHandle;
}

/**
 * Returns the maximum position value, which can be addressed.
 * Can be used as positive and negative limit.
 */
int MovementControl::getMaxPosition(){
	return this->maxPosition;
}

/**
 * Returns the maximum speed to move the table.
 * Can be used as positive and negative limit.
 */
int MovementControl::getMaxMovementSpeed(){
	return this->maxMovementSpeed;
}

/**
 * Returns the maximum ramp value.
 * The ramp value is always positive. (ramp >= 0)
 */
int MovementControl::getMaxRampValue(){
	return this->maxRampValue;
}

/**
 * Returns the current ramp value.
 * @param axis: Indicates the axis, which should be used
 */
int MovementControl::getRampValue(MC_axes_flag axis){
	// generate instruction
	std::stringstream ss;
	ss << "GR," << axis << "\r";
	std::string instructionString = ss.str();

	return sendToComPort(instructionString);
}

/**
 * Returns the current counter value.
 * @param axis: Indicates the axis, which should be used
 */
int MovementControl::getPosition(MC_axes_flag axis){
	// generate instruction
	std::stringstream ss;
	ss << "GC," << axis << "\r";
	std::string instructionString = ss.str();

	return sendToComPort(instructionString);
}

/**
 * Returns the current voltage value.
 */
int MovementControl::getVoltage(){
	return sendToComPort("GV\r");
}

/**
 * Set the current position to "counter".
 * @param axis:	    Indicates the axis, which should be used
 * @param counter:  The counter will be set to the value of "counter"
 */
int MovementControl::setPosition(MC_axes_flag axis, int counter){
	// generate instruction
	std::stringstream ss;
	ss << "SC," << axis << "," << counter << "\r";
	std::string instructionString = ss.str();

	return sendToComPort(instructionString);
}

/**
 * Open the com port to use the connected device.
 * @param device: Indicates the device which will be opened
 */
int MovementControl::openComPort(const char* device){
	comPortHandle = open( device, O_RDWR | O_NOCTTY );
	if (comPortHandle <= 0){
		std::cout << "Failed to open " << device << " - " << errno << " (" << strerror(errno) << ")\n";
		if(errno == 13){
			return -2;
		}
		return -1;
	}

	struct termios oldtio,newtio;

	tcgetattr( comPortHandle, &oldtio ); // save current port settings

	bzero( &newtio, sizeof(newtio) );
	// Set custom rate to 200kbps 8N1 - we're actually sending 8N2 but get 8N1 back
	newtio.c_cflag = B38400 | /*CRTSCTS |*/ CS8 | CLOCAL | CREAD;
	newtio.c_iflag = /*IGNPAR;*/ INPCK | ISTRIP | (IXON | IXOFF | IXANY);
	newtio.c_oflag = /*0;*/ OPOST | ONLCR;

	// set input mode (non-canonical, no echo,...)
	newtio.c_lflag = 0;

	newtio.c_cc[VTIME]    = 0;   // inter-character timer unused
	newtio.c_cc[VMIN]     = 5;   // blocking read until 5 chars received

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

	// Now use TIOCSSERIAL ioctl to set custom divisor
	// FTDI uses base_baud 24000000 so in theory a divisor

	struct serial_struct sio; // From /usr/include/linux/serial.h
	int ioctl_res = ioctl( comPortHandle, TIOCGSERIAL, &sio );
	if (ioctl_res < 0){
		std::cout << "Failed TIOCGSERIAL ioctl: error " << errno << " (" << strerror(errno) << ")\n";
		close( comPortHandle );
		return -1;
	}

	sio.flags = ((sio.flags & ~ASYNC_SPD_MASK) | ASYNC_SPD_CUST);
	sio.custom_divisor = sio.baud_base / 600000;
	ioctl_res = ioctl( comPortHandle, TIOCSSERIAL, &sio );
	if (ioctl_res < 0){
		std::cout << "Failed TIOCGSERIAL ioctl: error " << errno << " (" << strerror(errno) << ")\n";
		close( comPortHandle );
		return -1;
	}

	tcsetattr(STDOUT_FILENO,TCSANOW,&oldtio);

	// read() function will return immediately
	fcntl(comPortHandle, F_SETFL, FNDELAY);


	return EXIT_SUCCESS;
}

/**
 * Closes the comPort and set the comPortHandle to -1 (no comPort open)
 */
int MovementControl::closeComPort(){
	close(comPortHandle);
	comPortHandle = -1;

	return 0;
}

/**
 * @param axis:		Indicates the moved axis
 * @param speed:	Indicates the speed at which the table moves, negative values will move in opposite direction
 */
int MovementControl::moveWithSpeed(MC_axes_flag axis ,int speed){
	if(comPortHandle == -1){
		return -1;
	}
	if(speed > maxMovementSpeed){
		speed = maxMovementSpeed;
	}
	if(speed < -maxMovementSpeed){
		speed = -maxMovementSpeed;
	}

	// generate instruction
	std::stringstream ss;
	ss << "PV," << axis << "," << speed << "\r";
	std::string instructionString = ss.str();

	return sendToComPort(instructionString);
}

/**
 * Moves the axis by "counter" steps.
 * @param axis:		Indicates the moved axis
 * @param counter:	Represents the number of steps to be made, negative values will change the direction
 */
int MovementControl::moveSingleStep(MC_axes_flag axis, int counter){
	// generate instruction
	std::stringstream ss;
	ss << "PS," << axis << "," << counter << "\r";
	std::string instructionString = ss.str();

	return sendToComPort(instructionString);
}

/**
 * Moves the "axis" with "speed" to position "counter" relative to the current position.
 * @param axis:		Indicates the moved axis
 * @param speed:	Indicates the speed at which the table moves, negative values will move in opposite direction
 * @param counter:	Represents the destiny position
 */
int MovementControl::moveRelativeToPosition(MC_axes_flag axis, int speed, int counter){
	if(speed > maxMovementSpeed){
		speed = maxMovementSpeed;
	}
	if(speed < -maxMovementSpeed){
		speed = -maxMovementSpeed;
	}

	// generate instruction
	std::stringstream ss;
	ss << "PM," << axis << "," << speed << "," << counter << "\r";
	std::string instructionString = ss.str();

	return sendToComPort(instructionString);
}

/**
 * Moves the "axis" with the "speed" to position "counter", absolute to the counter position
 * @param axis:		Indicates the moved axis
 * @param speed:	Indicates the speed at which the table moves, negative values will move in opposite direction
 * @param counter:	Represents the destiny position
 */
int MovementControl::moveAbsoluteToPosition(MC_axes_flag axis, int speed, int counter){
	if(speed > maxMovementSpeed){
		speed = maxMovementSpeed;
	}
	if(speed < -maxMovementSpeed){
		speed = -maxMovementSpeed;
	}

	// generate instruction
	std::stringstream ss;
	ss << "PA," << axis << "," << speed << "," << counter << "\r";
	std::string instructionString = ss.str();

	return sendToComPort(instructionString);
}

/**
 * @param axis: Indicates the stopped axis
 */
int MovementControl::stopFast(MC_axes_flag axis){
	int decimal = 0;
	if(axis == MC_Z_AXIS){
		decimal = 4;
	} else {
		decimal = axis;
	}
	// generate instruction
	std::stringstream ss;
	ss << "FS," << decimal << "\r";
	std::string instructionString = ss.str();

	return sendToComPort(instructionString);
}

/**
 * Changes the ramp value.
 * @param axis:		Indicates the axis
 * @param value:	Represents the new ramp height
 */
int MovementControl::changeRampValue(MC_axes_flag axis, int value){
	if(value > maxRampValue){
		value = maxRampValue;
	}
	if(value < 0){
		value = 0;
	}

	// generate instruction
	std::stringstream ss;
	ss << "SR," << axis << "," << value << "\r";
	std::string instructionString = ss.str();

	return sendToComPort(instructionString);
}

/**
 * Sends the message to the comPort and returns the value of the response.
 * @param message:	The string which will be sent to the comPort.
 */
int MovementControl::sendToComPort(std::string message){
	if(comPortHandle == -1){
		return -1;
	}
	ssize_t writeAnswer = write(comPortHandle, message.c_str(), message.size());
	if(writeAnswer == -1){
		std::cout << "Can't write the message: " << message << " - " << errno << " (" << strerror(errno) << ")\n";
		errorCount++;
		if(errorCount > 20){
			return -2;
		}
		return 0;
	}
	errorCount = 0;

	usleep(10);

	return readAnswerValue(message);
}

/**
 * Returns the value of the response.
 * For example it could be the speed, when you use the PV command or the voltage when you use GV.
 */
int MovementControl::readAnswerValue(std::string message){
	std::string answer = readAnswer(message);

	int counter = 0;

	while(!(answer[0] == message[0] && answer[1] == message[1]) && counter < 5){
		answer = readAnswer(message);
		counter++;
	}
	if(counter >= 5){
		return 0;
	}

	std::string number = "";
	if(answer[0] == 'P' && answer[1] == 'V'){
		for(int i = 0; i < 6; i++){
			number += answer[i+5];
		}
	} else if( answer[0] == 'P' && answer[1] == 'S'){

	} else if( answer[0] == 'P' && answer[1] == 'M'){

	} else if( answer[0] == 'P' && answer[1] == 'A'){

	} else if( answer[0] == 'S' && answer[1] == 'R'){
		for(int i = 0; i < 5; i++){
			number += answer[i+6];
		}
	} else if( answer[0] == 'S' && answer[1] == 'C'){
		for(int i = 0; i < 11; i++){
			number += answer[i+5];
		}
	} else if( answer[0] == 'S' && answer[1] == 'F'){
		for(int i = 0; i < 1; i++){
			number += answer[i+5];
		}
	} else if( answer[0] == 'G' && answer[1] == 'R'){
		for(int i = 0; i < 5; i++){
			number += answer[i+6];
		}
	} else if( answer[0] == 'G' && answer[1] == 'C'){
		for(int i = 0; i < 11; i++){
			number += answer[i+5];
		}
	} else if( answer[0] == 'G' && answer[1] == 'V'){
		for(int i = 0; i < 6; i++){
			number += answer[i+3];
		}
	} else {

	}
	std::stringstream ss(number);
	int i = 0;
	ss >> i;

	return i;
}

/**
 * Reads the response of the comPort and returns it as a string.
 */
std::string MovementControl::readAnswer(std::string message){
	usleep(10);
	int rightAnswerSize = 0;
	int answerSize = 0;

	if(message[0] == 'P' && message[1] == 'V'){
		rightAnswerSize = 11;
	} else if( message[0] == 'P' && message[1] == 'S'){
		rightAnswerSize = 11;
	} else if( message[0] == 'P' && message[1] == 'M'){
		rightAnswerSize = 23;
	} else if( message[0] == 'P' && message[1] == 'A'){
		rightAnswerSize = 23;
	} else if( message[0] == 'S' && message[1] == 'R'){
		rightAnswerSize = 11;
	} else if( message[0] == 'S' && message[1] == 'C'){
		rightAnswerSize = 14;
	} else if( message[0] == 'S' && message[1] == 'F'){
		rightAnswerSize = 4;
	} else if( message[0] == 'G' && message[1] == 'R'){
		rightAnswerSize = 11;
	} else if( message[0] == 'G' && message[1] == 'C'){
		rightAnswerSize = 16;
	} else if( message[0] == 'G' && message[1] == 'V'){
		rightAnswerSize = 9;
	} else {
		rightAnswerSize = 0;
	}

	char buffer[64] = {0};
	int currentAnswerSize = read(comPortHandle, buffer, sizeof(buffer));
	if(currentAnswerSize > 0){
		answerSize = currentAnswerSize;
	}

	std::stringstream ss(buffer);
	std::string answer = ss.str();

	int counter = 0;
	while(rightAnswerSize > answerSize && rightAnswerSize != 0 && counter < 5){
		usleep(counter*100);

		for(int i = 0; i < 64; i++){
			buffer[i] = '\0';
		}

		currentAnswerSize = read(comPortHandle, buffer, sizeof(buffer));
		if(currentAnswerSize > 0){
			answerSize += currentAnswerSize;
		}

		std::stringstream ss(buffer);
		answer += ss.str();

		counter++;
	}

	return answer;
}
