#include <HardwareTimer.h>
#include <Servo.h>

#define POT_PIN       A0
#define PWM_PIN       D3
#define HEARTBEAT_LED D13
#define BUTTON_PIN    D2
#define ENCODER_CLK   D4
#define ENCODER_DT    D5
#define ENCODER_SW    D7
#define SERVO_PIN     D9

volatile uint16_t adcValue      = 0;
volatile bool     systemRunning = false;
volatile int      encoderPos    = 90;

Servo          myServo;
HardwareTimer *sampleTimer;

void timerISR(void)
{
    adcValue = analogRead(POT_PIN);

    if (systemRunning)
        analogWrite(PWM_PIN, adcValue >> 2);
    else
        analogWrite(PWM_PIN, 0);

    digitalWrite(HEARTBEAT_LED, !digitalRead(HEARTBEAT_LED));
}

void buttonISR(void)
{
    systemRunning = !systemRunning;
}

void encoderISR(void)
{
    int dtState = digitalRead(ENCODER_DT);

    if (dtState == HIGH)
        encoderPos--;
    else
        encoderPos++;

    encoderPos = constrain(encoderPos, 0, 180);
    myServo.write(encoderPos);
}

void encoderSwISR(void)
{
    encoderPos = 90;
    myServo.write(90);
}

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

    pinMode(HEARTBEAT_LED, OUTPUT);
    digitalWrite(HEARTBEAT_LED, LOW);

    pinMode(PWM_PIN, OUTPUT);
    analogWrite(PWM_PIN, 0);

  pinMode(BUTTON_PIN,   INPUT_PULLUP);
    pinMode(ENCODER_CLK,  INPUT_PULLUP);
    pinMode(ENCODER_DT,   INPUT_PULLUP);
    pinMode(ENCODER_SW,   INPUT_PULLUP);

    myServo.attach(SERVO_PIN);
    myServo.write(90);

    attachInterrupt(digitalPinToInterrupt(BUTTON_PIN),
                    buttonISR, FALLING);

    attachInterrupt(digitalPinToInterrupt(ENCODER_CLK),
                    encoderISR, FALLING);

    attachInterrupt(digitalPinToInterrupt(ENCODER_SW),
                    encoderSwISR, FALLING);

    sampleTimer = new HardwareTimer(TIM16);
    sampleTimer->setOverflow(1, HERTZ_FORMAT);
    sampleTimer->attachInterrupt(timerISR);
    sampleTimer->resume();

    Serial.println("NMotion Motor Drive Monitor");
    Serial.println("Encoder: rotate to move servo");
    Serial.println("Button D2: RUN / STOP");
}

void loop()
{
    static uint32_t lastPrint = 0;

    if (millis() - lastPrint >= 500)
{
    lastPrint = millis();
    uint16_t duty = (adcValue * 100U) / 1023U;

    char buf[64];
    snprintf(buf, sizeof(buf),
             "ADC:%4u | Duty:%3u%% | State: %-4s | Servo:%4ddeg",
             (unsigned)adcValue,
             (unsigned)duty,
             systemRunning ? "RUN" : "STOP",
             encoderPos);
    Serial.println(buf);
}
}