// Demo code for
// https://arduino.stackexchange.com/questions/21026/timer-ctc-set-and-clear-modes-how-to-un-clear-and-un-set?
// and https://forum.arduino.cc/t/delay-in-timer-interrupt/1273170/88?u=davex
//
// Note that if you want to do someting PWM-like and with CTC mode and
// clear or set the pin at overflow notmal mode might be easier to manage, since the toffling off
//

byte ovfflag = false;

void setup() {
  // put your setup code here, to run once:

  // Timer 1 CTC mode 12 Clearing OC1B at 0.25Hz using OCR1A to set frequency
  // Output on OC1A, OC1B   Uno 9,10
  // Clear at OCR1B
  // WGM =15 0b0100

  pinMode(2, INPUT_PULLUP);
  pinMode(3, INPUT_PULLUP);
  //DDRB |= bit(DDB5)| bit(DDB6);  // Teensy2.0 atmega32u OC1A and OC1B outputs
  DDRB |= bit(DDB1) | bit(DDB2); // atmega168/UNO OC1A and OC1B 9,10 (Tested by OP/Luis)
  // TCCR1A =  bit(COM1A0) | bit(COM11)  ; // Toggle OC1A, Clear on OC1B
  // TCCR1B =  bit(WGM12)  | bit(CS12);  // Set /1 prescaler
  TCCR1C = 0;  // Note that it is good to initialize the timer
  // into a known mode before trying to set interesting TOPs
  //OCR1A  = 62500U ;    // Set TOP count to 16000000/(PreScaler *Ftimer)
  TCCR1A =  bit(COM1A0) | bit(COM1B1) | bit(COM1B0)  ; // Toggle OC1A, Set on OC1B

  TCCR1A =  bit(COM1A0) | bit(COM1B0)  ; // Toggle OC1A, Toggle on OC1B
  TCCR1C = bit(FOC1B); // should toggle OC1B
  TCCR1A =  bit(COM1A0) | bit(COM1B0)  ; // Toggle OC1A, Toggle on OC1B

  ICR1  = 62500U ;    // Set TOP count to 16000000/(PreScaler *Ftimer)
  OCR1B  = ICR1 / 2; //  ~50% duty cycle
  OCR1A  = ICR1 / 16; //

  //TCCR1A |= bit(COM1B0)  ; // update to Set on OC1B
  //TCCR1A |= bit(COM1B1) ; TCCR1A &= ~bit(COM1B1) ;   ; // update to Clear on OC1B
  TCCR1B =  bit(WGM13) | bit(WGM12)   | bit(CS12) | bit(CS10); // Set /1024 prescaler
  TCNT1 = 0 ;
  Serial.begin(115200);
  Serial.println("Testing CTC mode 12(ICR1) OC1A/OC1B control "
                 "\nToggling and FOC1x control"
                );

  Serial.print("ICR1:");
  Serial.println(ICR1);
  Serial.print("OCR1A:");
  Serial.println(OCR1A);
  Serial.print("OCR1B:");
  Serial.println(OCR1B);
  TIMSK1 |= bit(ICIE1);
}

void loop() {
  static uint32_t lastChange = 0;
  uint32_t now = millis();
  int butt = digitalRead(2);
  static byte lastButt = -1;
  if (lastButt != butt && now - lastChange > 20) {
    lastChange = now;
    if (butt == LOW) {
      //OCR1B = 62500U/4-1; // 25% duty cycle.
      if (digitalRead(2) == LOW) {
        //digitalWrite(10,HIGH);
        //if(digitalRead(2)==LOW){PORTB |= bit(2);} // doesn't set OC1B high
        //if(digitalRead(2)==LOW){TCNT1=0;} // doesn't set OC1B high
        TCCR1C = bit(FOC1B); // should toggle OC1B
        TCCR1C = bit(FOC1A); // should toggle OC1A
      }
      Serial.print('v');
    } else { // butt = high
      Serial.print('^');
    }
    lastButt = butt;
  }
  if (ovfflag) {
    Serial.print('o');
    ovfflag = false;
  }


  // put your main code here, to run repeatedly:
  report();
}


void report(void) {
  static uint32_t lastChange = 0;
  uint32_t now = millis();
  static int oldState = -1;
  if (now - lastChange > 500) {
    lastChange = now;
    int newState = digitalRead(10) << 1 | digitalRead(9);
    Serial.print(digitalRead(9));
    Serial.print(digitalRead(10));
    Serial.print(' ');
    if (newState != oldState) {
      oldState = newState;
      if (newState == 0) {
        Serial.println();

      }
    }
  }

}


ISR(TIMER1_CAPT_vect) {
  if (digitalRead(3) == LOW) {
    TCCR1C = bit(FOC1B);
    TCCR1C = bit(FOC1A); // should toggle on
    if (digitalRead(9) == LOW) TCCR1C = bit(FOC1A);
    if (digitalRead(10) == LOW) TCCR1C = bit(FOC1B);
    ovfflag = true;

  }
}