#include <framework/devicehandlers/DeviceHandlerIF.h>
#include <framework/objectmanager/ObjectManagerIF.h>
#include <framework/rmap/RMAPChannelIF.h>
#include <framework/serviceinterface/ServiceInterfaceStream.h>
#include <framework/tmtcservices/AcceptsTelecommandsIF.h>
#include <mission/obc/ccsdsboard/CCSDSHandler.h>
#include <math.h>
#include <config/datapool/dataPoolInit.h>

const float CCSDSHandler::DEFAULT_RATES[BoardHandler::USED_VIRTUAL_CHANNELS_PER_BOARD] =
		{ CCSDSHandler::DEFAULT_BYTES_PER_SECOND, CCSDSHandler::DEFAULT_BYTES_PER_SECOND,
				CCSDSHandler::DEFAULT_BYTES_PER_SECOND, CCSDSHandler::DEFAULT_BYTES_PER_SECOND };

CCSDSHandler::CCSDSHandler(object_id_t setObjectId, uint8_t setSwitchId1,
		uint8_t setSwitchId2, object_id_t setChannel, uint16_t setSCID,
		BoardHandler::DataPoolIds setPool) :
		SystemObject(setObjectId), tmPartHealth(setObjectId + 1, 0), commandQueue(), state(
				STATE_IDLE), commState(SEND_WRITE), mode(MODE_OFF), submode(
				SUBMODE_NONE), boardHandler((uint8_t*) frameBuffer,
				sizeof(frameBuffer), setSCID, setPool), dataLinkLayer(
				this->frameBuffer, boardHandler.getClcw(), 0, setSCID), frameLength(
				0), channelId(setChannel), memoryHelper(this, &commandQueue), pendingWrite(
		false), pendingRead(false), modeHelper(this), healthHelper(this,
				setObjectId), parameterHelper(this), powerSwitcher(setSwitchId1,
				setSwitchId2), switchOffWasReported(false), fdir(setObjectId,
				setChannel) {
	memset(this->frameBuffer, 0, sizeof(frameBuffer));
	DataSet mySet;
	PoolVector<float, BoardHandler::USED_VIRTUAL_CHANNELS_PER_BOARD> rateRatio(
			datapool::DATA_RATE_ASSIGN, &mySet, PoolVariableIF::VAR_WRITE);
	for (uint8_t i = 0; i < BoardHandler::USED_VIRTUAL_CHANNELS_PER_BOARD;
			++i) {
		virtualChannels[i] = new VCGeneration(i,
				BoardHandler::TM_PACKETS_PER_CALL_PER_CHANNEL);
		rateLimitOverload[i] = 0;
		rateRatio[i] = DEFAULT_RATES[i];
	}
	mySet.commit(PoolVariableIF::VALID);
	//Set real-time channel to high responsiveness.
	virtualChannels[0]->setIdlePacketIntervalMs(
			VCGeneration::IDLE_INTERVAL_RT_CHANNEL);
}

CCSDSHandler::~CCSDSHandler() {
	for (uint8_t i = 0; i < BoardHandler::USED_VIRTUAL_CHANNELS_PER_BOARD;
			++i) {
		delete virtualChannels[i];
	}
}

void CCSDSHandler::printFrameBuffer(void) {
	debug << "frame_buffer contains: " << std::endl;
	for (uint32_t i = 0; i < this->frameLength; ++i) {
		debug << "frame_buffer[" << std::dec << i << "]: " << std::hex
				<< std::showbase << (uint16_t) this->frameBuffer[i] << std::dec
				<< std::endl;
	}
}

