/*
 * Title: 	ArduinoDeviceHandler.cpp
 * Author: 	Marco Modè
 * Last version: 24/09/2021
 * Project: ESBO-DS
 *-------------------------------------------------------------------------------------------------------------------
 *
 */

#include <mission/DeviceHandler/ArduinoDeviceHandler.h>

ArduinoDH::ArduinoDH(object_id_t objectId, object_id_t comIF, CookieIF *cookie) :
		DeviceHandlerBase(objectId, comIF, cookie),
		TempValueVec(datapool::Temperature_value, &ArduinoDataSet, PoolVariableIF::VAR_WRITE),
		TempTimeVec(datapool::Temperature_Timestamp, &ArduinoDataSet, PoolVariableIF::VAR_WRITE) {
	mode = _MODE_START_UP;
}
ArduinoDH::~ArduinoDH() {
}

void ArduinoDH::doStartUp() {
	std::cout << "Arduino device -> Switching-ON" << std::endl;
	setMode(_MODE_TO_ON);
	return;
}

void ArduinoDH::doShutDown() {
	std::cout << "Arduino device -> Switching-OFF" << std::endl;
	setMode(_MODE_SHUT_DOWN);
	return;
}
ReturnValue_t ArduinoDH::buildNormalDeviceCommand(DeviceCommandId_t *id) {
	return NOTHING_TO_SEND;
}

ReturnValue_t ArduinoDH::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
	return NOTHING_TO_SEND;
}

void ArduinoDH::doTransition(Mode_t modeFrom, Submode_t submodeFrom) {
	if (mode == _MODE_TO_NORMAL) {
		setMode(_MODE_TO_NORMAL);
		std::cout << "Arduino device is in Normal mode" << std::endl;
	} else {
		setMode(_MODE_TO_NORMAL);
	}
}

ReturnValue_t ArduinoDH::buildCommandFromCommand(
		DeviceCommandId_t deviceCommand, const uint8_t *commandData,
		size_t commandDataLen) {
	return HasActionsIF::EXECUTION_FINISHED;
}

void ArduinoDH::fillCommandAndReplyMap() {
}

ReturnValue_t ArduinoDH::scanForReply(const uint8_t *start, size_t len,
		DeviceCommandId_t *foundId, size_t *foundLen) {

	/* In this case the data are sent from the Arduino board without any command.
	 * No replies of data received are required.
	 * This function checks the validity of the data packet received.
	 */

	*foundLen = len;
	// The ID is defined as a macro. It includes info about the array size.
	std::map<DeviceCommandId_t, size_t> id { { *foundId, len } };

	/* Check validity of the packet in terms of length (the data have
	 * been already checked in ArduinoComIF.cpp in order to provide the
	 * start of the data buffer.
	 */
	if (*foundLen == 7740) {
		return APERIODIC_REPLY;
	} else {
		return DeviceHandlerIF::LENGTH_MISSMATCH;
	}
}

