/*
 * URCLinterface.cpp
 *
 *  Created on: Jan 14, 2019
 *      Author: emanuele
 *
 *  Implementazione C++ del protocollo di comunicazione socket per Universal Robot e-series.
 *  Protocollo client interface, comunicazione 10Hz su porte 30001 - 30002, informazioni e messaggi parziali,
 *  invio di comandi URscript
 *
 *  20190114 - Prima Scittura
 *  20190114 - Inizio implementazione della connessione socket con libreria boost, scritta funzione connect.
 *  20190115 - Continuo scirttura funzione dataRead(), verificato il protocollo rispetto al manuale, la funzione legge e divide corretamente i pacchetti
 *  			tra di loro. ora bisogna separare i sottopacchetti.
 *  20190116 - La separazione dei sottopacchetti viene fatta da delle classi specializzate che scompattano e cambiano
 *  			l'endianness dei dati se necessario.
 *  20190117 - Prima scirttura delle classi che scompattano i dati, scirtta versionMessage, risolti alcuni dubbi
 *  			e problemi con i puntatori. A quanto pare non è disponibile una libreria che faccia quello che
 *  			fa struct.unpack di python
 *  20190118 - Completata la scrittura delle classi che scompattano i dati, le strutture combaciano con il
 *  			manuale. Inizio della scrittura finzioni che mandano i comandi URscript al controller,
 *  			valutare se vale la pena implementare per ogn tipo di funzione movimento
 *  			prevista in URscript o no..
 *  20190121 - Passaggio a nuova Vm con Ubuntu 18.04LTS e g++ 7.3.0, risolti alcuni problemi di compilazione,
 *  			passaggio a librerie boost-1.68
 *  			Scritta funzione di invio script grezzi al controller, inizio scrittura a thread che continua a ricevere
 *  			i dati dal robot.
 */

#include "URCLinterface.h"

using namespace subpackage;
using namespace std;
using boost::format;
using std::string;


UR_CLinterface::UR_CLinterface(shared_ptr<spdlog::logger> logger) {
	log = logger;
	log->info("Initializing UR Client Interface"); // @suppress("Ambiguous problem")
	rsocket = new tcp::socket(io_service);
}

UR_CLinterface::~UR_CLinterface() {
	// TODO Auto-generated destructor stub
	if (rsocket->is_open()) this->disconnect();
	delete rsocket;
}

bool UR_CLinterface::connect(const char* ip, uint16_t port) {
	try {
		rsocket->connect(
				tcp::endpoint(boost::asio::ip::address::from_string(ip), port));
		if ((bool) rsocket->is_open()) {
			log->info("Connected to {0} on port {1}", ip, port);
			return true;
		}
	} catch (boost::system::error_code* e) {
		log->error(e->message());
		return false;
	}
	return false;
}

void UR_CLinterface::disconnect(void) {
	rsocket->close(error);
	if (error){
		log->error("Cannot close socket, code: {}", error.message()); // @suppress("Assignment in condition")
		return;
	}
	log->info("Disconnected"); // @suppress("Ambiguous problem")
}

void UR_CLinterface::sendCommand(std::string command) {
	char* dataOut=NULL;
	if (rsocket->is_open()){
		command = command + '\n';
		dataOut = (char*)malloc(command.size());
		strncpy(dataOut, command.c_str(), command.size());
		log->debug("Sending: {}, s:{}",dataOut,command.size());
		boost::asio::write(*rsocket, boost::asio::buffer(dataOut, command.size()), error);
		free(dataOut);
	}
	if (!error) return;
	else {
		log->error("Failed to send data to socket, code: {}", error.message());
	}

}

