#include <Arduino.h>
#include <U8g2lib.h>
//#include "fonts.h"  
#include <Wire.h>
#include "SSD1306Ascii.h"
#include "SSD1306AsciiWire.h"
#include "RF24.h"
#include "speedclock.h"
#include "pitch.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 counter_time_offset = 0;        // number of used values for the mean value calculation
signed long sum_time_offset = 0;            // sum of offset values
signed long mean_time_offset = 0;           // mean value for the offset 
signed long current_time_offset = 0;        // current offset ... 
signed long running_time_offset = 0;        // offset that will be used for this run ... 
unsigned long start_time = 0;                 // if the timer is running this is that start time ...
unsigned long runner_start_time = 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
signed long runner_run_time = 0;              // this is the time the runner really needed or the time started to early - depending on sign ... 
unsigned long run_time = 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
unsigned long connection_established = 0;

boolean topbuttonwaspressed = false;


timer_state_e timer_state = TIMER_WAIT;       // timer needs to be initialized ... 
timer_state_e timer_new_state = TIMER_INIT;       // timer needs to be initialized ... 


transcv_s radio_data;

void setup(){

  Serial.begin(115200);

  // this is the top button - will be pressed by the speed climber as soon she/he reaches the top ...
  pinMode(STOPBUTTON_IN, INPUT_PULLUP); 
  pinMode(STARTBUTTON_IN, INPUT_PULLUP);
  pinMode(CANCELBUTTON_IN, INPUT_PULLUP);
  pinMode(FAILSTARTBUTTON_IN, INPUT_PULLUP);

  pinMode(WARN_LED, OUTPUT);
  pinMode(FAIL_LED, OUTPUT);
  pinMode(READY_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);
  Serial.print(F(" The station select[1,0] pins (pin "));
  Serial.print(STATION_SEL0);
  Serial.print(F(","));
  Serial.print(STATION_SEL1);
  Serial.print(F(") are set to level: '"));
  Serial.print(radio_sel0);
  Serial.print(radio_sel1);
  Serial.println("'");

  if((radio_sel0 == 1) & (radio_sel1 == 0)){
    stationNumber = TOPSTATION; 
    Serial.println(F("The level of the station select pin makes the current node set to the TOPSTATION."));
  }
  else{
    Serial.println(F("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(15, 15);               //the first is the time between reties in multiple of 250ms, the second is the numer of attempts
  if(stationNumber == TOPSTATION){
    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 = millis();         // set the current milli second count  
  radio_data.topbuttonpressedtime = 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) {

  
/****************** 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.
    
    // check for pressed button ...  
    if(topbuttonwaspressed == false){
      if( (millis() - radio_data.topbuttonpressedtime) > MIN_DELAY_BETWEEN_PRESSED_MS){
        // ignore if the button was "pressed" a few millis before - this is keybouncing and would give a false result and if the button is pressed for a longer time that would effect the time as well
        if(digitalRead(STOPBUTTON_IN) == STOPBUTTON_PRESSED){
          // button was pressed - store the time 
          radio_data.topbuttonpressedtime = millis();
          topbuttonwaspressed = true;
          Serial.print("Stopp button was pressed at:");
          Serial.println(radio_data.topbuttonpressedtime);
          digitalWrite(RUN_LED, RUN_LED_ON);
        }
      }
    } else {
      if(digitalRead(STOPBUTTON_IN) != STOPBUTTON_PRESSED){
        topbuttonwaspressed = false;
        digitalWrite(RUN_LED, RUN_LED_OFF);
      }
    }
    
    // if the button was not pressed only each few second data will be send to BASESTATION ... 
    if(topbuttonwaspressed || ((millis()-radio_data.topstationtime) >= MIN_DELAY_BETWEEN_SEND_MS)){
      // store current millis to be send as reference ...
      radio_data.topstationtime = millis();                       // set the current milli second count   
      Serial.print(millis());
      Serial.print("ms - Send data to bottom station: topstationtime: ");
      Serial.print( radio_data.topstationtime );
      Serial.print(" stoppressedtime: ");
      Serial.print( radio_data.topbuttonpressedtime );
      if(topbuttonwaspressed)
      {
        Serial.println(" .Stopp button was pressed.");
      }
      else {
        Serial.println(" .Stopp button was NOT pressed.");
      }

      // send data ...                                       
      if (!radio.write(&radio_data,sizeof(radio_data) )){                     // Send the counter variable to the other radio 
        Serial.println(F("Failed to send data to BASESSTATION ... will retry"));
        digitalWrite(FAIL_LED, FAIL_LED_ON);
        digitalWrite(READY_LED, READY_LED_OFF);
      }  
      else
      {
        Serial.println("Data sent to BASESSTATION");   
        digitalWrite(FAIL_LED, FAIL_LED_OFF);
        digitalWrite(READY_LED, READY_LED_ON);
      }
       
    }
    
  }


/****************** Code for the BASESTATION is here - the display and the start button is connected here. All caclulation will be done here ***************************/

  if ( stationNumber == BASESTATION ) {
    byte pipeNo;
    // read data from TOP_STATION ... 
    if( radio.available()){
        // check if radio data is available - if so read the data
        connection_established = millis();
        while( radio.available(&pipeNo)){              // Read all available payloads
          radio.read( &radio_data, sizeof(radio_data) );                                  // Read the data the TOPSTATION sent
        } 
        current_time_offset = radio_data.topstationtime - millis();                             // the offset between TOP_STATION and BASESTATION 
        Serial.print("Current time on host in millis:");
        Serial.print(millis());
        Serial.print(F(" Current time on client in millis: "));
        Serial.println(radio_data.topstationtime);
        Serial.print("Offset is: ");
        Serial.println(current_time_offset);
        Serial.print(F(" Button was pressed last time on client in millis: "));
        Serial.println(radio_data.topbuttonpressedtime); 
    }

    // offset calculation ... only needed if the variation is bigger than allowed or not enough values available already ...
    // check current offset of the TOP_STATIOn and the BASESTATION if more than allowed ... 
    if(counter_time_offset == 0){
      mean_time_offset = current_time_offset;
    }
    if(abs(current_time_offset - mean_time_offset) < MAX_DIFFERENCE_OFFSET_MS){
      // 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 ... 
        sum_time_offset = sum_time_offset + current_time_offset;
        counter_time_offset++;
        mean_time_offset = sum_time_offset/counter_time_offset;
        Serial.print(F("Offset calulation. We already have "));
        Serial.print(counter_time_offset);
        Serial.print(F(" of "));
        Serial.print(REQUIRED_NUMBER_MEANVALS);
        Serial.print(F(" values used for offset calculation. Mean value of offset based on that is: "));
        Serial.println(mean_time_offset);
      }
    } else {
      // the current offset is out of range so we need to restart the mean calculation and set the timer to unready state ...
      Serial.print("Difference current offset is ");
      Serial.println( abs(current_time_offset - mean_time_offset) );
      Serial.print(F("Will restart offset calculation because the variation of the current offset: "));
      Serial.print(current_time_offset);
      Serial.print(F(" is more than the allowed: "));
      Serial.print(MAX_DIFFERENCE_OFFSET_MS);
      Serial.print(F(" compared to the mean offset: "));
      Serial.println(mean_time_offset);
      counter_time_offset = 0;
      sum_time_offset = 0;
      mean_time_offset = 0;
    } 

    // set state to new_state
    if(timer_state != timer_new_state){
      update_statemessage(timer_new_state);    
    }

    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_INIT:
        // check if we are ready ... 
        if(counter_time_offset > REQUIRED_NUMBER_MEANVALS){
          // 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(counter_time_offset < REQUIRED_NUMBER_MEANVALS){
          // check if offset is OK - if not .. set state back to INIT
          timer_new_state = TIMER_INIT;
        }
        else{
          // check if the FALSESTATE button is pressed - somebody is ready to run ... 
          if(digitalRead(FAILSTARTBUTTON_IN) == FAILSTARTBUTTON_PRESSED){
            //wait a few milliseconds to prevent keybouncing - this is a very simplistic method here
            delay(10);
            //read again and check if still active ... 
            if(digitalRead(FAILSTARTBUTTON_IN) == FAILSTARTBUTTON_PRESSED){
              timer_new_state = TIMER_READY;
            }
          }
        }
        break;
      case TIMER_READY:
        if(digitalRead(FAILSTARTBUTTON_IN) == FAILSTARTBUTTON_PRESSED){
          // check if the start button was pressed ... there is at least still someone waiting for the run .
          if(digitalRead(STARTBUTTON_IN) == STARTBUTTON_PRESSED){
            // now enable the interrupt for the FALSESTART button
            attachInterrupt(digitalPinToInterrupt(FAILSTARTBUTTON_IN), false_start_isr, CHANGE);
            timer_new_state = TIMER_STARTED;
          }
        }
        else{
          timer_new_state = TIMER_IDLE;        
        }
        break;
      case TIMER_STARTED:
        //initialize the start countdown
        timer_new_state = TIMER_RUNNING;
        startSequence();
        break;        
      case TIMER_RUNNING: 
        noTone(PIEZO_PIN);
        if(counter_time_offset < REQUIRED_NUMBER_MEANVALS){
          // check if offset is still OK - if not .. set state to TIMER_RUNNING
          warn_during_run = true;
        }
        if(millis() - start_time > TIMER_TIMEOUT){
          timer_new_state = TIMER_TIMEDOUT;
        }
        if(digitalRead(CANCELBUTTON_IN) == CANCELBUTTON_PRESSED){
          timer_new_state = TIMER_CANCELLED;
        }
        Serial.println(radio_data.topbuttonpressedtime);
        if(radio_data.topbuttonpressedtime > running_time_offset){
          if((radio_data.topbuttonpressedtime - running_time_offset) > start_time){
            run_time = (radio_data.topbuttonpressedtime - running_time_offset) - start_time;
            runner_run_time = runner_start_time - run_time;
            timer_new_state = TIMER_STOPPED;
          }
        }
        break;
      case TIMER_STOPPED:
        //calculate the run_time and switch to WAIT
        delay(10);
        if(digitalRead(CANCELBUTTON_IN) != CANCELBUTTON_PRESSED){
          timer_new_state = TIMER_WAIT;
        }
        break;
      case TIMER_FAIL:
        //fail start case .... 
        failSequence();
        run_time = 99999;
        runner_run_time = runner_start_time - start_time;
        delay(10);
        if(digitalRead(CANCELBUTTON_IN) != CANCELBUTTON_PRESSED){        
          timer_new_state = TIMER_WAIT;
        }
        break;        
      case TIMER_CANCELLED:
        // what to do in chancel mode ?
        run_time = 99999;
        runner_run_time = runner_start_time - start_time;
        delay(10);
        if(digitalRead(CANCELBUTTON_IN) != CANCELBUTTON_PRESSED){
          timer_new_state = TIMER_WAIT;
        }
        break;
      case TIMER_TIMEDOUT:
        // time out
        run_time = millis() - start_time;
        runner_run_time = runner_start_time - start_time;
        delay(10);
        if(digitalRead(CANCELBUTTON_IN) != CANCELBUTTON_PRESSED){
          timer_new_state = TIMER_WAIT;
        }
        break;
      case TIMER_WAIT:
        // disable interrupt if not already done
        detachInterrupt(digitalPinToInterrupt(FAILSTARTBUTTON_IN));
        // wait until the chancel button was pressed to go ahead
        if(digitalRead(CANCELBUTTON_IN) == CANCELBUTTON_PRESSED){
          timer_new_state = TIMER_IDLE;
        }
        break;
    }
  }
}


//####################### HELPER FUNCTIONS ###########################

void update_statemessage(timer_state_e timer_state){
  switch(timer_state){
    case TIMER_INIT:
      Serial.println("*** TIMER_INIT ***");
      break;
    case TIMER_IDLE:
      Serial.println("*** TIMER_IDLE ***");
      break;
    case TIMER_READY:
      Serial.println("*** TIMER_READY ***");
      break;
    case TIMER_STARTED:
      Serial.println("*** TIMER_STARTED ***");
      break;
    case TIMER_RUNNING:
      Serial.println("*** TIMER_RUNNING ***");
      break;
    case TIMER_CANCELLED:
      Serial.println("*** TIMER_CANCELLED ***");
      break;
    case TIMER_STOPPED:
      Serial.println("*** TIMER_STOPPED ***");
      break;
    case TIMER_TIMEDOUT:
      Serial.println("*** TIMER_TIMEDOUT ***");
      break;
    case TIMER_FAIL:
      Serial.println("*** TIMER_FAIL ***");
      break;
    case TIMER_WAIT:
      Serial.println("*** TIMER_WAIT ***");
      break;
    default:
      break;
  }
}

void update_screen(timer_state_e state){
  bool scr_update = true;
  int ypos = 64-42/2;
  String header = "no state";
  String content = "";
  String footer = "";
  
  char header_to_char[50];
  char content_to_char[50];
  char footer_to_char[50];
  
  float curr_time_local = 0.0;

  switch(state){
    case TIMER_INIT:
      header = "Init";
      content = "...";
      footer = "please wait";
      break;
    case TIMER_IDLE:
      header = "Idle!";
      content = "00.00 sec";
      footer = "Waiting for climber";
      break;
    case TIMER_READY:
      header = "Ready!";
      content = "00.00 sec";
      footer = "Waiting for start";
      break;
    case TIMER_STARTED:
      header = "Starting ...";
      content = "00.00 sec";
      footer = "...";
      break;
    case TIMER_RUNNING:
      header = "Running ...";
      curr_time_local = (millis() - start_time)/1000.000;
      content = curr_time_local;
      content += " sec";
      curr_time_local = (runner_start_time - start_time)/1000.000;
      footer = "Reaction time: ";
      footer += curr_time_local;
      footer += " sec";
      break;
    case TIMER_CANCELLED:
      header = "Cancelled!";
      break;
    case TIMER_STOPPED:
      header = "Stopped!";
      curr_time_local = run_time/1000.000;
      content = curr_time_local;
      content += " sec";
      curr_time_local = (runner_start_time - start_time)/1000.000;
      footer = "Reaction time: ";
      footer += curr_time_local;
      footer += " sec";
      break;
    case TIMER_TIMEDOUT:
      header = "Timed out!";
      content = "Invalid!";
      break;
    case TIMER_FAIL:
      header = "False start!";
      curr_time_local = (start_time - runner_start_time)/1000.000;
      content = curr_time_local;
      footer = "seconds too early";
      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(header_to_char, sizeof(header_to_char));
    content.toCharArray(content_to_char, sizeof(content_to_char));
    footer.toCharArray(footer_to_char, sizeof(footer_to_char));
    //Serial.print("DISPLAY: ");    
    //Serial.println(string_to_char); 
    
    display.setFont(System5x7);
    display.set1X();
    int xpos = (128 - (display.strWidth(header_to_char)))/2 - 10;
    display.home();
    display.setLetterSpacing(1);
    display.setCursor(64 - (display.strWidth(header_to_char) / 2), 0);
    display.print(header_to_char);
    display.setCursor(1,0);
    if(millis() - connection_established >= CONN_TIMEOUT || connection_established == 0){
      display.print("  ");
    }
    else {
      display.print("Y");
    }
    display.setCursor(0,1);
    display.setLetterSpacing(0);
    display.print("----------------------------");
    
    //end of the Header
    //display.setLetterSpacing(1);
    display.set2X();
    display.setCursor(64 - (display.strWidth(content_to_char) / 2), 3);
    display.print(content_to_char);
    //end of the Content
    display.set1X();
    display.setCursor(0,6);
    display.setLetterSpacing(0);
    display.print("----------------------------");
    display.setCursor(64 - (display.strWidth(footer_to_char) / 2), 7);
    display.print(footer_to_char);
  }
}

void set_state_LEDs(timer_state_e state, boolean warn)
{
  // set the LEDS corresponding to the state of the timer ... as long as the system is not waiting for input ...  
  if(TIMER_WAIT != state){
    digitalWrite(READY_LED, LEDStates[state][0]);
    digitalWrite(RUN_LED, LEDStates[state][1]);
    digitalWrite(FAIL_LED, LEDStates[state][2]);

    if(warn == true){
      digitalWrite(WARN_LED, WARN_LED_ON);
    }
    else
    {
      digitalWrite(WARN_LED, WARN_LED_OFF);
    }
  }
}

void startSequence(void)
{
  // set the startime - this is the current time plus the length of this sequence
  running_time_offset = mean_time_offset;
  start_time = millis() + STARTSEQ_LENGTH_MS;
  Serial.print("Start time is: ");
  Serial.println(start_time);
  // this is sequence of usually three tones after a wait time 1sec , in between the tones there is also a delay of 1 sec. Each tone is 200ms seconds long, except the last 
  update_statemessage(timer_new_state);
  if(timer_new_state == TIMER_RUNNING){
    wait(STARTSEQ_STARTPAUSE_MS);
  }
  // first tone
  update_statemessage(timer_new_state);
  if(timer_new_state == TIMER_RUNNING){  
    tone(PIEZO_PIN, STARTSEQ_TON_1_2_FREQUENCY,STARTSEQ_TON_1_2_LENGTH_MS );
    wait(STARTSEQ_TONEPAUSE_MS);
  }
  //second tone
  update_statemessage(timer_new_state);
  if(timer_new_state == TIMER_RUNNING){
    tone(PIEZO_PIN, STARTSEQ_TON_1_2_FREQUENCY,STARTSEQ_TON_1_2_LENGTH_MS );
    wait(STARTSEQ_TONEPAUSE_MS);
  }
  //third tone
  update_statemessage(timer_new_state);
  if(timer_new_state == TIMER_RUNNING){
    tone(PIEZO_PIN, STARTSEQ_TON_3_FREQUENCY,STARTSEQ_TON_3_LENGTH_MS );
    wait(STARTSEQ_TON_3_LENGTH_MS);
  }
}

void failSequence(void)
{
  // first tone
  tone(PIEZO_PIN, FAILSEQ_TON_FREQUENCY,FAILSEQ_TON_LENGTH_MS );
  delay(FAILSEQ_TONEPAUSE_MS);
  //second tone
  tone(PIEZO_PIN, FAILSEQ_TON_FREQUENCY,FAILSEQ_TON_LENGTH_MS );
  delay(FAILSEQ_TONEPAUSE_MS);
  noTone(PIEZO_PIN);  
}

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
  Serial.println("** Interrupt service routine started: false_start_ISR **");
  runner_start_time = millis();
  if((timer_state == TIMER_STARTED) & (timer_new_state == TIMER_RUNNING)){
    timer_new_state = TIMER_FAIL;
    noTone(PIEZO_PIN);
    Serial.println("** Interrupt service routine detected false_start. Set new state to TIMER_FAIL **");
    update_statemessage(timer_new_state);
    detachInterrupt(digitalPinToInterrupt(FAILSTARTBUTTON_IN));
  } else {
    if((timer_state == TIMER_RUNNING) | (timer_new_state == TIMER_RUNNING) ){
      // disable this interrupt;
      detachInterrupt(digitalPinToInterrupt(FAILSTARTBUTTON_IN));
    }
  }
}

void wait(unsigned long ms){
   unsigned long current = 0;
   unsigned long started = millis();
   do{
    current = millis()-started;
   }while(current < ms);
}