/*
 * clientMessage.cpp
 *
 *  Created on: Jan 16, 2019
 *      Author: emanuele
 */


#include "clientMessage.h"


namespace subpackage {

	clientSubpackage::clientSubpackage() {
	}

	clientSubpackage::~clientSubpackage() {
	}

	uint32_t clientSubpackage::parse() {
		return 0;
	}

	versionSub::~versionSub() {
	}

	configurationSub::~configurationSub() {
	}

	calibrationSub::~calibrationSub() {
	}

	robotModeSub::~robotModeSub() {
	}

	forceModeSub::~forceModeSub() {
	}

	kineInfoSub::~kineInfoSub() {
	}

	jointDataSub::~jointDataSub() {
	}

	cartesianInfoSub::~cartesianInfoSub() {
	}

	additionalInfoSub::~additionalInfoSub() {
	}

	toolDataSub::~toolDataSub() {
	}

	toolModeSub::~toolModeSub() {
	}

	toolComSub::~toolComSub() {
	}

	masterBoardSub::~masterBoardSub() {
	}

	uint32_t subpackage::clientSubpackage::getUInt32(unsigned char** dataPointer) {
		uint32_t d;
		memcpy(&d, *dataPointer, sizeof(uint32_t));
		*dataPointer+=sizeof(uint32_t);
		return big_to_native(d);
	}

	uint64_t subpackage::clientSubpackage::getUInt64(unsigned char** dataPointer) {
		uint64_t d;
		memcpy(&d, *dataPointer, sizeof(uint64_t));
		*dataPointer+=sizeof(uint64_t);
		return big_to_native(d);

	}

	int32_t subpackage::clientSubpackage::getInt32(unsigned char** dataPointer) {
		int32_t d;
		memcpy(&d, *dataPointer, sizeof(int32_t));
		*dataPointer+=sizeof(int32_t);
		return big_to_native(d);
	}

	double subpackage::clientSubpackage::getDouble(unsigned char** dataPointer) {
		union{
			uint64_t i;
			double dd;
		} d;
		memcpy(&d, *dataPointer, sizeof(double));
		d.i=big_to_native(d.i);
		*dataPointer+=sizeof(double);
		return d.dd;
	}

	float subpackage::clientSubpackage::getFloat(unsigned char** dataPointer) {
		union{
			uint32_t i;
			float dd;
		} d;
		memcpy(&d, *dataPointer, sizeof(float));
		d.i=big_to_native(d.i);
		*dataPointer+=sizeof(float);
		return d.dd;
	}

	char subpackage::clientSubpackage::getChar(unsigned char** dataPointer) {
		char val=**dataPointer;
		*dataPointer+=1;
		return val;
	}

	unsigned char subpackage::clientSubpackage::getUChar(unsigned char** dataPointer) {
		unsigned char val=**dataPointer;
		*dataPointer+=1;
		return val;
	}

	void clientSubpackage::copyString(std::string **out, unsigned char** in, char len) {
		char* s=NULL;
		if (*out != NULL)
			delete(out);
		s = (char*)malloc(len+1);
		strncpy(s, (const char*)*in, len);
		s[(unsigned char)len]='\0';
		*out = new std::string(s);
		free(s);
		*in+=len;

	}
	//////////////////////////////////////////////////////////////////////////////
	/////////////// Overload of Parse Method /////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////////
	uint32_t subpackage::robotModeSub::parse() {
		robotModeData_t *v = &dataOut->robotMode;
		unsigned char* begin = dataIn;
		uint32_t size;

		size = v->packageSize = this->getInt32(&dataIn);
		v->packageType = this->getUChar(&dataIn);
		v->timestamp = this->getUInt64(&dataIn);
		v->isRealRobotConnected = this->getUChar(&dataIn);
		v->isRealRobotEnabled =  this->getUChar(&dataIn);
		v->isRobotPowerOn  = this->getUChar(&dataIn);
		v->isEmergencyStopped = this->getUChar(&dataIn);
		v->isProtectiveStopped = this->getUChar(&dataIn);
		v->isProgramRunning = this->getUChar(&dataIn);
		v->isProgramPaused = this->getUChar(&dataIn);
		v->robotMode = this->getUChar(&dataIn);
		v->controlMode = this->getUChar(&dataIn);
		v->targetSpeedFraction = this->getDouble(&dataIn);
		v->speedScaling = this->getDouble(&dataIn);
		v->targetSpeedFractionLimit = this->getDouble(&dataIn);
		v->internalValue = this->getUChar(&dataIn);

		// check if read data is aligned with package size
		if (dataIn-begin != size) throw dataIn-begin-size;
		return dataIn-begin;
		}

