#include "PCDUHandler.h"
#include <fsfwconfig/objects/systemObjectList.h>
#include <mission/devices/devicedefinitions/GomSpacePackets.h>
#include <fsfw/ipc/QueueFactory.h>
#include <fsfw/housekeeping/HousekeepingSnapshot.h>
#include <fsfwconfig/OBSWConfig.h>

PCDUHandler::PCDUHandler(object_id_t setObjectId, size_t cmdQueueSize) :
        SystemObject(setObjectId), poolManager(this, nullptr), pdu2HkTableDataset(this), cmdQueueSize(
                cmdQueueSize) {

    commandQueue = QueueFactory::instance()->createMessageQueue(cmdQueueSize,
            MessageQueueMessage::MAX_MESSAGE_SIZE);
}

PCDUHandler::~PCDUHandler() {
}

ReturnValue_t PCDUHandler::performOperation(uint8_t counter) {

    if (counter == DeviceHandlerIF::PERFORM_OPERATION) {
        readCommandQueue();
        return RETURN_OK;
    }

    return RETURN_OK;
}

ReturnValue_t PCDUHandler::initialize() {

    ReturnValue_t result;

    IPCStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
    if (IPCStore == nullptr) {
        return ObjectManagerIF::CHILD_INIT_FAILED;
    }

    result = poolManager.initialize(commandQueue);
    if (result != HasReturnvaluesIF::RETURN_OK) {
        return result;
    }

    HasLocalDataPoolIF* pdu2Handler = objectManager->get<HasLocalDataPoolIF>(objects::PDU2_HANDLER);
    if(pdu2Handler == nullptr) {
        sif::error << "PCDUHandler::initialize: Invalid pdu2Handler" << std::endl;
        return RETURN_FAILED;
    }
    result = pdu2Handler->getSubscriptionInterface()->subscribeForSetUpdateMessages(
            PDU2::HK_TABLE_DATA_SET_ID, this->getObjectId(), commandQueue->getId(), true);
    if (result != RETURN_OK) {
        sif::error << "PCDUHandler::initialize: Failed to subscribe for set update messages from "
                << "PDU2Handler" << std::endl;
        return result;
    }

    return RETURN_OK;
}

void PCDUHandler::initializeSwitchStates() {
    switchStates[pcduSwitches::Q7S] = pcduSwitches::INIT_STATE_Q7S;
    switchStates[pcduSwitches::PAYLOAD_PCDU_CH1] = pcduSwitches::INIT_STATE_PAYLOAD_PCDU_CH1;
    switchStates[pcduSwitches::RW] = pcduSwitches::INIT_STATE_RW;
    switchStates[pcduSwitches::TCS_BOARD_8V_HEATER_IN] = pcduSwitches::INIT_STATE_TCS_BOARD_8V_HEATER_IN;
    switchStates[pcduSwitches::SUS_REDUNDANT] = pcduSwitches::INIT_STATE_SUS_REDUNDANT;
    switchStates[pcduSwitches::DEPLOYMENT_MECHANISM] = pcduSwitches::INIT_STATE_DEPLOYMENT_MECHANISM;
    switchStates[pcduSwitches::PAYLOAD_PCDU_CH6] = pcduSwitches::INIT_STATE_PAYLOAD_PCDU_CH6;
    switchStates[pcduSwitches::ACS_BOARD_SIDE_B] = pcduSwitches::INIT_STATE_ACS_BOARD_SIDE_B;
    switchStates[pcduSwitches::PAYLOAD_CAMERA] = pcduSwitches::INIT_STATE_PAYLOAD_CAMERA;
}

void PCDUHandler::readCommandQueue() {
    ReturnValue_t result;
    CommandMessage command;

    result = commandQueue->receiveMessage(&command);
    if (result != RETURN_OK) {
        return;
    }

    result = poolManager.handleHousekeepingMessage(&command);
    if (result == RETURN_OK) {
        return;
    }
}

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

void PCDUHandler::handleChangedDataset(sid_t sid, store_address_t storeId) {

    if (sid == sid_t(objects::PDU2_HANDLER, PDU2::HK_TABLE_DATA_SET_ID)) {
        updatePdu2HousekeepingDataset(storeId);
        updatePdu2SwitchStates();
    }
    else {
        sif::error << "PCDUHandler::handleChangedDataset: Invalid sid" << std::endl;
    }
}

