#define MIN_VOLTAGE 1.0
#define VOLTAGE_HYSTERESIS 0.1
#define DISPLAYED_STATE_DURATION 3000UL

#define RGB_LED_RED 3
#define RGB_LED_GREEN 4
#define RGB_LED_BLUE 5

#define BIDIRECTIONAL_VOLTAGE_LED_SWITCH 1

typedef enum
{
    STATE_OK,
    STATE_WARNING_1,
    STATE_WARNING_2,
    STATE_NOT_OK
} State;

State currentState = STATE_OK;

double getBattVoltage(bool getLast)
{
    static double lastVoltage = 0.0;

    if (getLast)
    {
        return lastVoltage;
    }

    double voltage = 0.0;

    for (uint8_t i = 0; i < 20; i++)
    {
        voltage += analogRead(A0);
    }

    voltage /= 20.0;
    voltage = (voltage * (5.0 / 1023.0));
    lastVoltage = voltage;

    return voltage;
}

uint8_t getBattPercent(bool getLast)
{
    double v = getBattVoltage(getLast);
    int result = (v - 0.0) * (100.0 - 0.0) / (5.0 - 0.0) + 0.0;
    return max(min(result, 100), 0);
}

double percentToVoltage(double percent)
{
    return (percent - 0.0) * (5.0 - 0.0) / (100.0 - 0.0) + 0.0;
}

void handleVoltageLed(bool forceUpdate)
{
    static State tempState = STATE_OK;
    static unsigned long lastStateChangeTime = 0;

    double voltage = getBattVoltage(false);
    double warningVoltage1 = percentToVoltage(80.0);
    double warningVoltage2 = percentToVoltage(50.0);

    State previousState = tempState;

    if ((forceUpdate) ||
        (voltage <= MIN_VOLTAGE - VOLTAGE_HYSTERESIS) ||
        (voltage >= MIN_VOLTAGE + VOLTAGE_HYSTERESIS && voltage <= warningVoltage2 - VOLTAGE_HYSTERESIS) ||
        (voltage >= warningVoltage2 + VOLTAGE_HYSTERESIS && voltage <= warningVoltage1 - VOLTAGE_HYSTERESIS) ||
        (voltage >= warningVoltage1 + VOLTAGE_HYSTERESIS))
    {
        if (voltage < MIN_VOLTAGE)
        {
            tempState = STATE_NOT_OK;
        }
        else if (voltage <= warningVoltage2)
        {
            tempState = STATE_WARNING_2;
        }
        else if (voltage <= warningVoltage1)
        {
            tempState = STATE_WARNING_1;
        }
        else
        {
            tempState = STATE_OK;
        }
    }

    if (tempState != previousState)
    {
        lastStateChangeTime = millis();
    }

    if (forceUpdate || ((millis() - lastStateChangeTime) >= DISPLAYED_STATE_DURATION))
    {
        #if BIDIRECTIONAL_VOLTAGE_LED_SWITCH == 0
            if (tempState > currentState)
            {
                currentState = tempState;
            }
        #else
            currentState = tempState;
        #endif

        if (currentState == STATE_WARNING_1)
        {
            digitalWrite(RGB_LED_RED, LOW);
            digitalWrite(RGB_LED_GREEN, HIGH);
            digitalWrite(RGB_LED_BLUE, LOW);
        }
        else if (currentState == STATE_WARNING_2 || currentState == STATE_NOT_OK)
        {
            digitalWrite(RGB_LED_RED, HIGH);
            digitalWrite(RGB_LED_GREEN, LOW);
            digitalWrite(RGB_LED_BLUE, LOW);
        }
        else if (currentState == STATE_OK)
        {
            digitalWrite(RGB_LED_RED, LOW);
            digitalWrite(RGB_LED_GREEN, LOW);
            digitalWrite(RGB_LED_BLUE, HIGH);
        }
    }

    Serial.print("PREV: ");
    Serial.print(previousState);
    Serial.print(" TEMP: ");
    Serial.print(tempState);
    Serial.print(" CURR: ");
    Serial.print(currentState);
    Serial.print(" VOLT: ");
    Serial.print(voltage);
    Serial.print(" COUN: ");
    Serial.println((millis() - lastStateChangeTime) / 1000);
}


void setup() {
  Serial.begin(9600);

  pinMode(RGB_LED_RED, OUTPUT);
  pinMode(RGB_LED_GREEN, OUTPUT);
  pinMode(RGB_LED_BLUE, OUTPUT);

  digitalWrite(RGB_LED_RED, LOW);
  digitalWrite(RGB_LED_GREEN, LOW);
  digitalWrite(RGB_LED_BLUE, LOW);

  handleVoltageLed(true);

}

void loop() {
  handleVoltageLed(false);

}