#include <TimerOne.h>             // this will enable us to simply use the Timer1. The Timer1 is NOT used by any Arduino internal functions we are using in thsi sketch (the Servo lib is usually using the Timer1) - so we are free to use it for our needs as we like :-)
#include <Wire.h>
#include "SSD1306Ascii.h"
#include "SSD1306AsciiWire.h"
#include "RF24.h"
#include "speedclock.h"

// internal defines for the OLED display ...
SSD1306AsciiWire display;
/****************** User Config for NRF24***************************/
/***      Set this radio as radio number RADIO0 or RADIO1         ***/
radio_type_e stationNumber = BASESTATION;  //---> TOPSTATION has the button connected, BASESTATION is the default ... 
uint8_t radio_sel0, radio_sel1;          // code of type of station

/* Hardware configuration: Set up nRF24L01 radio on SPI bus plus pins 7 & 8 */
RF24 radio(RF24_CNS,RF24_CE);
/**********************************************************/
byte addresses[][12] = {"top_station","basestation"};   // Radio pipe addresses for the 2 nodes to communicate.

unsigned long stats_last_plotted_at_ms = 0;

unsigned long startloop_ms = 0;

boolean offset_sync_sequence = true;                    // set to true as the offset sync / calibration is not done - sending data to the basestation is more often as after the sync is done ... 

boolean blink_on = false;                               // set to TRUE if the system clock cycle signals to set LEDs in LED_BLINK mode to be active - means top be switched on
unsigned long blink_on_swiched_at_ms = 0;               // the last system time (in milliseconds) the blink_on was switched
uint8_t *leds_states = LEDStates[TIMER_INIT];

boolean time_offset_ok = false;                         // true as long as the offset is correctly calculated
volatile boolean new_current_offset_available = false;
uint16_t counter_time_offset = 0;                       // number of used values for the mean value calculation

signed long long sum_time_offset_us = 0;                // sum of offset values
float sum_time_slope = 0;                             // sum of slopes

volatile unsigned long last_top_time_us = 0;              // current top time ... 
volatile unsigned long current_top_time_us = 0;           // current top time ... 
volatile unsigned long last_bottom_time_us = 0;           // current top time ... 
volatile unsigned long current_bottom_time_us = 0;        // current top time ... 

volatile signed long current_time_offset_us = 0;        // current offset ... 

signed long mean_time_offset_us = 0;                    // mean value for the offset (we have an linear sync function to map the TOPSTATION time to BOTTOMSTATIONTIME : t
float mean_time_slope = 0;                            // mean slope  

signed long running_time_offset_us = 0;                 // offset that will be used for this run ... 
volatile unsigned long start_time_us = 0;               // if the timer is running this is that start time ... (use volatile for all shared variables that deals with hardware)
volatile unsigned long runner_start_time_us = 0;        // this is the time the runner left the pad - so the status of the falsetstart pin goes to high again  - but this is OK and a real start (use volatile for all shared variables that deals with hardware)
unsigned long run_time_us = 0;                          // if the timer is running this is that start time ...
boolean warn_during_run = false;                        // will be set to true if there is a warning during the run - usually an offset sync error
volatile unsigned long connection_last_established_at_ms = 0;    // time the last active connection was established
volatile boolean connection_available = false;                   // if there were no data for longer then CONN_TIMEOUT the connection will be flaged as lost ... 
boolean keep_connection_off = true;                     // if sett to true the connection to the top station will be kept off for a timeout time to signal that we are in the init sequnce again ... 
uint8_t failed_offsets = MAX_ALLOWED_FAILED_OFFSETS;    // number of offset values that did not fullfill the MAX_DIFFERENCE_OFFSET_MS criterion
volatile boolean false_start = false;                   // set to true if a false start occurs (use volatile for all shared variables that deals with hardware)
volatile uint8_t startsequence_count = 0;               // shows thze actual step in the startsquence. Number of steps is defined in STARTSEQ_STEPS (use volatile for all shared variables that deals with hardware)
volatile boolean startsequence_done = false;            // set to TRUE if the startsequnce was completed successfully (without a false start)
volatile boolean failsequence_done = false;
volatile uint8_t failsequence_count = 0;

boolean topbuttonwaspressed = false;                    // set to true if the stop button was pressed

uint8_t button_state[NO_LAST_BUTTON] = {BUTTON_NOTPRESSED};
unsigned long button_last_changed_at_ms[NO_LAST_BUTTON] = {0};
uint8_t button_last_changed_to[NO_LAST_BUTTON] = {BUTTON_NOTPRESSED};

timer_state_e timer_state = TIMER_INIT;                 // current state needs to be initialized to somethin different then new_state due to the fact that some pieces of the code check for differnt values of state and _new_state to detect an update... 
timer_state_e timer_new_state = TIMER_NOCONNECTION;     // next state - in the startup phase the first state - will be TIMER_NOCONNECTION ... checking if a connection to TOPSTATION is established 

