#include "OBSWConfig.h"

#include <mission/devices/SyrlinksHkHandler.h>
#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/datapool/PoolReadGuard.h>


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

SyrlinksHkHandler::~SyrlinksHkHandler() {
}


void SyrlinksHkHandler::doStartUp(){
	if(mode == _MODE_START_UP){
		setMode(MODE_ON);
	}
}

void SyrlinksHkHandler::doShutDown(){
    setMode(_MODE_POWER_DOWN);
}

ReturnValue_t SyrlinksHkHandler::buildNormalDeviceCommand(
		DeviceCommandId_t * id) {
    switch (nextCommand) {
    case(SYRLINKS::READ_RX_STATUS_REGISTERS):
        *id = SYRLINKS::READ_RX_STATUS_REGISTERS;
        nextCommand = SYRLINKS::READ_TX_STATUS;
        break;
    case(SYRLINKS::READ_TX_STATUS):
        *id = SYRLINKS::READ_TX_STATUS;
        nextCommand = SYRLINKS::READ_TX_WAVEFORM;
        break;
    case(SYRLINKS::READ_TX_WAVEFORM):
        *id = SYRLINKS::READ_TX_WAVEFORM;
        nextCommand = SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE;
        break;
    case(SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE):
        *id = SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE;
        nextCommand = SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE;
        break;
    case(SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE):
        *id = SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE;
        nextCommand = SYRLINKS::READ_RX_STATUS_REGISTERS;
        break;
    default:
        sif::debug << "SyrlinksHkHandler::buildNormalDeviceCommand: rememberCommandId has invalid"
            << "command id" << std::endl;
        break;
    }
    return buildCommandFromCommand(*id, NULL, 0);
}

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

ReturnValue_t SyrlinksHkHandler::buildCommandFromCommand(
		DeviceCommandId_t deviceCommand, const uint8_t * commandData,
		size_t commandDataLen) {
	switch(deviceCommand) {
	case(SYRLINKS::RESET_UNIT): {
	    resetCommand.copy(reinterpret_cast<char*>(commandBuffer), resetCommand.size(), 0);
		rawPacketLen = resetCommand.size();
		rawPacket = commandBuffer;
		return RETURN_OK;
	}
	case(SYRLINKS::SET_TX_MODE_STANDBY): {
	    setTxModeStandby.copy(reinterpret_cast<char*>(commandBuffer), setTxModeStandby.size(), 0);
		rawPacketLen = setTxModeStandby.size();
		rawPacket = commandBuffer;
		return RETURN_OK;
	}
	case(SYRLINKS::SET_TX_MODE_MODULATION): {
	    setTxModeModulation.copy(reinterpret_cast<char*>(commandBuffer), setTxModeModulation.size(), 0);
		rawPacketLen = setTxModeModulation.size();
		rawPacket = commandBuffer;
		return RETURN_OK;
	}
	case(SYRLINKS::SET_TX_MODE_CW): {
	    setTxModeCw.copy(reinterpret_cast<char*>(commandBuffer), setTxModeCw.size(), 0);
		rawPacketLen = setTxModeCw.size();
		rawPacket = commandBuffer;
		return RETURN_OK;
	}
	case(SYRLINKS::READ_RX_STATUS_REGISTERS): {
	    readRxStatusRegCommand.copy(reinterpret_cast<char*>(commandBuffer), readRxStatusRegCommand.size(), 0);
		rawPacketLen = readRxStatusRegCommand.size();
		rawPacket = commandBuffer;
		return RETURN_OK;
	}
	case(SYRLINKS::READ_TX_STATUS): {
	    readTxStatus.copy(reinterpret_cast<char*>(commandBuffer), readTxStatus.size(), 0);
		rawPacketLen = readTxStatus.size();
		rememberCommandId = SYRLINKS::READ_TX_STATUS;
		rawPacket = commandBuffer;
		return RETURN_OK;
	}
	case(SYRLINKS::READ_TX_WAVEFORM): {
	    readTxWaveform.copy(reinterpret_cast<char*>(commandBuffer), readTxStatus.size(), 0);
		rawPacketLen = readTxWaveform.size();
		rememberCommandId = SYRLINKS::READ_TX_WAVEFORM;
		rawPacket = commandBuffer;
		return RETURN_OK;
	}
	case(SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE): {
	    readTxAgcValueHighByte.copy(reinterpret_cast<char*>(commandBuffer), readTxStatus.size(), 0);
		rawPacketLen = readTxAgcValueHighByte.size();
		rememberCommandId = SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE;
		rawPacket = commandBuffer;
		return RETURN_OK;
	}
	case(SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE): {
	    readTxAgcValueLowByte.copy(reinterpret_cast<char*>(commandBuffer), readTxStatus.size(), 0);
		rawPacketLen = readTxAgcValueLowByte.size();
		rememberCommandId = SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE;
		rawPacket = commandBuffer;
		return RETURN_OK;
	}
	default:
		return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
	}
	return HasReturnvaluesIF::RETURN_FAILED;
}

