#include "mpod.hpp"
#include <SNMP.h>
#include <WiFiUdp.h>

MPOD::MPOD() {
    bool _on = false;
    bool _up = false;
    bool _down = false;
    float _measurementSenseVoltage = 0;
    float _measurementCurrent = 0;
    float _voltage = 0;
    float _current = 0;
    float _voltageRiseRate = 0;
}


// Create an SNMP SETREQUEST message to setup MPOD
SNMP::Message* MPOD::setup() {
    // Use read/write community, not read-only
    SNMP::Message* message = new SNMP::Message(SNMP::VERSION2C, "guru", SNMP::TYPE_SETREQUEST);
    // In SETREQUEST, use node type and set the value.
    // OUTPUT SWITCH, integer type, 0 is OFF and 1 is ON.
    message->add(OID::NAMES[OID::OUTPUTSWITCH], new IntegerBER(0));
    // OUTPUT VOLTAGE is an opaque float embedded in an opaque node
    message->add(OID::NAMES[OID::OUTPUTVOLTAGE], new OpaqueBER(new OpaqueFloatBER(10.0)));
    // OUTPUT CURRENT is an opaque float embedded in an opaque node
    message->add(OID::NAMES[OID::OUTPUTCURRENT], new OpaqueBER(new OpaqueFloatBER(0.001)));
    // OUTPUT VOLTAGE RISE RATE is negative. Don't ask me why...
    message->add(OID::NAMES[OID::OUTPUTVOLTAGERISERATE], new OpaqueBER(new OpaqueFloatBER(-1.0)));
    return message;
}

    // Create an SNMP GETREQUEST message
SNMP::Message* MPOD::read() {
    SNMP::Message* message = new SNMP::Message(SNMP::VERSION2C, "public", SNMP::TYPE_GETREQUEST);
    // In GETREQUEST, values are always of type NULL.
    message->add(OID::NAMES[OID::OUTPUTSTATUS]);
    message->add(OID::NAMES[OID::OUTPUTMEASUREMENTSENSEVOLTAGE]);
    message->add(OID::NAMES[OID::OUTPUTMEASUREMENTCURRENT]);
    return message;
}

    // Create an SNMP SETREQUEST message to switch on or off the MPOD
SNMP::Message* MPOD::output(const bool on) {
    SNMP::Message* message = new SNMP::Message(SNMP::VERSION2C, "guru", SNMP::TYPE_SETREQUEST);
    // In SETREQUEST, use node type and set the value.
    // OUTPUT SWITCH, integer type, 0 is OFF and 1 is ON.
    message->add(OID::NAMES[OID::OUTPUTSWITCH], new IntegerBER(on ? 1 : 0));
    return message;
}

    // Parse incoming message
bool MPOD::message(const SNMP::Message *message) {
    unsigned int found = 0;
    unsigned int index = 0;
    // Get the variable binding list from the message.
    VarBindList *varbindlist = message->getVarBindList();
    for (unsigned int index = 0; index < varbindlist->count(); ++index) {
        // Each variable binding is a sequence of 2 objects:
        // - First one is and ObjectIdentifierBER. It holds the OID
        // - Second is the value of any type
        VarBind *varbind = (*varbindlist)[index];
        // There is a convenient function to get the OID as a const char*
        const char *name = varbind->getName();
        switch (OID::match(name)) {
        case OID::OUTPUTSTATUS: {
            // OUTPUTSTATUS is defined in MIB as BITS but encoded as OCTETSTRING by MPOD
            OctetStringBER *status = static_cast<OctetStringBER*>(varbind->getValue());
            _on = status->getBit(0);
            _up = status->getBit(11);
            _down = status->getBit(12);
        }
            found++;
            break;
        case OID::OUTPUTMEASUREMENTSENSEVOLTAGE:
            // Use private helper function to extract float value
            _measurementSenseVoltage = getFloatFromVarBind(varbind);
            found++;
            break;
        case OID::OUTPUTMEASUREMENTCURRENT:
            _measurementCurrent = getFloatFromVarBind(varbind);
            found++;
            break;
        case OID::OUTPUTSWITCH:
            // Use private helper function to extract integer value
            _on = getIntegerFromVarBind(varbind);
            found++;
            break;
        case OID::OUTPUTVOLTAGE:
            _voltage = getFloatFromVarBind(varbind);
            found++;
            break;
        case OID::OUTPUTCURRENT:
            _current = getFloatFromVarBind(varbind);
            found++;
            break;
        case OID::OUTPUTVOLTAGERISERATE:
            _voltageRiseRate = getFloatFromVarBind(varbind);
            found++;
            break;
        }
    }
    // Return true if nodes found, that means this is a valid response from MPOD
    return found;
}

bool MPOD::isOn() const {
    return _on;
}

bool MPOD::isUp() const {
    return _up;
}

bool MPOD::isDown() const {
    return _down;
}

float MPOD::getMeasurementSenseVoltage() const {
    return _measurementSenseVoltage;
}

float MPOD::getMeasurementCurrent() const {
    return _measurementCurrent;
}

float MPOD::getVoltage() const {
    return _voltage;
}

float MPOD::getCurrent() const {
    return _current;
}

float MPOD::getVoltageRiseRate() const {
    return _voltageRiseRate;
}

// Use appropriate cast to get integer value
unsigned int MPOD::getIntegerFromVarBind(const VarBind *varbind) {
    return static_cast<IntegerBER*>(varbind->getValue())->getValue();
}

// Use appropriate casts to get embedded opaque float value
float MPOD::getFloatFromVarBind(const VarBind *varbind) {
    return static_cast<OpaqueFloatBER*>(static_cast<OpaqueBER*>(varbind->getValue())->getBER())->getValue();
}

String MPOD::toJSON() {
    String json = "\"status\":{\n";
    json += "\"is_on\":" + String(isOn()) + ",";
    json += "\"is_up\":" + String(isUp()) + ",";
    json += "\"is_down\":" + String(isDown()) + ",";
    json += "\"sense_voltage\":" + String(getMeasurementSenseVoltage()) + ",";
    json += "\"set_voltage\":" + String(getVoltage()) + ",";
    json += "\"sense_current\":" + String(getMeasurementCurrent()) + ",";
    json += "\"set_current\":" + String(getCurrent()) + ",";
    json += "\"voltage_rise_rate\":" + String(getVoltageRiseRate());
    json += "}";

    return json;
}


WiFiUDP udp;
SNMP::Manager snmp;
MPOD mpod;

// Event handler to process SNMP messages
void onMessage(const SNMP::Message *message, const IPAddress remote, const uint16_t port) {
    if (mpod.message(message)) {
        Serial.println();
        Serial.print("MPOD status");
        Serial.print(mpod.isOn() ? " on" : " off");
        if (mpod.isUp()) {
            Serial.print(" up");
        }
        if (mpod.isDown()) {
            Serial.print(" down");
        }
        Serial.println();
        Serial.print("HV voltage ");
        Serial.print(mpod.getMeasurementSenseVoltage());
        Serial.print(" V (");
        Serial.print(mpod.getVoltage());
        Serial.print(") current ");
        Serial.print(mpod.getMeasurementCurrent());
        Serial.print(" A (");
        Serial.print(mpod.getCurrent());
        Serial.print(") rise rate ");
        Serial.print(mpod.getVoltageRiseRate());
        Serial.println(" V/s");
    } else {
        Serial.println("Received non-MPOD traffic");
    }
}

void initializeSNMP() {
    snmp.begin(&udp);
    snmp.onMessage(onMessage);
    Serial.println("SNMP Server Started");
}