#include <cstring>
#include <sstream>

#include <sys/mman.h>
#include <fcntl.h>

#include "PdecHandler.h"
#include "OBSWConfig.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "fsfw/tmtcservices/TmTcMessage.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw/ipc/QueueFactory.h"


PdecHandler::PdecHandler(object_id_t objectId, object_id_t tcDestinationId,
        LinuxLibgpioIF* gpioComIF, gpioId_t pdecReset, std::string uioConfigMemory,
        std::string uioRamMemory, std::string uioRegisters) :
        SystemObject(objectId), tcDestinationId(tcDestinationId), gpioComIF(gpioComIF), pdecReset(
                pdecReset), uioConfigMemory(uioConfigMemory), uioRamMemory(uioRamMemory), uioRegisters(
                uioRegisters), actionHelper(this, nullptr) {
    commandQueue = QueueFactory::instance()->createMessageQueue(QUEUE_SIZE);
}

PdecHandler::~PdecHandler() {
}

ReturnValue_t PdecHandler::initialize() {

    tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
    if (tcStore == nullptr) {
        sif::error << "PdecHandler::initialize: Invalid TC store" << std::endl;
        return ObjectManagerIF::CHILD_INIT_FAILED;
    }

    tcDestination = ObjectManager::instance()->get<AcceptsTelecommandsIF>(
            tcDestinationId);

    if (tcDestination == nullptr) {
        sif::error << "PdecHandler::initialize: Invalid tc destination specified" << std::endl;
        return ObjectManagerIF::CHILD_INIT_FAILED;
    }

    ReturnValue_t result = RETURN_OK;

    result = getRegisterAddress();
    if (result != RETURN_OK) {
        return ObjectManagerIF::CHILD_INIT_FAILED;
    }

    result = getConfigMemoryBaseAddress();
    if (result != RETURN_OK) {
        return ObjectManagerIF::CHILD_INIT_FAILED;
    }

    result = getRamBaseAddress();
    if (result != RETURN_OK) {
        return ObjectManagerIF::CHILD_INIT_FAILED;
    }

    writePdecConfig();

    result = releasePdec();
    if (result != RETURN_OK) {
        return ObjectManagerIF::CHILD_INIT_FAILED;
    }

    result = actionHelper.initialize(commandQueue);
    if (result != RETURN_OK) {
        return result;
    }

    return RETURN_OK;
}

MessageQueueId_t PdecHandler::getCommandQueue() const {
    return commandQueue->getId();
}

ReturnValue_t PdecHandler::getRegisterAddress() {
    int fd = open(uioRegisters.c_str(), O_RDWR);
    if (fd < 1) {
        sif::warning << "PdecHandler::getRegisterAddress: Invalid UIO device file" << std::endl;
        return RETURN_FAILED;
    }

    registerBaseAddress = static_cast<uint32_t*>(mmap(NULL, REGISTER_MAP_SIZE,
            PROT_WRITE | PROT_READ, MAP_SHARED, fd, 0));

    if (registerBaseAddress == MAP_FAILED) {
        sif::error << "PdecHandler::getRegisterAddress: Failed to map uio address" << std::endl;
        return RETURN_FAILED;
    }

    return RETURN_OK;
}

ReturnValue_t PdecHandler::getConfigMemoryBaseAddress() {
    int fd = open(uioConfigMemory.c_str(), O_RDWR);
    if (fd < 1) {
        sif::warning << "PdecHandler::getConfigMemoryBaseAddress: Invalid UIO device file" << std::endl;
        return RETURN_FAILED;
    }

    memoryBaseAddress = static_cast<uint32_t*>(mmap(NULL, CONFIG_MEMORY_MAP_SIZE, PROT_WRITE | PROT_READ,
            MAP_SHARED, fd, 0));

    if (memoryBaseAddress == MAP_FAILED) {
        sif::error << "PdecHandler::getConfigMemoryBaseAddress: Failed to map uio address" << std::endl;
        return RETURN_FAILED;
    }

    return RETURN_OK;
}