void SyrlinksHkHandler::fillCommandAndReplyMap() {
    this->insertInCommandAndReplyMap(SYRLINKS::RESET_UNIT, 1, nullptr, SYRLINKS::ACK_SIZE, false,
            true, SYRLINKS::ACK_REPLY);
    this->insertInCommandAndReplyMap(SYRLINKS::SET_TX_MODE_STANDBY, 1, nullptr, SYRLINKS::ACK_SIZE,
            false, true, SYRLINKS::ACK_REPLY);
    this->insertInCommandAndReplyMap(SYRLINKS::SET_TX_MODE_MODULATION, 1, nullptr,
            SYRLINKS::ACK_SIZE, false, true, SYRLINKS::ACK_REPLY);
    this->insertInCommandAndReplyMap(SYRLINKS::SET_TX_MODE_CW, 1, nullptr, SYRLINKS::ACK_SIZE,
            false, true, SYRLINKS::ACK_REPLY);
    this->insertInCommandAndReplyMap(SYRLINKS::READ_TX_STATUS, 1, &txDataset,
            SYRLINKS::READ_ONE_REGISTER_REPLY_SIE);
    this->insertInCommandAndReplyMap(SYRLINKS::READ_TX_WAVEFORM, 1, &txDataset,
            SYRLINKS::READ_ONE_REGISTER_REPLY_SIE);
    this->insertInCommandAndReplyMap(SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE, 1, &txDataset,
            SYRLINKS::READ_ONE_REGISTER_REPLY_SIE);
    this->insertInCommandAndReplyMap(SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE, 1, &txDataset,
            SYRLINKS::READ_ONE_REGISTER_REPLY_SIE);
    this->insertInCommandAndReplyMap(SYRLINKS::READ_RX_STATUS_REGISTERS, 1, &rxDataset,
            SYRLINKS::RX_STATUS_REGISTERS_REPLY_SIZE);
}

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

    ReturnValue_t result = RETURN_OK;

    if(*start != '<') {
        sif::error << "SyrlinksHkHandler::scanForReply: Missing start frame character" << std::endl;
        return MISSING_START_FRAME_CHARACTER;
    }

	switch(*(start + 1)) {
	case('A'):
        *foundLen = SYRLINKS::ACK_SIZE;
        *foundId = SYRLINKS::ACK_REPLY;
		break;
	case('E'):
        *foundLen = SYRLINKS::RX_STATUS_REGISTERS_REPLY_SIZE;
        *foundId = SYRLINKS::READ_RX_STATUS_REGISTERS;
        break;
	case('R'):
	    *foundId = rememberCommandId;
        *foundLen = SYRLINKS::READ_ONE_REGISTER_REPLY_SIE;
        break;
	default:
	    sif::error << "SyrlinksHkHandler::scanForReply: Unknown reply identifier" << std::endl;
		result = IGNORE_REPLY_DATA;
		break;
	}

    return result;
}

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

    ReturnValue_t result;

    switch (id) {
    case (SYRLINKS::ACK_REPLY):
        result = verifyReply(packet, SYRLINKS::ACK_SIZE);
        if (result != RETURN_OK) {
            sif::error << "SyrlinksHkHandler::interpretDeviceReply: Acknowledgement reply has "
                    "invalid crc" << std::endl;
            return CRC_FAILURE;
        }
        result = parseReplyStatus(reinterpret_cast<const char*>(packet + SYRLINKS::MESSAGE_HEADER_SIZE));
        if (result != RETURN_OK) {
            return result;
        }
        break;
    case(SYRLINKS::READ_RX_STATUS_REGISTERS):
        result = verifyReply(packet, SYRLINKS::RX_STATUS_REGISTERS_REPLY_SIZE);
        if (result != RETURN_OK) {
            sif::error << "SyrlinksHkHandler::interpretDeviceReply: Read rx status registers reply "
                    << "has invalid crc" << std::endl;
            return CRC_FAILURE;
        }
        parseRxStatusRegistersReply(packet);
        break;
    case(SYRLINKS::READ_TX_STATUS):
        result = verifyReply(packet, SYRLINKS::READ_ONE_REGISTER_REPLY_SIE);
        if (result != RETURN_OK) {
            sif::error << "SyrlinksHkHandler::interpretDeviceReply: Read tx status reply "
                    << "has invalid crc" << std::endl;
            return CRC_FAILURE;
        }
        parseTxStatusReply(packet);
        break;
    case(SYRLINKS::READ_TX_WAVEFORM):
        result = verifyReply(packet, SYRLINKS::READ_ONE_REGISTER_REPLY_SIE);
        if (result != RETURN_OK) {
            sif::error << "SyrlinksHkHandler::interpretDeviceReply: Read tx waveform reply "
                    << "has invalid crc" << std::endl;
            return CRC_FAILURE;
        }
        parseTxWaveformReply(packet);
        break;
    case(SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE):
        result = verifyReply(packet, SYRLINKS::READ_ONE_REGISTER_REPLY_SIE);
        if (result != RETURN_OK) {
            sif::error << "SyrlinksHkHandler::interpretDeviceReply: Read tx AGC high byte reply "
                    << "has invalid crc" << std::endl;
            return CRC_FAILURE;
        }
        parseAgcHighByte(packet);
        break;
    case(SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE):
        result = verifyReply(packet, SYRLINKS::READ_ONE_REGISTER_REPLY_SIE);
        if (result != RETURN_OK) {
            sif::error << "SyrlinksHkHandler::interpretDeviceReply: Read tx AGC low byte reply "
                    << "has invalid crc" << std::endl;
            return CRC_FAILURE;
        }
        parseAgcLowByte(packet);
        break;
	default: {
        sif::debug << "SyrlinksHkHandler::interpretDeviceReply: Unknown device reply id"
                << std::endl;
		return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
	}
	}

	return RETURN_OK;
}

