// Modified from ROCKBLOCK_MENGES_REV5 and barebones_ROCKBLOCK_REV3G

/*
 RockBLOCK modem code for use with and Adafruit Trinket (Arduino clone)
 https://www.sparkfun.com/products/13745
 https://www.adafruit.com/product/2010
 https://www.adafruit.com/product/1137
 https://www.adafruit.com/product/2900

 Irridium SBD libraries available here:
 https://github.com/mikalhart/IridiumSBD/releases/tag/v1.0

 Please acknowlege Hans Huth and Sean Keane if you use/modify this code
*/

// Libraries

#include <IridiumSBD.h>
#include <SoftwareSerial.h>
#include <SSD1306Ascii.h>       // for OLED display
#include <SSD1306AsciiAvrI2c.h> // for OLED display

// Display Settings
  #define OLEDScanner 11      // determines power state of OLED
  #define I2C_ADDRESS 0x3C
  SSD1306AsciiAvrI2c oled;
  
// Rockblock settings

#define STATUS 13             // onboard pin to determine if modem engaged.
#define ROCKBLOCK_RX_PIN A1   // Recieve data pin from Rockblock (seial data from RockBLOCK)
#define ROCKBLOCK_TX_PIN A2   // Transmit data pin to Rockblock (serial data to RockBLOCK)
#define ROCKBLOCK_SLEEP_PIN 4 // on/off pin for power savings
#define ROCKBLOCK_BAUD 19200  // serial modem communication baud rate
#define CONSOLE_BAUD 115200   // serial terminal communications baud rate
#define DIAGNOSTICS true      // Set "true" to see serial diagnostics

// Timer Variables

unsigned long resetLoopTime = 0;  // Resests the loop timer back to 0 so it can be compared with telemInterval each loop
unsigned long loopStartTime = 0; // Used to register start time at top of loop()
unsigned long telemInterval = 1800000; // status reporting interval (24hours where 1000 = 1 second) **Can be modified remotely
unsigned long pollingInterval = 60000;  // (60 secs) interval that rangeFind takes readings for change detect **Can be modifed remotely

SoftwareSerial ssIridium(ROCKBLOCK_RX_PIN, ROCKBLOCK_TX_PIN);  // type Arduino Stream
IridiumSBD isbd(ssIridium, ROCKBLOCK_SLEEP_PIN);               // this is my RockBLOCK

// Rangefinder pins/variables

 #define RangeTrig 6    // Attach to pin 4 on range sensor
 #define RangePin A3    // Attach to pin 3 on range sensor

// Ultrasonic variables
int8_t arraysize = 9;   // quanitity of values for median (odd numbers)
uint16_t rangevalue[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0}; // group array for 9 ultrasonic values to calculate median/mode
uint16_t mode;          // calculated mode distance (value that occurs most often) (cm or volts)
uint16_t modeOld;       // used to store old rangeFind values if needed (i.e. if there is a change) (cm or volts)
int change;             // used to measure change. INITALLY SET TO 10 SO THE LOOP WILL RUN WHEN INITALLY SET UP
                        // GETS SET TO ACTUAL CHANGE IN mode AND modeOld DURING FIRST LOOP.   
int changeCond = 10;    // Conditional change value to start transmission, can be changed through message retrival. **Can be modifed remotly
String myHeight;        // string for adding stage field to myUrl
String myChange;        // string for adding change field to myURL

// String and character array for posting to RockBLOCK server
String myUrl;           // String where all fields will be captured and posted
char url[256];          // for passing to modem stream
char url_old [256];     // If telemetry failed, this array stores the old value 
                        // for telemetry retry while ultraonic measures a new value

// Message Retrival Variables
uint8_t buffer[200];    // buffer for saving messages.
size_t bufferSize = sizeof(buffer); // Determines the size of the buffer. This gets set to 0 if no message is recieved. 
                                    // or if the message takes to long to download.

// Variables for battery voltage
long myVcc = 0.00;
String myVolts = "";

// Variables to determine message status
bool timeout = false; // If telemetry times out, this becomes true.
int err;              // Error code status for Iridium processes.
byte Setup = 1;       // Allows the loop to run immediatly for setup