void PCDUHandler::updatePdu2HousekeepingDataset(store_address_t storeId) {
    ReturnValue_t result;

    HousekeepingSnapshot packetUpdate(reinterpret_cast<uint8_t*>(&timeStampPdu2HkDataset),
            sizeof(timeStampPdu2HkDataset), &pdu2HkTableDataset);
    const uint8_t* packet_ptr = NULL;
    size_t size;
    result = IPCStore->getData(storeId, &packet_ptr, &size);
    if (result != RETURN_OK) {
        sif::error << "PCDUHandler::handleChangedDataset: Failed to get data from IPCStore."
                << std::endl;
    }
    pdu2HkTableDataset.read();
    result = packetUpdate.deSerialize(&packet_ptr, &size, SerializeIF::Endianness::MACHINE);
    if (result != RETURN_OK) {
        sif::error << "PCDUHandler::handleChangedDataset: Failed to deserialize packet in "
                << "pdu2HkTableDataset" << std::endl;
    }
    pdu2HkTableDataset.commit();
    result = IPCStore->deleteData(storeId);
    if (result != RETURN_OK) {
        sif::error << "PCDUHandler::handleChangedDataset: Failed to delete data in IPCStore"
                << std::endl;
    }
}

void PCDUHandler::updatePdu2SwitchStates() {
    //TODO: pool read helper
    if (pdu2HkTableDataset.read() == RETURN_OK) {
        switchStates[pcduSwitches::Q7S] = pdu2HkTableDataset.outEnabledQ7S.value;
        switchStates[pcduSwitches::PAYLOAD_PCDU_CH1] = pdu2HkTableDataset.outEnabledPlPCDUCh1.value;
        switchStates[pcduSwitches::RW] = pdu2HkTableDataset.outEnabledReactionWheels.value;
        switchStates[pcduSwitches::TCS_BOARD_8V_HEATER_IN] = pdu2HkTableDataset.outEnabledTCSBoardHeaterIn.value;
        switchStates[pcduSwitches::SUS_REDUNDANT] = pdu2HkTableDataset.outEnabledSUSRedundant.value;
        switchStates[pcduSwitches::DEPLOYMENT_MECHANISM] = pdu2HkTableDataset.outEnabledDeplMechanism.value;
        switchStates[pcduSwitches::PAYLOAD_PCDU_CH6] = pdu2HkTableDataset.outEnabledPlPCDUCh6.value;
        switchStates[pcduSwitches::ACS_BOARD_SIDE_B] = pdu2HkTableDataset.outEnabledAcsBoardSideB.value;
        switchStates[pcduSwitches::PAYLOAD_CAMERA] = pdu2HkTableDataset.outEnabledPayloadCamera.value;
    }
    else {
        sif::debug << "PCDUHandler::updatePdu2SwitchStates: Failed to read PDU2 Hk Dataset"
                << std::endl;
    }
    pdu2HkTableDataset.commit();
}

void PCDUHandler::updatePdu1SwitchStates() {

}

LocalDataPoolManager* PCDUHandler::getHkManagerHandle() {
    return &poolManager;
}

void PCDUHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onOff) const {

    ReturnValue_t result;
    uint16_t memoryAddress;
    size_t parameterValueSize = sizeof(uint8_t);
    uint8_t parameterValue;
    GomspaceDeviceHandler* pdu;

    switch (switchNr) {
    case pcduSwitches::TCS_BOARD_8V_HEATER_IN:
        memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_TCS_BOARD_HEATER_IN;
        pdu = objectManager->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
        break;
    default:
        sif::error << "PCDUHandler::sendSwitchCommand: Invalid switch number " << std::endl;
        return;
    }

    switch (onOff) {
    case PowerSwitchIF::SWITCH_ON:
        parameterValue = 1;
        break;
    case PowerSwitchIF::SWITCH_OFF:
        parameterValue = 0;
        break;
    default:
        sif::error << "PCDUHandler::sendSwitchCommand: Invalid state commanded" << std::endl;
        return;
    }

    const uint8_t* parameterValuePtr = &parameterValue;
    GomspaceSetParamMessage setParamMessage(memoryAddress, &parameterValuePtr, parameterValueSize);

    size_t serializedLength = 0;
    uint8_t command[4];
    uint8_t* commandPtr = command;
    size_t maxSize = sizeof(command);
    setParamMessage.serialize(&commandPtr, &serializedLength, maxSize,
            SerializeIF::Endianness::BIG);

    store_address_t storeAddress;
    result = IPCStore->addData(&storeAddress, command,sizeof(command));

    CommandMessage message;
    ActionMessage::setCommand(&message, GOMSPACE::PARAM_SET, storeAddress);

    result = commandQueue->sendMessage(pdu->getCommandQueue(), &message, 0);
    if (result != RETURN_OK) {
        sif::debug << "PCDUHandler::sendSwitchCommand: Failed to send message to PDU Handler"
                << std::endl;
    }
}

