#include <avr/io.h>
#include <avr/interrupt.h>


volatile uint16_t OF1, OF2, OFcounter, OFcounter_start, stop_counter, stop_countdown, pulse_counter_beam_on, pulse_counter_beam_off, pulse_counter;  
//OVERFLOW COUNTERS
volatile uint16_t Capt1, Capt2, Capt3, StartCapt, StopCapt, StopCapt0;  
//VARIABLES TO HOLD TIMESTAMPS
volatile uint8_t Flag, Flag2;          //CAPTURE FLAG

float periode, pulsewidth_negative, pulsewidth_positive, beam_on, beam_off, Totaltime, PRF, actual_time;
#define icpPin 8                    // PB0 ICP D8  Interrupt input
#define solenoidPin 13
char key;
boolean start, stop_, stop_firstcapture;
/*-------------------------------------------------------------------------------
 INTIALIZING TIMER
 -------------------------------------------------------------------------------*/

void InitTimer1(void)
{
  TCNT1 = 0;                       //SETTING INTIAL TIMER VALUE
  TCCR1B |= (1 << ICES1);          //SETTING FIRST CAPTURE ON RISING EDGE ,(TCCR1B = TCCR1B | (1<<ICES1)
  TIMSK1 |= (1 << ICIE1) | (1 << TOIE1); //ENABLING INPUT CAPTURE AND OVERFLOW INTERRUPTS
}


/*--------------------------------------------------------------------------------
 STARTING TIMER
----------------------------------------------------------------------------------*/

void StartTimer1(void)
{
  TCCR1B |= (1 << CS10);          //STARTING TIMER WITH PRESCALER 1
  //    TCCR1B|=(1<<CS11)|(1<<CS10);    //STARTING TIMER WITH PRESCALER 64
  //    TCCR1B|=(1<<CS12);              //STARTING TIMER WITH PRESCALER 256
  //    TCCR1B|=(1<<CS12)|(1<<CS10);    //STARTING TIMER WITH PRESCALER 1024
  sei();                          //ENABLING GLOBAL INTERRUPTS
}
/*---------------------------------------------------------------------------------
 CAPTURE ISR
------------------------------------------------------------------------------*/

ISR(TIMER1_CAPT_vect)
{
  //  TCNT1 = 0;

  if (Flag == 0 && start == true && stop_ == false)
  {
    StartCapt = ICR1;      //SAVING CAPTURED TIMESTAMP
    TCCR1B &= ~(1 << ICES1); //CHANGE CAPTURE ON FALLING EDGE
    OFcounter_start = OFcounter;
    OFcounter = 0;        //RESETING OVERFLOWS

    pulse_counter_beam_on++;

  }

  if (Flag == 0 && start == false && stop_ == false)
  {
    Capt1 = ICR1;         //SAVING CAPTURED TIMESTAMP
    TCCR1B &= ~(1 << ICES1); //CHANGE CAPTURE ON FALLING EDGE
    OFcounter = 0;           //RESETING OVERFLOWS
    pulse_counter_beam_on++;
  }
  if (Flag == 1 && start == false && stop_ == false)
  {
    Capt2 = ICR1;          //SAVING CAPTURED TIMESTAMP
    TCCR1B |= (1 << ICES1); //CHANGING CAPTURE ON RISING EDGE
    OF1 = OFcounter;       //SAVING FIRST OVERFLOW COUNTER
  }
  if (Flag == 2 && start == false && stop_ == false)
  {
    Capt3 = ICR1;                     //SAVING CAPTURED TIMESTAMP
    OF2 = OFcounter;
    TIMSK1 &= ~((1 << ICIE1) | ~(1 << TOIE1)); //STOP INPUT CAPTURE AND OVERFLOW INTERRUPTS
    pulse_counter_beam_on++;
  }



  if (Flag2 == 0 && stop_ == true && start == false)
  {
    StopCapt = ICR1;         //SAVING CAPTURED TIMESTAMP
    stop_counter = stop_countdown;
    pulse_counter_beam_off++;
    TCCR1B &= ~(1 << ICES1); //CHANGE CAPTURE ON FALLING EDGE
    Flag2++;


  }
  Flag++;                          //INCREMENTING FLAG
  start = false;

}
/*------------------------------------------------------------------------------------
 OVERFLOW ISR
------------------------------------------------------------------------------------*/

