#include <Arduino.h>
#include <U8g2lib.h>
//#include "fonts.h"  
#include <SPI.h>
#include "RF24.h"
#include "speedclock.h"
#include "pitch.h"

// internal defines for the OLED display ...
U8G2_SSD1306_128X64_NONAME_1_SW_I2C display(U8G2_R0, /* clock=*/ SCL, /* data=*/ SDA, /* reset=*/ U8X8_PIN_NONE);   // All Boards without Reset of the Display
//U8G2_SSD1306_128X64_NONAME_1_HW_I2C display(U8G2_R0,/* reset=*/ U8X8_PIN_NONE);   // All Boards without Reset of the 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
unsigned long sum_time_offset = 0;            // sum of offset values
unsigned long mean_time_offset = 0;           // mean value for the offset 
unsigned long current_time_offset = 0;        // current offset ... 
unsigned 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

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.print(F("The level of the station select pin makes the current node set to the TOPSTATION."));
  }
  else{
    Serial.print(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();                       // Start listening  

  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 OLED and display Welcome Message ...
  display.begin();
  
}

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.
    bool topbuttonwaspressed = false;
    // check for pressed button ...  
    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;
      }
    }

    // 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   

      // 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"));
      }      
    }
    
  }


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

  if ( stationNumber == BASESTATION ) {

    // read data from TOP_STATION ... 
    if( radio.available()){
        // check if radio data is available - if so read the data
        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(F(" Current time on client in millis: "));
        Serial.println(radio_data.topstationtime);
        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(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(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_statemassage(timer_new_state);    
    }
    timer_state = timer_new_state;

    // set LEDs
    set_state_LEDs(timer_state, warn_during_run );

    switch(timer_state){
      case TIMER_INIT:
        update_screen(timer_state);
        // 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_READY;
        }
         break;
      case TIMER_READY:
        update_screen(timer_state);
        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(300);
            //read again and check if still active ... 
            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;
              }
            }
          }
        }
        break;
      case TIMER_STARTED:
        update_screen(timer_state);
        //initialize the start countdown
        timer_new_state = TIMER_RUNNING;
        startSequence();
        break;        
      case TIMER_RUNNING: 
        noTone(PIEZO_PIN);
        update_screen(timer_state);         
        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;
        }
        if((radio_data.topbuttonpressedtime - running_time_offset) > millis()){
          timer_new_state = TIMER_STOPPED;
        }
        break;
      case TIMER_STOPPED:
        //calculate the run_time and switch to WAIT
        run_time = (radio_data.topbuttonpressedtime - running_time_offset) - start_time;
        runner_run_time = runner_start_time - run_time;
        update_screen(timer_state);
        timer_new_state = TIMER_WAIT;
        break;
      case TIMER_FAIL:
        //fail start case .... 
        failSequence();
        run_time = 99999;
        runner_run_time = runner_start_time - start_time;
        update_screen(timer_state);
        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;
        update_screen(timer_state);
        timer_new_state = TIMER_WAIT;
        break;
      case TIMER_TIMEDOUT:
        // time out
        run_time = millis() - start_time;
        runner_run_time = runner_start_time - start_time;
        update_screen(timer_state);             
        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_READY;
        }
        break;
    }
  }
}


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

void update_statemassage(timer_state_e timer_state){
  switch(timer_state){
    case TIMER_INIT:
      Serial.println("*** TIMER_INIT ***");
      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 timer_state){
  bool scr_update = true;
  int ypos = 64-42/2;
  String top_line = "no state";
  char string_to_char[50];

  switch(timer_state){
    case TIMER_INIT:
      top_line = "Init";
      break;
    case TIMER_READY:
      top_line = "Ready!";
      break;
    case TIMER_STARTED:
      top_line = "Started ...";
      break;
    case TIMER_RUNNING:
      top_line = "Running ...";
      break;
    case TIMER_CANCELLED:
      top_line = "Cancelled!";
      break;
    case TIMER_STOPPED:
      top_line = "Stopped!";
      break;
    case TIMER_TIMEDOUT:
      top_line = "Time out!";
      break;
    case TIMER_FAIL:
      top_line = "False start!";
      break;
    default:
      scr_update = false;
      break;
  }

  if(scr_update == true){
    //snprintf( string_to_char, sizeof(string_to_char),"%s", top_line);
    top_line.toCharArray(string_to_char, sizeof(string_to_char));
    //Serial.print("DISPLAY: ");    
    //Serial.println(string_to_char); 
    display.setFontPosCenter();
    display.setFont(u8g2_font_ncenB08_tr);
    int xpos = (128 - (display.getStrWidth(string_to_char)))/2 - 10;
        
    display.firstPage();
    do {
      display.setFont(u8g2_font_ncenB08_tr);
      display.setCursor(xpos,ypos);
      display.print(string_to_char);
    } while ( display.nextPage() );
  }
}

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 
  if(timer_new_state == TIMER_RUNNING){
    delay(STARTSEQ_STARTPAUSE_MS);
  }
  // first tone
  if(timer_new_state == TIMER_RUNNING){  
    tone(PIEZO_PIN, STARTSEQ_TON_1_2_FREQUENCY,STARTSEQ_TON_1_2_LENGTH_MS );
    delay(STARTSEQ_TONEPAUSE_MS);
  }
  //second tone
  if(timer_new_state == TIMER_RUNNING){
    tone(PIEZO_PIN, STARTSEQ_TON_1_2_FREQUENCY,STARTSEQ_TON_1_2_LENGTH_MS );
    delay(STARTSEQ_TONEPAUSE_MS);
  }
  //third tone
  if(timer_new_state == TIMER_RUNNING){
    tone(PIEZO_PIN, STARTSEQ_TON_3_FREQUENCY,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_STARTED){
    timer_new_state = TIMER_FAIL;
    detachInterrupt(digitalPinToInterrupt(FAILSTARTBUTTON_IN));
    noTone(PIEZO_PIN);
  } else {
    if(timer_state == TIMER_RUNNING | timer_new_state == TIMER_RUNNING ){
      // disable this interrupt;
      detachInterrupt(digitalPinToInterrupt(FAILSTARTBUTTON_IN));
    }
  }
}