//Variables to modify conditions through SMS
long varChange; // Variable for saving recieved message data and saving it to global variables. 
int n = 25; // Variable for setting adjustSendReceivetimeout or how long a message sending attempt will last.

void setup()
{

 // Start the serial port at 115200 baud rate so that I can 
 // see output on the a terminal from modem initiation 
 Serial.begin(CONSOLE_BAUD);
 Serial.println("REM Setup");
  
 // OLED Button pins
 pinMode(OLEDScanner, INPUT);
 
 // LED on 13 for status on modem communication
 pinMode(STATUS, OUTPUT); 

 // Sleep pin on modem
 pinMode(ROCKBLOCK_SLEEP_PIN, OUTPUT);

 // Ultrasonic pins
 pinMode(RangeTrig, OUTPUT);   // Pin for triggering sensor                     
 pinMode(RangePin, INPUT);     // Pin for reading sensor (NOTE: analog is input by default)     
 digitalWrite(RangeTrig, LOW); // Pin on sensor is pulled high internally,
                               // sensor will stop ranging when low (power savings)
 
 // Setup the RockBLOCK

 isbd.adjustSendReceiveTimeout(n); // This deterimnes how long RockBLOCK will try to send message before timeout
                                   // Here, set to 25 seconds per attempt for timeout
  
 isbd.setPowerProfile(0); // Since we are running off battery in field, set to 0 
                          // Available power settings: 
                          // 1 = low current  (90 mA USB; 60 secs between transmit retries)
                          // 0 = high current (high current battery; 20 secs between transmit retries)
 
 rangeFind();

 modeOld = mode; // Set old rangefinder value to current in the event transmission doesn't work

 if (digitalRead(OLEDScanner) == 1) { // Check that the OLED is on
  oledReset();     
  oled.println(F("REM SETUP:"));
  oled.println(F("Lets test connection"));
  oled.print(F("Stage Level= "));
  oled.print(mode);
  oled.print(F(" cm"));
  delay(2000);  // Wait seconds so the user can read the message
 }
}

