/**
 * @file battery.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief Contains the implementation of the class Battery
 * @version 0.1
 * @date 2022-02-05
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#include "battery.h"

Battery::Battery(uint8_t pin, uint32_t r1, uint32_t r2) {
    this->pin = pin;
    this->r1 = r1;
    this->r2 = r2;
    this->calculateBatteryVoltage();
}

void Battery::loop() {
    if (millis() - this->lastMillis < this->delay)
        return;

    this->calculateBatteryVoltage();
    this->calculateBatteryPercent();

    this->lastMillis = millis();
}

double Battery::getBatteryVoltage() {
    double res = this->batteryVoltage;
    return (int)(res*100+0.5)/100.0;
}

double Battery::readInputVoltage() {
    // Reference voltage is 3v3 so maximum reading is 3v3 = 4095 in range 0 to 4095
    double reading = analogRead(this->pin);
    if(reading < 1 || reading > 4095) return 0;
    return  - 0.000000000000016 * pow(reading,4)
            + 0.000000000118171 * pow(reading,3)
            - 0.000000301211691 * pow(reading,2)
            + 0.001109019271794 * reading
            + 0.034143524634089;
}

void Battery::calculateBatteryVoltage() {
    this->batteryVoltage = (readInputVoltage() * (double) (this->r1 + this->r2)) / (double) this->r2;
}

void Battery::calculateBatteryPercent() {
    int8_t size = sizeof(this->capacityVoltages) / sizeof(*this->capacityVoltages);
    uint8_t i;
    for (i = 0; i < size; i++) {
        if (this->batteryVoltage <= this->capacityVoltages[i])
            break;
    }
    if (i == 0) {
        if (this->batteryVoltage > 6)
            std::cout << "Critical low battery!" << std::endl;
    } else if (i == size - 1) {

    } else {
        double diffToLowerVal = this->batteryVoltage - this->capacityVoltages[i - 1];
        double diffToHigherVal = this->capacityVoltages[i] - this->batteryVoltage;
        if (diffToLowerVal > diffToHigherVal)
            i--;
    }
    this->batteryPercent = i * (100 / (size - 1));
}