// Code for Arduino MEGA (ATmega2560)
// Cycles per instruction


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 NOPTest()
{
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();
  _NOP();

}

void loop() {

  saveTimers();

  runCounter1();

  // Reset counter
  TCNT1 = 0;

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

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


  volatile uint8_t a, b;
  volatile uint16_t r;

  for (a = 0; a <= 255; a++)
  {
    for (b = 0; b <= 255; b++)
    {
      r=a*b;

    }

  }

  /*
    int i = 10;
    while (1)
    {
      _NOP();

      if (!(--i))
        break;
    }
  */

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

  pauseCounter1();


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

  cycles -= 3;
  cycles /= 5;


  restoreTimers();

  speed = (cycles * 625) ;

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

  Serial.println(speed % 10);

  delay(1000);

}