#include <U8glib.h>

const uint8_t PWM_OUTPUT_PIN = 9;
const uint8_t PWM_FEEDBACK_PIN = A0;
const uint8_t CONTROL_PIN_UP = 2;
const uint8_t CONTROL_PIN_DOWN = 3;

const uint16_t CONTROL_UPDATE_INTERVAL_MS = 100;

const uint8_t PWM_FEEDBACK_SAMPLE_COUNT = 5;
const uint8_t PWM_MIN_DUTY_CYCLE = 0;
const uint8_t PWM_MAX_DUTY_CYCLE = 255;
const uint8_t PWM_DUTY_CYCLE_STEP = 8;
const uint16_t ADC_SAMPLE_DELAY_US = 50;

const float EMA_FILTER_ALPHA = 0.15f;

const uint8_t GRAPH_ORIGIN_X = 5;
const uint8_t GRAPH_ORIGIN_Y = 10;
const uint8_t GRAPH_WIDTH = 118;
const uint8_t GRAPH_HEIGHT = 40;
const uint8_t GRAPH_WAVE_PERIOD_PIXELS = 20;
const int8_t TEXT_SET_Y_OFFSET = -2;
const int8_t TEXT_MEASURE_Y_OFFSET = 10;
const uint8_t TEXT_PERCENTAGE_X_OFFSET = 75;
const uint8_t TEXT_BUFFER_SIZE = 30;

U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK | U8G_I2C_OPT_FAST);

uint8_t gTargetPwmValue = 127;
uint8_t gDisplayMeasuredPwmValue = 127;
float gFilteredMeasuredPwmFloat = 127.0f;
uint8_t gLastDisplayedMeasuredValue = 127;

bool isUpdateIntervalPassed(unsigned long lastUpdateTime, uint16_t intervalMs)
{
    return (millis() - lastUpdateTime >= intervalMs);
}

void recordUpdateTime(unsigned long &lastUpdateTime)
{
    lastUpdateTime = millis();
}

uint8_t readRawPwmFeedback()
{
    long adcTotal = 0;
    for (uint8_t i = 0; i < PWM_FEEDBACK_SAMPLE_COUNT; i++)
    {
        int adcReading = analogRead(PWM_FEEDBACK_PIN);
        adcTotal += adcReading;
        delayMicroseconds(ADC_SAMPLE_DELAY_US);
    }

    int averageAdcReading = adcTotal / PWM_FEEDBACK_SAMPLE_COUNT;
    averageAdcReading = constrain(averageAdcReading, 0, 1023);

    return map(averageAdcReading, 0, 1023, PWM_MIN_DUTY_CYCLE, PWM_MAX_DUTY_CYCLE);
}

void applyEmaFilter(uint8_t rawMeasurement)
{
    gFilteredMeasuredPwmFloat = (EMA_FILTER_ALPHA * (float)rawMeasurement) +
                                ((1.0f - EMA_FILTER_ALPHA) * gFilteredMeasuredPwmFloat);

    gDisplayMeasuredPwmValue = (uint8_t)(gFilteredMeasuredPwmFloat + 0.5f);
    gDisplayMeasuredPwmValue = constrain(gDisplayMeasuredPwmValue, PWM_MIN_DUTY_CYCLE, PWM_MAX_DUTY_CYCLE);
}

void setPwmOutput(uint8_t dutyCycle)
{
    analogWrite(PWM_OUTPUT_PIN, dutyCycle);
}

bool updateTargetPwmFromControls()
{
    static unsigned long lastControlCheckTime = 0;
    static uint8_t previousTargetPwmValue = gTargetPwmValue;
    bool valueChanged = false;

    if (isUpdateIntervalPassed(lastControlCheckTime, CONTROL_UPDATE_INTERVAL_MS))
    {
        int pinStateUp = digitalRead(CONTROL_PIN_UP);
        int pinStateDown = digitalRead(CONTROL_PIN_DOWN);

        previousTargetPwmValue = gTargetPwmValue;

        if (pinStateUp == LOW && pinStateDown == HIGH)
        {
            if (gTargetPwmValue < PWM_MAX_DUTY_CYCLE)
            {
                gTargetPwmValue = min((int)PWM_MAX_DUTY_CYCLE, (int)gTargetPwmValue + PWM_DUTY_CYCLE_STEP);
            }
        }
        else if (pinStateDown == LOW && pinStateUp == HIGH)
        {
            if (gTargetPwmValue > PWM_MIN_DUTY_CYCLE)
            {
                gTargetPwmValue = max((int)PWM_MIN_DUTY_CYCLE, (int)gTargetPwmValue - PWM_DUTY_CYCLE_STEP);
            }
        }

        if (gTargetPwmValue != previousTargetPwmValue)
        {
            valueChanged = true;
            setPwmOutput(gTargetPwmValue);
        }

        recordUpdateTime(lastControlCheckTime);
    }

    return valueChanged;
}