timer_mode_e timer_mode = MODE_COMPETE;                 // mode of the BASESTATION - this can be changed in IDLE state by pressing the CANCEL button
unsigned long timer_mode_changed_at_ms = 0;
transcv_s radio_data;

void setup(){

  Serial.begin(115200);
  
  // set the BUTTON pins as pullup input pins ... 
  for(uint8_t button = 0; button<NO_LAST_BUTTON;button++){
    pinMode(BUTTONPins[button], INPUT_PULLUP);
  }

  // set the LED pins as output pins ... 
  for(uint8_t led = 0; led<NO_LAST_LED;led++){
    pinMode(LEDPins[led], OUTPUT);
  }

  // Get the station type (base or top) as set by the station select pin - BASESTATION is default

  pinMode(STATION_SEL0, INPUT);
  pinMode(STATION_SEL0, INPUT);
  radio_sel0 = digitalRead(STATION_SEL0);
  radio_sel1 = digitalRead(STATION_SEL1);

  if((radio_sel0 == 1) & (radio_sel1 == 0)){
    stationNumber = TOPSTATION; 
    Serial.println("The level of the station select pin makes the current node set to the TOPSTATION.");
  }
  else{
    Serial.println("The level of the station select pin makes the current node set to the BASESTATION");
  }
  
  // Setup and configure the NRF radio
  // radio setup ...
  radio.begin();
  radio.setRetries(1, 1);               //the first is the time between reties in multiple of 250ms, the second is the numer of attempts
  if(stationNumber == TOPSTATION){
    // Attach the STOP button interrupt
    attachInterrupt(digitalPinToInterrupt(BUTTONPins[BUTTON_STOP]), stop_isr, FALLING );
    // Set the PA Level of the sendin TOP_STATION
    radio.setPALevel(RF24_PA_LEVEL);
    radio.openWritingPipe(addresses[1]);        // Both radios listen on the same pipes by default, but opposite addresses
    radio.openReadingPipe(1,addresses[0]);      // Open a reading pipe on address 0, pipe 1
    radio.stopListening();                      // top station will never receive data.      
  }else{
    radio.openWritingPipe(addresses[0]);
    radio.openReadingPipe(1,addresses[1]);
    radio.startListening();  
  }  
  radio_data.topstationtime_us = micros();      // set the current micro second count  
  radio_data.topbuttonpressedtime_us = 0;       // set the time the button was pressed last time to 0

  //initialise Wire and OLED
  Wire.begin();
  Wire.setClock(400000L);  
  display.begin(&Adafruit128x64, DISPLAY_I2C_ADDRESS);
  display.clear();
}

