// High-frequency PWM using Timer2 on pins 3 & 11
const int pwmPin1 = 11;  // OC2B
const int pwmPin2 = 3;   // OC2A
bool readADC2 = true;

void setup() {
  pinMode(pwmPin1, OUTPUT);
  pinMode(pwmPin2, OUTPUT);

  // ----- Timer2 Fast PWM (8-bit), no prescaler -----
  TCCR2A = _BV(WGM20) | _BV(WGM21) | _BV(COM2A1) | _BV(COM2B1);  // Fast PWM mode, enable PWM on Pin 11, enable PWM on Pin 3
  //TCCR2B = _BV(CS20);  // No prescaler → 16MHz / 256 = 62.5 kHz
  TCCR2B = _BV(CS21);  // Prescaler 8 → (16MHz / 8) / 256 = 7.812 kHz

  ADCSRA = (1 << ADEN) | (1 << ADPS2);  // Set ADC prescaler to 16 (1MHz ADC clock), 13 cycles to read a value so a readout takes 13 uS
  // Atmel/Microchip recommends:
  // 50–200 kHz for full 10-bit accuracy
  // Up to 1 MHz for reduced accuracy (≈ 8–9 bits)
  Serial.begin(9600);
}

int currentSense2 = 0;
int currentSense3 = 0;

int countdownOffPWM1 = 1024;
int countdownOffPWM2 = 1024;

void loop() {
  //int adc0 = analogRead(A0);
  ADMUX = (1 << REFS0) | (A0 & 0x0F);  // Set ADC reference to AVcc and channel to ADC channel 0, example REFS0 = 1  → 0100 0000 | Channel 3 → 0000 0011 | ADMUX     → 0100 0011
  ADCSRA |= (1 << ADSC);               // Start ADC conversion
  while (ADCSRA & (1 << ADSC))
    ;  //wait or the ADC conversion to finish
  int adc0 = ADC;

  //int adc1 = analogRead(A1);
  ADMUX = (1 << REFS0) | (A1 & 0x0F);  // Set ADC reference to AVcc and channel to ADC channel 0, example REFS0 = 1  → 0100 0000 | Channel 3 → 0000 0011 | ADMUX     → 0100 0011
  ADCSRA |= (1 << ADSC);               // Start ADC conversion
  while (ADCSRA & (1 << ADSC))
    ;  //wait or the ADC conversion to finish
  int adc1 = ADC;

  if (adc0 <= 100) {  //Because the controller has noise on the ADC reading below this value and we want the cars to be still in position.
    adc0 = 0;
  }
  if (adc1 <= 100) {  //Because the controller has noise on the ADC reading below this value and we want the cars to be still in position.
    adc1 = 0;
  }

  uint8_t pwm1 = map(adc0, 0, 1023, 0, 255);
  uint8_t pwm2 = map(adc1, 0, 1023, 0, 255);

  if (countdownOffPWM1 == 1024) {
    OCR2B = pwm1;  // Output is LOW from 0 to pwm1 value (between 0 and 256) and then goes HIGH from pwm1 value to 255 on Pin 3
  } else {
    countdownOffPWM1 = countdownOffPWM1 - 1;
    if(countdownOffPWM1 == 0){
      countdownOffPWM1 = 1024;
      TCCR2A |= _BV(COM2A1);   // Reconnect PWM to pin 11
    }
  }

  if (countdownOffPWM1 == 1024) {
    OCR2A = pwm2;  // Output on pin 11 TCCR2A |= _BV(COM2B1);
  }
  else {
    countdownOffPWM2 = countdownOffPWM2 - 1;
    if(countdownOffPWM2 == 0){
      countdownOffPWM2 = 1024;
      TCCR2A |= _BV(COM2B1);   // Reconnect PWM to pin 3
    }
  }

  if (readADC2) {  //Every two cycle we read ADC2 and ADC3
    //int adc2 = analogRead(A2);
    while (TCNT2 < 217); // wait until timer 2 is 20uS from resetting to perform the ADC conversion in 13 cycles thus in 13uS
    ADMUX = (1 << REFS0) | (A2 & 0x0F);  // Set ADC reference to AVcc and channel to ADC channel 0, example REFS0 = 1  → 0100 0000 | Channel 3 → 0000 0011 | ADMUX     → 0100 0011
    ADCSRA |= (1 << ADSC);               // Start ADC conversion
    while (ADCSRA & (1 << ADSC));  //wait or the ADC conversion to finish
    currentSense2 = ADC;

    if(currentSense2 > 128){
      countdownOffPWM1 = 1023;
      TCCR2A &= ~_BV(COM2A1); //Disable PWM from pin 11
      PORTB &= ~_BV(PB3);   // Pin 11 = PB3, force pin 11 LOW
    }

    readADC2 = false;
  } else {
    //int adc2 = analogRead(A3);
    while (TCNT2 < 217); // wait until timer 2 is 20uS from resetting to perform the ADC conversion in 13 cycles thus in 13uS
    ADMUX = (1 << REFS0) | (A3 & 0x0F);  // Set ADC reference to AVcc and channel to ADC channel 0, example REFS0 = 1  → 0100 0000 | Channel 3 → 0000 0011 | ADMUX     → 0100 0011
    ADCSRA |= (1 << ADSC);               // Start ADC conversion
    while (ADCSRA & (1 << ADSC));  //wait or the ADC conversion to finish
    currentSense3 = ADC;

    if(currentSense3 > 128){
      countdownOffPWM2 = 1023;
      TCCR2A &= ~_BV(COM2B1);; //Disable PWM from pin 3
      PORTD &= ~_BV(PD3);   // Pin 3 = PD3, force pin 3 LOW
    }

    readADC2 = true;
  }

}  //end main loop