LocalPoolDataSetBase* SyrlinksHkHandler::getDataSetHandle(sid_t sid) {
    if (sid == rxDataset.getSid()) {
        return &rxDataset;
    }
    else if (sid== txDataset.getSid()) {
        return &txDataset;
    }
    else {
        sif::error << "SyrlinksHkHandler::getDataSetHandle: Invalid sid" << std::endl;
        return nullptr;
    }
}

std::string SyrlinksHkHandler::convertUint16ToHexString(uint16_t intValue) {
    std::stringstream stream;
    stream << std::setfill('0') << std::setw(4) << std::hex << std::uppercase << intValue;
    return stream.str();
}

uint8_t SyrlinksHkHandler::convertHexStringToUint8(const char* twoChars) {
    uint32_t value;
    std::string hexString(twoChars, 2);
    std::stringstream stream;
    stream << std::hex << hexString;
    stream >> value;
    return static_cast<uint8_t>(value);
}

uint16_t SyrlinksHkHandler::convertHexStringToUint16(const char* fourChars) {
    uint16_t value = 0;
    value = convertHexStringToUint8(fourChars) << 8 | convertHexStringToUint8(fourChars+2);
    return value;
}

uint32_t SyrlinksHkHandler::convertHexStringToUint32(const char* characters, uint8_t numberOfChars) {

    uint32_t value = 0;

    switch (numberOfChars) {
    case 6:
        value = convertHexStringToUint8(characters) << 16
                | convertHexStringToUint8(characters + 2) << 8
                | convertHexStringToUint8(characters + 4);
        return value;
    case 8:
        value = convertHexStringToUint8(characters) << 24
                | convertHexStringToUint8(characters + 2) << 16
                | convertHexStringToUint8(characters + 4) << 8
                | convertHexStringToUint8(characters + 4);
        return value;
    default:
        sif::debug << "SyrlinksHkHandler::convertHexStringToUint32: Invalid number of characters. "
                << "Must be either 6 or 8" << std::endl;
        return 0;
    }
}