void loop(void) {

/****************** Shared code for all stations ********************************************************************************/
  startloop_ms = millis();
  
  if(millis() - blink_on_swiched_at_ms > LED_BLINK_ALL_MS){
    blink_on_swiched_at_ms = millis();
    blink_on = !blink_on;
  }

  // set state to new_state
  /*
  if(timer_state != timer_new_state){
    Serial.print(millis());
    Serial.print("ms : current state:");
    Serial.print(timer_state);
    Serial.print(" new state:");
    Serial.println(timer_new_state);
  }
  */

  // update button states ...
  update_buttons();
    
/****************** Code for the TOPSTATION is here - the stop button is connected to the top station ***************************/
  if (stationNumber == TOPSTATION){                         // Radio is the top station and sends continously its time and the time the stop button was pressed.

    // send data to base_station
    send_values();

    timer_state = timer_new_state;

    // set LEDs
    set_state_LEDs(timer_state, false );

    switch(timer_state){
      case TIMER_NOCONNECTION:
        // as long as there is no connection to BASE_STATION we will end up here
        if(true == connection_available){
          timer_new_state = TIMER_INIT;
        }
        offset_sync_sequence = true;
        counter_time_offset = 0;
        break;
      case TIMER_INIT:
        if(false == connection_available){
          timer_new_state = TIMER_NOCONNECTION;
        } else {
          if(false == offset_sync_sequence){
            if(button_state[BUTTON_STOP] == BUTTON_NOTPRESSED){
              topbuttonwaspressed = false;
              timer_new_state = TIMER_IDLE;
            }
          }
        }
        break;
      case TIMER_IDLE:
        //check for the pressed button - or better enable the interrupt to handle that
        if(false == connection_available){
          timer_new_state = TIMER_NOCONNECTION;
        } else {
          if(true == topbuttonwaspressed){
            timer_new_state = TIMER_STOPPED;
          }
        }
        break;
      case TIMER_STOPPED:
        // wait a few millis and  ... after that go back to idle ... 
        if((signed long)(micros() - radio_data.topbuttonpressedtime_us) > MIN_DELAY_BETWEEN_PRESSED_US){
          if(button_state[BUTTON_STOP] == BUTTON_NOTPRESSED){
            timer_new_state = TIMER_IDLE;
            topbuttonwaspressed = false;
          }
        }
        break;
    }

  }

/****************** Code for the BASESTATION is here - the display and the start button is connected here. All caclulation will be done here ***************************/
  if ( stationNumber == BASESTATION ) {

    // update connection status
    handle_connection();
    
    // calculate offset and set 'last connection' time stamp
    //update_offset_values();

    // update the OLED screen 
    update_screen(timer_new_state);

    timer_state = timer_new_state;

    // set LEDs
    set_state_LEDs(timer_state, warn_during_run );

    switch(timer_state){
      case TIMER_NOCONNECTION:
        // as long as there is no connection to TOP_STATION we will end up here
        if(connection_available == true){
          timer_new_state = TIMER_INIT;
        }
        break;
      case TIMER_INIT:
        // init the system offset ... 
        if(connection_available == false){
          // if the connection was lost ... switch to noconnection state 
          timer_new_state = TIMER_NOCONNECTION;
        } 
        else{
          // if the offset is claculated, cancel not pressed and failstart not pressed switch to IDLE mode ... 
          if((time_offset_ok == true) && 
             (button_state[BUTTON_CANCEL] == BUTTON_NOTPRESSED) &&
             (button_state[BUTTON_FAIL] == BUTTON_NOTPRESSED) )
          {
            // check if offset is OK - if not .. set state back to INIT
            timer_new_state = TIMER_IDLE;
          }
        }
        break;
      case TIMER_IDLE:
        warn_during_run = false;
        if(connection_available == false){
          // if the connection was lost ... switch to noconnection state 
          timer_new_state = TIMER_NOCONNECTION;
        } 
        else{
          if(time_offset_ok == false){
            // check if offset is OK - if not .. set state back to INIT
            timer_new_state = TIMER_INIT;
            keep_connection_off = true;
            connection_last_established_at_ms = millis();
          }
          else{
            // check if the FALSESTATE button is pressed OR we are in trainingsmode - somebody is ready to run, but STARTBUTTON is NOT pressed ... 
            if(((button_state[BUTTON_FAIL] != BUTTON_NOTPRESSED) || (timer_mode != MODE_COMPETE)) &&
               (button_state[BUTTON_START] == BUTTON_NOTPRESSED) && 
               (button_state[BUTTON_CANCEL] == BUTTON_NOTPRESSED)) 
            {
                timer_new_state = TIMER_READY;
            } else {
              if((button_state[BUTTON_CANCEL] == BUTTON_LONGPRESSED) &&
                 (button_state[BUTTON_START] == BUTTON_NOTPRESSED) &&
                 (button_state[BUTTON_FAIL] == BUTTON_NOTPRESSED))
              {
                // switch to settings mode ... 
                timer_new_state = TIMER_SETTINGS;
              }
            }
          }
        }
        break;
      case TIMER_READY:
        if((button_state[BUTTON_FAIL] == BUTTON_NOTPRESSED) &&
           (timer_mode == MODE_COMPETE))
        {
          // false start was released again - go back to IDLE ... so far this is not a false start - run was not started yet    
          timer_new_state = TIMER_IDLE;
        } else {
          // check if the start button was pressed ... there is at least still someone waiting for the run .
          if(button_state[BUTTON_START] != BUTTON_NOTPRESSED){
            // now enable the interrupt for the FALSESTART button
            startsequence_count = 0;
            startsequence_done = false;
            running_time_offset_us = mean_time_offset_us;
            false_start = false;
            if(timer_mode == MODE_COMPETE){
              attachInterrupt(digitalPinToInterrupt(BUTTONPins[BUTTON_FAIL]), false_start_isr, RISING );
            }
            Timer1.initialize();
            timer_new_state = TIMER_STARTED;
            // set the startime - this is the current time plus the length of this sequence
            start_time_us = micros() + STARTSEQ_LENGTH_US;
            // call the start sequence interrupt routine ... 
            Timer1.attachInterrupt(start_isr,STARTSEQ_PAUSE_US[startsequence_count]); // startISR to run every given microseconds  
          } else {
            if((button_state[BUTTON_CANCEL] == BUTTON_LONGPRESSED) &&
               (button_state[BUTTON_START] == BUTTON_NOTPRESSED) &&
               (button_state[BUTTON_FAIL] == BUTTON_NOTPRESSED))
            {
              // switch to settings mode ... 
              timer_new_state = TIMER_SETTINGS;
            }
          }
        }
        break;
      case TIMER_STARTED:
        //initialize the start ISR and the timer interrupt ...  
        //----> to be removed : startSequence();
        if( false_start == true) {
          failsequence_done = false;
          failsequence_count = 0;
          timer_new_state = TIMER_FAIL;
          Timer1.attachInterrupt(failSequence,FAILSEQ_PAUSE_US[failsequence_count]); 
        } else {
          if(startsequence_done == true){
            timer_new_state = TIMER_RUNNING;
          }
        }
        break;        
      case TIMER_RUNNING: 
        if(time_offset_ok != true){
          // check if offset is still OK - if not .. set warning
          warn_during_run = true;
        }
        if((signed long)(micros() - start_time_us) > TIMER_TIMEOUT_US){
          timer_new_state = TIMER_TIMEDOUT;
        } else {
          if(button_state[BUTTON_CANCEL] != BUTTON_NOTPRESSED){
            timer_new_state = TIMER_CANCELLED;
          } else {
            if(radio_data.topbuttonpressedtime_us > running_time_offset_us){
              if((signed long)(radio_data.topbuttonpressedtime_us - running_time_offset_us) > start_time_us){
                run_time_us = (radio_data.topbuttonpressedtime_us - running_time_offset_us) - start_time_us;
                timer_new_state = TIMER_STOPPED;
              }
            }
          }
        }
        break;
      case TIMER_STOPPED:
        //calculate the run_time and switch to WAIT
        if(button_state[BUTTON_CANCEL] == BUTTON_NOTPRESSED){
          timer_new_state = TIMER_WAIT;
        }
        break;
      case TIMER_FAIL:
        //fail start case .... 
        run_time_us = TIMER_MAX_TIME_US;
        if(true == failsequence_done){
          if(button_state[BUTTON_CANCEL] == BUTTON_NOTPRESSED){        
            timer_new_state = TIMER_WAIT;
          }
        }
        break;        
      case TIMER_CANCELLED:
        // what to do in chancel mode ?
        run_time_us = TIMER_MAX_TIME_US;
        if(button_state[BUTTON_CANCEL] == BUTTON_NOTPRESSED){
          timer_new_state = TIMER_WAIT;
        }
        break;
      case TIMER_TIMEDOUT:
        // time out
        run_time_us = micros() - start_time_us;
        if(button_state[BUTTON_CANCEL] == BUTTON_NOTPRESSED){
          timer_new_state = TIMER_WAIT;
        }
        break;
      case TIMER_WAIT:
        // wait until the chancel button was pressed to go ahead
        if((button_state[BUTTON_CANCEL] != BUTTON_NOTPRESSED) &&
           (button_state[BUTTON_START] == BUTTON_NOTPRESSED) &&
           (button_state[BUTTON_FAIL] == BUTTON_NOTPRESSED)
           )
        {
          timer_new_state = TIMER_IDLE;
        }
        break;
    case TIMER_SETTINGS:
      // switch between the different modes for now - compete, training and calibration ... 
      if((button_state[BUTTON_CANCEL] == BUTTON_PRESSED) &&
         (button_state[BUTTON_START] == BUTTON_NOTPRESSED))
      {
        // go back to idle 
        timer_new_state = TIMER_IDLE;
      } else {
        if((button_state[BUTTON_CANCEL] == BUTTON_NOTPRESSED) &&
            (button_state[BUTTON_START] == BUTTON_PRESSED))
        {
          if((millis() - timer_mode_changed_at_ms) > KEY_TOGGLE_MS){
            // switch system mode ... 
            switch(timer_mode){
              case MODE_COMPETE:
                timer_mode = MODE_TRAINING;
                break;
              case MODE_TRAINING:
                timer_mode = MODE_CALIBRATION;
                break;
              case MODE_CALIBRATION:
                timer_mode = MODE_COMPETE;
                break;              
            }
            timer_mode_changed_at_ms = millis();
          }
        }
            
      }
      break;
    }      
  }

  /*
  // ignore the first seconds ... 
  if(time_offset_ok == true){
    if( (STATS_PLOTS_EVERY_MS > 0) &&
        ((signed long)(millis() -stats_last_plotted_at_ms) > STATS_PLOTS_EVERY_MS))
    {
      //Serial.println("mean_time_offset-current_time_offset");
      //Serial.println(calc_mean_offset(current_top_time_us));
      //Serial.print(" ");
      //Serial.print(mean_time_offset_us);
      //Serial.print(" ");
      //Serial.println(current_time_offset_us);
      stats_last_plotted_at_ms = millis();
    }
  }
  */
}