ReturnValue_t PdecHandler::getRamBaseAddress() {
    int fd = open(uioRamMemory.c_str(), O_RDWR);

    ramBaseAddress = static_cast<uint32_t*>(mmap(NULL, RAM_MAP_SIZE, PROT_WRITE | PROT_READ,
            MAP_SHARED, fd, 0));

    if (ramBaseAddress == MAP_FAILED) {
        sif::error << "PdecHandler::getRamBaseAddress: Failed to map RAM base address" << std::endl;
        return RETURN_FAILED;
    }
    return RETURN_OK;
}

void PdecHandler::writePdecConfig()  {

    PdecConfig pdecConfig;

    *(memoryBaseAddress + FRAME_HEADER_OFFSET)= pdecConfig.getConfigWord(0);
    *(memoryBaseAddress + FRAME_HEADER_OFFSET + 1) = pdecConfig.getConfigWord(1);

    // Configure all MAP IDs as invalid
    for (int idx = 0; idx <= MAX_MAP_ADDR; idx += 4) {
        *(memoryBaseAddress + MAP_ADDR_LUT_OFFSET + idx + 1 / 4) = NO_DESTINATION << 24
                | NO_DESTINATION << 16 | NO_DESTINATION << 8 | NO_DESTINATION;

    }

    // All TCs with MAP ID 7 will be routed to the PM module (can then be read from memory)
    uint8_t routeToPm = calcMapAddrEntry(PM_BUFFER);
    *(memoryBaseAddress + MAP_ADDR_LUT_OFFSET + 1) = (NO_DESTINATION << 24) | (NO_DESTINATION << 16) | (NO_DESTINATION << 8)
            | routeToPm;

    // Write map id clock frequencies
    for (int idx = 0; idx <= MAX_MAP_ADDR; idx += 4) {
        *(memoryBaseAddress + MAP_CLK_FREQ_OFFSET + idx / 4) = MAP_CLK_FREQ << 24
                | MAP_CLK_FREQ << 16 | MAP_CLK_FREQ << 8 | MAP_CLK_FREQ;

    }
}

ReturnValue_t PdecHandler::resetFarStatFlag() {
    uint32_t pdecFar = *(registerBaseAddress + PDEC_FAR_OFFSET);
    if (pdecFar != FAR_RESET) {
        sif::warning << "PdecHandler::resetFarStatFlag: FAR register did not match expected value."
                << " Read value: 0x" << std::hex << static_cast<unsigned int>(pdecFar) << std::endl;
        return RETURN_FAILED;
    }
#if OBSW_DEBUG_PDEC_HANDLER == 1
    sif::debug << "PdecHandler::resetFarStatFlag: read FAR with value: 0x" << std::hex << pdecFar
            << std::endl;
#endif /* OBSW_DEBUG_PDEC_HANDLER == 1 */
    return RETURN_OK;
}

ReturnValue_t PdecHandler::releasePdec()  {
    ReturnValue_t result = RETURN_OK;
    result = gpioComIF->pullHigh(pdecReset);
    if (result != RETURN_OK) {
        sif::error << "PdecHandler::releasePdec: Failed to release PDEC reset signal" << std::endl;
    }
    return result;
}

ReturnValue_t PdecHandler::performOperation(uint8_t operationCode) {

    ReturnValue_t result = RETURN_OK;

    readCommandQueue();

    switch(state) {
    case State::INIT:
        resetFarStatFlag();
        if (result != RETURN_OK) {
            // Requires reconfiguration and reinitialization of PDEC
            triggerEvent(INVALID_FAR);
            state = State::WAIT_FOR_RECOVERY;
            return result;
        }
        state = State::RUNNING;
        break;
    case State::RUNNING:
        if (newTcReceived()) {
            handleNewTc();
        }
        checkLocks();
        break;
    case State::WAIT_FOR_RECOVERY:
        break;
    default:
        sif::debug << "PdecHandler::performOperation: Invalid state" << std::endl;
        break;
    }

    return RETURN_OK;
}

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

    result = commandQueue->receiveMessage(&commandMessage);
    if (result == RETURN_OK) {
        result = actionHelper.handleActionMessage(&commandMessage);
        if (result == RETURN_OK) {
            return;
        }
        CommandMessage reply;
        reply.setReplyRejected(CommandMessage::UNKNOWN_COMMAND,
                commandMessage.getCommand());
        commandQueue->reply(&reply);
        return;
    }
}