ReturnValue_t SyrlinksHkHandler::parseReplyStatus(const char* status) {
    switch (*status) {
    case '0':
        return RETURN_OK;
    case '1':
        sif::debug << "SyrlinksHkHandler::parseReplyStatus: Uart faming or parity error"
                << std::endl;
        return UART_FRAMIN_OR_PARITY_ERROR_ACK;
    case '2':
        sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad character detected" << std::endl;
        return BAD_CHARACTER_ACK;
    case '3':
        sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad parameter value (unexpected value "
                << "detected" << std::endl;
        return BAD_PARAMETER_VALUE_ACK;
    case '4':
        sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad end of frame" << std::endl;
        return BAD_END_OF_FRAME_ACK;
    case '5':
        sif::debug << "SyrlinksHkHandler::parseReplyStatus: Unknown command id or attempt to access"
                << " a protected register" << std::endl;
        return UNKNOWN_COMMAND_ID_ACK;
    case '6':
        sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad CRC" << std::endl;
        return BAD_CRC_ACK;
    default:
        sif::debug << "SyrlinksHkHandler::parseReplyStatus: Status reply contains an invalid "
            << "status id" << std::endl;
        return RETURN_FAILED;
    }
}

ReturnValue_t SyrlinksHkHandler::verifyReply(const uint8_t* packet, uint8_t size) {
    int result = 0;
    /* Calculate crc from received packet */
    uint16_t crc = CRC::crc16ccitt(packet, size - SYRLINKS::SIZE_CRC_AND_TERMINATION,
            CRC_INITIAL_VALUE);
    std::string recalculatedCrc = convertUint16ToHexString(crc);

    const char* startOfCrc = reinterpret_cast<const char*>(packet + size - SYRLINKS::SIZE_CRC_AND_TERMINATION);
    const char* endOfCrc = reinterpret_cast<const char*>(packet + size - 1);

    std::string replyCrc(startOfCrc, endOfCrc);

    result = recalculatedCrc.compare(replyCrc);
    if (result != 0) {
        return RETURN_FAILED;
    }
    return RETURN_OK;
}

void SyrlinksHkHandler::parseRxStatusRegistersReply(const uint8_t* packet) {
    PoolReadGuard readHelper(&rxDataset);
    uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE;
    rxDataset.rxStatus = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
    offset += 2;
    rxDataset.rxSensitivity = convertHexStringToUint32(reinterpret_cast<const char*>(packet + offset), 6);
    offset += 6;
    rxDataset.rxFrequencyShift = convertHexStringToUint32(reinterpret_cast<const char*>(packet + offset), 6);
    offset += 6;
    rxDataset.rxIqPower = convertHexStringToUint16(reinterpret_cast<const char*>(packet + offset));
    offset += 4;
    rxDataset.rxAgcValue = convertHexStringToUint16(reinterpret_cast<const char*>(packet + offset));
    offset += 4;
    offset += 2; // reserved register
    rxDataset.rxDemodEb= convertHexStringToUint32(reinterpret_cast<const char*>(packet + offset), 6);
    offset += 6;
    rxDataset.rxDemodN0= convertHexStringToUint32(reinterpret_cast<const char*>(packet + offset), 6);
    offset += 6;
    rxDataset.rxDataRate = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));