void loop() {
  
 Serial.println(F("In Loop"));
 if (digitalRead(OLEDScanner) == 1) {  //Check to see if OLED is on before sending data
  oledReset(); 
  oled.print(F("Stage Level="));
  oled.print(mode);
  oled.println(F(" cm"));

  if(Setup == 1) { // Setup is equal to 1 the first run through the loop. It will be set to 1 if the first message is sent
   oled.println(F("Sending Message to"));
   oled.println(F("Test Connection"));
  }
  else { // This message prints after setup is complete to remind the user to shut off the OLED before leaving
   oled.println(F("Power off OLED"));
   oled.println(F("before leaving site"));
  }
  delay(5000); // Allow the user to read this message
 }

 loopStartTime = millis(); // Setting loop start time.
                           // millis() returns the number of milliseconds since the start of current program.

 rangeFind();              // Determines rangefinder value
 change = mode-modeOld;    // Calculate change in rangeFind height based on last reading (cm) (Will run initally because modeOld is initally 0.)
 Serial.print("Interval is "); Serial.println(telemInterval); 
 Serial.print("Change condition = "); Serial.println(changeCond);
 Serial.print("Polling Interval = "); Serial.println(pollingInterval);

 // Uncomment (abs(change) >= changeCond) to have message sent if changeCond is exceeded

 if (/*abs(change) >= changeCond ||*/ ((loopStartTime-resetLoopTime) >= telemInterval) || Setup == 1) {
  // If I made it here, I'm setting things up for first telemetry test
  // OR my interval for readings has been exceeded - time to report status
  // OR my change condition has been exceeded (assuming respective code has been uncommented)
  resetLoopTime = loopStartTime; // reset the timer 
  if (digitalRead(OLEDScanner) == 1) { 
   oledReset();  //Start the OLED
   oled.println(F("In the Loop"));
   oled.println(F("Gathering Data"));
   delay(2000);
  }

 if(timeout == true) // if message failed to send during last attempt, increase the attempt to send interval.
  {
   n +=25; // If message doesnt send add 25 seconds to timeout.
   if(n >= 300) {
    n = 300;  // never allow n to get over the default value of 300 seconds or 5 mins
   }
  isbd.adjustSendReceiveTimeout(n);
  }
  
  Serial.println(F("Transmit")); // Conditions met - start the transmission sequence

  // Start the serial port
  ssIridium.begin(ROCKBLOCK_BAUD);
  
  // Start talking to RockBLOCK
  Serial.println("Beginning to talk to the RockBLOCK...");
  ssIridium.listen();

  if (digitalRead(OLEDScanner) == 1) {
   oledReset();
   oled.println(F("Talking to RockBlock"));
   oled.println(F("This may take a min"));
  }
    
  // Try sending a text if awoken okay
  if (isbd.begin() == ISBD_SUCCESS) { 
   digitalWrite(STATUS, HIGH); // modem awoke okay    
   isbd.useMSSTMWorkaround(false); //isbd.begin() causes this to be set true.
   // To prevent lockout keep it set to false.
     
   if (digitalRead(OLEDScanner) == 1) {
    oledReset();
    oled.print(F("RockBlock Turned on"));
    delay(2000);
   }
   // Monitoring functions
   
   myVcc = readVcc();    // returns REM battery voltage
   
   // varialbes to String  
   myChange = String(change); 
   myHeight = String(mode);
    
   // battery
   myVolts = String(myVcc);

   // build post string
   myUrl = "," + myVolts + "," + myHeight + "," + myChange + ",0"; // 0 required as terminator byte for RockBLOCK servers

   if (digitalRead(OLEDScanner) == 1) {
    oledReset();
    oled.println(F("The String is:"));
    oled.println(F("volt, cm, change(cm)"));
    oled.println(myUrl);
    delay(5000);
   }
  
   myUrl.toCharArray(url,256);
   if(timeout == true) {
    messageRetry(); // Function for resending previous unsent message (only attempts resends one time)
   }


 /* Note whenever a message is sent the avaliable incoming messages are also downloaded. 
  *  If no message is in the inbox the buffer size (or sizeof(buffer)) = 0. 
  *  handleMessage() will only be called if a message was sucessfully downloaded.
  */
  
  // 35 corresponds to # so any data sent must start with "#" (e.g. #time8640000)
  
   if(buffer[0] == 35) { // Sending messages also downloads available messages
    Serial.print("telemInterval = "); Serial.println(telemInterval);
    Serial.print("Change cond = "); Serial.println(changeCond);
    Serial.print("Polling Interval = "); Serial.println(pollingInterval);

    for(int kk=1;kk<201; kk++) {
     Serial.println(buffer[kk]);
    }
    
    handleMessage();        // Function for converting recieved messages
    
    Serial.print("telemInterval = "); Serial.println(telemInterval);
    Serial.print("Reading rate = "); Serial.println(pollingInterval);
    Serial.print("Change cond = "); Serial.println(changeCond);
   }
 
   // Okay - let's send the most recent data
   if (digitalRead(OLEDScanner) == 1) {
    oledReset();
    oled.println(F("Trying to send"));
    oled.println(F("field data for a"));
    oled.println(F("channel update"));
   }
 
   timeout= false; // Reset timeout variable (if a previous timeout message was not sent do not try it again).
   
   adaptiveRetry();  // function for sending/recieving data to server including retries

   /* Again since a message was attempted to be sent it also checks if a message is in the inbox. 
    *  If a message was recieved the convert it and save the command.
    */

   if(buffer[0] == 35) {
    Serial.print("telemInterval = "); Serial.println(telemInterval);
    Serial.print("Change cond = "); Serial.println(changeCond);
    Serial.print("Reading rate = "); Serial.println(pollingInterval);
    Serial.print("buffer = "); for(int kk=1;kk<201; kk++){Serial.println(buffer[kk]);}
 
    handleMessage();

    Serial.print("telemInterval = "); Serial.println(telemInterval);
    Serial.print("Change cond = "); Serial.println(changeCond);
    Serial.print("Reading rate = "); Serial.println(pollingInterval);
   }

   // End incoming message handling 
   
   if (timeout == true){
    if (digitalRead(OLEDScanner) == 1) {
     oledReset();
     oled.println(F("Message timeouted"));
     oled.println(F("saving message"));
     oled.println(F("for retry later"));
     delay(5000);
    }       
    memcpy(url_old, url, 256); // Copies unsent message for retry next loop. (Erases old url_old, if one was saved).
   }
  
   else if (digitalRead(OLEDScanner) == 1 && timeout == false) { // Confirm message was sent
    oledReset();
    oled.println(F("Message Sent"));
    oled.println(F("Powering down modem"));
   }
   
   // All done - go to sleep 
   
   isbd.sleep();
   ssIridium.end();
   digitalWrite(STATUS, LOW); // modem asleep     

   modeOld = mode; // save old rangeFind value

   if ((Setup == 1 && timeout == false) && digitalRead(OLEDScanner) == 1) { 
   // If the first run through the loop is succesful, prompt user to turn off OLED and set Setup = 0    
    oled.clear();                             
    oled.println(F("Message Sent"));
    oled.println(F("Setup complete"));
    oled.println(F("Turn off OLED"));
    oled.println(F("With on/off switch"));
    Setup = 0; 
    delay(15000);
   }
   
   else if ((Setup == 1 && timeout == true)  && digitalRead(OLEDScanner) == 1) {
   // If the message wasn't sent during setup, the attempt will be tried in a few seconds, but prompt user to move antenna
    oled.clear(); 
    oled.println(F("Couldnt send message"));
    oled.println(F("Move Antenna"));
    oled.println(F("Restart in 30 sec"));
    delay(30000);
   }
  if(Setup == 1 && timeout == false)
   // make sure Setup is set to zero irrespective of state of OLED
   Setup = 0;
  }
 }
 else {
   // there hasn't been a signficant change OR I am still within timer interval
   Serial.println(F("Waiting for condition"));
   delay(pollingInterval); // check condition after polling interval 

   if(digitalRead(OLEDScanner) == 1) { 
    oled.clear();                             
    oled.println(F("Waiting for condition"));
    delay(15000);   
   } 
 }
} // end loop

