#define NOP() __asm__ __volatile__ ("nop\n\t")

#define DEBUG

uint16_t timer1_prescaler = 0;

// Timer 1 duration in milliseconds
uint32_t timer1_duration = 0;


byte interruptPin = 19;

volatile uint32_t duration = 0;  // Total duration
volatile uint32_t timerDuration; // Timer value when toggle

volatile byte pinState = 0;
volatile boolean timerReset = false;
volatile boolean pinToggle = false;

uint32_t t1;

#ifdef DEBUG
uint32_t us;
#endif


int32_t initTimer1(uint32_t duration_us, uint16_t prescaler)
{
  uint32_t OCR_val;

  ///////////////////
  // Timer 1
  ///////////////////

  TCCR1A = 0;          // Reset TCCR1A
  TCCR1B = 0;          // Reset TCCR1B

  // CS02 CS01 CS00 Description
  // 0 0 0 No clock source (Timer/Counter stopped)
  // 0 0 1 clkI/O/(No prescaling)
  // 0 1 0 clkI/O/8 (From prescaler)
  // 0 1 1 clkI/O/64 (From prescaler)
  // 1 0 0 clkI/O/256 (From prescaler)
  // 1 0 1 clkI/O/1024 (From prescaler)
  // 1 1 0 External clock source on T0 pin. Clock on falling edge
  // 1 1 1 External clock source on T0 pin. Clock on rising edge

  // Automatically reser counter when reach OCR1A value
  // if this options is added, OCR1A need to be subtracted by 1
  TCCR1B |= bit(WGM12);


  // To calculate OCR value,16MHz fixed CPU clock
  // 625/10000UL is 1/16MHz in us

  OCR_val = duration_us / (prescaler * 625 / 10000UL);

  if ((OCR_val > 65535UL) || (OCR_val==0))
    // Handle error ?
    ;

  OCR1A = (uint16_t)(OCR_val -1);

  switch (prescaler)
  {
    case 1: // no prescaler
      TCCR1B |= 0b00000001;
      break;
    case 8: // Prescaler 8
      TCCR1B |= 0b00000010;
      break;
    case 64: // Prescaler 64
      TCCR1B |= 0b00000011;
      break;
    case 256: // Prescaler 256
      TCCR1B |= 0b00000100;
      break;
    case 1024: // Prescaler 1024
      TCCR1B |= 0b00000101;
      break;

    default:
      // nothing
    ;
  }

  ///////////////////

  return OCR1A;
}

void setup()
{

  // Reset all
  duration = 0;  // Total duration
  timerDuration = 0; // Timer value when toggle

  pinState = 0;
  timerReset = false;
  pinToggle = false;

  noInterrupts();

  pinMode(interruptPin, INPUT);
  attachInterrupt(digitalPinToInterrupt(interruptPin), on_change_state, CHANGE);

  Serial.print(F("Pin "));
  Serial.print(interruptPin);
  Serial.print(F(" have interrupt "));
  Serial.println(digitalPinToInterrupt(interruptPin));

  pinState = digitalRead(interruptPin);

  if (pinState)
    Serial.print(F("HIGH"));
  else
    Serial.print(F("LOW"));

  Serial.println();

  Serial.begin(2000000);

  timer1_duration = 10000;
  timer1_prescaler = 64;

  int32_t OCR_val = initTimer1(timer1_duration, timer1_prescaler);
  Serial.print("OCR1A: ");
  Serial.println(OCR_val);

  interrupts();

  TCNT1 = 0;
  runTimer1();

  #ifdef DEBUG
  us = micros();
  #endif

}

void printVal(uint32_t d1)

{
  if (!pinState)
    Serial.write('H');
  else
    Serial.write('L');

  Serial.print(d1);

  #ifdef DEBUG
  Serial.write(' ');
  Serial.print(t1);
  #endif

  Serial.write(13);
  Serial.write(10);


}

void loop()
{


  if (timerReset)
  {

    timerReset = false;

    //Add timer duration 
    duration += timer1_duration;

    if (pinToggle)
    {
      uint32_t d1 = duration;
      duration = 0;

      pinToggle = false;

      pinState = bitRead(PIND, PD2);


      #ifdef DEBUG
      uint32_t t2 = micros();
      t1 = t2 - us;
      us = t2;
      #endif

      printVal(d1);
    }
  }

  if (pinToggle)
  {

    pinToggle = false;

    // For Arduino Mega 2560 and pin 19, related chip pin is PD2.
    // pinState = PIND & (1 << PD2); produce the same code as follows
    pinState = bitRead(PIND, PD2);

    //pinState = !pinState;
    //pinState = digitalRead(interruptPin);
    //pinState = digitalReadDirect(pinInput);

#ifdef DEBUG
    uint32_t t2 = micros();
    t1 = t2 - us;
    us = t2;
#endif

    boolean reset = timerReset;
    timerReset = false;

    uint32_t d1 = duration;
    duration = 0;

    //Avoid conflict if both events happens at the same time
    // Real ATmega have list of interrupt prioritied added belows

    if (!reset)
    {

      switch (timer1_prescaler)
      {
        case 1: // no prescaler
          TCCR1B |= 0b00000001;
          break;
        case 8: // Prescaler 8
          d1 += (timerDuration)  >> 1; // prescaler 8
          break;
        case 64: // Prescaler 64
          d1 += (timerDuration) << 2; // prescaler 64
          break;
        case 256: // Prescaler 256
          d1 += (timerDuration)  << 4; // prescaler 256
          break;
        case 1024: // Prescaler 1024
          d1 += (timerDuration)  << 6; // prescaler 1024
          break;

        default:
          // disable it
          TCCR1B |= 0b00000000;
      }

      // Serial.println(timerDuration);
    }

    printVal(d1);


  }

}

inline void runTimer1()
{
  TIMSK1 |= (1 << OCIE1A); // run timer1
};

inline void pauseTimer1()
{
  TIMSK1 &= ~(1 << OCIE1A); // stop timer1
};

void on_change_state()
{

  // Secure reading value correctly

  cli();
  NOP();
  timerDuration = TCNT1;
  sei();

  TCNT1 = 0;
  pinToggle = true;
}

ISR(TIMER1_COMPA_vect)
{
  //  TCNT1  = 0; // No need when WS12 is active
  timerReset = true;
}