#include "ina219.hpp"

void ForteINA219::setup() {
    Wire.begin(pinSetup.sda, pinSetup.scl);
    if (!ina219.init()) {
        // Sensor init went wrong
        ESP_LOGW(sensorInformation.getHardwareNameString().c_str(),
                 "Initialization failed");
        return;
    }
}

out_data_ina219 ForteINA219::readData() {
    data.shuntVoltage_mV = Measurement{ina219.getShuntVoltage_mV(), NO_CHANNEL,
                                       INA219_BASE_ADDRESS,
                                       MeasurementType::SHUNT_VOLTAGE, ErrorType::DATA_OK};
    data.busVoltage_V = Measurement{ina219.getBusVoltage_V(), NO_CHANNEL, INA219_BASE_ADDRESS,
                                    MeasurementType::BUS_VOLTAGE, ErrorType::DATA_OK};
    data.current_mA = Measurement{ina219.getCurrent_mA(), NO_CHANNEL, INA219_BASE_ADDRESS,
                                  MeasurementType::CURRENT_mA, ErrorType::DATA_OK};
    data.power_mW = Measurement{ina219.getBusPower(), NO_CHANNEL, INA219_BASE_ADDRESS,
                                MeasurementType::POWER_mW, ErrorType::DATA_OK};
    auto loadVoltage_V_value =
            data.busVoltage_V.getValue() + (data.shuntVoltage_mV.getValue() / 1000);
    data.loadVoltage_V = Measurement{
            loadVoltage_V_value, NO_CHANNEL,
            INA219_BASE_ADDRESS, MeasurementType::LOAD_VOLTAGE_V,
            ErrorType::DATA_OK};

    data.ina219_overflow = ina219.getOverflow();
    if (data.ina219_overflow) {
        // set all measurements to overflow
        data.shuntVoltage_mV.setErrorType(ErrorType::INA219_OVERFLOW);
        data.busVoltage_V.setErrorType(ErrorType::INA219_OVERFLOW);
        data.current_mA.setErrorType(ErrorType::INA219_OVERFLOW);
        data.power_mW.setErrorType(ErrorType::INA219_OVERFLOW);
        data.loadVoltage_V.setErrorType(ErrorType::INA219_OVERFLOW);
        return data;
    } else {
        return data;
    }
}

std::list<Message> ForteINA219::buildMessages() {
    std::list<Message> messages;
    out_data_ina219 measurements = readData();


    messages.emplace_back(measurements.shuntVoltage_mV, sensorInformation, Time::getInstance().getEpochSeconds());
    messages.emplace_back(measurements.busVoltage_V, sensorInformation, Time::getInstance().getEpochSeconds());
    messages.emplace_back(measurements.current_mA, sensorInformation, Time::getInstance().getEpochSeconds());
    messages.emplace_back(measurements.power_mW, sensorInformation, Time::getInstance().getEpochSeconds());
    messages.emplace_back(measurements.loadVoltage_V, sensorInformation, Time::getInstance().getEpochSeconds());
//    messages.emplace_back(ina219OverflowData, sensorInformation, 0); TODO: Do we need this as an extra message?

    return messages;
}

void ForteINA219::setPinSetup(pin_setup_ina219 pins){
    this->pinSetup = pins;
}


SensorInformation ForteINA219::getSensorInformation() const {
    return sensorInformation;
}