	uint32_t subpackage::jointDataSub::parse() {
		jointData_t *v = &dataOut->jointData;
		unsigned char* begin = dataIn;
		uint32_t size;
		uint32_t c;

		size = v->packageSize = this->getInt32(&dataIn);
		v->packageType = this->getUChar(&dataIn);
		for (c=0; c<JOINT_NUMBER; c++){
			v->jointParam[c].actual = this->getDouble(&dataIn);
			v->jointParam[c].target = this->getDouble(&dataIn);
			v->jointParam[c].speedActual = this->getDouble(&dataIn);
			v->jointParam[c].current = this->getFloat(&dataIn);
			v->jointParam[c].voltage = this->getFloat(&dataIn);
			v->jointParam[c].motorTemp = this->getFloat(&dataIn);
			v->jointParam[c].nc = this->getFloat(&dataIn);
			v->jointParam[c].mode = this->getUChar(&dataIn);
		}

		// check if read data is aligned with package size
		if (dataIn-begin != size) throw dataIn-begin-size;
		return dataIn-begin;
		}

	uint32_t subpackage::cartesianInfoSub::parse() {
		cartesianInfo_t *v = &dataOut->cartesianInfo;
		unsigned char* begin = dataIn;
		uint32_t size;
		uint32_t c;

		size = v->packageSize = this->getInt32(&dataIn);
		v->packageType = this->getUChar(&dataIn);
		for (c=0; c<JOINT_NUMBER; c++){
            v->cartPosition.pos[c] = this->getDouble(&dataIn);
		}
		for (c=0; c<JOINT_NUMBER; c++){
            v->cartTCPoffset.ofst[c] = this->getDouble(&dataIn);
		}

		// check if read data is aligned with package size
		if (dataIn-begin != size) throw dataIn-begin-size;
		return dataIn-begin;
	}

	uint32_t subpackage::masterBoardSub::parse() {
		masterBoardData_t *v = &dataOut->masterBoard;
		unsigned char* begin = dataIn;
		uint32_t size;

		size = v->packageSize = this->getInt32(&dataIn);
		v->packageType = this->getUChar(&dataIn);
		v->digitalInputBits = this->getUInt32(&dataIn);
		v->digitalOutputBits = this->getUInt32(&dataIn);
		v->analogInputRange0 = this->getUChar(&dataIn);
		v->analogInputRange1 = this->getUChar(&dataIn);
		v->analogInput0 = this->getDouble(&dataIn);
		v->analogInput1 = this->getDouble(&dataIn);
		v->analogOutputDomain0 = this->getChar(&dataIn);
		v->analogOutputDomain1 = this->getChar(&dataIn);
		v->analogOutput0 = this->getDouble(&dataIn);
		v->analogOutput1 = this->getDouble(&dataIn);
		v->masterBoardTemperature = this->getFloat(&dataIn);
		v->robotVoltage48V = this->getFloat(&dataIn);
		v->robotCurrent = this->getFloat(&dataIn);
		v->masterIOCurrent = this->getFloat(&dataIn);
		v->safetyMode = this->getUChar(&dataIn);
		v->InReducedMode = this->getUChar(&dataIn);
		v->euromap67InterfaceInstalled = this->getChar(&dataIn);
		#ifdef EUROMAP
		v->euromapInputBits = this->getUInt32(&dataIn);
		v->euromapOutputBits = this->getUInt32(&dataIn);
		v->euromapVoltage24V = this->getFloat(&dataIn);
		v->euromapCurrent = this->getFloat(&dataIn);
		#endif
		v->nc = this->getUInt32(&dataIn);
		v->operationalModeSelectorInput = this->getUChar(&dataIn);
		v->threePositionEnblingDeviceInput = this->getUChar(&dataIn);

		// check if read data is aligned with package size
		if (dataIn-begin != size) throw dataIn-begin-size;
		return dataIn-begin;
	}

