/*
 * DeviceHandler.cpp
 *
 * Created on: 02/06/2021
 * Author: Marco Modè
 *
 */

#include <mission/DeviceHandler/ArduinoDeviceHandler.h>
#include <OBSWConfig.h>
#include <fsfw/datapool/DataSet.h>
#include <fsfw/datapool/PoolVector.h>
#include <bsp_linux/fsfwconfig/datapool/dataPoolInit.h>
#include <cstdlib>


ArduinoDH::ArduinoDH(object_id_t objectId, object_id_t comIF, CookieIF *cookie) :
		DeviceHandlerBase(objectId, comIF, cookie) {
		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) {
		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) {

	//Unless a command was sent explicitely, we don't expect any replies and ignore
	//the packet. On a real device, there might be replies which are sent without a previous
	//command.

	*foundLen = 2580;

	// check validity
	if (len == *foundLen){
		// start character: '['
		if (*start == 91 ){
			return APERIODIC_REPLY;
		} else{
			return DeviceHandlerIF::LENGTH_MISSMATCH;
		}
	} else {
		return IGNORE_REPLY_DATA;
	}
	/*if (len == *foundLen){
		// start character: '['
		if (*start == 91 ){
			if (*start + 9 == 49) {
				return APERIODIC_REPLY;
			}
		} else{
			return DeviceHandlerIF::LENGTH_MISSMATCH;
		}
	} else {
		return IGNORE_REPLY_DATA;
	}*/
}

