#include <Arduino.h>

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++

#define BUTTON_PIN 2
#define HEATER_PWM_PIN 3
#define GEAR_1_LED 4
#define GEAR_2_LED 5
#define GEAR_3_LED 6
#define GEAR_0_SWITCH 7
#define GEAR_1_SWITCH 8
#define GEAR_2_SWITCH 9
#define GEAR_3_SWITCH 10
#define VOLTAGE_MEAS_PIN A1
#define CONTROL_MEAS_PIN A2

#define DEBUG_MODE
#define CONTROL_MODE 1 // [1] przycisk | [2] przełącznik czteropozycyjny
#define DEBOUNCE_DELAY 50
#define LONG_PRESS_TIME 1000
#define MAX_GEAR 3
#define DEST_VOLTAGE_1 8.0
#define DEST_VOLTAGE_2 10.0
#define DEST_VOLTAGE_3 12.0
#define ADJ_5V 5.00
#define VOLTAGE_MEAS_R1 100000.0
#define VOLTAGE_MEAS_R2 10000.0

#if CONTROL_MODE == 1
    typedef enum
    {
        NOT_PRESSED,
        SHORT_PRESS,
        LONG_PRESS
    } ButtonState;
#endif

uint8_t currentGear = 0; // 0 = STOP
double inputVoltage = 0.0, controlVoltage = 0.0;
double pwmPercent = 0.0;

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++

#if CONTROL_MODE == 1
    ButtonState getButtonState()
    {
        static unsigned long buttonPressStartTime = 0;
        static bool buttonPressed = false;
        static bool longPressReturned = false;

        bool buttonState = !digitalRead(BUTTON_PIN);
        
        if (buttonState && !buttonPressed)
        {
            buttonPressStartTime = millis();
            buttonPressed = true;
        }

        if (!longPressReturned && buttonState && (millis() - buttonPressStartTime > LONG_PRESS_TIME))
        {
            longPressReturned = true;
            return LONG_PRESS;
        }
        
        if (buttonPressed && !buttonState)
        {
            unsigned long buttonReleaseTime = millis();
            buttonPressed = false;
            longPressReturned = false;

            if (buttonReleaseTime - buttonPressStartTime < DEBOUNCE_DELAY)
            {
                return NOT_PRESSED;
            }
            else if (buttonReleaseTime - buttonPressStartTime < LONG_PRESS_TIME)
            {
                return SHORT_PRESS;
            }
        }
        
        return NOT_PRESSED;
    }

    void handleButton()
    {
        ButtonState buttonState = getButtonState();

        if (buttonState == SHORT_PRESS)
        {
            currentGear = (currentGear == MAX_GEAR) ? (1) : (currentGear + 1);
        }
        else if (buttonState == LONG_PRESS)
        {
            currentGear = 0;
        }

        #ifdef DEBUG_MODE
            if (buttonState == SHORT_PRESS)
            {
                Serial.println("SHORT_PRESS");
            }
            else if (buttonState == LONG_PRESS)
            {
                Serial.println("LONG_PRESS");
            }
        #endif
    }
#elif CONTROL_MODE == 2
    void handleGearSwitch()
    {
        if      (!digitalRead(GEAR_0_SWITCH)) currentGear = 0;
        else if (!digitalRead(GEAR_1_SWITCH)) currentGear = 1;
        else if (!digitalRead(GEAR_2_SWITCH)) currentGear = 2;
        else if (!digitalRead(GEAR_3_SWITCH)) currentGear = 3;
    }
#endif

double getInputVoltage()
{
    double voltage = 0.0;

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

    voltage /= 20.0;
    voltage = (voltage * (ADJ_5V / 1023.0)) / (VOLTAGE_MEAS_R2 / (VOLTAGE_MEAS_R1 + VOLTAGE_MEAS_R2));

    return voltage;
}

double getControlVoltage()
{
    double voltage = 0.0;

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

    voltage /= 20.0;
    voltage = voltage * (ADJ_5V / 1023.0);

    return voltage;
}

void updateLed()
{
    switch (currentGear)
    {
    case 0:
        digitalWrite(GEAR_1_LED, LOW); digitalWrite(GEAR_2_LED, LOW); digitalWrite(GEAR_3_LED, LOW);
        break;
    
    case 1:
        digitalWrite(GEAR_1_LED, HIGH); digitalWrite(GEAR_2_LED, LOW); digitalWrite(GEAR_3_LED, LOW);
        break;

    case 2:
        digitalWrite(GEAR_1_LED, LOW); digitalWrite(GEAR_2_LED, HIGH); digitalWrite(GEAR_3_LED, LOW);
        break;

    case 3:
        digitalWrite(GEAR_1_LED, LOW); digitalWrite(GEAR_2_LED, LOW); digitalWrite(GEAR_3_LED, HIGH);
        break;
    }
}

void setup()
{
    #ifdef DEBUG_MODE
        Serial.begin(9600);
    #endif

    #if CONTROL_MODE == 1
        pinMode(BUTTON_PIN, INPUT_PULLUP);
    #elif CONTROL_MODE == 2
        pinMode(GEAR_0_SWITCH, INPUT_PULLUP);
        pinMode(GEAR_1_SWITCH, INPUT_PULLUP);
        pinMode(GEAR_2_SWITCH, INPUT_PULLUP);
        pinMode(GEAR_3_SWITCH, INPUT_PULLUP);
    #endif

    pinMode(HEATER_PWM_PIN, OUTPUT);
    pinMode(GEAR_1_LED, OUTPUT);
    pinMode(GEAR_2_LED, OUTPUT);
    pinMode(GEAR_3_LED, OUTPUT);

    analogWrite(HEATER_PWM_PIN, 0);
}

void loop()
{
    #if CONTROL_MODE == 1
        handleButton();
    #elif CONTROL_MODE == 2
        handleGearSwitch();
    #endif

    inputVoltage = getInputVoltage();
    controlVoltage = getControlVoltage();

    if (currentGear == 3 && inputVoltage < 12.0)
    {
        #ifdef DEBUG_MODE
            Serial.println("currentGear = 2 : inputVoltage < 12");
        #endif

        currentGear = 2;
    }

    if (controlVoltage < 1.0)
    {
        #ifdef DEBUG_MODE
            Serial.println("currentGear = 0 : controlVoltage < 1");
        #endif

        currentGear = 0;
    }

    switch (currentGear)
    {
    case 0:
        pwmPercent = 0.0;
        break;
    
    case 1:
        pwmPercent = DEST_VOLTAGE_1 / inputVoltage;
        break;

    case 2:
        pwmPercent = DEST_VOLTAGE_2 / inputVoltage;
        break;

    case 3:
        pwmPercent = DEST_VOLTAGE_3 / inputVoltage;
        break;
    }

    #ifdef DEBUG_MODE
        Serial.print("inputVoltage   = "); Serial.println(inputVoltage);
        Serial.print("controlVoltage = "); Serial.println(controlVoltage);
        Serial.print("pwmPercent     = "); Serial.println(pwmPercent);
        Serial.print("currentGear    = "); Serial.println(currentGear);
        Serial.print("PWM value      = "); Serial.println(constrain(255.0 * pwmPercent, 0.0, 255.0));
    #endif

    analogWrite(HEATER_PWM_PIN, constrain(255.0 * pwmPercent, 0.0, 255.0));
    updateLed();
}