void PCDUHandler::sendFuseOnCommand(uint8_t fuseNr) const {

}

ReturnValue_t PCDUHandler::getSwitchState( uint8_t switchNr ) const  {
    if (switchNr >= pcduSwitches::NUMBER_OF_SWITCHES) {
        sif::debug << "PCDUHandler::getSwitchState: Invalid switch number" << std::endl;
        return RETURN_FAILED;
    }
    if (switchStates[switchNr] == 1) {
        return PowerSwitchIF::SWITCH_ON;
    }
    else {
        return PowerSwitchIF::SWITCH_OFF;
    }
}

ReturnValue_t PCDUHandler::getFuseState( uint8_t fuseNr ) const  {
	return RETURN_OK;
}

uint32_t PCDUHandler::getSwitchDelayMs(void) const {
    return 5000;
}

object_id_t PCDUHandler::getObjectId() const {
    return SystemObject::getObjectId();
}

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

    localDataPoolMap.emplace(P60System::PDU2_CURRENT_OUT_Q7S, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_CURRENT_OUT_PAYLOAD_PCDU_CH1,
            new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_CURRENT_OUT_RW, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_CURRENT_OUT_TCS_BOARD_HEATER_IN,
            new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_CURRENT_OUT_SUS_REDUNDANT, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_CURRENT_OUT_DEPLOYMENT_MECHANISM,
            new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_CURRENT_OUT_PAYLOAD_PCDU_CH6,
            new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_CURRENT_OUT_ACS_BOARD_SIDE_B,
            new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_CURRENT_OUT_PAYLOAD_CAMERA, new PoolEntry<int16_t>( { 0 }));

    localDataPoolMap.emplace(P60System::PDU2_VOLTAGE_OUT_Q7S, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_VOLTAGE_OUT_PAYLOAD_PCDU_CH1,
            new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_VOLTAGE_OUT_RW, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_VOLTAGE_OUT_TCS_BOARD_HEATER_IN,
            new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_VOLTAGE_OUT_SUS_REDUNDANT, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_VOLTAGE_OUT_DEPLOYMENT_MECHANISM,
            new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_VOLTAGE_OUT_PAYLOAD_PCDU_CH6,
            new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_VOLTAGE_OUT_ACS_BOARD_SIDE_B,
            new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_VOLTAGE_OUT_PAYLOAD_CAMERA, new PoolEntry<int16_t>( { 0 }));

    localDataPoolMap.emplace(P60System::PDU2_VCC, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_VBAT, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_TEMPERATURE, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_CONV_EN_1, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_CONV_EN_2, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_CONV_EN_3, new PoolEntry<uint8_t>( { 0 }));

    localDataPoolMap.emplace(P60System::PDU2_OUT_EN_Q7S, new PoolEntry<uint8_t>( {
            pcduSwitches::INIT_STATE_Q7S }));
    localDataPoolMap.emplace(P60System::PDU2_OUT_EN_PAYLOAD_PCDU_CH1, new PoolEntry<uint8_t>( {
            pcduSwitches::INIT_STATE_PAYLOAD_PCDU_CH1 }));
    localDataPoolMap.emplace(P60System::PDU2_OUT_EN_RW, new PoolEntry<uint8_t>( {
            pcduSwitches::INIT_STATE_RW }));