ISR(TIMER1_OVF_vect)
{
  OFcounter++;//INCREMENTING OVERFLOW COUNTER
  stop_countdown++;

}

/*------------------------------------------------------------------------------------
 STOP MEASURE
-------------------------------------------------------------------------------------*/

void STOP_MEASURE(void) {

  stop_countdown = 0;
  TIFR1 = (1 << ICF1) | (1 << TOV1); //CLEARING INTERRUPT FLAGS TO AVOID ANY PENDING INTERRUPTS
  TIMSK1 |= (1 << ICIE1) | (1 << TOIE1); //ENABLING INPUT CAPTURE AND OVERFLOW INTERRUPTS
  TCCR1B &= ~(1 << ICES1);        //SETTING FIRST CAPTURE FALLING EDGE
  TCNT1 = 0;

  while (stop_countdown <= 500)
  {

    if (Flag2 == 1) {

      //Serial.println("  <BEAM_OFF>:");
      beam_off = ((StopCapt + (65536 * stop_counter)) * 0.0000625); // Prescaler 1
      //Serial.println(beam_off);
      Flag2 = 0;

    }


  }


  Serial.println(OFcounter_start);
  Serial.println("  <BEAM_ON [us]>:");
  beam_on = ((StartCapt + (65536 * OFcounter_start)) * 0.0000625) * 1000;
  Serial.println(beam_on);


  Serial.println("  <BEAM_ON [ms]>:");
  beam_on = ((StartCapt + (65536 * OFcounter_start)) * 0.0000625);
  Serial.println(beam_on);

  Serial.println("  <BEAM_OFF_LAST_CAPTURE [us]>:");
  beam_off = ((StopCapt + (65536 * stop_counter)) * 0.0000625) * 1000; // Prescaler 1
  Serial.println(beam_off);

  Serial.println("  <BEAM_OFF_LAST_CAPTURE [ms]>:");
  beam_off = ((StopCapt + (65536 * stop_counter)) * 0.0000625); // Prescaler 1
  Serial.println(beam_off);


  Serial.println("  <PRF>:");
  PRF = 1 / (periode / 1000000);
  Serial.println(PRF);

  Serial.println("  <PULSES_TOTAL>:");
  pulse_counter = pulse_counter_beam_on + pulse_counter_beam_off;
  Serial.println(pulse_counter);

  Serial.println("  <PULSES_BEAM_ON>:");
  Serial.println(pulse_counter_beam_on);

  Serial.println("  <PULSES_BEAM_OFF>:");
  Serial.println(pulse_counter_beam_off);

}

/*-------------------------------------------------------------------------------
 MEASURE
-------------------------------------------------------------------------------*/
void MEASURE(void) {

  if (Flag == 3)
  {


    Serial.println(StartCapt);
    pulsewidth_negative = ((Capt3 + (65536 * OF2) - (Capt2 + (65536 * OF1))) * 0.0000625) * 1000; // Prescaler 1
    Serial.print(pulsewidth_negative);
    Serial.print(";    ");


    pulsewidth_positive = ((Capt2 + (65536 * OF1) - Capt1) * 0.0000625) * 1000; // Prescaler 1
    Serial.print(pulsewidth_positive);
    Serial.print(";    ");


    periode = pulsewidth_positive + pulsewidth_negative; // Prescaler 1
    Serial.print(periode);
    Serial.print(";      ");


    Serial.println();

    beam_on = ((StartCapt + (65536 * OFcounter_start)) * 0.0000625) * 1000;
    //Serial.print(beam_on);
    //Serial.print(";      ");

    Flag = 0;                        //CLEARING FLAG
    OF1 = 0;                      //CLEARING OVERFLOW COUNTERS
    OF2 = 0;
    OFcounter = 0;
    OFcounter_start = 0;
    TIFR1 = (1 << ICF1) | (1 << TOV1); //CLEARING INTERRUPT FLAGS TO AVOID ANY PENDING INTERRUPTS
    TIMSK1 |= (1 << ICIE1) | (1 << TOIE1); //ENABLING INPUT CAPTURE AND OVERFLOW INTERRUPTS
    TCNT1 = 0;
  }
}