ReturnValue_t CCSDSHandler::packetProcessing(void) {
	rateSet.read();
	const float* usedRateRatios = NULL;
	if (rateSet.dataRates.isValid()) {
		usedRateRatios = rateSet.dataRates.value;
	} else {
		usedRateRatios = DEFAULT_RATES;
	}
	for (uint8_t i = 0; i < BoardHandler::USED_VIRTUAL_CHANNELS_PER_BOARD;
			++i) {
		//Set current cycle Limit first
		uint32_t newLimit = (usedRateRatios[i]) / SENDS_PER_SECOND;
		if (newLimit <= rateLimitOverload[i]) {
			//Calculate new overload.
			rateLimitOverload[i] -= newLimit;
			//Use newLimit as current rate, VC is utilized to its current limit.
			rateSet.dataRate[i] = usedRateRatios[i]; //equal to newLimit * SENDS_PER_SECOND
			//Don't send anything new.
			continue;
		}
		newLimit -= rateLimitOverload[i];
		virtualChannels[i]->tmSendLimitPerCycle = newLimit;
		//Send new packets.
		uint32_t tempRate = virtualChannels[i]->packetProcessing();
		if (tempRate > virtualChannels[i]->tmSendLimitPerCycle) {
			rateLimitOverload[i] = tempRate - virtualChannels[i]->tmSendLimitPerCycle;
			//VC is fully utilized, but not more. Overload will be accounted for in next cycles.
			rateSet.dataRate[i] = virtualChannels[i]->tmSendLimitPerCycle  * SENDS_PER_SECOND;
		} else {
			//Data rate extended to a Bytes/s range. Filtering is performed in controller.
			rateSet.dataRate[i] = tempRate * SENDS_PER_SECOND;
			rateLimitOverload[i] = 0;
		}
	}
	rateSet.commit(PoolVariableIF::VALID);
	return RETURN_OK;
}

ReturnValue_t CCSDSHandler::packetProcessingCheck(void) {
	ReturnValue_t status = RETURN_FAILED;
	for (uint8_t vc = 0; vc < BoardHandler::USED_VIRTUAL_CHANNELS_PER_BOARD;
			++vc) {
		status = this->virtualChannels[vc]->packetProcessingCheck();
		if (status != RETURN_OK) {
			error << "CCSDSHandler " << std::hex << this->getObjectId()
					<< " ::packetProcessingCheck: Error on VC " << (uint16_t) vc
					<< ". Error code: " << status << std::dec << std::endl;
		}
	}
	return RETURN_OK;
}

ReturnValue_t CCSDSHandler::performOperation(void) {
    readCommandQueue();
    handleTelemetry();
    handleTelecommands();
    return RETURN_OK;
}

void CCSDSHandler::addVirtualChannel(VcId_t virtualChannelId, VirtualChannel* virtualChannel) {
    auto status = virtualChannelMap.emplace(virtualChannelId, virtualChannel);
    if (status.second == false) {
        sif::warning << "CCSDSHandler::addVcInterface: Failed to add virtual channel with virtual "
                << "channel id " << static_cast<unsigned int>(virtualChannelId) << std::endl;
    }
}

void CCSDSHandler::handleTelemetry() {
    VirtualChannelMapIter iter;
    for (iter = virtualChannelMap.begin(); iter != VirtualChannelMap.end(); iter++) {
        iter->second.virtualChannel->performOperation();
    }
}

void CCSDSHandler::handleTelecommands() {

}

void CCSDSHandler::searchFrame() {
	frameLength = this->boardHandler.findFrame();
	while (frameLength != 0) {
		ReturnValue_t frame_status = this->dataLinkLayer.processFrame(
				frameLength);
		if (frame_status != RETURN_OK) {
			triggerEvent(DataLinkLayer::FRAME_PROCESSING_FAILED, frame_status,
					0);
		}
		boardHandler.resetFrameBuffer();
		frameLength = boardHandler.findFrame();
	}
}