//####################### HELPER FUNCTIONS ###########################
signed long calc_top2bot_time(signed long current_toptime){
  return((mean_time_slope*current_toptime) + mean_time_offset_us);
}

void nrf24_isr(void)                                
{
  //Serial.println("Got data from TOPSTATION");
  bool tx,fail,rx;
  // wait the connection time out time before receiving data - this is to tell the TOP_STATION to resend offset values in fast mode ... 
  if(keep_connection_off == false){
    radio.whatHappened(tx,fail,rx);                     // What happened?
    if ( rx ){
    // check if radio data is available - if so read the data
      // read data from TOP_STATION ... 
      //while( radio.available()){                                                        // Read all available payloads
      radio.read( &radio_data, sizeof(radio_data) );                                  // Read the data the TOPSTATION sent
      //} 
      if(new_current_offset_available == false){
        last_top_time_us = current_top_time_us;
        current_top_time_us = radio_data.topstationtime_us;
        last_bottom_time_us = current_bottom_time_us;
        current_bottom_time_us = micros(); 
        //new_current_offset_available = true;  
        Serial.print(" last_top_time_us:");
        Serial.print(last_top_time_us);
        Serial.print(" current_top_time_us:");    
        Serial.print(current_top_time_us);    
        Serial.print(" last_bottom_time_us.");        
        Serial.print(last_bottom_time_us);    
        Serial.print(" current_bottom_time_us:");    
        Serial.println(current_bottom_time_us);
        Serial.print("current offset:");
        Serial.println((signed long)(current_top_time_us - current_bottom_time_us));
      }
      connection_last_established_at_ms = millis();
      connection_available = true;
    }
  }
}