///// FUNCTIONS /////

void rangeFind(){ 
  
 int16_t pulse;  // number of pulses from sensor
 int i=0;

 digitalWrite(RangeTrig, HIGH); 
  
 while( i < arraysize ) {                                 
  pulse = analogRead(RangePin);  // read in time for pin to transition
  rangevalue[i]=pulse;           // pulses to centimeters (use 147 for inches)
  if( rangevalue[i] < 725 && rangevalue[i] >= 10 ) 
   // value in range
   i++;  
   delay(10);                    // wait between samples
 }
 isort(rangevalue,arraysize);        // sort samples
 mode = getMyMode(rangevalue,arraysize);  // get mode

 digitalWrite(RangeTrig, LOW); 
 //print_range(); //Debuggin Function
}
 
// Sorting function (Author: Bill Gentles, Nov. 12, 2010)

void isort(uint16_t *a, int8_t n){
 for (int i = 1; i < n; ++i)  {
  uint16_t j = a[i];
  int k;
  for (k = i - 1; (k >= 0) && (j < a[k]); k--) {
   a[k + 1] = a[k];
  }
  a[k + 1] = j;
 }
}
 
// Mode function, returning the mode or median.
uint16_t getMyMode(uint16_t *x,int n){
 int i = 0;
 int count = 0;
 int maxCount = 0;
 uint16_t mode = 0;
 int bimodal;
 int prevCount = 0;

 while(i<(n-1)){
  prevCount=count;
  count=0;
  while( x[i]==x[i+1] ) {
   count++;
   i++;
  }
  if( count > prevCount & count > maxCount) {
   mode=x[i];
   maxCount=count;
   bimodal=0;
  }
  if( count == 0 ) {
   i++;
  }
  
  if( count == maxCount ) {      //If the dataset has 2 or more modes.
   bimodal=1;
  }
  
  if( mode==0 || bimodal==1 ) {  // Return the median if there is no mode.
      mode=x[(n/2)];
  }
  
  return mode;
 }
}