ReturnValue_t CCSDSHandler::initialize() {
	PtmeIF *ptme = objectManager->get<PtmeIF>(ptmeId);
	if (ptme == nullptr) {
		return ObjectManagerIF::CHILD_INIT_FAILED;
	}
	status = boardHandler.initialize(channel);
	if (status != RETURN_OK) {
		return status;
	}
	status = modeHelper.initialize();
	if (status != RETURN_OK) {
		return status;
	}
	status = healthHelper.initialize();
	if (status != HasReturnvaluesIF::RETURN_OK) {
		return status;
	}
	status = memoryHelper.initialize();
	if (status != HasReturnvaluesIF::RETURN_OK) {
		return status;
	}
	status = parameterHelper.initialize();
	if (status != HasReturnvaluesIF::RETURN_OK) {
		return status;
	}
	status = powerSwitcher.initialize(objects::PCDU_HANDLER);
	if (status != HasReturnvaluesIF::RETURN_OK) {
		return status;
	}
	StorageManagerIF* tmStore = objectManager->get<StorageManagerIF>(
			objects::TM_STORE);
	status = dataLinkLayer.initialize();
	if ((tmStore != NULL) && status == RETURN_OK) {
		status = RETURN_OK;
		for (uint8_t i = 0; i < BoardHandler::USED_VIRTUAL_CHANNELS_PER_BOARD;
				++i) {
//			this->virtualChannels[i] = objectManager->get<VCGeneration>(
//					objects::CCSDS_VC_BASE + i);
			if (this->virtualChannels[i] != NULL) {
				this->virtualChannels[i]->setPacketStore(tmStore);
				this->virtualChannels[i]->setBoardHandler(&this->boardHandler);
			} else {
				status = RETURN_FAILED;
			}
		}
	} else {
		status = RETURN_FAILED;
		error << "CCSDSHandler " << std::hex << this->getObjectId() << std::dec
				<< " ::CCSDSHandler: Configuration failed." << std::endl;
	}
	status = fdir.initialize();
	if (status != HasReturnvaluesIF::RETURN_OK) {
		return status;
	}
	return status;
}

void CCSDSHandler::readCommandQueue(void) {
	CommandMessage commandMessage;
	ReturnValue_t result = RETURN_FAILED;

	result = commandQueue.receiveMessage(&commandMessage);
    if (result == RETURN_OK) {
        result = parameterHelper.handleParameterMessage(&commandMessage);
        if (result == RETURN_OK) {
            return;
        }
        CommandMessage reply;
        reply.setReplyRejected(CommandMessage::UNKNOW_COMMAND,
                commandMessage.getCommand());
        commandQueue.reply(&reply);
    }
}

MessageQueueId_t CCSDSHandler::getCommandQueue() const {
	return commandQueue.getId();
}

void CCSDSHandler::flushTmChannels() {
	for (uint8_t i = 0; i < BoardHandler::USED_VIRTUAL_CHANNELS_PER_BOARD;
			++i) {
		virtualChannels[i]->flush();
	}
}

ReturnValue_t CCSDSHandler::handleMemoryLoad(uint32_t address,
		const uint8_t* data, uint32_t size, uint8_t** dataPointer) {
	if (size != 4) {
		return INVALID_SIZE;
	}
	ReturnValue_t result = boardHandler.manualWriteToRegister(address, data);
	if (result == RETURN_OK) {
		pendingWrite = true;
		return DO_IT_MYSELF;
	}
	return result;
}

ReturnValue_t CCSDSHandler::handleMemoryDump(uint32_t address, uint32_t size,
		uint8_t** dataPointer, uint8_t* dumpTarget) {
	if (size != 4) {
		return INVALID_SIZE;
	}
	ReturnValue_t result = boardHandler.sendRegisterReadCommand(address);
	if (result == RETURN_OK) {
		pendingRead = true;
		return DO_IT_MYSELF;
	}
	return result;
}

ReturnValue_t CCSDSHandler::checkModeCommand(Mode_t commandedMode,
		Submode_t commandedSubmode, uint32_t* msToReachTheMode) {
	if (state != STATE_IDLE) {
		return IN_TRANSITION;
	}
	switch (commandedMode) {
	case MODE_ON:
		if ((commandedSubmode == SUBMODE_ACTIVE)
				|| (commandedSubmode == SUBMODE_PASSIVE)) {
			return RETURN_OK;
		} else {
			return INVALID_SUBMODE;
		}
		break;
	case MODE_OFF:
		if (commandedSubmode == SUBMODE_NONE) {
			return RETURN_OK;
		} else {
			return INVALID_SUBMODE;
		}
	default:
		return INVALID_MODE;
	}
}