void update_buttons(void){
  uint8_t curr_button_state;
  unsigned long button_pressed_at_ms = millis();
  String state_string;
  // we have some buttons to update that are used in the sketch ...
  for(uint8_t button = 0; button < NO_LAST_BUTTON; button++){
    // which state do we have ? 
    if(digitalRead(BUTTONPins[button]) != BUTTON_PRESSED){
      state_string = " not pressed.";
      curr_button_state = BUTTON_NOTPRESSED;
    } else {
      state_string = " pressed.";
      curr_button_state = BUTTON_PRESSED;
    }
    if( curr_button_state != button_last_changed_to[button] ){
      button_last_changed_at_ms[button] = button_pressed_at_ms;
      button_last_changed_to[button] = curr_button_state;
    }
    
    if(((curr_button_state == BUTTON_NOTPRESSED) && (button_state[button] != BUTTON_NOTPRESSED)) ||
       ((curr_button_state == BUTTON_PRESSED) && (button_state[button] == BUTTON_NOTPRESSED)))               // the button has changed its state
    {
      // is that bouncing or settled?
      if((unsigned long)(button_pressed_at_ms - button_last_changed_at_ms[button]) > KEY_BOUNCE_MS){
        // settled! -> change the stored state.
        button_state[button] = curr_button_state;
      } else {
      }
    } else {
      if((curr_button_state == BUTTON_PRESSED) &&
         (button_state[button] == BUTTON_PRESSED))
      {
        //check for long pressed button ... 
        if((unsigned long)(button_pressed_at_ms - button_last_changed_at_ms[button]) > KEY_LONGPRESSED_MS){
          button_state[button] = BUTTON_LONGPRESSED;
        }
      }
    }
  }
}

void handle_connection(void){
  if(keep_connection_off == true){
    // Attach the RF24 IRQ pin interrupt
    detachInterrupt(digitalPinToInterrupt(RF24_IRQ));    
    connection_available = false;
    if((millis() - connection_last_established_at_ms) > (2*CONN_TIMEOUT_MS)){
      keep_connection_off = false;
      // Attach the RF24 IRQ pin interrupt
      attachInterrupt(digitalPinToInterrupt(RF24_IRQ), nrf24_isr, LOW );
    }
  } else {
    // remove the RF24 connection flag if no data was received for longer time
    if((millis() - connection_last_established_at_ms >= CONN_TIMEOUT_MS) || (connection_last_established_at_ms == 0)){ 
      connection_available = false;
      Serial.println("ERROR: No connection established to TOPSTATION");
    } // radio available
  } // keep connection off
}

