volatile bool printB = false;
int adcvalue = 0;
float reference = 0.05;
float x1 = 0, x2 = 0, y0 = 0, y1 = 0, y2 = 0, error = 0;
float b0 = 30;
float b1 = -59.98;
float b2 = 29.98;
float a1 = 1.999;
float a2 = -0.999;

void setup() {
  // Initialize Serial communication
  // Serial.begin(115200);

  // Disable interrupts
  noInterrupts();

  // Clear Timer0 control registers
  TCCR0A = 0;
  TCCR0B = 0;
  TCNT0 = 0;

  // Set CTC mode
  TCCR0A |= (1 << WGM01);

  // Set compare match register for the desired sample rate (e.g., 1 kHz)
  OCR0A = 1599; // For 10 kHz: 16 MHz / (64 * (249 + 1)) = 1 kHz

  // Set prescaler to 1
  TCCR0B |= (1 << CS00);

  // Enable timer compare interrupt
  TIMSK0 |= (1 << OCIE0A);

  // Set Fast PWM mode with 10-bit resolution
  TCCR1A = (1 << WGM10) | (1 << WGM11);  // Fast PWM, 10-bit
  TCCR1B = (1 << WGM12) | (1 << CS10);   // No prescaling

  // Set non-inverting mode for PWM on pin 9 (OC1A)
  TCCR1A |= (1 << COM1A1);

  // Set the PWM duty cycle to 0%
  OCR1A = 725; // 10-bit resolution: 0-1023

  // Set pin 9 as output
  pinMode(9, OUTPUT);

  // Configure ADC
  ADMUX = (1 << REFS0); // Reference voltage set to AVcc
  ADCSRA = (1 << ADEN)  | // Enable ADC
          //  (1 << ADIE)  | // Enable ADC interrupt
          //  (1 << ADATE) | // Enable auto trigger
              (1 << ADPS0); // Prescaler of 2
  // ADCSRB |= (1 << ADTS0)
  //        | (1 << ADTS1); // Free running mode
  // ADCSRB = 0;
  DIDR0 = 0; // Disable digital input buffers on ADC0

  // Start ADC conversion
  // ADCSRA |= (1 << ADSC);

  // Enable interrupts
  interrupts();
}

void loop() {
  if (printB) {
    ADCSRA |= (1 << ADSC);
    while(ADCSRA & (1 << ADSC));
    // Read the ADC value
  float adcValue = (ADC * 5.0) / 1023.0;

  // Error calculation
  float error = reference - adcValue;

  float y0 = b0 * error + b1 * x1 + b2 * x2 + a1 * y1 + a2 * y2;
    //Serial.print(",");
    //Serial.println(y0);
    // Update states
    x2 = x1;
    x1 = error;
    y2 = y1;
    y1 = y0;

  // Scale and constrain the output to PWM range (0-255)
  // int pwmOutput = constrain(y0 * 255.0, 0, 255);
  int pwmOutput = (y0 / 10.0) * 1023.0;

  // Output the PWM signal
  // OCR1A = pwmOutput;
    // Serial.println(adcValue);
    printB = false;
  }
}

// Timer0 compare match interrupt service routine
ISR(TIMER0_COMPA_vect) {
  printB = true;
}

// ADC conversion complete interrupt service routine
// ISR(ADC_vect) {
//   adcvalue = ADC;
//   printB = true;
// }