#define startInterruptPin 2  // Use pin 8 for start interrupt
#define stopInterruptPin 3  // Use pin 9 for stop interrupt

bool read_1 = 0;
bool read_2 = 0;
bool read_ready = 1;
byte timeout_count = 0;
byte timeout_secs = 1;
byte MAX_TIMEOUT =  ceil(16e6/(65536 * 8.0) * timeout_secs);
uint16_t timer_start = 0;
uint16_t timer_stop = 0;


bool btn_pressed = 0;
bool prev_pressed = 0;
byte count = 2;
void setupTimer0()
{
  TCCR0B = 0; // Stop Timer 0
  TCNT0 = 0; // reset timer
  TCCR0A = (0 << COM0A0)  // toggle OC0A on compare match
         | (0 << COM0B0)  // toggle OC0B on compare match
         | (1 << WGM01);  // Mode 2 = CTC, TOP = OCRA
  OCR0A = 200; // T * 2 * Prescaler / F_CPU = T_OC => 16000000 / (2 * 8 * 5000)
  // OCR0A = F_CPU / (2 * Prescaler * F_OC) 
  OCR0B = 20; // Start opposite

  TCCR0B = (0 << CS02) | (0 << CS01) | (0 << CS00); // Prescaler = 8
  TIMSK0 = (1 << OCIE0A) | (1 << OCIE0B);
  interrupts();
}


void setup() {
  // put your setup code here, to run once:
  pinMode(startInterruptPin, INPUT);
  pinMode(stopInterruptPin, INPUT);
  pinMode(13, OUTPUT);

  
  digitalWrite(13, LOW);
  // Configure external interrupts to start/stop Timer1
  attachInterrupt(digitalPinToInterrupt(startInterruptPin), start, RISING);
  attachInterrupt(digitalPinToInterrupt(stopInterruptPin), stop, RISING);
  setupTimer0();
  DDRD = (1 << PORTD5)
       | (1 << PORTD6);
  PORTD = 0b01100000;
  pinMode(4, INPUT_PULLUP);
  // sei();
  TCCR1A = 0;
  TCCR1B = 0;
  // TCCR1B |= (1 << CS12) | (0 << CS11) | (0 << CS10); //Prescaler 256
  // TCCR1B |= (1 << CS12) | (0 << CS11) | (1 << CS10); //Prescaler 1024
  TCCR1B |= (0 << CS12) | (1 << CS11) | (0 << CS10); //Prescaler 8

  

  TIMSK1 |= (1 << TOIE1); // Enable timer 1 Overflow
  interrupts();
  Serial.begin(9600);
  Serial.println(MAX_TIMEOUT);
  
  // setupTimer2();

}

void loop() {
  // put your main code here, to run repeatedly:
  // if(no_input)
  // {
  //   Serial.println("FIRE");
  // }
  //TCNT1 = 6;
  //Serial.println(TCNT1);
  
  if((timeout_count >= MAX_TIMEOUT) & (read_ready | read_1 | read_2))
  {
    
    if(!read_1 & !read_2)
      Serial.println("FIRE");
    else if(!read_1 & read_2 )
      Serial.println("ERROR: FRONT SENSOR");
    else if(read_1 & !read_2 )
      Serial.println("ERROR: BACK SENSOR");
      

    read_1 = 0;
    read_2 = 0;
    timeout_count = 0;
    read_ready = 0;
    TCNT1 = 0;
  }

  btn_pressed = !digitalRead(4);
  //Serial.println(read_1);
  if(btn_pressed & !prev_pressed & !read_ready)
  {
    TCCR0B = (0 << CS02) | (1 << CS01) | (0 << CS00); // Prescaler = 8
    
    count = 0;
    prev_pressed = btn_pressed;
  }
  else if (!btn_pressed & prev_pressed & !read_ready)
  {
    prev_pressed = 0;
  }
  
  if(read_1 & read_2)
  {
    // Serial.println(timer_start);
    // Serial.println(timer_stop);
    read_1 = 0;
    read_2 = 0;
    timeout_count = 0;
    read_ready = 1;
    TCNT1 = 0;
    uint16_t delta = timer_stop - timer_start;
    if(timer_stop < timer_start)
    {
      delta = timer_start - timer_stop;
    }
    uint16_t time = delta >> 1; // CAMBIAR PRESCALER
    // float time = (62.5 * 8.0 * delta)/1e6;
    //Serial.println("read_1: " + String(timer_start));
    Serial.println("Δ: " + String(time) + " µs");
    
  }
}

ISR(TIMER0_COMPA_vect)
{
  if(count == 0)
    PORTD &= ~0b00100000;
  else if(count == 1)
    PORTD &= ~0b01000000;
}

ISR(TIMER0_COMPB_vect)
{
  
  // PORTD &= 0b10000000;
  if(count == 0)
    PORTD |= 0b00100000;
  else if(count == 1)
    PORTD |= 0b01000000;
  else
    TCCR0B = 0;
  count++;
}

void start()
{
  //digitalWrite(13, HIGH);
  timer_start = TCNT1;
  timeout_count = 0;
  read_1 = 1;

}

void stop()
{
  //digitalWrite(13, LOW);
  timer_stop = TCNT1;
  timeout_count = 0;
  read_2 = 1;
}

ISR(TIMER1_OVF_vect) {
  timeout_count++;
}