void update_offset_values(void){

  float int_time_slope = 1.0; 
  signed long int_time_delta_x = 0;
  signed long int_time_delta_y = 0;
  signed long int_time_offset_us = 0;
  unsigned long int_current_bot_time_us = 0;
  unsigned long int_calc_bot_time_us = 0;
  unsigned long int_current_top_time_us = 0;  
  signed long int_current_calc2real_offset = 0;

  if((new_current_offset_available == true) &&          // there is a new value
     (last_top_time_us != 0) &&                         // the last top time is updated already
     (current_top_time_us != 0) &&                      // the current top time is updated 
     (last_bottom_time_us != 0) &&                      // the last bottom time is updated
     (current_bottom_time_us != 0))                        // the current bottom time is updated
  {

        
    //calculate the last and the current offset based on what we got from Topstation
    int_current_bot_time_us = current_bottom_time_us;
    int_time_delta_y  = current_bottom_time_us-last_bottom_time_us;
    int_time_delta_x  = current_top_time_us-last_top_time_us;
    new_current_offset_available = false;
    
    if(counter_time_offset == 0){
      // this is the initial setup to start with something - in this case the last offset value that was received
      int_current_calc2real_offset = 0;
    } else {
      int_calc_bot_time_us  =  calc_top2bot_time(current_top_time_us);
      int_current_calc2real_offset = int_calc_bot_time_us - int_current_bot_time_us;
      //Serial.print("Counter:");
      //Serial.println(counter_time_offset);
      //Serial.print("Calculated bot time:");
      //Serial.println(int_calc_bot_time_us);
      //Serial.print("Real bot time:");
      //Serial.println(int_current_bot_time_us);
      Serial.print("So offset between calc and Real bot time:");
      Serial.println(abs(int_current_calc2real_offset));
      // Serial.print("function offset:");
      //Serial.println(mean_time_offset_us);
      //Serial.print("function slope:");
      //Serial.println((float)mean_time_slope);
    }
    // check current offset of the TOP_STATIOn and the BASESTATION if more than allowed ... 

    if(abs(int_current_calc2real_offset) < MAX_DIFFERENCE_OFFSET_US){
      // if the current value is in range - decrease the fail counter by 1 if it was not zero already
      if(failed_offsets > 0){
        Serial.println("INFO: The last received TOPSTATION offset time stamp was again in range. Decrease internal fail counter"); 
        failed_offsets--;
      }
      // the offset is in range - check if we have already enough values of if we need to add more ...
      if(counter_time_offset <= REQUIRED_NUMBER_MEANVALS){
        //add the next value to meanvalue calculation ... 
        int_time_slope = (float)(int_time_delta_y / int_time_delta_x);        
        int_time_offset_us = current_bottom_time_us - (int_time_slope*current_top_time_us);

        //Serial.print(" offset is: ");
        //Serial.print(int_time_offset_us);
        //Serial.print(" int_time_delta_y:");
        //Serial.print(int_time_delta_y);
        //Serial.print(" int_time_delta_x:");        
        //Serial.print(int_time_delta_x);
        //Serial.print(" int_time_slope:");
        //Serial.println(int_time_slope);
        
        sum_time_offset_us = sum_time_offset_us + int_time_offset_us;
        sum_time_slope = sum_time_slope + int_time_slope;
        counter_time_offset++;
        mean_time_offset_us = sum_time_offset_us/counter_time_offset;
        mean_time_slope = sum_time_slope/counter_time_offset;
        Serial.print("Offset calulation. We have ");
        Serial.print(counter_time_offset);
        Serial.print("/");
        Serial.print(REQUIRED_NUMBER_MEANVALS);
        Serial.print(" values. y=slope*x+offset. Mean offset value based on that is: ");
        Serial.print(mean_time_offset_us);
        Serial.print(" mean slope: ");
        Serial.println(mean_time_slope);        
      } else {
        time_offset_ok = true;
      }
      
    } else {
      // the current offset is out of range ...
      // if the values before also already failed the criterion but the max allowed number of such fails is not reached ... just increase the counter. 
      if(failed_offsets < MAX_ALLOWED_FAILED_OFFSETS){
        Serial.println("WARNING: The last received TOPSTATION offset time stamp was out of range. Increase internal fail counter"); 
        failed_offsets++;
      }
      else{
        // if the values before also already failed the criterion AND the max allowed number of such fails is reached ... we need to restart the mean calculation and set the timer to unready state ...
        Serial.println("TopStation and BaseStation are out off sync. Offset calculation will be (re)started.  ");
        //Serial.print("Last ");
        //Serial.print(MAX_ALLOWED_FAILED_OFFSETS);
        //Serial.print(" offsets (last one: ");
        //Serial.print(current_time_offset);
        //Serial.print(" ) were to far from the mean offset ");
        //Serial.println( abs(current_time_offset - mean_time_offset) );
        //Serial.print("This more than the allowed: ");
        //Serial.print(MAX_DIFFERENCE_OFFSET_MS);
        //Serial.print(" compared to the mean offset: ");
        //Serial.println(mean_time_offset);
        time_offset_ok = false;
        counter_time_offset = 0;
        sum_time_offset_us = 0;
        sum_time_slope = 0;
        mean_time_offset_us = 0;
        mean_time_slope = 0;
        failed_offsets = 0;
      } // out of range counter exceeds maximum value
    } // time offset of TOPSTATION out of range
  } else {
    new_current_offset_available = false;
  }
  

}// receive values