bool PdecHandler::newTcReceived() {
    uint32_t pdecFar = *(registerBaseAddress + PDEC_FAR_OFFSET);

    if (pdecFar >> STAT_POSITION != NEW_FAR_RECEIVED) {
        return false;
    }
    if (!checkFrameAna(pdecFar)) {
        return false;
    }
    return true;
}

void PdecHandler::checkLocks() {
    uint32_t clcw = getClcw();
    if (!(clcw & NO_RF_MASK) && (lastClcw & NO_RF_MASK)) {
        // Rf available changed from 0 to 1
        triggerEvent(CARRIER_LOCK);
    }
    if (!(clcw & NO_BITLOCK_MASK) && (lastClcw & NO_BITLOCK_MASK)) {
        // Bit lock changed from 0 to 1
        triggerEvent(BIT_LOCK_PDEC);
    }
    lastClcw = clcw;
}

bool PdecHandler::checkFrameAna(uint32_t pdecFar) {
    bool frameValid = false;
    FrameAna_t frameAna = static_cast<FrameAna_t>((pdecFar & FRAME_ANA_MASK) >> FRAME_ANA_POSITION);
    switch(frameAna) {
    case(FrameAna_t::ABANDONED_CLTU): {
        triggerEvent(INVALID_TC_FRAME, ABANDONED_CLTU);
        sif::debug << "PdecHandler::checkFrameAna: Abondoned CLTU" << std::endl;
        break;
    }
    case(FrameAna_t::FRAME_DIRTY): {
        triggerEvent(INVALID_TC_FRAME, FRAME_DIRTY);
        sif::debug << "PdecHandler::checkFrameAna: Frame dirty" << std::endl;
        break;
    }
    case(FrameAna_t::FRAME_ILLEGAL): {
        sif::debug << "PdecHandler::checkFrameAna: Frame illegal for one reason" << std::endl;
        handleIReason(pdecFar, FRAME_ILLEGAL_ONE_REASON);
        break;
    }
    case(FrameAna_t::FRAME_ILLEGAL_MULTI_REASON): {
        sif::debug << "PdecHandler::checkFrameAna: Frame illegal for multiple reasons"
                << std::endl;
        handleIReason(pdecFar, FRAME_ILLEGAL_MULTIPLE_REASONS);
        break;
    }
    case(FrameAna_t::AD_DISCARDED_LOCKOUT): {
        triggerEvent(INVALID_TC_FRAME, AD_DISCARDED_LOCKOUT);
        sif::debug << "PdecHandler::checkFrameAna: AD frame discarded because of lockout"
                << std::endl;
        break;
    }
    case(FrameAna_t::AD_DISCARDED_WAIT): {
        triggerEvent(INVALID_TC_FRAME, AD_DISCARDED_LOCKOUT);
        sif::debug << "PdecHandler::checkFrameAna: AD frame discarded because of wait"
                << std::endl;
        break;
    }
    case(FrameAna_t::AD_DISCARDED_NS_VR): {
        triggerEvent(INVALID_TC_FRAME, AD_DISCARDED_NS_VS);
        sif::debug << "PdecHandler::checkFrameAna: AD frame discarded because N(S) or V(R)"
                << std::endl;
        break;
    }
    case(FrameAna_t::FRAME_ACCEPTED): {
        sif::debug << "PdecHandler::checkFrameAna: Accepted TC frame" << std::endl;
        frameValid = true;
        break;
    }
    default: {
        sif::debug << "PdecHandler::checkFrameAna: Invalid frame analysis report" << std::endl;
        break;
    }
    }
    return frameValid;
}