#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_SYRLINKS == 1
    sif::info << "Syrlinks RX Status: 0x" << std::hex << (unsigned int)rxDataset.rxStatus.value << std::endl;
    sif::info << "Syrlinks RX Sensitivity: " << std::dec << rxDataset.rxSensitivity << std::endl;
    sif::info << "Syrlinks RX Frequency Shift: " << rxDataset.rxFrequencyShift << std::endl;
    sif::info << "Syrlinks RX IQ Power: " << rxDataset.rxIqPower << std::endl;
    sif::info << "Syrlinks RX AGC Value: " << rxDataset.rxAgcValue << std::endl;
    sif::info << "Syrlinks RX Demod Eb: " << rxDataset.rxDemodEb << std::endl;
    sif::info << "Syrlinks RX Demod N0: " << rxDataset.rxDemodN0 << std::endl;
    sif::info << "Syrlinks RX Datarate: " << (unsigned int)rxDataset.rxDataRate.value << std::endl;
#endif
}

void SyrlinksHkHandler::parseTxStatusReply(const uint8_t* packet) {
    PoolReadGuard readHelper(&txDataset);
    uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE;
    txDataset.txStatus = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_SYRLINKS == 1
    sif::info << "Syrlinks TX Status: 0x" << std::hex << (unsigned int) txDataset.txStatus.value
            << std::endl;
#endif
}

void SyrlinksHkHandler::parseTxWaveformReply(const uint8_t* packet) {
    PoolReadGuard readHelper(&txDataset);
    uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE;
    txDataset.txWaveform = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_SYRLINKS == 1
    sif::info << "Syrlinks TX Waveform: 0x" << std::hex << (unsigned int) txDataset.txWaveform.value
            << std::endl;
#endif
}

void SyrlinksHkHandler::parseAgcLowByte(const uint8_t* packet) {
    PoolReadGuard readHelper(&txDataset);
    uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE;
    txDataset.txAgcValue = agcValueHighByte << 8 | convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_SYRLINKS == 1
    sif::info << "Syrlinks TX AGC Value: " << txDataset.txAgcValue << std::endl;
#endif
}

void SyrlinksHkHandler::parseAgcHighByte(const uint8_t* packet) {
    PoolReadGuard readHelper(&txDataset);
    uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE;
    agcValueHighByte = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
}

void SyrlinksHkHandler::setNormalDatapoolEntriesInvalid(){

}

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

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

    localDataPoolMap.emplace(SYRLINKS::RX_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(SYRLINKS::RX_SENSITIVITY, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(SYRLINKS::RX_FREQUENCY_SHIFT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(SYRLINKS::RX_IQ_POWER, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(SYRLINKS::RX_AGC_VALUE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(SYRLINKS::RX_DEMOD_EB, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(SYRLINKS::RX_DEMOD_N0, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(SYRLINKS::RX_DATA_RATE, new PoolEntry<uint8_t>( { 0 }));

    localDataPoolMap.emplace(SYRLINKS::TX_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(SYRLINKS::TX_WAVEFORM, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(SYRLINKS::TX_AGC_VALUE, new PoolEntry<uint16_t>( { 0 }));

    return HasReturnvaluesIF::RETURN_OK;
}

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