void CCSDSHandler::startTransition(Mode_t commandedMode,
		Submode_t commandedSubmode) {
	if (commandedMode == mode) {
		if (mode == MODE_ON) {
			state = STATE_SELECT_SUBMODE;
		} else {
			//Turn off switch again anyway
			state = STATE_TURN_OFF;
		}
	} else {
		switch (commandedMode) {
		case MODE_ON:
			state = STATE_TURN_ON;
			break;
		case MODE_OFF:
			state = STATE_TURN_OFF;
			break;
		default:
			//Error case. Cannot happen?
			error << "CCSDSHandler::startTransition: Invalid commanded mode: "
					<< commandedMode << std::endl;
			return;
		}
	}
	triggerEvent(CHANGING_MODE, commandedMode, commandedSubmode);
}

void CCSDSHandler::getMode(Mode_t* modeReturn, Submode_t* submodeReturn) {
	*modeReturn = mode;
	*submodeReturn = submode;
}

void CCSDSHandler::setToExternalControl() {
	healthHelper.setHealth(EXTERNAL_CONTROL);
}

void CCSDSHandler::announceMode(bool recursive) {
	triggerEvent(MODE_INFO, mode, submode);
}

void CCSDSHandler::setParentQueue(MessageQueueId_t parentQueueId) {
	modeHelper.setParentQueue(parentQueueId);
	healthHelper.setParentQeueue(parentQueueId);
	tmPartHealth.setParentQueue(parentQueueId);
}

