#include "IMTQHandler.h"

#include <fsfwconfig/OBSWConfig.h>
#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfwconfig/OBSWConfig.h>

IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) :
        DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this) {
    if (comCookie == NULL) {
        sif::error << "IMTQHandler: Invalid com cookie" << std::endl;
    }
}

IMTQHandler::~IMTQHandler() {
}


void IMTQHandler::doStartUp(){
	if(mode == _MODE_START_UP){
	    //TODO: Set to MODE_ON again
		setMode(MODE_NORMAL);
	}
}

void IMTQHandler::doShutDown(){

}

ReturnValue_t IMTQHandler::buildNormalDeviceCommand(
		DeviceCommandId_t * id) {
    *id = IMTQ::GET_ENG_HK_DATA;
    return buildCommandFromCommand(*id, NULL, 0);
}

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

ReturnValue_t IMTQHandler::buildCommandFromCommand(
		DeviceCommandId_t deviceCommand, const uint8_t * commandData,
		size_t commandDataLen) {
	switch(deviceCommand) {
	case(IMTQ::START_ACTUATION_DIPOLE): {
        /* IMTQ expects low byte first */
        commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
        commandBuffer[1] = *(commandData + 1);
        commandBuffer[2] = *(commandData);
        commandBuffer[3] = *(commandData + 3);
        commandBuffer[4] = *(commandData + 2);
        commandBuffer[5] = *(commandData + 5);
        commandBuffer[6] = *(commandData + 4);
        commandBuffer[7] = *(commandData + 7);
        commandBuffer[8] = *(commandData + 6);
        rawPacket = commandBuffer;
        rawPacketLen = 9;
        return RETURN_OK;
    }
	case(IMTQ::GET_ENG_HK_DATA): {
	    commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA;
		rawPacket = commandBuffer;
		rawPacketLen = 1;
		return RETURN_OK;
	}
	case(IMTQ::GET_COMMANDED_DIPOLE): {
	    commandBuffer[0] = IMTQ::CC::GET_COMMANDED_DIPOLE;
		rawPacket = commandBuffer;
		rawPacketLen = 1;
		return RETURN_OK;
	}
	default:
		return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
	}
	return HasReturnvaluesIF::RETURN_FAILED;
}

void IMTQHandler::fillCommandAndReplyMap() {
    this->insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr,
            IMTQ::SIZE_STATUS_REPLY);
    this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset,
            IMTQ::SIZE_ENG_HK_DATA_REPLY);
    this->insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr,
            IMTQ::SIZE_ENG_HK_DATA_REPLY);
}

ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start,
        size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {

    ReturnValue_t result = RETURN_OK;

	switch(*start) {
	case(IMTQ::CC::START_ACTUATION_DIPOLE):
        *foundLen = IMTQ::SIZE_STATUS_REPLY;
        *foundId = IMTQ::START_ACTUATION_DIPOLE;
		break;
	case(IMTQ::CC::GET_ENG_HK_DATA):
        *foundLen = IMTQ::SIZE_ENG_HK_DATA_REPLY;
        *foundId = IMTQ::GET_ENG_HK_DATA;
		break;
	case(IMTQ::CC::GET_COMMANDED_DIPOLE):
        *foundLen = IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY;
        *foundId = IMTQ::GET_COMMANDED_DIPOLE;
		break;
	default:
        sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl;
        result = IGNORE_REPLY_DATA;
		break;
	}

    return result;
}

ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id,
        const uint8_t *packet) {

    ReturnValue_t result = RETURN_OK;

    result = parseStatusByte(packet);

    if (result != RETURN_OK) {
        return result;
    }

    switch (id) {
    case (IMTQ::START_ACTUATION_DIPOLE):
        // Replies only the status byte which is already handled with parseStatusByte
        break;
    case (IMTQ::GET_ENG_HK_DATA):
        fillEngHkDataset(packet);
        break;
    case (IMTQ::GET_COMMANDED_DIPOLE):
        handleGetCommandedDipoleReply(packet);
        break;
    default: {
        sif::debug << "IMTQHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
		return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
	}
	}

	return RETURN_OK;
}

void IMTQHandler::setNormalDatapoolEntriesInvalid(){

}

uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){
    return 500;
}

ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
        LocalDataPoolManager& poolManager) {

    localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::ANALOG_VOLTAGE_MV, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::DIGITAL_CURRENT_A, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::ANALOG_CURRENT_A, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::COIL_X_CURRENT_A, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::COIL_Y_CURRENT_A, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::COIL_Z_CURRENT_A, new PoolEntry<float>( { 0 }));
    localDataPoolMap.emplace(IMTQ::COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));

    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) {
    uint8_t cmdErrorField = *(packet + 1) & 0xF;
    switch (cmdErrorField)  {
    case 0:
        return RETURN_OK;
    case 1:
        sif::error << "IMTQHandler::parseStatusByte: Command rejected without reason" << std::endl;
        return REJECTED_WITHOUT_REASON;
    case 2:
        sif::error << "IMTQHandler::parseStatusByte: Command has invalid command code" << std::endl;
        return INVALID_COMMAND_CODE;
    case 3:
        sif::error << "IMTQHandler::parseStatusByte: Command has missing parameter" << std::endl;
        return PARAMETER_MISSING;
    case 4:
        sif::error << "IMTQHandler::parseStatusByte: Command has invalid parameter" << std::endl;
        return PARAMETER_INVALID;
    case 5:
        sif::error << "IMTQHandler::parseStatusByte: CC unavailable" << std::endl;
        return CC_UNAVAILABLE;
    case 7:
        sif::error << "IMTQHandler::parseStatusByte: IMQT replied internal processing error"
                << std::endl;
        return INTERNAL_PROCESSING_ERROR;
    default:
        sif::error << "IMTQHandler::parseStatusByte: CMD Error field contains unknown error code "
            << cmdErrorField << std::endl;
        return CMD_ERR_UNKNOWN;
    }
}

void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
    uint8_t offset = 2;
    engHkDataset.digitalVoltageMv = *(packet + offset + 1) | *(packet + offset);
    offset += 2;
    engHkDataset.analogVoltageMv = *(packet + offset + 1) | *(packet + offset);
    offset += 2;
    engHkDataset.digitalCurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
    offset += 2;
    engHkDataset.analogCurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
    offset += 2;
    engHkDataset.coilXcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
    offset += 2;
    engHkDataset.coilYcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
    offset += 2;
    engHkDataset.coilZcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
    offset += 2;
    engHkDataset.coilXTemperature = (*(packet + offset + 1) | *(packet + offset));
    offset += 2;
    engHkDataset.coilYTemperature = (*(packet + offset + 1) | *(packet + offset));
    offset += 2;
    engHkDataset.coilZTemperature = (*(packet + offset + 1) | *(packet + offset));
    offset += 2;
    engHkDataset.mcuTemperature = (*(packet + offset + 1) | *(packet + offset));

#if OBSW_VERBOSE_LEVEL >= 1 && IMQT_DEBUG == 1
    sif::info << "IMTQ digital voltage: " << engHkDataset.digitalVoltageMv << " mV" << std::endl;
    sif::info << "IMTQ analog voltage: " << engHkDataset.analogVoltageMv << " mV" << std::endl;
    sif::info << "IMTQ digital current: " << engHkDataset.digitalCurrentA << " A" << std::endl;
    sif::info << "IMTQ analog current: " << engHkDataset.analogCurrentA << " A" << std::endl;
    sif::info << "IMTQ coil X current: " << engHkDataset.coilXcurrentA << " A" << std::endl;
    sif::info << "IMTQ coil Y current: " << engHkDataset.coilYcurrentA << " A" << std::endl;
    sif::info << "IMTQ coil Z current: " << engHkDataset.coilZcurrentA << " A" << std::endl;
    sif::info << "IMTQ coil X temperature: " << engHkDataset.coilXTemperature << " °C"
            << std::endl;
    sif::info << "IMTQ coil Y temperature: " << engHkDataset.coilYTemperature << " °C"
            << std::endl;
    sif::info << "IMTQ coil Z temperature: " << engHkDataset.coilZTemperature << " °C"
            << std::endl;
    sif::info << "IMTQ coil MCU temperature: " << engHkDataset.mcuTemperature << " °C"
            << std::endl;
#endif
}

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



void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) {

    ReturnValue_t result = RETURN_OK;

    if (wiretappingMode == RAW) {
        /* Data already sent in doGetRead() */
        return result;
    }

    DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId);
    if (iter == deviceReplyMap.end()) {
        sif::debug << "IMTQHandler::handleDeviceTM: Unknown reply id" << std::endl;
        return;
    }
    MessageQueueId_t queueId = iter->second.command->second.sendReplyTo;

    if (queueId == NO_COMMANDER) {
        return;
    }

    result = actionHelper.reportData(queueId, replyId, data, dataSize);
    if (result != RETURN_OK) {
        sif::debug << "IMTQHandler::handleDeviceTM: Failed to report data" << std::endl;
    }
}