#include "mpod.hpp"

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

MPOD::MPOD() {
    bool _on = false;
    bool _interlocked = 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::OUTPUTVOLTAGE];
    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());

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

    return message;
}

// Create an SNMP SETREQUEST message to switch on or off the specified channel
SNMP::Message *MPOD::setChannelState(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;
}

// Create an SNMP SETREQUEST message to set the channel voltage
SNMP::Message *MPOD::setTargetVoltage(const uint16_t channel, const float targetVoltage) {
    SNMP::Message *message = new SNMP::Message(SNMP::Version::V2C, "guru", SNMP::Type::SetRequest);
    // In SETREQUEST, use node type and set the value.
    // OUTPUT VOLTAGE, float type.
    String snmp_cmd = OID::NAMES[OID::OUTPUTVOLTAGE];
    snmp_cmd += channel;
    message->add(snmp_cmd.c_str(), new OpaqueBER(new OpaqueFloatBER(targetVoltage)));
    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());
                if (status->getLength() == 0) {
                    break;
                }
                _on = status->getBit(0);
                _interlocked = status->getBit(1);
                _rampingUp = status->getBit(11);
                _rampingDown = status->getBit(12);
            }
                found++;
                break;
            case OID::OUTPUTMEASUREMENTSENSEVOLTAGE:
                if (varbind->getLength() != 25) {
                    break;
                }
                _measurementSenseVoltage = getFloatFromVarBind(varbind);
                found++;
                break;
            case OID::OUTPUTMEASUREMENTCURRENT:
                if (varbind->getLength() != 25) {
                    break;
                }
                _measurementCurrent = getFloatFromVarBind(varbind);
                found++;
                break;
            case OID::OUTPUTSWITCH:
                _on = getIntegerFromVarBind(varbind);
                found++;
                break;
            case OID::OUTPUTVOLTAGE:
                if (varbind->getLength() != 25) {
                    break;
                }
                _voltage = getFloatFromVarBind(varbind);
                found++;
                break;
            case OID::OUTPUTCURRENT:
                if (varbind->getLength() != 25) {
                    break;
                }
                _current = getFloatFromVarBind(varbind);
                found++;
                break;
            case OID::OUTPUTVOLTAGERISERATE:
                if (varbind->getLength() != 25) {
                    break;
                }
                _voltageRiseRate = getFloatFromVarBind(varbind);
                found++;
                break;
            default:
                Serial.println(name);
                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);

        // MPOD SNMP channel 0 does not exist (starts from 1; U0 is 1).
        // Mark the channel as 0 if invalid data was received.
        // This can be used later to validate a sent request.
        _channel = found ? _channel : 0;
    }
    // Return true if nodes found, that means this is a valid response from MPOD
    return found;
}

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

bool MPOD::isInterlocked() const { return _interlocked; }

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 += "\"is_interlocked\":" + String(isInterlocked()) + ",";
    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 (invalid channel?).");
    }
}

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

void setChannelStateAndWait(const IPAddress *ipAddr, const uint16_t channel, const uint8_t output) {
    // Send set command
    SNMP::Message *snmp_msg = mpod.setChannelState(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
    uint8_t loopCount = 0;
    bool ramping = true;
    bool settingChannelState = true;
    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();
        loopCount += 1;
        ramping = (mpod.isRampingUp() || mpod.isRampingDown());
        if (!ramping) {
            if ((mpod.isOn() == (bool)output || mpod.isInterlocked())) {
                settingChannelState = false;
            } else if (loopCount >= 5) {
                // There were no changes in 5 reads, it might be that the command (UDP)
                // got swallowed by the MPOD controller while it was doing something else.
                Serial.print("!Resend command to ");
                Serial.println(channel);
                SNMP::Message *snmp_msg = mpod.setChannelState(channel, output);
                snmp.send(snmp_msg, *ipAddr, SNMP::Port::SNMP);
                delete snmp_msg;
                delay(MPOD_UPDATE_LATENCY);
                snmp.loop();
                loopCount = 0;
            }
        }
    } while (settingChannelState);
}

void setChannelVoltageAndWait(const IPAddress *ipAddr, const uint16_t channel,
                              const float targetVoltage) {
    // Send set command
    SNMP::Message *snmp_msg = mpod.setTargetVoltage(channel, targetVoltage);

    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.isRampingDown() || mpod.isRampingUp());
}