	uint32_t subpackage::toolDataSub::parse() {
		toolData_t *v = &dataOut->toolData;
		unsigned char* begin = dataIn;
		uint32_t size;

		size = v->packageSize = this->getInt32(&dataIn);
		v->packageType = this->getUChar(&dataIn);
		v->analogInputRange0 = this->getUChar(&dataIn);
		v->analogInputRange1 = this->getUChar(&dataIn);
		v->analogInput0 = this->getDouble(&dataIn);
		v->analogInput1 = this->getDouble(&dataIn);
		v->toolVoltage48V = this->getFloat(&dataIn);
		v->toolOutputVoltage = this->getUChar(&dataIn);
		v->toolCurrent = this->getFloat(&dataIn);
		v->toolTemperature = this->getFloat(&dataIn);
		v->toolMode = this->getUChar(&dataIn);

		// check if read data is aligned with package size
		if (dataIn-begin != size) throw dataIn-begin-size;
		return dataIn-begin;
	}

	uint32_t subpackage::kineInfoSub::parse() {
		kinematicsInfo_t *v = &dataOut->kineInfo;
		kineDH_t *k=&dataOut->kineInfo.kinematics;
		unsigned char* begin = dataIn;
		uint32_t size;
		uint32_t c;

		size = v->packageSize = this->getInt32(&dataIn);
		v->packageType = this->getUChar(&dataIn);
		for(c=0; c<JOINT_NUMBER; c++){
			v->checksum[c]=this->getUInt32(&dataIn);
		}
		for(c=0; c<JOINT_NUMBER; c++){
			k->DHtheta[c]=this->getDouble(&dataIn);
		}
		for(c=0; c<JOINT_NUMBER; c++){
			k->DHa[c]=this->getDouble(&dataIn);
		}
		for(c=0; c<JOINT_NUMBER; c++){
			k->DHd[c]=this->getDouble(&dataIn);
		}
		for(c=0; c<JOINT_NUMBER; c++){
			k->DHalpha[c]=this->getDouble(&dataIn);
		}
		v->calibration_status = this->getUInt32(&dataIn);

		// check if read data is aligned with package size
		if (dataIn-begin != size) throw dataIn-begin-size;
		return dataIn-begin;
	}

	uint32_t subpackage::configurationSub::parse() {
		configurationData_t *v = &dataOut->configuration;
		kineDH_t *k=&dataOut->configuration.kinematics;
		unsigned char* begin = dataIn;
		uint32_t size;
		uint32_t c;

		size = v->packageSize = this->getInt32(&dataIn);
		v->packageType = this->getUChar(&dataIn);
		for(c=0; c<JOINT_NUMBER; c++){
			v->jointPositionLimits[c].min=this->getDouble(&dataIn);
			v->jointPositionLimits[c].max=this->getDouble(&dataIn);
		}
		for(c=0; c<JOINT_NUMBER; c++){
			v->jointSpeedLimits[c].min=this->getDouble(&dataIn);
			v->jointSpeedLimits[c].max=this->getDouble(&dataIn);
		}
		v->vJointDefault=this->getDouble(&dataIn);
		v->aJointDefault=this->getDouble(&dataIn);
		v->vToolDefault=this->getDouble(&dataIn);
		v->aToolDefault=this->getDouble(&dataIn);
		v->eqRadius=this->getDouble(&dataIn);
		for(c=0; c<JOINT_NUMBER; c++){
			k->DHa[c]=this->getDouble(&dataIn);
		}
		for(c=0; c<JOINT_NUMBER; c++){
			k->DHd[c]=this->getDouble(&dataIn);
		}
		for(c=0; c<JOINT_NUMBER; c++){
			k->DHalpha[c]=this->getDouble(&dataIn);
		}
		for(c=0; c<JOINT_NUMBER; c++){
			k->DHtheta[c]=this->getDouble(&dataIn);
		}
		v->masterboardVersion=this->getUInt32(&dataIn);
		v->controllerBoxType=this->getInt32(&dataIn);
		v->robotType=this->getInt32(&dataIn);
		v->robotSubType=this->getInt32(&dataIn);

		// check if read data is aligned with package size
		if (dataIn-begin != size) throw dataIn-begin-size;
		return dataIn-begin;
	}

