#include <csp/csp.h>
#include <csp/interfaces/csp_if_can.h>
#include <mission/devices/P60DockHandler.h>
#include "bsp_linux/comIF/cookies/P60DockCookie.h"
#include "bsp_linux/comIF/P60DockComIF.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;
	}
	p60DockCookie = dynamic_cast<P60DockCookie*> (comCookie);
	if(p60DockCookie == NULL){
		sif::error << "P60DockHandler failed to get P60DockCookie" << 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(READ_MODULE_CFG):{
		p60DockCookie->setReadModuleCfgMessage();
		internalState = InternalStates::READ_MODULE_CFG;
		this->pstStep = GET_READ;
		break;
	}
	case(READ_HK):{
		p60DockCookie->setReadHkMessage();
		internalState = InternalStates::READ_HK;
		this->pstStep = GET_READ;
		break;
	}
	case(PING): {
		break;
	}
	default:
		break;
	}
	return HasReturnvaluesIF::RETURN_OK;
}

void P60DockHandler::fillCommandAndReplyMap(){
	this->insertInCommandAndReplyMap(READ_MODULE_CFG, 3);
	this->insertInCommandAndReplyMap(READ_HK, 3);
}

ReturnValue_t P60DockHandler::scanForReply(const uint8_t *start,
        size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {
	switch(internalState) {
	case(InternalStates::READ_MODULE_CFG): {
			*foundId = READ_MODULE_CFG;
			*foundLen = moduleCfgTableSize;
		break;
	}
	case(InternalStates::READ_HK): {
			*foundId = READ_HK;
			*foundLen = hkTableSize;
		break;
	}
	default:
		return IGNORE_REPLY_DATA;
	}
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t P60DockHandler::interpretDeviceReply(DeviceCommandId_t id,
        const uint8_t *packet) {
	switch(id) {
	case(READ_MODULE_CFG): {
			handleDeviceTM((SerializeIF*)packet, id, true, true);
		break;
	}
	case(READ_HK): {
			handleDeviceTM((SerializeIF*)packet, id, true, true);
		break;
	}
	default:
		break;
	}
    return HasReturnvaluesIF::RETURN_OK;
}

void P60DockHandler::setNormalDatapoolEntriesInvalid(){

}