void adaptiveRetry() {
  
  // When a new message is sent, the modem attempts to recieve a message. 
  // If one is available, it gets saved to buffer and the size gets saved to buffersize.
  // buffersize has shown to be unreliable:
 
   /* 
      A message might send without a 0 code, so another sendRecieveSBD session is started.
      In this case, buffersize gets set to 0 even though buffer has a messaged saved in it. 
     
      To counteract this issue, all messages sent must start with a "#" and end with a "!" 
      or the program will not register it.
   */

  memset(buffer, 0, sizeof(buffer)); // Reset the buffer before a new message comes in 
                                     // or it will be added to the next open space in the buffer.
  
   // Details regarding this function for handling bad transmissions are avaialble here:
   // https://docs.rockblock.rock7.com/docs/adaptive-retry

  for(int i=0;i<5;i++) 
  { // consider adding OLED output here  
    Serial.print(F("Attempt# "));
    Serial.print(i+1);
    delay(5000);
    err = isbd.sendReceiveSBDText(url,buffer,bufferSize);
    Serial.print(F("Error Code = ")); Serial.println(err);
    // check status of sent message
    if (err != 0) { 
     // Latest attempt didn't work so echo status   
     Serial.println(F("sendSBDText failed."));
     Serial.println(F("Delaying before retry."));
     Serial.println(F("                      "));
     Serial.println(F("----------------------"));
     Serial.println(F("                      "));
     // Try again after specified delay
     if(i==0) 
      delay(random(0, 5000)); // 1st failure- trying second after random time 0-5 seconds
     else if(i<=2)
      delay(random(0, 30000)); // 2nd, 3rd failure- trying third, fourth after random time 0-30 seconds
     else if(i==3)
      delay(random(120000, 300000)); // 4th failure- trying fifth after random time between 120-300 seconds
     else if(i==4) {
      // If I'm here, the first five tries failed
      timeout = true;
      Serial.print(i+1);
      Serial.println(F(" attempts failed;"));  
      Serial.println(F("message not sent."));
      break;
     }
    }
   else
    // Message transmitted during first else, so
    // notificaiton will be provided in main loop.
    break;
  } // for loop closed
 
  // Since buffer would have been changed above if a message was recieved,
  // check that the first character was a # (Ascii value = 35).     
   
  if(buffer[0]==35){ 
  handleMessage(); // for incoming messages
 }
}

void messageRetry(){
 // If I'm here, timeout was set to true in prior iteration given failure after multiple tries,
 // so attempt to resend previous unsent post.
 if (digitalRead(OLEDScanner) == 1){
  oledReset();
  oled.clear();      
  oled.println(F("Sending old message"));
 }
 
 //Attempt to download any message while sending.  
 err = isbd.sendReceiveSBDText(url_old,buffer,bufferSize);  
    
 if (err != 0) {
  if (digitalRead(OLEDScanner) == 1){
   oledReset();
   oled.println(F("Old message not sent"));
   oled.println(F("trying one last time"));
  }
  isbd.sendReceiveSBDText(url_old,buffer,bufferSize);
  delay(15000); // Allow 15 seconds before thingspeak posts. 
 }
 else {
  delay(15000);
 }
}

/*
 * handleMessage() works by taking the buffer that the inbound message was saved in and initally reading the first four characters' 
 * ASCII values.  * These first four ASCII values are added together to form a command code. All the subsequent characters are read 
 * and they are turned into one value and saved into varChange. Using the unique command code a global variable is chosen and the 
 * value of varChange is saved inside to be used in the next loop cycles. Example: to change global variable for reporting interval 
 * (telemInterval) send in message #time600000 . The first four ASCII values for time add up to 116 + 105 + 109 + 101 = 431. 
 * The subsequent values [6,0,0,0,0,0] get converted from their ASCII values and saved as one number '600000'. This then changes the 
 * global Variable telemInterval to one hour (telemInterval = 600000);
 */