void PdecHandler::handleIReason(uint32_t pdecFar, ReturnValue_t parameter1) {
    IReason_t ireason = static_cast<IReason_t>((pdecFar & IREASON_MASK) >> IREASON_POSITION);
    switch(ireason) {
    case(IReason_t::NO_REPORT): {
        triggerEvent(INVALID_TC_FRAME, parameter1, NO_REPORT);
        sif::debug << "PdecHandler::handleIReason: No illegal report" << std::endl;
        break;
    }
    case(IReason_t::ERROR_VERSION_NUMBER): {
        triggerEvent(INVALID_TC_FRAME, parameter1, ERROR_VERSION_NUMBER);
        sif::debug << "PdecHandler::handleIReason: Error in version number and reserved A and B "
                << "fields" << std::endl;
        break;
    }
    case(IReason_t::ILLEGAL_COMBINATION): {
        triggerEvent(INVALID_TC_FRAME, parameter1, ILLEGAL_COMBINATION);
        sif::debug << "PdecHandler::handleIReason: Illegal combination (AC) of bypass and control "
                << "command flags" << std::endl;
        break;
    }
    case(IReason_t::INVALID_SC_ID): {
        triggerEvent(INVALID_TC_FRAME, parameter1, INVALID_SC_ID);
        sif::debug << "PdecHandler::handleIReason: Invalid spacecraft identifier " << std::endl;
        break;
    }
    case(IReason_t::INVALID_VC_ID_MSB): {
        triggerEvent(INVALID_TC_FRAME, parameter1, INVALID_VC_ID_MSB);
        sif::debug << "PdecHandler::handleIReason: VC identifier bit 0 to 4 did not match "
                << std::endl;
        break;
    }
    case(IReason_t::INVALID_VC_ID_LSB): {
        triggerEvent(INVALID_TC_FRAME, parameter1, INVALID_VC_ID_LSB);
        sif::debug << "PdecHandler::handleIReason: VC identifier bit 5 did not match " << std::endl;
        break;
    }
    case(IReason_t::NS_NOT_ZERO): {
        triggerEvent(INVALID_TC_FRAME, parameter1, NS_NOT_ZERO);
        sif::debug << "PdecHandler::handleIReason: N(S) of BC or BD frame not set to all zeros"
                << std::endl;
        break;
    }
    case(IReason_t::INCORRECT_BC_CC): {
        triggerEvent(INVALID_TC_FRAME, parameter1, INVALID_BC_CC);
        sif::debug << "PdecHandler::handleIReason: Invalid BC control command format" << std::endl;
        break;
    }
    default: {
        sif::debug << "PdecHandler::handleIReason: Invalid reason id" << std::endl;
        break;
    }
    }
}

void PdecHandler::handleNewTc() {
    ReturnValue_t result = RETURN_OK;

    uint32_t tcLength = 0;
    result = readTc(tcLength);
    if (result != RETURN_OK) {
        return;
    }
#if OBSW_DEBUG_PDEC_HANDLER == 1
    unsigned int mapId = tcSegment[0] & MAP_ID_MASK;
    sif::debug << "PdecHandler::handleNewTc: Received TC segment with map ID " << mapId
            << std::endl;
    printTC(tcLength);
#endif /* OBSW_DEBUG_PDEC_HANDLER */

    store_address_t storeId;
    result = tcStore->addData(&storeId, tcSegment + 1, tcLength - 1);
    if (result != RETURN_OK) {
        sif::warning << "PdecHandler::handleNewTc: Failed to add received space packet to store"
                << std::endl;
        return;
    }

    TmTcMessage message(storeId);

    result = MessageQueueSenderIF::sendMessage(tcDestination->getRequestQueue(), &message);
    if (result != HasReturnvaluesIF::RETURN_OK) {
        sif::warning << "PdecHandler::handleNewTc: Failed to send message to TC destination"
                << std::endl;
        tcStore->deleteData(storeId);
        return;
    }

    return;
}