	uint32_t subpackage::forceModeSub::parse() {
		forceModeData_t *v = &dataOut->forceMode;
		unsigned char* begin = dataIn;
		uint32_t size;
		uint32_t c;

		size = v->packageSize = this->getInt32(&dataIn);
		v->packageType = this->getUChar(&dataIn);
		for(c=0; c<JOINT_NUMBER; c++){
			v->forcePosition.force[c] = this->getDouble(&dataIn);
		}
		v->robotDexterity = this->getDouble(&dataIn);

		// check if read data is aligned with package size
		if (dataIn-begin != size) throw dataIn-begin-size;
		return dataIn-begin;
	}

	uint32_t subpackage::additionalInfoSub::parse() {
		additionalInfo_t *v = &dataOut->additionalInfo;
		unsigned char* begin = dataIn;
		uint32_t size;

		size = v->packageSize = this->getInt32(&dataIn);
		v->packageType = this->getUChar(&dataIn);
		v->feedriveButtonPressed = this->getUChar(&dataIn);
		v->feedriveButtonEnabled = this->getUChar(&dataIn);
		v->ioEnableFeedrive = this->getUChar(&dataIn);

		// check if read data is aligned with package size
		if (dataIn-begin != size) throw dataIn-begin-size;
		return dataIn-begin;
	}

	uint32_t subpackage::calibrationSub::parse() {
		calibrationData_t *v = &dataOut->calibrationData;
		unsigned char* begin = dataIn;
		uint32_t size;
		uint32_t c;

		size = v->packageSize = this->getInt32(&dataIn);
		v->packageType = this->getUChar(&dataIn);
		for(c=0; c<JOINT_NUMBER; c++){
			v->fcalData.force[c] = this->getDouble(&dataIn);
		}

		// check if read data is aligned with package size
		if (dataIn-begin != size) throw dataIn-begin-size;
		return dataIn-begin;
	}

	uint32_t subpackage::toolComSub::parse() {
		toolCommunicationInfo_t *v = &dataOut->toolCom;
		unsigned char* begin = dataIn;
		uint32_t size;

		size = v->packageSize = this->getInt32(&dataIn);
		v->packageType = this->getUChar(&dataIn);
		v->communicationIsEnabled = this->getUChar(&dataIn);
		v->baudRate = this->getInt32(&dataIn);
		v->parity = this->getInt32(&dataIn);
		v->stopBits = this->getInt32(&dataIn);
		v->rxIdleChars = this->getFloat(&dataIn);
		v->txIdleChars = this->getFloat(&dataIn);

		// check if read data is aligned with package size
		if (dataIn-begin != size) throw dataIn-begin-size;
		return dataIn-begin;
	}

	uint32_t subpackage::toolModeSub::parse() {
		toolModeInfo_t *v = &dataOut->toolMode;
		unsigned char* begin = dataIn;
		uint32_t size;

		size = v->packageSize = this->getInt32(&dataIn);
		v->packageType = this->getUChar(&dataIn);
		v->outputMode = this->getChar(&dataIn);
		v->digitalModeOut0 = this->getUChar(&dataIn);
		v->digitalModeOut1 = this->getUChar(&dataIn);

		// check if read data is aligned with package size
		if (dataIn-begin != size) throw dataIn-begin-size;
		return dataIn-begin;
	}

	uint32_t subpackage::versionSub::parse() {
		versionMessage_t *v = &dataOut->versonMessage;
		unsigned char* begin = dataIn;
		uint32_t size;
		char nameSize;
		size = v->packageSize = this->getInt32(&dataIn);
		v->packageType = this->getUChar(&dataIn);
		v->timestamp = this->getUInt64(&dataIn);
		v->source = this->getChar(&dataIn);
		v->robotMessageType = this->getChar(&dataIn);

		nameSize = v->projectNameSize = this->getChar(&dataIn);
		this->copyString(&v->projectName, &dataIn, nameSize);

		v->majorVersion = this->getUChar(&dataIn);
		v->minorVersion = this->getUChar(&dataIn);
		v->bugFixVersion = this->getUChar(&dataIn);
		v->buildNumber = this->getUChar(&dataIn);

		nameSize = (begin+v->packageSize-dataIn);
		this->copyString(&v->buildDate, &dataIn, nameSize);

		// check if read data is aligned with package size
		if (dataIn-begin != size) throw dataIn-begin-size;
		return dataIn-begin;
	}

} /* namespace clientmessage */