// Code for Arduino UNO (ATmega328p)
// Cycles per instruction only
// Confirmed, it works the same in real device and Wokwi

uint16_t cycles;
uint16_t speed;

byte TCCR1A_data;
byte TCCR1B_data;

byte TCCR0A_data;
byte TCCR0B_data;

void saveTimers()
{
  TCCR0A_data  = TCCR0A;
  TCCR0B_data  = TCCR0B;

  TCCR1A_data  = TCCR1A;
  TCCR1B_data  = TCCR1B;

  // Reset all and
  TCCR0A = 0;
  TCCR0B = 0;

  TCCR1A = 0;
  TCCR1B = 0;
}

void restoreTimers()
{
  TCCR1A = TCCR1A_data;
  TCCR1B = TCCR1B_data;

  TCCR0A = TCCR0A_data;
  TCCR0B = TCCR0B_data;
}

inline void runCounter1()
{
  //TCCR1B  |= (1 << CS10); // run counter1
  TCCR1B = 1 << CS10;
};

inline void pauseCounter1()
{
  //TCCR1B &= ~(1 << CS10); // pause counter1
  TCCR1B = 0;
};

void setup()
{
  Serial.begin(115200);
  pinMode(LED_BUILTIN, OUTPUT);

  Serial.println(F("Test speed for 10x NOP with timer1 counter"));
  Serial.println();
  Serial.println(F("Cycles, ns"));
}

volatile void callTest()
{
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();

}

void loop() {

  saveTimers();

  runCounter1();

  // Test further dummy instructions affects on TCNT

  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();

  // Reset counter
  TCNT1 = 0;

  // Testing code execution
  //--------------------

  //digitalWrite(LED_BUILTIN, state);
  //bitToggle(PORTB,5);
  //bitSet(PORTB,5);

  //10x NOP, should give 625ns on device with 16MHz crystal

/*
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  */

  callTest();

  //--------------------

  pauseCounter1();

  // Test further dummy instructions affects on TCNT

  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();

  // That should be 2 cycles to subtract and show 10 cycles and 625ns
  cycles = TCNT1 - 2 ;

  restoreTimers();

  speed = (cycles * 625) ;

  Serial.print(cycles);
  Serial.print(' ');
  Serial.print(speed / 10);
  Serial.print('.');

  Serial.println(speed % 10);

  delay(1000);

}