#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)
{
    digitalWrite(PWM_PIN, HIGH);
}

void buttonISR(void)
{
    Serial.println("D2 INTERRUPT FIRED");
    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;

        Serial.print("ADC:");
        Serial.print(adcValue);
        Serial.print(" | Duty:");
        Serial.print(duty);
        Serial.print("% | State:");
        Serial.print(systemRunning ? "RUN " : "STOP");
        Serial.print(" | Servo:");
        Serial.print(encoderPos);
        Serial.println("deg");
    }
}