void handleMessage(){
  
 long varChange = 0;    // variable to change values inside program
 int commandCode = 0;   // variable to determine which command was recieved

 // If no message was recieved buffer size is 0
 // if message is succesfully downloaded, convert to desired format  
 // Debugging code to read message through serial terminal
  
 Serial.println(F("Message received!"));
 Serial.print(F("Inbound message size is "));
 Serial.println(bufferSize);
 for (byte i=0; i<(byte)bufferSize; ++i)
  {
   Serial.print(F("Decimal = ")); Serial.println(buffer[i],DEC);  // displays message in decimal (ASCII) format
   Serial.print(F("HEX = ")); Serial.println(buffer[i], HEX);    // displays message in hex format
   Serial.print(F("Actual Message = "));
   if (isprint(buffer[i])) {
    Serial.print(("("));
    Serial.write(buffer[i]);
    Serial.println((")"));
    Serial.print(("("));
    Serial.print(buffer[i]);
    Serial.print((")"));
   }
   Serial.println((" "));
  }
  Serial.println();
    
  // data handling to store recieved data in variable
  
  if((byte)bufferSize!=0){
   for(byte i=1; i<5; ++i) {  // adds first four ASCII values together to form command code
    commandCode += buffer[i];
    oled.print(F("Command Code =")); Serial.print("command code  ="); Serial.println(commandCode);
   }
  
   for(byte i=5; i<(byte)bufferSize; ++i) { // adds subsequent characters together to get varChange value.
    byte ASCII_CONVERSION = buffer[i] - 48;   // Convert ASCII numbers to acutal numbers
    varChange = varChange *10 + ASCII_CONVERSION;           // Algorithm to get individual numbers as one value.
    oled.print(F("Variable Change = ")); Serial.print("VarChange = "); Serial.println(varChange);
   }
  }
 // Determine which code was sent and change respective variable.
   
 if(commandCode == 431) { // time: sum of ASCII values = 431
  Serial.print("right before interval is changed interval = "); Serial.println(telemInterval);
  telemInterval = varChange; // Change the conditional interval time to send a message
  Serial.print("in interval change new interval =  "); Serial.println(telemInterval);
 }
 if(commandCode == 416) { // chng: sum of ASCII values = 416
  // Must send in value 10x desired value (ex. sent: 10 actual 1.0)
  Serial.print("change condition is "); Serial.println(changeCond);
  changeCond = varChange / 10.0; // Change the conditional change value to send a message
  Serial.print("change condition was just changed to "); Serial.println(changeCond);
 }

 if(commandCode == 430) { // rari: sum of ASCII values = 430
  pollingInterval = varChange;   // Change the rate the rangeFind takes readings
 }

 if(commandCode == 418) { // isbd: sum of ASCII values = 418
  n = varChange; 
  isbd.adjustSendReceiveTimeout(n); // Adjust how long RockBLOCK will try to send message before timeout                                           
 }
}
void oledReset() {
  oled.begin(&Adafruit128x32, I2C_ADDRESS);
  oled.setFont(System5x7);
  oled.clear();
  oled.setCursor(0,0); 
}

long readVcc() {
 // Function provided by:
 // https://provideyourown.com/2012/secret-arduino-voltmeter-measure-battery-voltage/
 
 // Read 1.1V reference against AVcc
 // set the reference to Vcc and the measurement to the internal 1.1V reference
 #if defined(__AVR_ATmega32U4__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
   ADMUX = _BV(REFS0) | _BV(MUX4) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
 #elif defined (__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__)
   ADMUX = _BV(MUX5) | _BV(MUX0);
 #elif defined (__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__)
   ADMUX = _BV(MUX3) | _BV(MUX2);
 #else
   ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
 #endif  

 delay(2); // Wait for Vref to settle
 ADCSRA |= _BV(ADSC); // Start conversion

 while (bit_is_set(ADCSRA,ADSC)); // measuring

 uint8_t low  = ADCL; // must read ADCL first - it then locks ADCH  
 uint8_t high = ADCH; // unlocks both

 long result = (high<<8) | low;

 result = 1125300L / result; // Calculate Vcc (in mV); 1125300 = 1.1*1023*1000
 return result; // Vcc in millivolts
}

// diagnostic functions
#if DIAGNOSTICS

void ISBDConsoleCallback(IridiumSBD *device, char c)
{
  Serial.write(c);
}

void ISBDDiagsCallback(IridiumSBD *device, char c)
{
  Serial.write(c);
}

#endif