void CCSDSHandler::doStateMachine() {
	ReturnValue_t status = RETURN_FAILED;
	powerSwitcher.doStateMachine();
	switch (state) {
	case STATE_IDLE:
		//Do nothing? Do perform operation stuff?
		break;
	case STATE_TURN_ON:
		powerSwitcher.turnOn();
		modeHelper.startTimer(powerSwitcher.getSwitchDelay());
		state = STATE_WAIT_ON;
		break;
	case STATE_WAIT_ON:
		if (powerSwitcher.checkSwitchState() == RETURN_OK) {
			modeHelper.startTimer(LINK_UP_DELAY_MS);
			state = STATE_WAIT_LINK;
		}
		if (boardHandler.checkChannel() == RETURN_OK) {
			boardHandler.startStateTranstion();
			state = STATE_INITIALIZE_BOARD;
		} else {
			if (modeHelper.isTimedOut()) {
				triggerEvent(MODE_TRANSITION_FAILED,
						PowerSwitchIF::SWITCH_TIMEOUT, state);
				setMode(MODE_OFF, SUBMODE_NONE);
			}
		}
		break;
	case STATE_WAIT_LINK:
		if (boardHandler.checkAndResetChannel() == RETURN_OK) {
			boardHandler.startStateTranstion();
			state = STATE_INITIALIZE_BOARD;
		} else {
			if (modeHelper.isTimedOut()) {
				triggerEvent(MODE_TRANSITION_FAILED, DeviceHandlerIF::TIMEOUT,
						state);
				setMode(MODE_OFF, SUBMODE_NONE);
			}
		}
		break;
	case STATE_INITIALIZE_BOARD:
		status = boardHandler.initializeBoard();
		switch (status) {
		case RETURN_OK:
			state = STATE_SELECT_SUBMODE;
			break;
		case BoardHandler::IN_TRANSITION:
			//cannot last forever, so just wait.
			break;
		default:
			triggerEvent(MODE_TRANSITION_FAILED, status, state);
			state = STATE_TURN_OFF;
			break;
		}
		break;
	case STATE_SELECT_SUBMODE:
		boardHandler.startStateTranstion();
		if (modeHelper.commandedSubmode == SUBMODE_PASSIVE) {
			state = STATE_WAIT_SUBMODE_PASSIVE;
		} else {
			state = STATE_WAIT_SUBMODE_ACTIVE;
		}
		break;
	case STATE_WAIT_SUBMODE_PASSIVE:
		//Just one wait cycle to get rid of pending TM.
		state = STATE_DO_PASSIVATE;
		break;
	case STATE_DO_PASSIVATE:
		status = boardHandler.tmPassivate();
		switch (status) {
		case RETURN_OK:
			setMode(MODE_ON, SUBMODE_PASSIVE);
			break;
		case BoardHandler::IN_TRANSITION:
			//cannot last forever, so just wait.
			break;
		default:
			//If it comes here and fails then, there might be something wrong with the board.
			triggerEvent(MODE_TRANSITION_FAILED, status, state);
			state = STATE_TURN_OFF;
			break;
		}
		break;
	case STATE_WAIT_SUBMODE_ACTIVE:
		status = boardHandler.tmActivate();
		switch (status) {
		case RETURN_OK:
			setMode(MODE_ON, SUBMODE_ACTIVE);
			break;
		case BoardHandler::IN_TRANSITION:
			//cannot last forever, so just wait.
			break;
		default:
			//If it comes here and fails then, there might be something wrong with the board.
			triggerEvent(MODE_TRANSITION_FAILED, status, state);
			state = STATE_TURN_OFF;
			break;
		}
		break;
	case STATE_TURN_OFF:
		//Passivate first. Mainly optimization for System Testbed.
		boardHandler.startStateTranstion();
		state = STATE_DO_PASSIVATE_OFF;
		//No break
	case STATE_DO_PASSIVATE_OFF:
		status = boardHandler.tmPassivate();
		switch (status) {
		case RETURN_OK:
			state = STATE_TURN_SWITCH_OFF;
			break;
		case BoardHandler::IN_TRANSITION:
			//cannot last forever, so just wait.
			break;
		default:
			//If it comes here and fails then, there might be something wrong with the board.
			//Just turn it off.
			state = STATE_TURN_SWITCH_OFF;
			break;
		}
		break;
	case STATE_TURN_SWITCH_OFF:
		powerSwitcher.turnOff();
		modeHelper.startTimer(powerSwitcher.getSwitchDelay());
		state = STATE_WAIT_OFF;
		break;
	case STATE_WAIT_OFF:
		if ((boardHandler.checkAndResetChannel() == RMAPChannelIF::LINK_DOWN)
				|| (powerSwitcher.checkSwitchState() == RETURN_OK)) {
			setMode(MODE_OFF, SUBMODE_NONE);
			state = STATE_IDLE;
		} else {
			if (modeHelper.isTimedOut()) {
				triggerEvent(MODE_TRANSITION_FAILED, status, state);
				setMode(mode, submode);
			}
		}
		break;
	default:
		break;
	}
}

ReturnValue_t CCSDSHandler::addVirtualChannel(uint8_t virtualChannelId,
		VirtualChannelReceptionIF* object) {
	return this->dataLinkLayer.addVirtualChannel(virtualChannelId, object);
}

void CCSDSHandler::setMode(Mode_t newMode, Submode_t newSubmode) {
	triggerEvent(MODE_INFO, newMode, newSubmode);
	if (newMode == MODE_OFF) {
		boardHandler.setDataPoolVaraiblesInvalid();
	}
	modeHelper.modeChanged(newMode, newSubmode);
	state = STATE_IDLE;
	mode = newMode;
	submode = newSubmode;
}

Mode_t CCSDSHandler::getMode() const {
	return mode;
}

Submode_t CCSDSHandler::getSubmode() const {
	return submode;
}

MessageQueueId_t CCSDSHandler::getReportReceptionQueue(uint8_t virtualChannel) {
	if (virtualChannel < config::NUMBER_OF_VIRTUAL_CHANNELS) {
		return virtualChannels[virtualChannel]->getReportReceptionQueue();
	} else {
		sif::debug << "CCSDSHandler::getReportReceptionQueue: Invalid virtual channel requested";
	}
}

ReturnValue_t CCSDSHandler::getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
        ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues,
        uint16_t startAtIndex) {
    return RETURN_OK;
}