ReturnValue_t ArduinoDH::interpretDeviceReply(DeviceCommandId_t id,
		const uint8_t *packet) {

	/* The data stored in the read buffer are here copied in the variables with the SPC format.
	 * (The structures are defined in the header)
	 * The data of temperature, environment and orientation are then stored in three separated vectors.
	 * These vectors will be then saved in the framework datapool in order to be used by the controller.
	 */
	printf(
			"\n***********************************************************************************************\n");
	printf("TEMPERATURE parameters are: ");
	for (int i = 0; i < 36; i++) {
		memcpy(&Temp_ch.start_string, &packet[27 * i + 0], 8);
		memcpy(&Temp_ch.Typ, &packet[27 * i + 8], 1);
		memcpy(&Temp_ch.SPCChNumber, &packet[27 * i + 9], 1);
		memcpy(&Temp_ch.Value_Cnt, &packet[27 * i + 10], 1);
		memcpy(&Temp_ch.temperature, &packet[27 * i + 11], 4);
		memcpy(&Temp_ch.Timestamp, &packet[27 * i + 15], 4);
		memcpy(&Temp_ch.end_string, &packet[27 * i + 19], 8);
		/* The data (temperature, timestamp) are here saved in the datapool.
		 * They will be exploited by the thermal controller.
		 */
		memcpy(&TempValueVec[i], &Temp_ch.temperature, 4);
		memcpy(&TempTimeVec[i], &Temp_ch.Timestamp, 4);
		// The data are here printed (useful for tests).
		printf("\n\nStart: %7s", Temp_ch.start_string);
		printf("\nTyp: %u", Temp_ch.Typ);
		printf("\nSPCChNumber: %u", Temp_ch.SPCChNumber);
		printf("\nValue_Cnt: %u", Temp_ch.Value_Cnt);
		printf("\nTemperature: %f", Temp_ch.temperature);
		printf("\nTimestamp: %u", Temp_ch.Timestamp);
		printf("\nEnd: %7s", Temp_ch.end_string);
		vecTemp.emplace_back(Temp_ch);
	}
	ArduinoDataSet.commit(PoolVariableIF::VALID);
	/* The environment and orientation data reading and printing
	 * are currently commented out because they are not needed.
	 * Anyway they are available and they can be used if necessary.
	 */

	/*printf(
			"\n\n***********************************************************************************************\n");
	printf("ENVIRONMENTAL parameters are: ");
	for (int j = 0; j < 9; j++) {
		memcpy(&Env_ch.start_string, &packet[27 * (36 + j) + 0], 8);
		memcpy(&Env_ch.Typ, &packet[27 * (36 + j) + 8], 1);
		memcpy(&Env_ch.SPCChNumber, &packet[27 * (36 + j) + 9], 1);
		memcpy(&Env_ch.Value_Cnt, &packet[27 * (36 + j) + 10], 1);
		memcpy(&Env_ch.Value, &packet[27 * (36 + j) + 11], 4);
		memcpy(&Env_ch.Timestamp, &packet[27 * (36 + j) + 15], 4);
		memcpy(&Env_ch.end_string, &packet[27 * (36 + j) + 19], 8);
		// The data are here printed (useful for tests).
		if (j == 0 || j == 3 || j == 6) {
			printf("\n\nHUMIDITY: ");
		} else if (j == 1 || j == 4 || j == 7) {
			printf("\n\nPRESSURE: ");
		} else {
			printf("\n\nTEMPERATURE: ");
		}
		printf("\n\nStart: %7s", Env_ch.start_string);
		printf("\nTyp: %u", Env_ch.Typ);
		printf("\nSPCChNumber: %u", Env_ch.SPCChNumber);
		printf("\nValue_Cnt: %u", Env_ch.Value_Cnt);
		printf("\nTemperature: %f", Env_ch.Value);
		printf("\nTimestamp: %u", Env_ch.Timestamp);
		printf("\nEnd: %7s", Env_ch.end_string);
		vecEnv.emplace_back(Env_ch);
	}
	printf(
			"\n\n***********************************************************************************************\n");
	printf("ACCELEROMETER parameters are: ");
	for (int k = 0; k < 15; k++) {
		memcpy(&Ornt_ch.start_string, &packet[27 * (36 + 9) + 91 * k + 0], 8);
		memcpy(&Ornt_ch.Typ, &packet[27 * (36 + 9) + 91 * k + 8], 1);
		memcpy(&Ornt_ch.SPCChNumber, &packet[27 * (36 + 9) + 91 * k + 9], 1);
		memcpy(&Ornt_ch.Value_Cnt, &packet[27 * (36 + 9) + 91 * k + 10], 1);
		memcpy(&Ornt_ch.Value, &packet[27 * (36 + 9) + 91 * k + 11], 36);
		memcpy(&Ornt_ch.Timestamp, &packet[27 * (36 + 9) + 91 * k + 47], 36);
		memcpy(&Ornt_ch.end_string, &packet[27 * (36 + 9) + 91 * k + 83], 8);
		// The data are here printed (useful for tests).
		switch (k) {
		case 0:
			printf("\n\nACCELERATION: ");
			break;
		case 3:
			printf("\n\nGYROSCOPE: ");
			break;
		case 6:
			printf("\n\nMAGNETOMETER: ");
			break;
		case 9:
			printf("\n\nLINEAR ACCELERATION: ");
			break;
		case 12:
			printf("\n\nEULER ANGLES: ");
			break;
		}
		if (k == 0 || k == 3 || k == 6 || k == 9 || k == 12) {
			printf("\n\nX ==> ");
		} else if (k == 1 || k == 4 || k == 7 || k == 10 || k == 13) {
			printf("\n\nY ==> ");
		} else {
			printf("\n\nZ ==> ");
		}
		printf("\nStart: %7s", Ornt_ch.start_string);
		printf("\nTyp: %u", Ornt_ch.Typ);
		printf("\nSPCChNumber: %u", Ornt_ch.SPCChNumber);
		printf("\nValue_Cnt: %u", Ornt_ch.Value_Cnt);
		for (int i = 0; i < 9; i++) {
			printf("\nMeasurement number: %d", i);
			printf("\nValue: %f", Ornt_ch.Value[i]);
			printf("\nTimestamp: %u", Ornt_ch.Timestamp[i]);
		}
		printf("\nEnd: %7s", Ornt_ch.end_string);
		vecOrnt.emplace_back(Ornt_ch);
	}//*/

	std::cout << "\n\nEnd reading data.\n" << std::endl;

	return RETURN_OK;
}

void ArduinoDH::setNormalDatapoolEntriesInvalid() {
}