#include "PwrSupply_Laptop.h"

void C_PwrSupply_Laptop::Init() {
    pinMode(A0, OUTPUT);
    SetEnable(0);

    Pot.Init();
    Pot.SetWiper(127);

    ina.begin(MY_INA226_ADDRESS);
    ina.configure(INA226_AVERAGES_1, INA226_BUS_CONV_TIME_1100US, INA226_SHUNT_CONV_TIME_1100US, INA226_MODE_SHUNT_BUS_CONT);
    ina.calibrate(MY_INA226_SHUNT, MY_INA226_MAX_EXP_CURRENT);
}

void C_PwrSupply_Laptop::SetEnable(bool value) {
    digitalWrite(A0, value);

    if (IsEnabled()) regCountdown = COUNTDOWN_VALUE;
}

void C_PwrSupply_Laptop::ToggleEnable() {
    SetEnable(!IsEnabled());

    if (IsEnabled()) regCountdown = COUNTDOWN_VALUE;
}

bool C_PwrSupply_Laptop::IsEnabled() {
    return digitalRead(A0);
}

void C_PwrSupply_Laptop::RequestVoltage(float voltage) {
    setVoltage = voltage;
    int wiper = ((float)(LAPTOP_MAX_V - setVoltage) / 0.0708) + correction;
    Pot.SetWiper(constrain(wiper, 0, 127));
}

void C_PwrSupply_Laptop::CheckFuse() {
    if (power > FUSE_POWER_LAPTOP) {
        SetEnable(0);
        fuseTriggered = true;
    }
}

void C_PwrSupply_Laptop::Tasks() {
    voltage = ina.readBusVoltage();
    power = ina.readBusPower();
    current = fabs(ina.readShuntCurrent());

    if (IsEnabled()) {
        if (regCountdown)
            --regCountdown;
        else {
            float diff = voltage - setVoltage;
            if (diff >= 0.1) {
                correction++;
                regCountdown = COUNTDOWN_VALUE;
            } else if (diff <= -0.1) {
                correction--;
                regCountdown = COUNTDOWN_VALUE;
            }
            RequestVoltage(setVoltage);
        }
    } else
        correction = 0;
    Serial.print("Applying correction: ");
    Serial.println(correction);
}
