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

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

P60DockHandler::~P60DockHandler() {
}


void P60DockHandler::doStartUp(){
}

void P60DockHandler::doShutDown(){

}

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

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

ReturnValue_t P60DockHandler::buildCommandFromCommand(
		DeviceCommandId_t deviceCommand, const uint8_t * commandData,
		size_t commandDataLen) {
	switch(deviceCommand) {
	case(PING): {
		const uint8_t* pingData = commandData;
		uint8_t pingDataSz = commandDataLen;
		CspPingCommand cspPingCommand(pingData, pingDataSz);
		size_t cspPacketLen = 0;
		uint8_t* buffer = cspPacket;
		cspPingCommand.serialize(&buffer, &cspPacketLen, sizeof(cspPacket),
				SerializeIF::Endianness::BIG);
		if(cspPacketLen > MAX_PACKET_LEN){
			sif::error << "P60DockHandler: Received invalid ping message"
					<< std::endl;
			return HasReturnvaluesIF::RETURN_FAILED;
		}
		rawPacket = cspPacket;
		rawPacketLen = cspPacketLen;
		rememberCommandId = PING;
		break;
	}
	case(REBOOT): {
		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;
		break;
	}
	case(PARAM_SET):{
		SetParamMessageUnpacker setParamMessageUnpacker(commandData,
				commandDataLen);
		uint8_t tableId = setParamMessageUnpacker.getTableId();
		uint16_t address = setParamMessageUnpacker.getAddress();
		uint16_t checksum = GOMSPACE::IGNORE_CHECKSUM;
		uint16_t seq = 0;
		uint16_t total = 0;
		/* Reply only comprises the transaction state */
		uint16_t querySize = 1;
		const uint8_t* parameterPtr = setParamMessageUnpacker.getParameter();
		uint8_t parameterSize = setParamMessageUnpacker.getParameterSize();
		uint16_t payloadlength = sizeof(address) + parameterSize;
		CspSetParamCommand setParamCmd(querySize, PARAM_SET, tableId, payloadlength,
				checksum, seq, total, address, parameterPtr,
				parameterSize);
		size_t cspPacketLen = 0;
		uint8_t* buffer = cspPacket;
		setParamCmd.serialize(&buffer, &cspPacketLen, sizeof(cspPacket),
				SerializeIF::Endianness::BIG);
		if(cspPacketLen > MAX_PACKET_LEN){
			sif::error << "P60DockHandler: Received invalid set parameter "
					"command" << std::endl;
			return HasReturnvaluesIF::RETURN_FAILED;
		}
		rawPacket = cspPacket;
		rawPacketLen = cspPacketLen;
		rememberRequestedSize = querySize;
		rememberCommandId = PARAM_SET;
		break;
	}
	case(PARAM_GET):{
		/* Unpack the received action message */
		GetParamMessageUnpacker getParamMessage(commandData, commandDataLen);
		uint8_t tableId = getParamMessage.getTableId();
		uint16_t address = getParamMessage.getAddress();
		uint16_t length = sizeof(address);
		uint16_t checksum = GOMSPACE::IGNORE_CHECKSUM;
		uint16_t seq = 0;
		uint16_t total = 0;
		uint16_t querySize = getParamMessage.getParameterSize()
				+ CspGetParamCommand::GS_HDR_LENGTH;
		/* Generate the CSP command to send to the P60 Dock */
		CspGetParamCommand getParamCmd(querySize, PARAM_GET, tableId, length,
				checksum, seq, total, address);
		size_t cspPacketLen = 0;
		uint8_t* buffer = cspPacket;
		getParamCmd.serialize(&buffer, &cspPacketLen, sizeof(cspPacket),
				SerializeIF::Endianness::BIG);
		if(cspPacketLen > MAX_PACKET_LEN){
			sif::error << "P60DockHandler: Received invalid get parameter "
					"command" << std::endl;
			return HasReturnvaluesIF::RETURN_FAILED;
		}
		rawPacket = cspPacket;
		rawPacketLen = cspPacketLen;
		rememberRequestedSize = querySize;
		rememberCommandId = PARAM_GET;
		break;
	}
	case(GNDWDT_RESET): {
		WatchdogResetCommand watchdogResetCommand;
		size_t cspPacketLen = 0;
		uint8_t* buffer = cspPacket;
		watchdogResetCommand.serialize(&buffer, &cspPacketLen, sizeof(cspPacket),
				SerializeIF::Endianness::BIG);
		if(cspPacketLen > MAX_PACKET_LEN){
			sif::error << "P60DockHandler: Received invalid ground watchdog"
					"reset command" << std::endl;
			return HasReturnvaluesIF::RETURN_FAILED;
		}
		rawPacket = cspPacket;
		rawPacketLen = cspPacketLen;
		rememberRequestedSize = 0; // No bytes will be queried with the ground
								   // watchdog command.
		rememberCommandId = GNDWDT_RESET;
	}
	default:
		break;
	}
	return HasReturnvaluesIF::RETURN_OK;
}

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

ReturnValue_t P60DockHandler::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 P60DockHandler::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;
		uint8_t tempPayloadBuffer[payloadLength];
		/* Extract information from received data */
		CspGetParamReply cspGetParamReply(tempPayloadBuffer, payloadLength);
		size_t size = CspGetParamReply::GS_HDR_LENGTH + payloadLength;
		cspGetParamReply.deSerialize(&packet, &size,
				SerializeIF::Endianness::BIG);
		uint8_t action = cspGetParamReply.getAction();
		uint8_t tableId = cspGetParamReply.getTableId();
		uint16_t address = cspGetParamReply.getAddress();
		/* Pack relevant information into a tm reply 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 P60DockHandler::setNormalDatapoolEntriesInvalid(){

}