#include "UplinkRouter.hpp"
#include <FpConfig.hpp>

namespace Svc {

UplinkRouter::UplinkRouter(const char* name) : 
    UplinkRouterComponentBase(name)
{
    // Initialize statistics
    this->m_stats.commandsRouted = 0;
    this->m_stats.filesRouted = 0;
    this->m_stats.invalidPackets = 0;
}

void UplinkRouter::init(
    NATIVE_INT_TYPE queueDepth,
    NATIVE_INT_TYPE instance
) 
{
    UplinkRouterComponentBase::init(queueDepth, instance);
}

void UplinkRouter::packetIn_handler(
    const NATIVE_INT_TYPE portNum,
    Fw::Buffer& buffer
) 
{
    // Must have enough data for packet descriptor
    if (buffer.getSize() < sizeof(FwPacketDescriptorType)) {
        this->m_stats.invalidPackets++;
        this->log_WARNING_HI_InvalidPacket(buffer.getSize());
        return;
    }

    // Extract packet type
    FwPacketDescriptorType packetType;
    Fw::SerializeBufferBase& serBuffer = buffer.getSerializeRepr();
    serBuffer.deserialize(packetType);

    // Route based on packet type
    switch (packetType) {
        case Fw::ComPacket::FW_PACKET_COMMAND:
            this->routeCommand(buffer);
            break;

        case Fw::ComPacket::FW_PACKET_FILE:
            this->routeFile(buffer);
            break;

        default:
            this->m_stats.invalidPackets++;
            this->log_WARNING_HI_InvalidPacketType(packetType);
            break;
    }
}

void UplinkRouter::routeCommand(Fw::Buffer& buffer) 
{
    // Convert buffer to Com buffer for command port
    Fw::ComBuffer comBuffer;
    comBuffer.setData(buffer.getData(), buffer.getSize());

    // Send command
    if (this->isConnected_cmdOut_OutputPort(0)) {
        this->cmdOut_out(0, comBuffer);
        this->m_stats.commandsRouted++;
        this->log_ACTIVITY_HI_CommandRouted(this->m_stats.commandsRouted);
    }
}

void UplinkRouter::routeFile(Fw::Buffer& buffer) 
{
    // Only route if file port is connected
    if (this->isConnected_fileOut_OutputPort(0)) {
        // Skip packet type bytes since FileUplink doesn't expect them
        U8* data = buffer.getData();
        U32 size = buffer.getSize();
        
        // Create new buffer without packet type
        Fw::Buffer fileBuffer;
        fileBuffer.setData(data + sizeof(FwPacketDescriptorType));
        fileBuffer.setSize(size - sizeof(FwPacketDescriptorType));

        // Send file packet
        this->fileOut_out(0, fileBuffer);
        this->m_stats.filesRouted++;
        this->log_ACTIVITY_HI_FileRouted(this->m_stats.filesRouted);
    }
}

} // namespace Svc