#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;

// FIX FOR BUG 6: Flag to signal the main loop to update the servo safely outside of ISR context
volatile bool     servoNeedsUpdate = true; 

Servo myServo;
HardwareTimer *sampleTimer;

void timerISR(void)
{
  // BUG 1 FIXED: Changed from analogRead(A1) to POT_PIN (A0). Original read from A1 resulted in floating noise.
  adcValue = analogRead(POT_PIN); 
  
  if (systemRunning)
  {
    // BUG 2 FIXED: Scaled 10-bit ADC (0-1023) down to 8-bit PWM (0-255) using bit-shift (>> 2). 
    // Original code caused integer wrapping/overflow, making the LED cycle full-to-off 4 times.
    analogWrite(PWM_PIN, adcValue >> 2); 
  }
  else
  {
    analogWrite(PWM_PIN, 0);
  }
  
  // BUG 5 PART B: This toggle relies on the timer overflow configuration.
  digitalWrite(HEARTBEAT_LED, !digitalRead(HEARTBEAT_LED));
}

void buttonISR(void)
{
  systemRunning = !systemRunning;
  
  // BUG 7 FIXED: Immediately turn off the LED when entering STOP state.
  // Original code left a race condition/lag before the next timer ISR cleared the pin.
  if (!systemRunning) {
    analogWrite(PWM_PIN, 0);
  }
}

void encoderISR(void)
{
  int dtState = digitalRead(ENCODER_DT);
  
  // BUG 3 FIXED: Inverted the tracking logic check. Changed from 'if (dtState == HIGH)' to LOW.
  // Original logic caused the encoder to track backward or drop steps relative to physical rotation.
  if (dtState == LOW)
  {
    encoderPos++;
  }
  else
  {
    encoderPos--;
  }
  
  encoderPos = constrain(encoderPos, 0, 180);
  
  // BUG 6 PART A REMOVED: 'myServo.write(encoderPos);' has been removed from here.
  // Running complex library code or timed pulse I/O inside an ISR causes severe jitter and stalls the MCU.
  servoNeedsUpdate = true; 
}

void encoderSwISR(void)
{
  encoderPos = 90;
  // BUG 6 PART B REMOVED: 'myServo.write(90);' has been removed from here to prevent ISR context stalling.
  servoNeedsUpdate = true;
}

void setup()
{
  Serial.begin(115200);
  
  pinMode(HEARTBEAT_LED, OUTPUT);
  digitalWrite(HEARTBEAT_LED, LOW);
  
  pinMode(PWM_PIN, OUTPUT);
  analogWrite(PWM_PIN, 0);
  
  // BUG 4 FIXED: Changed interrupt trigger from RISING to FALLING.
  // Because the pin uses INPUT_PULLUP, pressing the button drops it to GND (FALLING edge).
  // Original code triggered the toggle only when the user *released* the button.
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(BUTTON_PIN), buttonISR, FALLING);
  
  pinMode(ENCODER_CLK, INPUT_PULLUP);
  pinMode(ENCODER_DT, INPUT_PULLUP);
  pinMode(ENCODER_SW, INPUT_PULLUP);
  
  myServo.attach(SERVO_PIN);
  myServo.write(90); // Initialize at center
  
  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), encoderISR, FALLING);
  attachInterrupt(digitalPinToInterrupt(ENCODER_SW), encoderSwISR, FALLING);
  
  sampleTimer = new HardwareTimer(TIM16);
  // BUG 5 PART A FIXED: Changed overflow from 1Hz to 2Hz.
  // Because the ISR toggles the pin state, a 2Hz interrupt frequency creates a perfect 1Hz physical blink cycle.
  sampleTimer->setOverflow(2, 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;
  
  // BUG 6 DEPLOYMENT: Safely update the physical servo positioning inside the non-blocking main loop.
  if (servoNeedsUpdate) {
    myServo.write(encoderPos);
    servoNeedsUpdate = false;
  }
  
  if (millis() - lastPrint >= 500)
  {
    lastPrint = millis();
    
    // Duty calculation matching standard 10-bit input mapping layout requirements
    uint16_t duty = ((uint32_t)adcValue * 100) / 1023;
    
    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");
  }
}