ReturnValue_t PdecHandler::readTc(uint32_t& tcLength) {
    uint32_t tcOffset = (*(registerBaseAddress + PDEC_BPTR_OFFSET) - PHYSICAL_RAM_BASE_ADDRESS) / 4;

#if OBSW_DEBUG_PDEC_HANDLER == 1
    sif::debug << "PdecHandler::readTc: TC offset: 0x" << std::hex << tcOffset << std::endl;
#endif /* OBSW_DEBUG_PDEC_HANDLER */

    tcLength = *(registerBaseAddress + PDEC_SLEN_OFFSET);

#if OBSW_DEBUG_PDEC_HANDLER == 1
    sif::debug << "PdecHandler::readTc: TC segment length: " << std::dec << tcLength << std::endl;
#endif /* OBSW_DEBUG_PDEC_HANDLER */

    if (tcLength > MAX_TC_SEGMENT_SIZE) {
        sif::warning << "PdecHandler::handleNewTc: Read invalid TC length from PDEC register"
                << std::endl;
        return RETURN_FAILED;
    }

    uint32_t idx = 0;
    uint32_t tcData = 0;
    for (idx = 0; idx <= tcLength; idx = idx + 4) {
        tcData = *(ramBaseAddress + tcOffset + idx / 4);
        if (idx == 0) {
            tcSegment[idx] = static_cast<uint8_t>((tcData >> 16) & 0xFF);
            tcSegment[idx + 1] = static_cast<uint8_t>((tcData >> 8) & 0xFF);
            tcSegment[idx + 2] = static_cast<uint8_t>(tcData & 0xFF);
        }
        else if (tcLength - idx + 1 == 3) {
            tcSegment[idx - 1] = static_cast<uint8_t>((tcData >> 24) & 0xFF);
            tcSegment[idx] = static_cast<uint8_t>((tcData >> 16) & 0xFF);
            tcSegment[idx + 1] = static_cast<uint8_t>((tcData >> 8) & 0xFF);
        }
        else if (tcLength - idx + 1 == 2) {
            tcSegment[idx - 1] = static_cast<uint8_t>((tcData >> 24) & 0xFF);
            tcSegment[idx] = static_cast<uint8_t>((tcData >> 16) & 0xFF);
        }
        else if (tcLength - idx + 1 == 1) {
            tcSegment[idx - 1] = static_cast<uint8_t>((tcData >> 24) & 0xFF);
        }
        else {
            tcSegment[idx - 1] = static_cast<uint8_t>((tcData >> 24) & 0xFF);
            tcSegment[idx] = static_cast<uint8_t>((tcData >> 16) & 0xFF);
            tcSegment[idx + 1] = static_cast<uint8_t>((tcData >> 8) & 0xFF);
            tcSegment[idx + 2] = static_cast<uint8_t>(tcData & 0xFF);
        }
    }

    // Backend buffer is handled back to PDEC3
    *(registerBaseAddress + PDEC_BFREE_OFFSET) = 0;

    return RETURN_OK;
}

void PdecHandler::printTC(uint32_t tcLength) {
    std::stringstream tcSegmentStream;
    tcSegmentStream << "TC segment data: 0x";
    for (uint32_t idx = 0; idx < tcLength; idx++) {
        tcSegmentStream << std::setfill('0') << std::setw(2) << std::hex
                << static_cast<unsigned int>(tcSegment[idx]);
    }
    sif::debug << tcSegmentStream.str() << std::endl;
}

uint8_t PdecHandler::calcMapAddrEntry(uint8_t moduleId) {
    uint8_t lutEntry = 0;
    uint8_t parity = getOddParity(moduleId | (1 << VALID_POSITION));
    lutEntry = (parity << PARITY_POSITION) | (1 << VALID_POSITION) | moduleId;
    return lutEntry;
}