ReturnValue_t ArduinoDH::interpretDeviceReply(DeviceCommandId_t id,
		const uint8_t *packet) {
	
	//sif::debug<<"DEBUG_DH: interprete for reply"<<std::endl;
	// The data stored in the read buffer are here copied in the variables with the SPC format.
	// After copying, the data of temperature, environment and accelerometer are stored in three separated vectors.
	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);
		vecTemp.emplace_back(Temp_ch);
	}
	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);
		vecEnv.emplace_back(Env_ch);
	}
	for (int k = 0; k < 15; k++) {
		memcpy(&Acc_ch.start_string, &packet[27 * (36 + 9) + 91 * k + 0], 8);
		memcpy(&Acc_ch.Typ, &packet[27 * (36 + 9) + 91 * k + 8], 1);
		memcpy(&Acc_ch.SPCChNumber, &packet[27 * (36 + 9) + 91 * k + 9], 1);
		memcpy(&Acc_ch.Value_Cnt, &packet[27 * (36 + 9) + 91 * k + 10], 1);
		memcpy(&Acc_ch.Value, &packet[27 * (36 + 9) + 91 * k + 11], 36);
		memcpy(&Acc_ch.Timestamp, &packet[27 * (36 + 9) + 91 * k + 47], 36);
		memcpy(&Acc_ch.end_string, &packet[27 * (36 + 9) + 91 * k + 83], 8);
		vecAcc.emplace_back(Acc_ch);
	}
	
	// All data are here printed to monitor from the three vectors of data measurements.
	/*printf(
			"\n***********************************************************************************************\n");
	printf("TEMPERATURE parameters are: ");

	for (int i = 0; i < 36; i++) {
		printf("\n\nStart: %7s", vecTemp[i].start_string);
		printf("\nTyp: %u", vecTemp[i].Typ);
		printf("\nSPCChNumber: %u", vecTemp[i].SPCChNumber);
		printf("\nValue_Cnt: %u", vecTemp[i].Value_Cnt);
		printf("\nTemperature: %f", vecTemp[i].temperature);
		printf("\nTimestamp: %u", vecTemp[i].Timestamp);
		printf("\nEnd: %7s", vecTemp[i].end_string);
	}
	printf(
			"\n\n***********************************************************************************************\n");
	printf("ENVIRONMENTAL parameters are: ");

	for (int j = 0; j < 3; j++) {
		printf("\n\nHUMIDITY: ");
		printf("\nStart: %7s", vecEnv[3 * j].start_string);
		printf("\nTyp: %u", vecEnv[3 * j].Typ);
		printf("\nSPCChNumber: %u", vecEnv[3 * j].SPCChNumber);
		printf("\nValue_Cnt: %u", vecEnv[3 * j].Value_Cnt);
		printf("\nValue: %f", vecEnv[3 * j].Value);
		printf("\nTimestamp: %u", vecEnv[3 * j].Timestamp);
		printf("\nEnd: %7s", vecEnv[3 * j].end_string);

		printf("\n\nPRESSURE: ");
		printf("\nStart: %7s", vecEnv[3 * j + 1].start_string);
		printf("\nTyp: %u", vecEnv[3 * j + 1].Typ);
		printf("\nSPCChNumber: %u", vecEnv[3 * j + 1].SPCChNumber);
		printf("\nValue_Cnt: %u", vecEnv[3 * j + 1].Value_Cnt);
		printf("\nValue: %f", vecEnv[3 * j + 1].Value);
		printf("\nTimestamp: %u", vecEnv[3 * j + 1].Timestamp);
		printf("\nEnd: %7s", vecEnv[3 * j + 1].end_string);

		printf("\n\nTEMPERATURE: ");
		printf("\nStart: %7s", vecEnv[3 * j + 2].start_string);
		printf("\nTyp: %u", vecEnv[3 * j + 2].Typ);
		printf("\nSPCChNumber: %u", vecEnv[3 * j + 2].SPCChNumber);
		printf("\nValue_Cnt: %u", vecEnv[3 * j + 2].Value_Cnt);
		printf("\nValue: %f", vecEnv[3 * j + 2].Value);
		printf("\nTimestamp: %u", vecEnv[3 * j + 2].Timestamp);
		printf("\nEnd: %7s", vecEnv[3 * j + 2].end_string);

	}
	printf(
			"\n\n***********************************************************************************************\n");
	printf("ACCELEROMETER parameters are: ");

	for (int k = 0; k < 5; k++) {
		switch (k) {
		case 0:
			printf("\n\nACCELERATION: ");
			break;
		case 1:
			printf("\n\nGYROSCOPE: ");
			break;
		case 2:
			printf("\n\nMAGNETOMETER: ");
			break;
		case 3:
			printf("\n\nLINEAR ACCELERATION: ");
			break;
		case 4:
			printf("\n\nEULER ANGLES: ");
			break;
		}
		printf("\n\nX ==> ");
		printf("\nStart: %7s", vecAcc[3 * k].start_string);
		printf("\nTyp: %u", vecAcc[3 * k].Typ);
		printf("\nSPCChNumber: %u", vecAcc[3 * k].SPCChNumber);
		printf("\nValue_Cnt: %u", vecAcc[3 * k].Value_Cnt);
		for (int i = 0; i < 9; i++) {
			printf("\nMeasurement number: %d", i);
			printf("\nValue: %f", vecAcc[3 * k].Value[i]);
			printf("\nTimestamp: %u", vecAcc[3 * k].Timestamp[i]);
		}
		printf("\nEnd: %7s", vecAcc[3 * k].end_string);

		printf("\n\nY ==> ");
		printf("\nStart: %7s", vecAcc[3 * k + 1].start_string);
		printf("\nTyp: %u", vecAcc[3 * k + 1].Typ);
		printf("\nSPCChNumber: %u", vecAcc[3 * k + 1].SPCChNumber);
		printf("\nValue_Cnt: %u", vecAcc[3 * k + 1].Value_Cnt);
		for (int i = 0; i < 9; i++) {
			printf("\nMeasurement number: %d", i);
			printf("\nValue: %f", vecAcc[3 * k + 1].Value[i]);
			printf("\nTimestamp: %u", vecAcc[3 * k + 1].Timestamp[i]);
		}
		printf("\nEnd: %7s", vecAcc[3 * k + 1].end_string);

		printf("\n\nZ ==> ");
		printf("\nStart: %7s", vecAcc[3 * k + 2].start_string);
		printf("\nTyp: %u", vecAcc[3 * k + 2].Typ);
		printf("\nSPCChNumber: %u", vecAcc[3 * k + 2].SPCChNumber);
		printf("\nValue_Cnt: %u", vecAcc[3 * k + 2].Value_Cnt);
		for (int i = 0; i < 9; i++) {
			printf("\nMeasurement number: %d", i);
			printf("\nValue: %f", vecAcc[3 * k + 2].Value[i]);
			printf("\nTimestamp: %u", vecAcc[3 * k + 2].Timestamp[i]);
		}
		printf("\nEnd: %7s", vecAcc[3 * k + 2].end_string);
	}*/

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

	// The data are here written to the data pool where they would be available to be used for other objects
	
	DataSet ArduinoDataSet;
	
	PoolVector <float, 36> TempValueVec(datapool::Temperature_value, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
	for (int i = 0; i < 36; i++) {
		memcpy(&TempValueVec[i], &vecTemp[i].temperature, 4);
	}
	ArduinoDataSet.commit(PoolVariableIF::VALID);
	

	/*PoolVector <unsigned int, 36> TempTimeVec(datapool::Temperature_Timestamp, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
	for (int i = 0; i < 36; i++) {
		memcpy(&TempTimeVec[i], &vecTemp[i].Timestamp, 4);
	}
	ArduinoDataSet.commit(PoolVariableIF::VALID);
	
	sif::debug<<"\nDEBUG_DHi: End of copy to datapool"<<std::endl;

	PoolVector <float, 9> EnvValueVec(datapool::Environmental_value, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
	for (int j = 0; j < 9; j++) {
		memcpy(&EnvValueVec[j], &vecEnv[j].Value, 4);
	}
	ArduinoDataSet.commit(PoolVariableIF::VALID);
	
	PoolVector <unsigned int, 9> EnvTimeVec(datapool::Environmental_Timestamp, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
	for (int j = 0; j < 9; j++) {
		memcpy(&EnvTimeVec[j], &vecEnv[j].Timestamp, 4);
	}
	ArduinoDataSet.commit(PoolVariableIF::VALID);
	
	sif::debug<<"\nDEBUG_DHj: End of copy to datapool"<<std::endl;

	PoolVector <float, 15> AccValueVec(datapool::Accelerometer_value, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
	for (int k = 0; k < 15; k++) {
		memcpy(&AccValueVec[k], &vecAcc[k].Value, 36);
	}
	ArduinoDataSet.commit(PoolVariableIF::VALID);
	sif::debug<<"\nDEBUG_DHk1: End of copy to datapool"<<std::endl;
	
	PoolVector <unsigned int, 15> AccTimeVec(datapool::Accelerometer_Timestamp, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
	for (int k = 0; k < 15; k++) {
		memcpy(&AccTimeVec[k], &vecAcc[k].Timestamp, 36);
	}
	ArduinoDataSet.commit(PoolVariableIF::VALID);
	sif::debug<<"\nDEBUG_DHk2: End of copy to datapool"<<std::endl;*/

	sif::debug<<"DEBUG_DH: End of copy to datapool"<<std::endl;

	return RETURN_OK;
}

void ArduinoDH::setNormalDatapoolEntriesInvalid() {}