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

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

float distance = 0.102;

int i = 0;
int j = 0;


void setupTimer0()
{
  TCCR0B = 0; // Stop Timer 0
  TCNT0 = 0; // reset timer
  TCCR0A = (1 << COM0A0)  // toggle OC0A on compare match
         | (1 << COM0B0)  // toggle OC0B on compare match
         | (1 << WGM01);  // Mode 2 = CTC, TOP = OCRA
  OCR0A = 100; // T * 2 * Prescaler / F_CPU = T_OC
  // OCR0A = F_CPU / (2 * Prescaler * F_OC) 
  OCR0B = 1; // Start opposite

  TCCR0B = (0 << CS02) | (1 << CS01) | (0 << CS00); // Prescaler = 8
  DDRD = (1 << PORTD5)  // OC0A = PD5 output
       | (1 << PORTD6);
}

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);
  // sei();
  TCCR1A = 0;
  TCCR1B = 0;
  // TCCR1B |= (1 << CS12) | (0 << CS11) | (0 << CS10); //Prescaler 256
  TCCR1B |= (0 << CS12) | (1 << CS11) | (0 << CS10); //Prescaler 8

  

  TIMSK1 |= (1 << TOIE1); // Enable timer 1 Overflow

  setupTimer0();
  Serial.begin(115200);
  Serial.println(MAX_TIMEOUT);
  
  interrupts();
  

}

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, SHOOT AGAIN");
    else if(read_1 & !read_2 )
      Serial.println("ERROR: BACK SENSOR, SHOOT AGAIN");
      

    read_1 = 0;
    read_2 = 0;
    timeout_count = 0;
    read_ready = 0;
    TCNT1 = 0;
    i = 0;
    j = 0;
  }
  //Serial.println(read_1);
  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;
    }
    float delta_time = float(delta * 8.0/16e6*1e6); // CAMBIAR PRESCALER
    float velocity = distance/delta_time * 3.28084;
    // float time = (62.5 * 8.0 * delta)/1e6;
    // Serial.println("Front Sensor triggered " + String(i) + " times, read_1 = " + String(read_1));
    // Serial.println("Velocity: " + String(velocity) + " FPS");
    // Serial.println(delta);
    // Serial.println("Rear Sensor triggered " + String(j) + " times, read_2 = " + String(read_2));
    Serial.println("Front count: " + String(timer_start));
    Serial.println("Rear count: " + String(timer_stop));
    Serial.println("Delta count: " + String(delta));
    Serial.println("Delta time: " + String(delta_time));
    
    
  }
  
}



void start()
{
  //digitalWrite(13, HIGH);
  
  if(i < 1)
  {
    timer_start = TCNT1;
    timeout_count = 0;
    read_1 = 1;
    i++;
  }
  //else
    //i = 0;
}

void stop()
{
  //digitalWrite(13, LOW);
  
  if(j < 1)
  {
    timer_stop = TCNT1;
    timeout_count = 0;
    // if(read_1 ==1)
    // {
    //   read_2 = 1;
    // }
    read_2 = 1;
    j++;
  }
  //else
    //j = 0;
}

ISR(TIMER1_OVF_vect) {
  timeout_count++;
}