void update_screen(timer_state_e state){
  bool scr_update = true;
  uint8_t ypos = 64-42/2;
  String header = "no state";
  String content = "";
  String footer = "";
  
  char string_to_char[50];
  
  signed long curr_time_local_s = 0;

  switch(state){
    case TIMER_NOCONNECTION:
      header = "No connection";
      content = "Waiting..";
      footer = "Waiting for TOPSTATION";
      break;    
    case TIMER_INIT:
      header = "Init";
      content = "...";
      footer = "please wait";
      break;
    case TIMER_IDLE:
      header = "Idle!";
      content = "00.000 sec";
      footer = "Waiting for climber";
      break;
    case TIMER_READY:
      header = "Ready!";
      content = "00.000 sec";
      footer = "Waiting for start";
      break;
    case TIMER_STARTED:
      header = "Starting ...";
      content = "00.000 sec";
      footer = "...";
      break;
    case TIMER_RUNNING:
      header = "Running ...";
      curr_time_local_s = (micros() - start_time_us);
      content = micros2string(curr_time_local_s);
      content += " sec";
      curr_time_local_s = (runner_start_time_us - start_time_us);
      footer = "Reaction time: ";
      footer += micros2string(curr_time_local_s);
      footer += " sec";
      break;
    case TIMER_CANCELLED:
      header = "Cancelled!";
      break;
    case TIMER_STOPPED:
      header = "Stopped!";
      curr_time_local_s = run_time_us;
      content = micros2string(curr_time_local_s);
      content += " sec";
      curr_time_local_s = (runner_start_time_us - start_time_us);
      footer = "Reaction time: ";
      footer += micros2string(curr_time_local_s);
      footer += " sec";
      break;
    case TIMER_TIMEDOUT:
      header = "Timed out!";
      content = "Invalid!";
      break;
    case TIMER_FAIL:
      header = "False start!";
      curr_time_local_s = (start_time_us - runner_start_time_us);
      content = micros2string(curr_time_local_s);
      footer = "seconds too early";
      break;
    case TIMER_SETTINGS:
      header = "Settings";
      content = timer_mode_string[timer_mode];
      footer = "Press Start to toggle.";
      break;      
    default:
      scr_update = false;
      break;
  }

  if(scr_update == true){
    if(timer_new_state != timer_state){
      display.clear(0,200,0,0);
      display.clear(0,200,3,5);
      display.clear(0,200,7,7);
    }
    //snprintf( string_to_char, sizeof(string_to_char),"%s", header);
    header.toCharArray(string_to_char, sizeof(string_to_char));
    
    display.setFont(System5x7);
    display.set1X();
    int xpos = (128 - (display.strWidth(string_to_char)))/2 - 10;
    display.home();
    display.setLetterSpacing(1);
    display.setCursor(64 - (display.strWidth(string_to_char) / 2), 0);
    display.print(string_to_char);
    display.setCursor(1,0);
    //check if there is a connection to the topstation
    if(connection_available != true){
      //if not remove the "Y"
      display.print("  ");
    }
    else {
      //if print the "Y"
      display.print("Y");
    }
    //check which timer_mode we are in 
    display.setCursor(122,0);
    display.print(timer_mode_short[timer_mode]);
    display.setCursor(0,1);
    display.setLetterSpacing(0);
    display.print("----------------------------");
    
    //end of the Header
    content.toCharArray(string_to_char, sizeof(string_to_char));
    //display.setLetterSpacing(1);
    display.set2X();
    display.setCursor(64 - (display.strWidth(string_to_char) / 2), 3);
    display.print(string_to_char);
    //end of the Content

    footer.toCharArray(string_to_char, sizeof(string_to_char));    
    display.set1X();
    display.setCursor(0,6);
    display.setLetterSpacing(0);
    display.print("----------------------------");
    display.setCursor(64 - (display.strWidth(string_to_char) / 2), 7);
    display.print(string_to_char);
  }
}

void set_state_LEDs(timer_state_e state, boolean warn){
  //Serial.print("led state is: ");
  //Serial.println(state);
  // set the LEDS corresponding to the state of the timer ... as long as the system is not waiting for input ...  
  if(TIMER_WAIT != state){
    leds_states = LEDStates[state];
  }
  // loop over all the LEDs and set state ... 
  for(uint8_t led = 0; led<NO_LAST_LED;led++){
    // warn is special so far - there is no special state for the warn led . Handle if warn flag is set - switch LED on in this case ...   
    if((WARN_LED == led)&&(true==warn)){
      digitalWrite(LEDPins[led], HIGH);
    } else {
      switch(leds_states[led]){
        case LED_BLINK:
          digitalWrite(LEDPins[led], blink_on);
          break;
        case LED_OFF:
          digitalWrite(LEDPins[led], LOW);
          break;
        case LED_ON:
          digitalWrite(LEDPins[led], HIGH);
          break;
      }
    }
  }
}

