#include "mpod.hpp"

#include <SNMP.h>
#include <WiFiUdp.h>

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

// Create an SNMP GETREQUEST message
SNMP::Message *MPOD::read(uint16_t channel) {
    SNMP::Message *message =
        new SNMP::Message(SNMP::Version::V2C, "public", SNMP::Type::GetRequest);
    // In GETREQUEST, values are always of type NULL.
    String snmp_cmd = OID::NAMES[OID::OUTPUTSTATUS];
    snmp_cmd += channel;
    message->add(snmp_cmd.c_str());

    snmp_cmd = OID::NAMES[OID::OUTPUTMEASUREMENTSENSEVOLTAGE];
    snmp_cmd += channel;
    message->add(snmp_cmd.c_str());

    snmp_cmd = OID::NAMES[OID::OUTPUTMEASUREMENTCURRENT];
    snmp_cmd += channel;
    message->add(snmp_cmd.c_str());

    return message;
}

// Create an SNMP SETREQUEST message to switch on or off the MPOD
SNMP::Message *MPOD::output(const uint16_t channel, const bool on) {
    SNMP::Message *message = new SNMP::Message(SNMP::Version::V2C, "guru", SNMP::Type::SetRequest);
    // In SETREQUEST, use node type and set the value.
    // OUTPUT SWITCH, integer type, 0 is OFF and 1 is ON.
    String snmp_cmd = OID::NAMES[OID::OUTPUTSWITCH];
    snmp_cmd += channel;
    message->add(snmp_cmd.c_str(), 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);
                _rampingUp = status->getBit(11);
                _rampingDown = 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;
        }

        // Get the channel ID from the last value in the varbind name
        // No validation is done here as the varbind must be legal to get
        // that far.
        const char *channel = strrchr(varbind->getName(), '.') + 1;
        _channel = atoi(channel);
    }
    // Return true if nodes found, that means this is a valid response from MPOD
    return found;
}

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

bool MPOD::isRampingUp() const { return _rampingUp; }

bool MPOD::isRampingDown() const { return _rampingDown; }

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; }

uint16_t MPOD::getChannel() const { return _channel; }

// 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 = "{\n";
    json += "\"channel\":" + String(getChannel()) + ",";
    json += "\"is_on\":" + String(isOn()) + ",";
    json += "\"ramping_up\":" + String(isRampingUp()) + ",";
    json += "\"ramping_down\":" + String(isRampingDown()) + ",";
    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.getChannel());
        Serial.print(mpod.isOn() ? " on" : " off");
        if (mpod.isRampingUp()) {
            Serial.print(" ramping up");
        }
        if (mpod.isRampingDown()) {
            Serial.print(" ramping 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");
}

/* setChannelAndWait
 * Set a channel's parameters and wait until it's settled.
 */
void setChannelAndWait(const IPAddress *ipAddr, const uint16_t channel, const uint8_t output) {
    // Send set command
    SNMP::Message *snmp_msg = mpod.output(channel, output);
    snmp.send(snmp_msg, *ipAddr, SNMP::Port::SNMP);
    delete snmp_msg;

    // wait for channel reply
    delay(MPOD_UPDATE_LATENCY);
    snmp.loop();

    // Poll channel until it's settled
    do {
        SNMP::Message *snmp_msg = mpod.read(channel);
        snmp.send(snmp_msg, *ipAddr, SNMP::Port::SNMP);
        delete snmp_msg;
        delay(MPOD_UPDATE_LATENCY);
        snmp.loop();
    } while (mpod.isOn() != (bool)output || mpod.isRampingDown() || mpod.isRampingUp());
}