#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,
        uint16_t hkTableReplySize, LocalPoolDataSetBase* hkTableDataset) :
        DeviceHandlerBase(objectId, comIF, comCookie), maxConfigTableAddress(maxConfigTableAddress),
        maxHkTableAddress(maxHkTableAddress), hkTableReplySize(hkTableReplySize),
        hkTableDataset(hkTableDataset) {
    if (comCookie == nullptr) {
        sif::error << "GomspaceDeviceHandler::GomspaceDeviceHandler: Invalid com cookie"
                << std::endl;
    }
    if (hkTableDataset == nullptr) {
        sif::error << "GomspaceDeviceHandler::GomspaceDeviceHandler: Invalid hk table data set"
                << 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(GOMSPACE::PING): {
        result = generatePingCommand(commandData, commandDataLen);
        if(result != HasReturnvaluesIF::RETURN_OK) {
            return result;
        }
        break;
    }
    case(GOMSPACE::REBOOT): {
        generateRebootCommand();
        break;
    }
    case(GOMSPACE::PARAM_SET):{
        result = generateSetParamCommand(commandData, commandDataLen);
        if(result != HasReturnvaluesIF::RETURN_OK){
            return result;
        }
        break;
    }
    case(GOMSPACE::PARAM_GET):{
        result = generateGetParamCommand(commandData, commandDataLen);
        if(result != HasReturnvaluesIF::RETURN_OK){
            return result;
        }
        break;
    }
    case(GOMSPACE::GNDWDT_RESET): {
        result = generateResetWatchdogCmd();
        if(result != HasReturnvaluesIF::RETURN_OK){
            return result;
        }
        break;
    }
    case(GOMSPACE::PRINT_HK_TABLE): {
        result = printStatus(deviceCommand);
        break;
    }
    case(GOMSPACE::REQUEST_HK_TABLE): {
        result = generateRequestFullHkTableCmd(hkTableReplySize);
        if(result != HasReturnvaluesIF::RETURN_OK){
            return result;
        }
        break;
    }
    default:
        return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
    }
    return HasReturnvaluesIF::RETURN_OK;
}

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

ReturnValue_t GomspaceDeviceHandler::scanForReply(const uint8_t *start,
        size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {
    switch(rememberCommandId) {
    case(GOMSPACE::PING):
			        *foundId = GOMSPACE::PING;
    *foundLen = PING_REPLY_SIZE;
    rememberCommandId = GOMSPACE::NONE;
    break;
    case(GOMSPACE::PARAM_GET): {
        *foundId = GOMSPACE::PARAM_GET;
        *foundLen = rememberRequestedSize + GOMSPACE::GS_HDR_LENGTH;
        rememberCommandId = GOMSPACE::NONE;
        break;
    }
    case(GOMSPACE::PARAM_SET): {
        *foundId = GOMSPACE::PARAM_SET;
        *foundLen = rememberRequestedSize;
        rememberCommandId = GOMSPACE::NONE;
        break;
    }
    case(GOMSPACE::REQUEST_HK_TABLE): {
        *foundId = GOMSPACE::REQUEST_HK_TABLE;
        *foundLen = rememberRequestedSize + GOMSPACE::GS_HDR_LENGTH;
        rememberCommandId = GOMSPACE::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(GOMSPACE::PING): {
        SerializeElement<uint32_t> replyTime = *packet;
        handleDeviceTM(&replyTime, id, true);
        break;
    }
    case(GOMSPACE::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 = GOMSPACE::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(GOMSPACE::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;
    }
    case(GOMSPACE::REQUEST_HK_TABLE): {
        letChildHandleHkReply(id, packet);
        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 = GOMSPACE::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 + GOMSPACE::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 = GOMSPACE::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 = GOMSPACE::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 = GOMSPACE::GNDWDT_RESET;
    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t GomspaceDeviceHandler::generateRequestFullHkTableCmd(uint16_t hkTableReplySize) {

    uint16_t querySize = hkTableReplySize;
    uint8_t tableId = HK_TABLE_ID;
    RequestFullTableCommand requestFullTableCommand(querySize, tableId);

    size_t cspPacketLen = 0;
    uint8_t* buffer = cspPacket;
    ReturnValue_t result = requestFullTableCommand.serialize(&buffer,
            &cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG);
    if(result != HasReturnvaluesIF::RETURN_OK){
        sif::error << "GomspaceDeviceHandler::generateRequestFullHkTableCmd Failed to serialize "
                "full table request command " << std::endl;
        return result;
    }
    rawPacket = cspPacket;
    rawPacketLen = cspPacketLen;
    rememberRequestedSize = querySize;
    rememberCommandId = GOMSPACE::REQUEST_HK_TABLE;
    return RETURN_OK;
}

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

LocalPoolDataSetBase* GomspaceDeviceHandler::getDataSetHandle(sid_t sid) {
    if(sid == hkTableDataset->getSid()) {
        return hkTableDataset;
    }
    else {
        return nullptr;
    }
}

void GomspaceDeviceHandler::setModeNormal() {
    mode = MODE_NORMAL;
}

ReturnValue_t GomspaceDeviceHandler::printStatus(DeviceCommandId_t cmd) {
    sif::info << "No printHkTable implementation given.." << std::endl;
    return HasReturnvaluesIF::RETURN_OK;
}