void failSequence(void){
  // first tone
  if(failsequence_count <FAILSEQ_STEPS){
    // play the tone ...  
    tone( PIEZO_PIN, FAILSEQ_NOTES[failsequence_count],FAILSEQ_DURATION_MS[failsequence_count] );
    if(failsequence_count > 0){
      Timer1.setPeriod(FAILSEQ_PAUSE_US[failsequence_count]);
    }
    // increase the counter
    failsequence_count++;
  } else {
    // set the done bit and stop and detache the timer1
    Timer1.detachInterrupt(); 
    failsequence_done = true;
    noTone(PIEZO_PIN);   
  }
}

void stop_isr(void){
  // this is the interrupt routine for the topstation STOP button
  // this will save the time when the runner has pushed the button
  if(timer_state == TIMER_IDLE){
    radio_data.topbuttonpressedtime_us = micros();
    //Serial.print(radio_data.topbuttonpressedtime);
    //Serial.println(" ms <- current time ** stop_ISR ** stop button pushed: ");
    topbuttonwaspressed = true;   
  }
}

void false_start_isr(void) {
  // this is the interrupt routine for the FALSESTART button
  // this will save the time when the runner is really started
  if(timer_new_state != TIMER_READY){
    runner_start_time_us = micros();
    //Serial.print(runner_start_time);
    //Serial.println(" ms <- current time ** false_start_ISR ** false start button released: ");
    if(false == startsequence_done){
      false_start = true;
      //Serial.print(start_time);
      //Serial.println(" <- starttime ** Interrupt service routine detected false_start. **");
    }
    detachInterrupt(digitalPinToInterrupt(BUTTONPins[BUTTON_FAIL]));
  }
}

void start_isr(void){
  // this is the timer interrupt routine that is called during the startsequence
  //Serial.print(millis());
  //Serial.print(" <- current time ; startsquence count -> ");
  //Serial.println(startsequence_count);
  if(false == false_start){
    if(startsequence_count < STARTSEQ_STEPS){
      // (re)init the interrupt timer ... 
      if(startsequence_count > 0){
        // play the tone ...  
        //Serial.println(STARTSEQ_DURATION[startsequence_count]);
        tone( PIEZO_PIN, STARTSEQ_NOTES[startsequence_count],STARTSEQ_DURATION_MS[startsequence_count] );         
        Timer1.setPeriod(STARTSEQ_PAUSE_US[startsequence_count]);
      }
      // increase the counter
      startsequence_count++;
    } else {
      // set the done bit and stop and detache the timer1
      Timer1.detachInterrupt();
      //Serial.print(millis());
      //Serial.println("ms: set startsequence done to true");
      startsequence_done = true;
    }
  } else {
    Timer1.detachInterrupt(); 
  }
}

void send_values(void){
  if((((micros()-radio_data.topstationtime_us) > MIN_DELAY_BETWEEN_SEND_US) && (offset_sync_sequence || topbuttonwaspressed)) || ((micros()-radio_data.topstationtime_us) >= MAX_DELAY_BETWEEN_SEND_US)){
    // store current millis to be send as reference ...
    radio_data.topstationtime_us = micros();                       // set the current micro second count   
    //Serial.print("senddate_to_base at:");
    //Serial.println(millis());
    //Serial.print(" -> topstationtime:");
    //Serial.print(radio_data.topstationtime_us);
    //Serial.print("us  stoppressedtime:");
    //Serial.print(radio_data.topbuttonpressedtime_us);
    //Serial.print("us  offset counter value :");
    //Serial.println(counter_time_offset);
  
    // send data ...                                       
    if (!radio.write(&radio_data,sizeof(radio_data))){                     // Send the counter variable to the other radio 
      if(((millis() - connection_last_established_at_ms) >= (CONN_TIMEOUT_MS-100))){ 
        connection_available = false;
        //Serial.println("Failed to send data to BASESSTATION ... will retry");
      } else {
        if(offset_sync_sequence){
          if(counter_time_offset > 0){
            counter_time_offset--;
          }
        }
      }
    }  
    else
    {
      //Serial.print("Data sent to BASESSTATION");   
      connection_last_established_at_ms = millis();
      connection_available = true;
      // check offset sync counter ...
      if(counter_time_offset < (2*REQUIRED_NUMBER_MEANVALS)){
        counter_time_offset++;          
      } else {
        // offset sync done
        offset_sync_sequence = false;
      }
        
    }
  }   
}

String micros2string(signed long microsecs){
  char       buf[32] = "";
  String bufstring; 
  sprintf(buf, "%032d", microsecs);
  sprintf(buf, "%c%c:%c%c%c", buf[24],buf[25],buf[26],buf[27],buf[28]);
  bufstring = (String)buf;
  return bufstring;
}

