#include <U8glib.h>

U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK | U8G_I2C_OPT_FAST);

const uint8_t PWM_PIN = 9;
const uint8_t PWM_FEEDBACK_PIN = A0;
const uint8_t JOYSTICK_X_PIN = A1;

const uint16_t CONTROL_UPDATE_INTERVAL_MS = 100;
const uint16_t DISPLAY_UPDATE_INTERVAL_MS = 200;
const uint8_t PWM_SAMPLE_COUNT = 5;

uint8_t pwmControlValue = 127;
uint8_t prevPwmControlValue = 127;
uint8_t measuredPwmValue = 0;

const int JOYSTICK_CENTER = 512;
const int JOYSTICK_DEADZONE = 100;
const uint8_t MAX_PWM_CHANGE_RATE = 5;

const uint8_t GRAPH_X = 5;
const uint8_t GRAPH_Y = 10;
const uint8_t GRAPH_WIDTH = 118;
const uint8_t GRAPH_HEIGHT = 40;
const uint8_t WAVE_PERIOD = 20;

bool isUpdateDue(unsigned long lastUpdate, unsigned long interval)
{
    return (millis() - lastUpdate >= interval);
}

void updateLastTime(unsigned long &lastUpdate)
{
    lastUpdate = millis();
}

int8_t getJoystickPWMChange()
{
    int joystickValue = analogRead(JOYSTICK_X_PIN);
    int offset = joystickValue - JOYSTICK_CENTER;

    if (abs(offset) < JOYSTICK_DEADZONE)
    {
        return 0;
    }

    int8_t changeAmount = map(abs(offset),
                              JOYSTICK_DEADZONE,
                              512 - JOYSTICK_DEADZONE,
                              1,
                              MAX_PWM_CHANGE_RATE);

    return (offset > 0) ? changeAmount : -changeAmount;
}

uint8_t readPWMFeedback()
{
    long total = 0;
    for (uint8_t i = 0; i < PWM_SAMPLE_COUNT; i++)
    {
        int reading = analogRead(PWM_FEEDBACK_PIN);
        total += reading;
        delayMicroseconds(50);
    }

    int averageReading = total / PWM_SAMPLE_COUNT;
    averageReading = constrain(averageReading, 0, 1023);

    return map(averageReading, 0, 1023, 0, 255);
}

void outputPWM(uint8_t pin, uint8_t value)
{
    analogWrite(pin, value);
}

bool updatePwmSignal()
{
    static unsigned long lastControlUpdate = 0;
    bool valueChanged = false;

    if (isUpdateDue(lastControlUpdate, CONTROL_UPDATE_INTERVAL_MS))
    {
        prevPwmControlValue = pwmControlValue;

        int joystickValue = analogRead(JOYSTICK_X_PIN);

        int16_t newValue = map(joystickValue, 0, 1023, 0, 255);

        newValue = constrain(newValue, 0, 255);
        pwmControlValue = (uint8_t)newValue;

        outputPWM(PWM_PIN, pwmControlValue);
        valueChanged = (pwmControlValue != prevPwmControlValue);

        updateLastTime(lastControlUpdate);
    }

    return valueChanged;
}

void drawGraphAxes()
{
    u8g.drawLine(GRAPH_X, GRAPH_Y, GRAPH_X, GRAPH_Y + GRAPH_HEIGHT);
    u8g.drawLine(GRAPH_X, GRAPH_Y + GRAPH_HEIGHT, GRAPH_X + GRAPH_WIDTH, GRAPH_Y + GRAPH_HEIGHT);
}

void drawSquareWave(uint8_t dutyCycle)
{
    uint8_t highWidth = map(dutyCycle, 0, 255, 0, WAVE_PERIOD);
    uint8_t numPeriods = GRAPH_WIDTH / WAVE_PERIOD;

    for (uint8_t i = 0; i < numPeriods; i++)
    {
        uint8_t startX = GRAPH_X + i * WAVE_PERIOD;

        if (highWidth > 0)
        {
            u8g.drawLine(startX, GRAPH_Y + GRAPH_HEIGHT, startX, GRAPH_Y);
            u8g.drawLine(startX, GRAPH_Y, startX + highWidth, GRAPH_Y);
            u8g.drawLine(startX + highWidth, GRAPH_Y, startX + highWidth, GRAPH_Y + GRAPH_HEIGHT);
        }

        u8g.drawLine(startX + highWidth, GRAPH_Y + GRAPH_HEIGHT, startX + WAVE_PERIOD, GRAPH_Y + GRAPH_HEIGHT);
    }
}

void drawPwmText()
{
    char buf[30];
    uint8_t setPercentage = map(pwmControlValue, 0, 255, 0, 100);
    uint8_t measuredPercentage = map(measuredPwmValue, 0, 255, 0, 100);

    u8g.setFont(u8g_font_6x10);

    snprintf(buf, sizeof(buf), "Set: %d", pwmControlValue);
    u8g.drawStr(GRAPH_X, GRAPH_Y - 2, buf);

    snprintf(buf, sizeof(buf), "(%d%%)", setPercentage);
    u8g.drawStr(GRAPH_X + 75, GRAPH_Y - 2, buf);

    snprintf(buf, sizeof(buf), "Measure: %d", measuredPwmValue);
    u8g.drawStr(GRAPH_X, GRAPH_Y + GRAPH_HEIGHT + 10, buf);

    snprintf(buf, sizeof(buf), "(%d%%)", measuredPercentage);
    u8g.drawStr(GRAPH_X + 75, GRAPH_Y + GRAPH_HEIGHT + 10, buf);
}

void updateDisplay()
{
    u8g.firstPage();
    do
    {
        drawGraphAxes();
        drawSquareWave(measuredPwmValue);
        drawPwmText();
    } while (u8g.nextPage());
}

void setup()
{
    pinMode(PWM_PIN, OUTPUT);
    pinMode(JOYSTICK_X_PIN, INPUT);
    pinMode(PWM_FEEDBACK_PIN, INPUT);

    analogReference(DEFAULT);
}

void loop()
{
    measuredPwmValue = readPWMFeedback();

    bool pwmChanged = updatePwmSignal();

    static unsigned long lastDisplayUpdate = 0;
    if (pwmChanged || isUpdateDue(lastDisplayUpdate, DISPLAY_UPDATE_INTERVAL_MS))
    {
        updateDisplay();
        updateLastTime(lastDisplayUpdate);
    }
}