#define SET0(REG,BIT) REG &= ~(1<<BIT)
#define SET1(REG,BIT) REG |= (1<<BIT)
#define TEST(REG,BIT) (REG&(1<<BIT))

bool test = true;

void setup() 
{
  // Set PD4 and PD5 as outputs
  SET1(DDRD, 4);
  SET1(DDRD, 5);

  // Set PD7 as input with pull-up resistor enabled
  SET0(DDRD, 7);
  SET1(PORTD, 7);

  // ADC setup for input A0
  SET0(ADMUX, REFS1);
  SET0(ADMUX, REFS0);
  SET0(ADMUX, ADLAR);
  SET0(ADMUX, MUX3);
  SET0(ADMUX, MUX2);
  SET0(ADMUX, MUX1);
  SET0(ADMUX, MUX0);

  // ADC setup for input A1
  SET0(ADMUX, REFS1);
  SET0(ADMUX, REFS0);
  SET0(ADMUX, ADLAR);
  SET0(ADMUX, MUX3);
  SET0(ADMUX, MUX2);
  SET0(ADMUX, MUX1);
  SET1(ADMUX, MUX0); // A1 is selected (MUX0 is set)

  // Enable ADC and set prescaler to 128
  SET1(ADCSRA, ADEN);
  SET0(ADCSRA, ADATE);
  SET0(ADCSRA, ADIE);
  SET1(ADCSRA, ADPS2);
  SET1(ADCSRA, ADPS1);
  SET1(ADCSRA, ADPS0); 
}

void loop() 
{
  // Start ADC conversion for A0
  SET1(ADCSRA, ADSC);
  while(TEST(ADCSRA, ADIF) == 0) {}
  int vysledok = ADCL;
  vysledok = ADCL + (ADCH << 8);

  // Select A1 for the next conversion
  SET1(ADMUX, MUX0);
  SET1(ADCSRA, ADSC);
  while(TEST(ADCSRA, ADIF) == 0) {}
  int speedValue = ADCL;
  speedValue = ADCL + (ADCH << 8);

  // Calculate delay based on speedValue
  int delayTime = map(speedValue, 0, 1023, 10, 1000); // Map the potentiometer value to a delay range

  if(vysledok > 512)
  {
    SET1(PORTD, 4);
    SET1(PORTD, 5);
    delay(50);
    SET0(PORTD, 5);
  }
  else if(vysledok < 512)
  {
    SET0(PORTD, 4);
    SET0(PORTD, 5);
    delay(50);
    SET1(PORTD, 5);
  }

  // Adjust the delay for the stepper motor speed
  delay(delayTime);

  // Deselect A1 and select A0 for the next conversion
  SET0(ADMUX, MUX0);
}