#include "PDU1Handler.h"
#include <mission/devices/devicedefinitions/GomSpacePackets.h>
#include <OBSWConfig.h>

PDU1Handler::PDU1Handler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) :
        GomspaceDeviceHandler(objectId, comIF, comCookie, PDU::MAX_CONFIGTABLE_ADDRESS,
                PDU::MAX_HKTABLE_ADDRESS, PDU::HK_TABLE_REPLY_SIZE, &pdu1HkTableDataset), pdu1HkTableDataset(
                this) {
}

PDU1Handler::~PDU1Handler() {
}

ReturnValue_t PDU1Handler::buildNormalDeviceCommand(
        DeviceCommandId_t * id) {
    *id = GOMSPACE::REQUEST_HK_TABLE;
    return buildCommandFromCommand(*id, NULL, 0);
}

void PDU1Handler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *packet) {

    parseHkTableReply(packet);
    handleDeviceTM(&pdu1HkTableDataset, id, true);

#if OBSW_VERBOSE_LEVEL >= 1 && PDU1_DEBUG == 1
    pdu1HkTableDataset.read();
    sif::info << "PDU1 VCC: " << pdu1HkTableDataset.vcc << " mV" << std::endl;
    float vbat = pdu1HkTableDataset.vbat.value * 0.1;
    sif::info << "PDU1 VBAT: " << vbat << std::endl;
    float temperatureC = pdu1HkTableDataset.temperature.value * 0.1;
    sif::info << "PDU1 Temperature: " << temperatureC << " °C" << std::endl;
    sif::info << "PDU1 csp1 watchdog pings before reboot: "
            << unsigned(pdu1HkTableDataset.csp1WatchdogPingsLeft.value) << std::endl;
    sif::info << "PDU1 csp2 watchdog pings before reboot: "
            << unsigned(pdu1HkTableDataset.csp2WatchdogPingsLeft.value) << std::endl;
    pdu1HkTableDataset.commit();
#endif
}