void drawGraphAxes()
{
    u8g.drawLine(GRAPH_ORIGIN_X, GRAPH_ORIGIN_Y, GRAPH_ORIGIN_X, GRAPH_ORIGIN_Y + GRAPH_HEIGHT);
    u8g.drawLine(GRAPH_ORIGIN_X, GRAPH_ORIGIN_Y + GRAPH_HEIGHT, GRAPH_ORIGIN_X + GRAPH_WIDTH, GRAPH_ORIGIN_Y + GRAPH_HEIGHT);
}

void drawSquareWaveGraph(uint8_t dutyCycle)
{
    uint8_t highWidthPixels = map(dutyCycle, PWM_MIN_DUTY_CYCLE, PWM_MAX_DUTY_CYCLE, 0, GRAPH_WAVE_PERIOD_PIXELS);
    uint8_t numPeriods = GRAPH_WIDTH / GRAPH_WAVE_PERIOD_PIXELS;

    const uint8_t yHigh = GRAPH_ORIGIN_Y;
    const uint8_t yLow = GRAPH_ORIGIN_Y + GRAPH_HEIGHT;

    for (uint8_t i = 0; i < numPeriods; i++)
    {
        uint8_t startX = GRAPH_ORIGIN_X + i * GRAPH_WAVE_PERIOD_PIXELS;
        uint8_t endHighX = startX + highWidthPixels;
        uint8_t endPeriodX = startX + GRAPH_WAVE_PERIOD_PIXELS;

        if (highWidthPixels > 0)
        {
            u8g.drawLine(startX, yLow, startX, yHigh);
            u8g.drawLine(startX, yHigh, endHighX, yHigh);
            if (highWidthPixels < GRAPH_WAVE_PERIOD_PIXELS)
            {
                u8g.drawLine(endHighX, yHigh, endHighX, yLow);
            }
        }
        u8g.drawLine(endHighX, yLow, endPeriodX, yLow);
    }
}

void drawPwmInfoText()
{
    char textBuffer[TEXT_BUFFER_SIZE];

    uint8_t setPercentage = map(gTargetPwmValue, PWM_MIN_DUTY_CYCLE, PWM_MAX_DUTY_CYCLE, 0, 100);
    uint8_t measuredPercentage = map(gDisplayMeasuredPwmValue, PWM_MIN_DUTY_CYCLE, PWM_MAX_DUTY_CYCLE, 0, 100);

    u8g.setFont(u8g_font_6x10);

    snprintf(textBuffer, TEXT_BUFFER_SIZE, "Set: %d", gTargetPwmValue);
    u8g.drawStr(GRAPH_ORIGIN_X, GRAPH_ORIGIN_Y + TEXT_SET_Y_OFFSET, textBuffer);
    snprintf(textBuffer, TEXT_BUFFER_SIZE, "(%d%%)", setPercentage);
    u8g.drawStr(GRAPH_ORIGIN_X + TEXT_PERCENTAGE_X_OFFSET, GRAPH_ORIGIN_Y + TEXT_SET_Y_OFFSET, textBuffer);

    snprintf(textBuffer, TEXT_BUFFER_SIZE, "Measure: %d", gDisplayMeasuredPwmValue);
    u8g.drawStr(GRAPH_ORIGIN_X, GRAPH_ORIGIN_Y + GRAPH_HEIGHT + TEXT_MEASURE_Y_OFFSET, textBuffer);
    snprintf(textBuffer, TEXT_BUFFER_SIZE, "(%d%%)", measuredPercentage);
    u8g.drawStr(GRAPH_ORIGIN_X + TEXT_PERCENTAGE_X_OFFSET, GRAPH_ORIGIN_Y + GRAPH_HEIGHT + TEXT_MEASURE_Y_OFFSET, textBuffer);
}

void renderOledDisplay()
{
    u8g.firstPage();
    do
    {
        drawGraphAxes();
        drawSquareWaveGraph(gTargetPwmValue);
        drawPwmInfoText();
    } while (u8g.nextPage());
}

void setup()
{
    pinMode(PWM_OUTPUT_PIN, OUTPUT);
    pinMode(CONTROL_PIN_UP, INPUT_PULLUP);
    pinMode(CONTROL_PIN_DOWN, INPUT_PULLUP);
    pinMode(PWM_FEEDBACK_PIN, INPUT);

    analogReference(DEFAULT);

    gFilteredMeasuredPwmFloat = (float)gTargetPwmValue;
    gDisplayMeasuredPwmValue = gTargetPwmValue;
    gLastDisplayedMeasuredValue = gTargetPwmValue;

    setPwmOutput(gTargetPwmValue);
}

void loop()
{
    uint8_t rawMeasurement = readRawPwmFeedback();

    applyEmaFilter(rawMeasurement);

    bool targetPwmChanged = updateTargetPwmFromControls();

    bool displayUpdateNeeded = targetPwmChanged || (gDisplayMeasuredPwmValue != gLastDisplayedMeasuredValue);

    if (displayUpdateNeeded)
    {
        renderOledDisplay();
        gLastDisplayedMeasuredValue = gDisplayMeasuredPwmValue;
    }
}