uint8_t PdecHandler::getOddParity(uint8_t number) {
    uint8_t parityBit = 0;
    uint8_t countBits = 0;
    for (unsigned int idx = 0; idx < sizeof(number) * 8; idx++) {
        countBits += (number >> idx) & 0x1;
    }
    parityBit = ~(countBits & 0x1) & 0x1;
    return parityBit;
}

uint32_t PdecHandler::getClcw() {
    return *(registerBaseAddress + PDEC_CLCW_OFFSET);
}

void PdecHandler::printClcw() {
    uint32_t clcw = getClcw();
    uint8_t type = static_cast<uint8_t>((clcw >> 31) & 0x1);
    uint8_t versionNo = static_cast<uint8_t>((clcw >> 29) & 0x3);
    uint8_t status = static_cast<uint8_t>((clcw >> 26) & 0x7);
    uint8_t cop = static_cast<uint8_t>((clcw >> 24) & 0x3);
    uint8_t vcId = static_cast<uint8_t>((clcw >> 18) & 0x3F);
    uint8_t noRf = static_cast<uint8_t>((clcw >> 15) & 0x1);
    uint8_t noBitLock = static_cast<uint8_t>((clcw >> 14) & 0x1);
    uint8_t lockoutFlag = static_cast<uint8_t>((clcw >> 13) & 0x1);
    uint8_t waitFlag = static_cast<uint8_t>((clcw >> 12) & 0x1);
    uint8_t retransmitFlag = static_cast<uint8_t>((clcw >> 11) & 0x1);
    uint8_t farmBcnt = static_cast<uint8_t>((clcw >> 9) & 0x3);
    // Expected frame sequence number in te next AD frame
    uint8_t repValue = static_cast<uint8_t>(clcw & 0xFF);
    sif::info << std::setw(30) << std::left << "CLCW type: " << std::hex
            << "0x" << static_cast<unsigned int>(type) << std::endl;
    sif::info << std::setw(30) << std::left << "CLCW version no: " << std::hex
            << "0x" << static_cast<unsigned int>(versionNo) << std::endl;
    sif::info << std::setw(30) << std::left << "CLCW status: " << std::hex
            << "0x" << static_cast<unsigned int>(status) << std::endl;
    sif::info << std::setw(30) << std::left << "CLCW COP: " << std::hex
            << "0x" << static_cast<unsigned int>(cop) << std::endl;
    sif::info << std::setw(30) << std::left << "CLCW virtual channel ID: " << std::hex
            << "0x" << static_cast<unsigned int>(vcId) << std::endl;
    sif::info << std::setw(30) << std::left << "CLCW no RF: " << std::hex
            << "0x" << static_cast<unsigned int>(noRf) << std::endl;
    sif::info << std::setw(30) << std::left << "CLCW no bit lock: " << std::hex
            << "0x" << static_cast<unsigned int>(noBitLock) << std::endl;
    sif::info << std::setw(30) << std::left << "CLCW lockout flag: " << std::hex
            << "0x" << static_cast<unsigned int>(lockoutFlag) << std::endl;
    sif::info << std::setw(30) << std::left << "CLCW wait flag: " << std::hex
            << "0x" << static_cast<unsigned int>(waitFlag) << std::endl;
    sif::info << std::setw(30) << std::left << "CLCW retransmit flag: " << std::hex
            << "0x" << static_cast<unsigned int>(retransmitFlag) << std::endl;
    sif::info << std::setw(30) << std::left << "CLCW FARM B count: " << std::hex
            << "0x" << static_cast<unsigned int>(farmBcnt) << std::endl;
    sif::info << std::setw(30) << std::left << "CLCW rep value: " << std::hex
            << "0x" << static_cast<unsigned int>(repValue) << std::endl;
}


ReturnValue_t PdecHandler::executeAction(ActionId_t actionId,
        MessageQueueId_t commandedBy, const uint8_t* data, size_t size) {
    switch(actionId) {
    case PRINT_CLCW:
        printClcw();
        return EXECUTION_FINISHED;
    default:
        return COMMAND_NOT_IMPLEMENTED;
    }
}