void PDU1Handler::parseHkTableReply(const uint8_t *packet) {
    uint16_t dataOffset = 0;
    pdu1HkTableDataset.read();
    /* Fist 10 bytes contain the gomspace header. Each variable is preceded by the 16-bit table
     * address. */
    dataOffset += 12;
    pdu1HkTableDataset.currentOutTCSBoard3V3 = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.currentOutSyrlinks = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.currentOutStarTracker = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.currentOutMGT = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.currentOutSUSNominal = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.currentOutSolarCellExp = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.currentOutPLOC = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.currentOutACSBoardSideA = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.currentOutChannel8 = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;

    pdu1HkTableDataset.voltageOutTCSBoard3V3 = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.voltageOutSyrlinks = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.voltageOutStarTracker = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.voltageOutMGT = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.voltageOutSUSNominal = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.voltageOutSolarCellExp = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.voltageOutPLOC = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.voltageOutACSBoardSideA = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.voltageOutChannel8 = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;

    pdu1HkTableDataset.vcc = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.vbat = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.temperature = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;

    pdu1HkTableDataset.converterEnable1 = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.converterEnable2 = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.converterEnable3 = *(packet + dataOffset);
    dataOffset += 3;

    pdu1HkTableDataset.outEnabledTCSBoard3V3 = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.outEnabledSyrlinks = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.outEnabledStarTracker = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.outEnabledMGT = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.outEnabledSUSNominal = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.outEnabledSolarCellExp = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.outEnabledPLOC = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.outEnabledAcsBoardSideA = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.outEnabledChannel8 = *(packet + dataOffset);
    dataOffset += 3;

    pdu1HkTableDataset.bootcause = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
    dataOffset += 6;
    pdu1HkTableDataset.bootcount = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
    dataOffset += 6;
    pdu1HkTableDataset.uptime = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
    dataOffset += 6;
    pdu1HkTableDataset.resetcause = *(packet + dataOffset + 1) << 8 | *(packet + dataOffset);
    dataOffset += 4;
    pdu1HkTableDataset.battMode = *(packet + dataOffset);
    /* +10 because here begins the second gomspace csp packet */
    dataOffset += 3 + 10;

    pdu1HkTableDataset.latchupsTcsBoard3V3 = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.latchupsSyrlinks = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.latchupsStarTracker = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.latchupsMgt = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.latchupsSusNominal = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.latchupsSolarCellExp = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.latchupsPloc = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.latchupsAcsBoardSideA = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;
    pdu1HkTableDataset.latchupsChannel8 = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
    dataOffset += 4;

    pdu1HkTableDataset.device0 = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.device1 = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.device2 = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.device3 = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.device4 = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.device5 = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.device6 = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.device7 = *(packet + dataOffset);
    dataOffset += 3;

    pdu1HkTableDataset.device0Status = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.device1Status = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.device2Status = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.device3Status = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.device4Status = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.device5Status = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.device6Status = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.device7Status = *(packet + dataOffset);
    dataOffset += 3;

    pdu1HkTableDataset.gndWdtReboots = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
    dataOffset += 6;
    pdu1HkTableDataset.i2cWdtReboots = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
    dataOffset += 6;
    pdu1HkTableDataset.canWdtReboots = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
    dataOffset += 6;
    pdu1HkTableDataset.csp1WdtReboots = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
    dataOffset += 6;
    pdu1HkTableDataset.csp2WdtReboots = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
    dataOffset += 6;
    pdu1HkTableDataset.groundWatchdogSecondsLeft = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
    dataOffset += 6;
    pdu1HkTableDataset.i2cWatchdogSecondsLeft = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
    dataOffset += 6;
    pdu1HkTableDataset.canWatchdogSecondsLeft = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 | *(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
    dataOffset += 6;
    pdu1HkTableDataset.csp1WatchdogPingsLeft = *(packet + dataOffset);
    dataOffset += 3;
    pdu1HkTableDataset.csp2WatchdogPingsLeft = *(packet + dataOffset);

    pdu1HkTableDataset.commit();
    pdu1HkTableDataset.setChanged(true);
}

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

    localDataPoolMap.emplace(P60System::PDU1_CURRENT_OUT_TCS_BOARD_3V3, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_CURRENT_OUT_SYRLINKS, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_CURRENT_OUT_STAR_TRACKER, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_CURRENT_OUT_MGT, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_CURRENT_OUT_SUS_NOMINAL, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_CURRENT_OUT_SOLAR_CELL_EXP, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_CURRENT_OUT_PLOC, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_CURRENT_OUT_ACS_BOARD_SIDE_A, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_CURRENT_OUT_CHANNEL8, new PoolEntry<int16_t>( { 0 }));

    localDataPoolMap.emplace(P60System::PDU1_VOLTAGE_OUT_TCS_BOARD_3V3, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_VOLTAGE_OUT_SYRLINKS, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_VOLTAGE_OUT_STAR_TRACKER, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_VOLTAGE_OUT_MGT, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_VOLTAGE_OUT_SUS_NOMINAL, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_VOLTAGE_OUT_SOLAR_CELL_EXP, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_VOLTAGE_OUT_PLOC, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_VOLTAGE_OUT_ACS_BOARD_SIDE_A, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_VOLTAGE_OUT_CHANNEL8, new PoolEntry<int16_t>( { 0 }));

    localDataPoolMap.emplace(P60System::PDU1_VCC, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_VBAT, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_TEMPERATURE, new PoolEntry<int16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_CONV_EN_1, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_CONV_EN_2, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_CONV_EN_3, new PoolEntry<uint8_t>( { 0 }));

    localDataPoolMap.emplace(P60System::PDU1_OUT_EN_TCS_BOARD_3V3, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_OUT_EN_SYRLINKS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_OUT_EN_STAR_TRACKER, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_OUT_EN_MGT, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_OUT_EN_SUS_NOMINAL, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_OUT_EN_SOLAR_CELL_EXP, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_OUT_EN_PLOC, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_OUT_EN_ACS_BOARD_SIDE_A, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_OUT_EN_CHANNEL8, new PoolEntry<uint8_t>( { 0 }));

    localDataPoolMap.emplace(P60System::PDU1_BOOTCAUSE, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_BOOTCNT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_UPTIME, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_RESETCAUSE, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_BATT_MODE, new PoolEntry<uint8_t>( { 0 }));

    localDataPoolMap.emplace(P60System::PDU1_LATCHUP_TCS_BOARD_3V3, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_LATCHUP_SYRLINKS, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_LATCHUP_STAR_TRACKER, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_LATCHUP_MGT, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_LATCHUP_SUS_NOMINAL, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_LATCHUP_SOLAR_CELL_EXP, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_LATCHUP_PLOC, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_LATCHUP_ACS_BOARD_SIDE_A, new PoolEntry<uint16_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_LATCHUP_CHANNEL8, new PoolEntry<uint16_t>( { 0 }));

    localDataPoolMap.emplace(P60System::PDU1_DEVICE_0, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_DEVICE_1, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_DEVICE_2, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_DEVICE_3, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_DEVICE_4, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_DEVICE_5, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_DEVICE_6, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_DEVICE_7, new PoolEntry<uint8_t>( { 0 }));

    localDataPoolMap.emplace(P60System::PDU1_DEVICE_0_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_DEVICE_1_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_DEVICE_2_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_DEVICE_3_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_DEVICE_4_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_DEVICE_5_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_DEVICE_6_STATUS, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_DEVICE_7_STATUS, new PoolEntry<uint8_t>( { 0 }));

    localDataPoolMap.emplace(P60System::PDU1_WDT_CNT_GND, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_WDT_CNT_I2C, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_WDT_CNT_CAN, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_WDT_CNT_CSP1, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_WDT_CNT_CSP2, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_WDT_GND_LEFT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_WDT_I2C_LEFT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_WDT_CAN_LEFT, new PoolEntry<uint32_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_WDT_CSP_LEFT1, new PoolEntry<uint8_t>( { 0 }));
    localDataPoolMap.emplace(P60System::PDU1_WDT_CSP_LEFT2, new PoolEntry<uint8_t>( { 0 }));

    return HasReturnvaluesIF::RETURN_OK;
}