#include <mission/devices/GomspaceDeviceHandler.h>
#include <mission/devices/devicedefinitions/GomSpacePackets.h>

GomspaceDeviceHandler::GomspaceDeviceHandler(object_id_t objectId, object_id_t comIF,
		CookieIF * comCookie, uint16_t maxConfigTableAddress,
		uint16_t maxHkTableAddress) :
		DeviceHandlerBase(objectId, comIF, comCookie), maxConfigTableAddress(
				maxConfigTableAddress), maxHkTableAddress(maxHkTableAddress) {
	mode = MODE_NORMAL;
	if (comCookie == NULL) {
		sif::error << "GomspaceDeviceHandler invalid com cookie" << std::endl;
	}
}

GomspaceDeviceHandler::~GomspaceDeviceHandler() {
}


void GomspaceDeviceHandler::doStartUp(){
}

void GomspaceDeviceHandler::doShutDown(){

}

ReturnValue_t GomspaceDeviceHandler::buildNormalDeviceCommand(
		DeviceCommandId_t * id) {
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GomspaceDeviceHandler::buildTransitionDeviceCommand(
		DeviceCommandId_t * id){
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(
		DeviceCommandId_t deviceCommand, const uint8_t * commandData,
		size_t commandDataLen) {
	ReturnValue_t result;
	switch(deviceCommand) {
	case(PING): {
		result = generatePingCommand(commandData, commandDataLen);
		if(result != HasReturnvaluesIF::RETURN_OK) {
			return result;
		}
		break;
	}
	case(REBOOT): {
		generateRebootCommand();
		break;
	}
	case(PARAM_SET):{
		result = generateSetParamCommand(commandData, commandDataLen);
		if(result != HasReturnvaluesIF::RETURN_OK){
			return result;
		}
		break;
	}
	case(PARAM_GET):{
		result = generateGetParamCommand(commandData, commandDataLen);
		if(result != HasReturnvaluesIF::RETURN_OK){
			return result;
		}
		break;
	}
	case(GNDWDT_RESET): {
		result = generateResetWatchdogCmd();
		if(result != HasReturnvaluesIF::RETURN_OK){
			return result;
		}
		break;
	}
	default:
		return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
	}
	return HasReturnvaluesIF::RETURN_FAILED;
}

void GomspaceDeviceHandler::fillCommandAndReplyMap(){
	this->insertInCommandAndReplyMap(PING, 3);
	this->insertInCommandMap(REBOOT);
	this->insertInCommandAndReplyMap(PARAM_SET, 3);
	this->insertInCommandAndReplyMap(PARAM_GET, 3);
	this->insertInCommandMap(GNDWDT_RESET);
}

ReturnValue_t GomspaceDeviceHandler::scanForReply(const uint8_t *start,
        size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {
	switch(rememberCommandId) {
	case(PING):
			*foundId = PING;
			*foundLen = PING_REPLY_SIZE;
			rememberCommandId = NONE;
		break;
	case(PARAM_GET): {
			*foundId = PARAM_GET;
			*foundLen = rememberRequestedSize + CspGetParamReply::GS_HDR_LENGTH;
			rememberCommandId = NONE;
		break;
	}
	case(PARAM_SET): {
			*foundId = PARAM_SET;
			*foundLen = rememberRequestedSize;
			rememberCommandId = NONE;
		break;
	}
	default:
		return IGNORE_REPLY_DATA;
	}
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GomspaceDeviceHandler::interpretDeviceReply(DeviceCommandId_t id,
        const uint8_t *packet) {
	switch(id) {
	case(PING): {
		SerializeElement<uint32_t> replyTime = *packet;
		handleDeviceTM(&replyTime, id, true);
		break;
	}
	case(PARAM_GET): {
		// -2 to subtract address size from gomspace parameter reply packet
		uint16_t payloadLength = (*(packet + 2) << 8 | *(packet + 3)) - 2;
		if(payloadLength > sizeof(uint32_t)){
			sif::error << "GomspaceDeviceHandler: PARAM_GET: Invalid payload "
					<< "size in reply" << std::endl;
			return INVALID_PAYLOAD_SIZE;
		}
		uint8_t tempPayloadBuffer[payloadLength];
		/* Extract information from received data */
		CspGetParamReply cspGetParamReply(tempPayloadBuffer, payloadLength);
		size_t size = CspGetParamReply::GS_HDR_LENGTH + payloadLength;
		ReturnValue_t result = cspGetParamReply.deSerialize(&packet, &size,
				SerializeIF::Endianness::BIG);
		if(result != HasReturnvaluesIF::RETURN_OK){
			sif::error << "GomspaceDeviceHandler: Failed to deserialize get parameter"
					<< "reply" << std::endl;
			return result;
		}
		uint8_t action = cspGetParamReply.getAction();
		uint8_t tableId = cspGetParamReply.getTableId();
		uint16_t address = cspGetParamReply.getAddress();
		/* Pack relevant information into a tm packet */
		ParamReply paramReply(action, tableId, address, payloadLength,
				tempPayloadBuffer);
		handleDeviceTM(&paramReply, id, true);
		break;
	}
	case(PARAM_SET): {
		/* When setting a parameter, the p60dock sends back the state of the
		 * operation */
		if(*packet != PARAM_SET_OK){
			return HasReturnvaluesIF::RETURN_FAILED;
		}
		break;
	}
	default:
		break;
	}
    return HasReturnvaluesIF::RETURN_OK;
}

void GomspaceDeviceHandler::setNormalDatapoolEntriesInvalid(){

}

ReturnValue_t GomspaceDeviceHandler::generateSetParamCommand(
		const uint8_t * commandData, size_t commandDataLen) {
	SetParamMessageUnpacker setParamMessageUnpacker;
	ReturnValue_t result = setParamMessageUnpacker.deSerialize(&commandData,
			&commandDataLen, SerializeIF::Endianness::BIG);
	if(result != HasReturnvaluesIF::RETURN_OK){
		sif::error << "GomspaceDeviceHandler: Failed to deserialize set parameter "
				"message" << std::endl;
		return result;
	}
	/* Get and check address */
	uint16_t address = setParamMessageUnpacker.getAddress();
	if(address > maxConfigTableAddress){
		sif::error << "GomspaceDeviceHandler: Invalid address for set parameter "
				<< "action" << std::endl;
		return INVALID_ADDRESS;
	}
	uint16_t checksum = GOMSPACE::IGNORE_CHECKSUM;
	uint16_t seq = 0;
	uint16_t total = 0;
	/* CSP reply only contains the transaction state */
	uint16_t querySize = 1;
	const uint8_t* parameterPtr = setParamMessageUnpacker.getParameter();
	uint8_t parameterSize = setParamMessageUnpacker.getParameterSize();
	uint16_t payloadlength = sizeof(address) + parameterSize;

	/* Generate command for CspComIF */
	CspSetParamCommand setParamCmd(querySize, payloadlength, checksum, seq,
			total, address, parameterPtr, parameterSize);
	size_t cspPacketLen = 0;
	uint8_t* buffer = cspPacket;
	result = setParamCmd.serialize(&buffer, &cspPacketLen, sizeof(cspPacket),
			SerializeIF::Endianness::BIG);

	if(result != HasReturnvaluesIF::RETURN_OK){
		sif::error << "GomspaceDeviceHandler: Failed to serialize command for "
				<< "CspComIF" << std::endl;
		return result;
	}
	if(cspPacketLen > MAX_PACKET_LEN){
		sif::error << "GomspaceDeviceHandler: Invalid length of set parameter "
				"command" << std::endl;
		return PACKET_TOO_LONG;
	}
	rawPacket = cspPacket;
	rawPacketLen = cspPacketLen;
	rememberRequestedSize = querySize;
	rememberCommandId = PARAM_SET;
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GomspaceDeviceHandler::generateGetParamCommand(
		const uint8_t * commandData, size_t commandDataLen){
	ReturnValue_t result;
	/* Unpack the received action message */
	GetParamMessageUnpacker getParamMessage;
	result = getParamMessage.deSerialize(&commandData, &commandDataLen,
			SerializeIF::Endianness::BIG);
	if(result != HasReturnvaluesIF::RETURN_OK) {
		sif::error << "Failed to deserialize message to extract information "
				"from get parameter message" << std::endl;
		return result;
	}
	/* Get an check table id to read from */
	uint8_t tableId = getParamMessage.getTableId();
	if(tableId != CONFIG_TABLE_ID && tableId != HK_TABLE_ID){
		sif::error << "GomspaceDeviceHandler: Invalid table id in get parameter"
				" message" << std::endl;
		return INVALID_TABLE_ID;
	}
	/* Get and check address */
	uint16_t address = getParamMessage.getAddress();
	if(address > maxHkTableAddress && tableId == HK_TABLE_ID){
		sif::error << "GomspaceDeviceHandler: Invalid address to get parameter from "
				<< "housekeeping table" << std::endl;
		return INVALID_ADDRESS;
	}
	if(address > maxConfigTableAddress && tableId == CONFIG_TABLE_ID){
		sif::error << "GomspaceDeviceHandler: Invalid address to get parameter from "
				<< "configuration table" << std::endl;
		return INVALID_ADDRESS;
	}
	uint16_t length = sizeof(address);
	uint16_t checksum = GOMSPACE::IGNORE_CHECKSUM;
	uint16_t seq = 0;
	uint16_t total = 0;
	uint8_t parameterSize = getParamMessage.getParameterSize();
	if(parameterSize > sizeof(uint32_t)) {
		sif::error << "GomspaceDeviceHandler: GET_PARAM: Invalid parameter "
				<< "size" << std::endl;
		return INVALID_PARAM_SIZE;
	}
	uint16_t querySize = parameterSize + CspGetParamCommand::GS_HDR_LENGTH;

	/* Generate the CSP command to send to the P60 Dock */
	CspGetParamCommand getParamCmd(querySize, tableId, length,
			checksum, seq, total, address);
	size_t cspPacketLen = 0;
	uint8_t* buffer = cspPacket;
	result = getParamCmd.serialize(&buffer, &cspPacketLen, sizeof(cspPacket),
			SerializeIF::Endianness::BIG);
	if(result != HasReturnvaluesIF::RETURN_OK){
		sif::error << "GomspaceDeviceHandler: Failed to serialize command to "
				<< "get parameter" << std::endl;
	}
	if(cspPacketLen > MAX_PACKET_LEN){
		sif::error << "GomspaceDeviceHandler: Received invalid get parameter "
				"command" << std::endl;
		return PACKET_TOO_LONG;
	}
	rawPacket = cspPacket;
	rawPacketLen = cspPacketLen;
	rememberRequestedSize = querySize;
	rememberCommandId = PARAM_GET;
	return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GomspaceDeviceHandler::generatePingCommand(
		const uint8_t * commandData, size_t commandDataLen) {
	CspPingCommand cspPingCommand(commandData, commandDataLen);
	size_t cspPacketLen = 0;
	uint8_t* buffer = cspPacket;
	ReturnValue_t result = cspPingCommand.serialize(&buffer, &cspPacketLen,
			sizeof(cspPacket),
			SerializeIF::Endianness::BIG);
	if(result != HasReturnvaluesIF::RETURN_OK){
		sif::error << "GomspaceDeviceHandler: Failed to serialize ping command"
				<< std::endl;
		return result;
	}
	if(cspPacketLen > MAX_PACKET_LEN){
		sif::error << "GomspaceDeviceHandler: Received invalid ping message"
				<< std::endl;
		return PACKET_TOO_LONG;
	}
	rawPacket = cspPacket;
	rawPacketLen = cspPacketLen;
	rememberCommandId = PING;
	return HasReturnvaluesIF::RETURN_OK;
}

void GomspaceDeviceHandler::generateRebootCommand(){
	uint8_t cspPort = GOMSPACE::REBOOT_PORT;
	uint16_t querySize = 0;
	*cspPacket = GOMSPACE::REBOOT_PORT;
	*(cspPacket + 1) = querySize;
	size_t cspPacketLen = sizeof(cspPort) + sizeof(cspPacketLen);
	rawPacket = cspPacket;
	rawPacketLen = cspPacketLen;
}

ReturnValue_t GomspaceDeviceHandler::generateResetWatchdogCmd(){
	WatchdogResetCommand watchdogResetCommand;
	size_t cspPacketLen = 0;
	uint8_t* buffer = cspPacket;
	ReturnValue_t result = watchdogResetCommand.serialize(&buffer,
			&cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG);
	if(result != HasReturnvaluesIF::RETURN_OK){
		sif::error << "GomspaceDeviceHandler: Failed to serialize watchdog reset "
				<< "command" << std::endl;
		return result;
	}
	rawPacket = cspPacket;
	rawPacketLen = cspPacketLen;
	rememberRequestedSize = 0; // No bytes will be queried with the ground
							   // watchdog command.
	rememberCommandId = GNDWDT_RESET;
	return HasReturnvaluesIF::RETURN_OK;
}

uint32_t GomspaceDeviceHandler::getTransitionDelayMs(Mode_t modeFrom,
		Mode_t modeTo) {
	return 0;
}