void UR_CLinterface::parseData(void){
	uint32_t packageSize=lastDataLen;
	uint32_t subPackageSize;
	unsigned char packageType;
	unsigned char subPackageType;
	uint32_t currIndex=0;
	clientSubpackage* subP=NULL;

	//main message type is on 4th byte of the message
	packageType = lastData[sizeof(uint32_t)];

	switch(packageType){
		case VERSION_MESSAGE: //begin version message parse
			if (firstMessage){
				log->info("Version Message: size[{}]",packageSize); // @suppress("Ambiguous problem")
				subP = new versionSub(lastData, &clientData);
				subP->parse();
				firstMessage=false;
				long t=(long)clientData.versonMessage.timestamp;
                std::string timestr=std::string(asctime(gmtime(&t)));
                timestr.pop_back();
                log->info("Connection Time: {}", timestr);
				log->info("Controller Version {0}.{1}", clientData.versonMessage.majorVersion,
						clientData.versonMessage.minorVersion);
				log->info("ProjectName: {}", clientData.versonMessage.projectName->c_str());
			}
			break;

		case ROBOT_MESSAGE: // begin subpacket parse
			// move index to skip package leader
			currIndex = sizeof(uint32_t)+sizeof(char);
			log->debug("Robot Message: size[{}]",packageSize);
			// iterate until end of package
			while (currIndex < lastDataLen){
				// free memory
				if (subP != NULL) {
					delete subP;
					subP = NULL;
				}

				//extract subpackage size
				memcpy(&subPackageSize, lastData+currIndex, sizeof(uint32_t));
				subPackageSize=big_to_native(subPackageSize);
				// extract subpackage type
				subPackageType=lastData[currIndex+sizeof(uint32_t)];

				try{
					switch (subPackageType){
						case ROBOT_MODE_DATA:
							log->debug("SubP_ModeData"); // @suppress("Ambiguous problem")
							subP = new robotModeSub(&lastData[currIndex], &clientData);
							break;
						case JOINT_DATA:
							log->debug("SubP_JointData"); // @suppress("Ambiguous problem")
							subP = new jointDataSub(&lastData[currIndex], &clientData);
							break;
						case TOOL_DATA:
							log->debug("SubP_ToolData"); // @suppress("Ambiguous problem")
							subP = new toolDataSub(&lastData[currIndex], &clientData);
							break;
						case MASTER_BOARD_DATA:
							log->debug("SubP_MasterBoardData"); // @suppress("Ambiguous problem")
							subP = new masterBoardSub(&lastData[currIndex], &clientData);
							break;
						case CARTESIAN_INFO:
							log->debug("SubP_CartesianInfo"); // @suppress("Ambiguous problem")
							subP = new cartesianInfoSub(&lastData[currIndex], &clientData);
							break;
						case KINEMATICS_INFO:
							log->debug("SubP_KinematicsInfo"); // @suppress("Ambiguous problem")
							subP = new kineInfoSub(&lastData[currIndex], &clientData);
							break;
						case CONFIGURATION_DATA:
							log->debug("SubP_ConfigurationData"); // @suppress("Ambiguous problem")
							subP = new configurationSub(&lastData[currIndex], &clientData);
							break;
						case FORCE_MODE_DATA:
							log->debug("SubP_ForceModeData"); // @suppress("Ambiguous problem")
							subP = new forceModeSub(&lastData[currIndex], &clientData);
							break;
						case ADDITIONAL_INFO:
							log->debug("SubP_AdditionalInfo"); // @suppress("Ambiguous problem")
							subP = new additionalInfoSub(&lastData[currIndex], &clientData);
							break;
						case CALIBRATION_DATA:
							log->debug("SubP_CalibrationData"); // @suppress("Ambiguous problem")
							subP = new calibrationSub(&lastData[currIndex], &clientData);
							break;
						case TOOL_COM_INFO:
							log->debug("SubP_ToolComInfo"); // @suppress("Ambiguous problem")
							subP = new toolComSub(&lastData[currIndex], &clientData);
							break;
						case TOOL_MODE_INFO:
							log->debug("SubP_ToolModeInfo"); // @suppress("Ambiguous problem")
							subP = new toolModeSub(&lastData[currIndex], &clientData);
							break;
						case SAFETY_DATA:
							//not yet implemented
							break;
						default:
							log->error("Unknown Subpackage: {}", subPackageType);
							break;
					}
					if (subP != NULL) subP->parse();
					currIndex+=subPackageSize;
				} catch (long sizediff){
					log->error("Package Length Mismatch: {}",sizediff);
					currIndex+=subPackageSize;
				}
			}
			log->debug("End of Package"); // @suppress("Ambiguous problem")
			break;

		default:
			log->error("Unknown package type: {}",packageType);
			break;
	}
	clientData.lastUpdate = time(NULL);
}

void UR_CLinterface::readData(void) {
	uint32_t packlen;
	uint32_t readata = 0;
	string rawdata;

	if (rsocket->is_open()){
		// clear last data vector
		if(lastData!=NULL)
			free(lastData);
		//read first incoming byte for packet length
		boost::asio::read(*rsocket, receive_buffer, boost::asio::transfer_exactly(HEADERSIZE), error);
		if (!error) {
			const unsigned char* f = boost::asio::buffer_cast<const unsigned char*>(receive_buffer.data());
			memcpy(&packlen, f, HEADERSIZE * sizeof(char));
			// all number are represented in network byte order, conversion required
			packlen = ntohl(packlen);
			// allocate memory for incoming package
			lastData = (unsigned char*)malloc(packlen);
			// blocking read of exactly packlen bytes
			readata += boost::asio::read(*rsocket, receive_buffer,boost::asio::transfer_exactly(packlen), error);
			//retreive all data from buffer
			const unsigned char* ff = boost::asio::buffer_cast<const unsigned char*>(receive_buffer.data());
			// flush buffer for reuse
			receive_buffer.consume(packlen);

			// print packet dump
			log->debug("Successful read: {0} bytes", readata);
			if (log->level() == spdlog::level::trace) {
				char hh[5];
				for (uint32_t k = 0; k < packlen; k++) {
					sprintf(hh, "%02x|", ff[k]);
					rawdata.append(hh);
					if (k % 16 == 0 && k != 0)
						rawdata.append("\n");
				}
				log->trace("\n{}\n", rawdata);
			}
			//export data to vector
			memcpy(lastData, ff, packlen);
			lastDataLen=packlen;
		} else {
			log->error("Failed to read data from socket, code: {}", error.message());
		}
	} else {
		log->error("Robot not connected!, call connect(ip,port) instead"); // @suppress("Ambiguous problem")
	}
}