#if TE0720 == 1
    localDataPoolMap.emplace(P60System::PDU2_OUT_EN_TCS_BOARD_HEATER_IN, new PoolEntry<uint8_t>( { 1 }));
#else
    localDataPoolMap.emplace(P60System::PDU2_OUT_EN_TCS_BOARD_HEATER_IN, new PoolEntry<uint8_t>( {pcduSwitches::INIT_STATE_TCS_BOARD_8V_HEATER_IN}));
#endif
    localDataPoolMap.emplace(P60System::PDU2_OUT_EN_SUS_REDUNDANT, new PoolEntry<uint8_t>( {
            pcduSwitches::INIT_STATE_SUS_REDUNDANT }));
    localDataPoolMap.emplace(P60System::PDU2_OUT_EN_DEPLOYMENT_MECHANISM, new PoolEntry<uint8_t>( {
            pcduSwitches::INIT_STATE_DEPLOYMENT_MECHANISM }));
    localDataPoolMap.emplace(P60System::PDU2_OUT_EN_PAYLOAD_PCDU_CH6, new PoolEntry<uint8_t>( {
            pcduSwitches::INIT_STATE_PAYLOAD_PCDU_CH6 }));
    localDataPoolMap.emplace(P60System::PDU2_OUT_EN_ACS_BOARD_SIDE_B, new PoolEntry<uint8_t>( {
            pcduSwitches::INIT_STATE_ACS_BOARD_SIDE_B }));
    localDataPoolMap.emplace(P60System::PDU2_OUT_EN_PAYLOAD_CAMERA, new PoolEntry<uint8_t>( {
            pcduSwitches::INIT_STATE_PAYLOAD_CAMERA }));

    localDataPoolMap.emplace(P60System::PDU2_BOOTCAUSE, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_BOOTCNT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_UPTIME, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_RESETCAUSE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_BATT_MODE, new PoolEntry<uint8_t>( { 0 }));

    localDataPoolMap.emplace(P60System::PDU2_LATCHUP_Q7S, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_LATCHUP_PAYLOAD_PCDU_CH1, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_LATCHUP_RW, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_LATCHUP_TCS_BOARD_HEATER_IN, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_LATCHUP_TCS_BOARD_HEATER_IN, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_LATCHUP_SUS_REDUNDANT, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_LATCHUP_DEPLOYMENT_MECHANISM, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_LATCHUP_PAYLOAD_PCDU_CH6, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_LATCHUP_ACS_BOARD_SIDE_B, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_LATCHUP_PAYLOAD_CAMERA, new PoolEntry<uint16_t>( { 0 }));

    localDataPoolMap.emplace(P60System::PDU2_DEVICE_0, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_DEVICE_1, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_DEVICE_2, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_DEVICE_3, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_DEVICE_4, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_DEVICE_5, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_DEVICE_6, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_DEVICE_7, new PoolEntry<uint8_t>( { 0 }));

    localDataPoolMap.emplace(P60System::PDU2_DEVICE_0_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_DEVICE_1_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_DEVICE_2_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_DEVICE_3_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_DEVICE_4_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_DEVICE_5_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_DEVICE_6_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_DEVICE_7_STATUS, new PoolEntry<uint8_t>( { 0 }));

    localDataPoolMap.emplace(P60System::PDU2_WDT_CNT_GND, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_WDT_CNT_I2C, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_WDT_CNT_CAN, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_WDT_CNT_CSP1, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_WDT_CNT_CSP2, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_WDT_GND_LEFT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_WDT_I2C_LEFT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_WDT_CAN_LEFT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_WDT_CSP_LEFT1, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU2_WDT_CSP_LEFT2, new PoolEntry<uint8_t>( { 0 }));

    return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t PCDUHandler::initializeAfterTaskCreation() {

    if(executingTask != nullptr) {
        pstIntervalMs = executingTask->getPeriodMs();
    }
    this->poolManager.initializeAfterTaskCreation();

    initializeSwitchStates();

    return HasReturnvaluesIF::RETURN_OK;
}

uint32_t PCDUHandler::getPeriodicOperationFrequency() const {
    return pstIntervalMs;
}

void PCDUHandler::setTaskIF(PeriodicTaskIF* task){
    executingTask = task;
}

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