// 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
 a newer version of this library has been released and the functions renamed or restructured. Use the version 1.0 for this code. Also DO NOT
 update Iridium SBD using the arduino library manager

 Please acknowlege Hans Huth and Sean Keane if you use/modify this code
 Modified and annotated by Carl Hooper
*/

// 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 = 86400000; // status reporting interval (24hours where 1000 = 1 second) **Can be modified remotely
unsigned long pollingInterval = 10000;  // (60 secs) interval that rangeFind takes readings for change detect **Can be modifed remotely +2 seconds if OLED is still on.

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 (must be an odd number)
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=0;       // 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.print("Change = "); Serial.println(change);
Serial.print("mode = "); Serial.println(mode);
Serial.print("modeold = "); Serial.println(modeOld);
Serial.print("Millis = "); Serial.println(loopStartTime);
delay(10000);

 
 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);


 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 then 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(3000);
   }
   
   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"));
  if(digitalRead(OLEDScanner) == 1) { 
  oled.clear();                             
  oled.println(F("Waiting for condition"));
  delay(2000);   
  }  
 delay(pollingInterval); //check condition after polling interval 
 }
} // end loop

///// FUNCTIONS /////

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


 while( i < arraysize ) {  
  digitalWrite(RangeTrig, HIGH); 
  delay(1000);  
  pulse = analogRead(RangePin);  // read in time for pin to transition
  rangevalue[i]=pulse;          // pulse is set to the analog read value which is the distance in cm 
  if( rangevalue[i] < 725 && rangevalue[i] >= 10 ) 
  i++;   // ensure no value out of range
  digitalWrite(RangeTrig, LOW);
  delay(1000); // delay between measurements
 }
 isort(rangevalue,arraysize);        // sort samples
 mode = getMyMode(rangevalue,arraysize);  // get mode

 //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 reading the buffer and leading four characters of the data stream of the inbound message. (the message is limited to 
 * a max of 270 inbound bytes) The data stream is transmitted as ASCII values.  * The first value is always 35 which is the ASCII value of # . 
 * The next four ASCII values are then read, and are added together to form a command code. All the remaining characters are then read, converted to text
 * 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 a message from the RockBLOCK 'send message' tab located at  
 * https://rockblock.rock7.com/Operations  
 * Using text mode enter #time3600000 in the text box.  You'll notice the text being converted to an ASCII string below. Click send message. 
 * Repeat this step a second time.
 * By default when the rockblock executes a send/receive function the first waiting message is discarded by default.
 * This function is hard coded into the iridium system.  The second, duplicate message, is then received as the inbound message. 
 * 35 the ASCII value of # is ignored at the begining of the message as it is the leading buffer. 
 * The next four ASCII values are added together. In this example the characters t i m e send as ASCII add up to 116 + 105 + 109 + 101 = 431. 
 * The subsequent values [3,6,0 0,0,0,0] get converted from their ASCII values and saved as one number '3600000'. This then changes the 
 * global Variable telemInterval to 3600000 milliseconds or one hour (telemInterval = 3600000);
 * 
 * time, chng, isbd, and rari are defined as remote alterable variables within this function. Pay close attention to the unit values for each 
 * variable as they are not all the same.
 */

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