/*---------------------------------------------------------------------------------
 MAIN FUNCTION
---------------------------------------------------------------------------------*/
int main(void)
{
  volatile uint8_t FINALTIME;          //VARIABLE TO HOLD THE FINAL TIME
  pinMode(icpPin, INPUT);
  pinMode(solenoidPin, OUTPUT);

  InitTimer1();                        //CALLING FUNCTION INITTIMER1 TO INITIALIZE TIMER 1
  StartTimer1();                       //CALLING FUNCTION STARTTIMER1 TO START TIMER 1

  Serial.begin(115200);

  while (1)
  {


    while (!Serial.available());
    key = Serial.read();

    switch (key) {
      case '1':

        start = true;
        stop_ = false;
        //calculate duty cycle if all timestamps captured



        digitalWrite(solenoidPin, HIGH);
        Serial.print("  <PULSEWIDTH_negative>: ");
        Serial.print("  <PULSEWIDTH_positive>: ");
        Serial.print("  <PERIODE>: ");
        Serial.println();

        TCNT1 = 0;
        OFcounter = 0;
        OFcounter_start = 0;

        while (1) {

          key = Serial.read();


          if (key == '0')
          {
            start = false;
            stop_ = true;
            Flag2 = 0;
            digitalWrite(solenoidPin, LOW);

            STOP_MEASURE();

            pulse_counter_beam_on = 0;
            pulse_counter_beam_off = 0;
            pulse_counter = 0;
            beam_off = 0;
            beam_on = 0;
            stop_countdown = 0;
            StartCapt = 0;
            StopCapt0 = 0;
            StopCapt = 0;
            Totaltime = 0;
            beam_on = 0;
            stop_ = false;
            Flag = 0;                        //CLEARING FLAG
            Flag2 = 0;
            OF1 = 0;                      //CLEARING OVERFLOW COUNTERS
            OF2 = 0;
            Capt1 = 0;
            Capt2 = 0;
            Capt3 = 0;
            OFcounter = 0;
            OFcounter_start = 0;
            stop_counter = 0;
            periode = 0;
            TIFR1 = (1 << ICF1) | (1 << TOV1); //CLEARING INTERRUPT FLAGS TO AVOID ANY PENDING INTERRUPTS
            TIMSK1 |= (1 << ICIE1) | (1 << TOIE1); //ENABLING INPUT CAPTURE AND OVERFLOW INTERRUPTS
            TCNT1 = 0;
            InitTimer1();
            StartTimer1();
            break;
          }

          MEASURE();

        }
        Serial.println("FINISHED");
        break;
    }
  }

}

void InitTimer1(void)
{
  TCNT1 = 0;                       //SETTING INTIAL TIMER VALUE
  TCCR1B |= (1 << ICES1);          //SETTING FIRST CAPTURE ON RISING EDGE ,(TCCR1B = TCCR1B | (1<<ICES1)
  TIMSK1 |= (1 << ICIE1) | (1 << TOIE1); //ENABLING INPUT CAPTURE AND OVERFLOW INTERRUPTS
}


/* to clear the interrupt flags do this:

    TIFR1 |= (1 << ICF1) | (1 << TOV1);

*/