/*
 * Ref:
 * Manual - https://www.seeedstudio.com/blog/2019/07/05/u8g2-for-seeeduino-boards/
 * Dual OLED - https://github.com/olikraus/u8g2/issues/77
 *
 * Every power cycle:
 * 1. Read configuration from EEPROM
 *  
 */

#include <Arduino.h>
#include <SPI.h>

#include <U8g2lib.h>
#include <Wire.h>
#include <EEPROM.h>
#include <BleGamepad.h> 

// For OTA and Settings over WiFi
#include <WiFi.h>
#include <WiFiClient.h>
#include <WebServer.h>
#include <Update.h>

// For NRF24L01 (E01) - EByte
// #include "nRF24L01.h"
#include "RF24.h"
#include "printf.h"

// Declaration for NRF24L01
#define NRF24_CE    5                       // GPIO5 (CE)
#define NRF24_CSN   4                       // GPIO4 (CSN)

// NRF24L01 registers we need - For 2.4G Scanning
#define _NRF24_CONFIG      0x00
#define _NRF24_EN_AA       0x01
#define _NRF24_RF_CH       0x05
#define _NRF24_RF_SETUP    0x06
#define _NRF24_RPD         0x09

// For channel scanner
#define MAX_CHANNELS    125
#define MAX_SAMPLES     100

int channel_loads[MAX_CHANNELS];            // Channel 0 ~ Channel 124
uint64_t pipeIn = 0xE8E8F0F0E1LL;     // TODO: set it to EEPROM
RF24 radio(NRF24_CE, NRF24_CSN);            // Starting up the module on GPIO5 (CE), GPIO4 (CSN)
bool nrf_enable = false;
bool nrf_scanner = false;
bool binding = false;
bool channel_used[MAX_CHANNELS];
uint8_t graph_type = 0;
uint32_t delay_maxrf_time = 0;
bool delay_maxrf = false;

TaskHandle_t SignalTask;

// TTTT TTTT TTTT ---- YYYY YYYY YYYY ---- PPPP PPPP PPPP ---- RRRR RRRR RRRR ---- SSSS SSSS = 72-bits ( 9 bytes )
// Data Structure
struct TxMessage {
    uint16_t throttle;
    uint16_t yaw;
    uint16_t pitch;
    uint16_t roll;
    uint8_t switches;
};
TxMessage txmessage;

struct SyncData {
    uint8_t freq_channel;
    uint8_t dummy;
    uint8_t fixed;
};
SyncData syncdata;

bool wifi_enable = false;
bool webserver_setup = false;

// Starting up Web Server
WebServer server(80);

// WebPages ----- START

/*** Firmware Upgrade Page ***/
const char* firmware_upgrade_html PROGMEM = R"rawliteral(
<script>
var iBytesUploaded = 0;
var iBytesTotal = 0;
var iPreviousBytesLoaded = 0;
var iMaxFilesize = 1048576; // 1MB
var oTimer = 0;
var sResultFileSize = '';
function secondsToTime(secs) {
  var hr = Math.floor(secs / 3600);
  var min = Math.floor((secs - (hr * 3600))/60);
  var sec = Math.floor(secs - (hr * 3600) -  (min * 60));
  if (hr < 10) {hr = "0" + hr; }
  if (min < 10) {min = "0" + min;}
  if (sec < 10) {sec = "0" + sec;}
  if (hr) {hr = "00";}
  return hr + ':' + min + ':' + sec;
};
function bytesToSize(bytes) {
  var sizes = ['Bytes', 'KB', 'MB'];
  if (bytes == 0) return 'n/a';
  var i = parseInt(Math.floor(Math.log(bytes) / Math.log(1024)));
  return (bytes / Math.pow(1024, i)).toFixed(1) + ' ' + sizes[i];
};
function fileSelected() {
  document.getElementById('upload_response').style.display = 'none';
  document.getElementById('error').style.display = 'none';
  document.getElementById('error2').style.display = 'none';
  document.getElementById('abort').style.display = 'none';
  document.getElementById('warnsize').style.display = 'none';
  var oFile = document.getElementById('image_file').files[0];
  var rFilter = /^(image\/bmp|image\/gif|image\/jpeg|image\/png|image\/tiff)$/i;
  if (! rFilter.test(oFile.type)) {
    document.getElementById('error').style.display = 'block';
    return;
  }
  if (oFile.size > iMaxFilesize) {
    document.getElementById('warnsize').style.display = 'block';
    return;
  }
  var oImage = document.getElementById('preview');
  var oReader = new FileReader();
  oReader.onload = function(e){
    oImage.src = e.target.result;
    oImage.onload = function () { // binding onload event
      sResultFileSize = bytesToSize(oFile.size);
      document.getElementById('fileinfo').style.display = 'block';
      document.getElementById('filename').innerHTML = 'Name: ' + oFile.name;
      document.getElementById('filesize').innerHTML = 'Size: ' + sResultFileSize;
      document.getElementById('filetype').innerHTML = 'Type: ' + oFile.type;
      document.getElementById('filedim').innerHTML = 'Dimension: ' + oImage.naturalWidth + ' x ' + oImage.naturalHeight;
    };
  };
  oReader.readAsDataURL(oFile);
}
function startUploading() {
  iPreviousBytesLoaded = 0;
  document.getElementById('upload_response').style.display = 'none';
  document.getElementById('error').style.display = 'none';
  document.getElementById('error2').style.display = 'none';
  document.getElementById('abort').style.display = 'none';
  document.getElementById('warnsize').style.display = 'none';
  document.getElementById('progress_percent').innerHTML = '';
  var oProgress = document.getElementById('progress');
  oProgress.style.display = 'block';
  oProgress.style.width = '0px';
  var vFD = new FormData(document.getElementById('upload_form')); 
  var oXHR = new XMLHttpRequest();
  oXHR.upload.addEventListener('progress', uploadProgress, false);
  oXHR.addEventListener('load', uploadFinish, false);
  oXHR.addEventListener('error', uploadError, false);
  oXHR.addEventListener('abort', uploadAbort, false);
  oXHR.open('POST', '/update');
  oXHR.send(vFD);
  oTimer = setInterval(doInnerUpdates, 300);
}
function doInnerUpdates() { 
  var iCB = iBytesUploaded;
  var iDiff = iCB - iPreviousBytesLoaded;
  if (iDiff == 0)
    return;
  iPreviousBytesLoaded = iCB;
  iDiff = iDiff * 2;
  var iBytesRem = iBytesTotal - iPreviousBytesLoaded;
  var secondsRemaining = iBytesRem / iDiff;
  var iSpeed = iDiff.toString() + 'B/s';
  if (iDiff > 1024 * 1024) {
    iSpeed = (Math.round(iDiff * 100/(1024*1024))/100).toString() + 'MB/s';
  } else if (iDiff > 1024) {
    iSpeed =  (Math.round(iDiff * 100/1024)/100).toString() + 'KB/s';
  }
  document.getElementById('speed').innerHTML = iSpeed;
  document.getElementById('remaining').innerHTML = '| ' + secondsToTime(secondsRemaining);
}
function uploadProgress(e) {
  if (e.lengthComputable) {
    iBytesUploaded = e.loaded;
    iBytesTotal = e.total;
    var iPercentComplete = Math.round(e.loaded * 100 / e.total);
    var iBytesTransfered = bytesToSize(iBytesUploaded);
    document.getElementById('progress_percent').innerHTML = iPercentComplete.toString() + '%';
    document.getElementById('progress').style.width = (iPercentComplete * 4).toString() + 'px';
    document.getElementById('b_transfered').innerHTML = iBytesTransfered;
    if (iPercentComplete == 100) {
      var oUploadResponse = document.getElementById('upload_response');
      oUploadResponse.innerHTML = '<h1>Please wait...processing</h1>';
      oUploadResponse.style.display = 'block';
    }
  } else {
    document.getElementById('progress').innerHTML = 'unable to compute';
  }
}
function uploadFinish(e) { 
  var oUploadResponse = document.getElementById('upload_response');
  oUploadResponse.innerHTML = e.target.responseText;
  oUploadResponse.style.display = 'block';
  document.getElementById('progress_percent').innerHTML = '100%';
  document.getElementById('progress').style.width = '400px';
  document.getElementById('filesize').innerHTML = sResultFileSize;
  document.getElementById('remaining').innerHTML = '| 00:00:00';
  clearInterval(oTimer);
}
function uploadError(e) { 
  document.getElementById('error2').style.display = 'block';
  clearInterval(oTimer);
}  
function uploadAbort(e) {
  document.getElementById('abort').style.display = 'block';
  clearInterval(oTimer);
}
</script>
<style>
.container {
  margin: auto;
  width: 50%;
}
.upload_form_cont {
  background: -moz-linear-gradient(#ffffff, #f2f2f2);
  background: -ms-linear-gradient(#ffffff, #f2f2f2);
  background: -webkit-gradient(linear, left top, left bottom, color-stop(0%, #ffffff), color-stop(100%, #f2f2f2));
  background: -webkit-linear-gradient(#ffffff, #f2f2f2);
  background: -o-linear-gradient(#ffffff, #f2f2f2);
  filter: progid:DXImageTransform.Microsoft.gradient(startColorstr='#ffffff', endColorstr='#f2f2f2');
  -ms-filter: "progid:DXImageTransform.Microsoft.gradient(startColorstr='#ffffff', endColorstr='#f2f2f2')";
  background: linear-gradient(#ffffff, #f2f2f2);

  color:#000;
  overflow:hidden;
}
#upload_form {
  float:center;
  padding:20px;
  width:700px;
}
#preview {
  background-color:#fff;
  display:block;
  float:right;
  width:200px;
}
#upload_form > div {
  margin-bottom:10px;
}
#speed,#remaining {
  float:left;
  width:100px;
}
#b_transfered {
  float:right;
  text-align:right;
}
.clear_both {
  clear:both;
}
input {
  border-radius:10px;
  -moz-border-radius:10px;
  -ms-border-radius:10px;
  -o-border-radius:10px;
  -webkit-border-radius:10px;

  border:1px solid #ccc;
  font-size:14pt;
  padding:5px 10px;
}
input[type=button] {
  background: -moz-linear-gradient(#ffffff, #dfdfdf);
  background: -ms-linear-gradient(#ffffff, #dfdfdf);
  background: -webkit-gradient(linear, left top, left bottom, color-stop(0%, #ffffff), color-stop(100%, #dfdfdf));
  background: -webkit-linear-gradient(#ffffff, #dfdfdf);
  background: -o-linear-gradient(#ffffff, #dfdfdf);
  filter: progid:DXImageTransform.Microsoft.gradient(startColorstr='#ffffff', endColorstr='#dfdfdf');
  -ms-filter: "progid:DXImageTransform.Microsoft.gradient(startColorstr='#ffffff', endColorstr='#dfdfdf')";
  background: linear-gradient(#ffffff, #dfdfdf);
}
#image_file {
  width:400px;
}
#progress_info {
  font-size:10pt;
}
#fileinfo,#error,#error2,#abort,#warnsize {
  color:#aaa;
  display:none;
  font-size:10pt;
  font-style:italic;
  margin-top:10px;
}
#progress {
  border:1px solid #ccc;
  display:none;
  float:left;
  height:14px;

  border-radius:10px;
  -moz-border-radius:10px;
  -ms-border-radius:10px;
  -o-border-radius:10px;
  -webkit-border-radius:10px;

  background: -moz-linear-gradient(#66cc00, #4b9500);
  background: -ms-linear-gradient(#66cc00, #4b9500);
  background: -webkit-gradient(linear, left top, left bottom, color-stop(0%, #66cc00), color-stop(100%, #4b9500));
  background: -webkit-linear-gradient(#66cc00, #4b9500);
  background: -o-linear-gradient(#66cc00, #4b9500);
  filter: progid:DXImageTransform.Microsoft.gradient(startColorstr='#66cc00', endColorstr='#4b9500');
  -ms-filter: "progid:DXImageTransform.Microsoft.gradient(startColorstr='#66cc00', endColorstr='#4b9500')";
  background: linear-gradient(#66cc00, #4b9500);
}
#progress_percent {
  float:right;
}
#upload_response {
  margin-top: 10px;
  padding: 20px;
  overflow: hidden;
  display: none;
  border: 1px solid #ccc;

  border-radius:10px;
  -moz-border-radius:10px;
  -ms-border-radius:10px;
  -o-border-radius:10px;
  -webkit-border-radius:10px;

  box-shadow: 0 0 5px #ccc;
  background: -moz-linear-gradient(#bbb, #eee);
  background: -ms-linear-gradient(#bbb, #eee);
  background: -webkit-gradient(linear, left top, left bottom, color-stop(0%, #bbb), color-stop(100%, #eee));
  background: -webkit-linear-gradient(#bbb, #eee);
  background: -o-linear-gradient(#bbb, #eee);
  filter: progid:DXImageTransform.Microsoft.gradient(startColorstr='#bbb', endColorstr='#eee');
  -ms-filter: "progid:DXImageTransform.Microsoft.gradient(startColorstr='#bbb', endColorstr='#eee')";
  background: linear-gradient(#bbb, #eee);
}
</style>
<body>
  <header>
    <h2 align='center'>*** OpenFlightTX - Firmware Uploader ***</h2>
  </header>
  <div class='container'>
    <div class='contr'><h2>Please select a file to upload the firmware.</h2></div>
      <div class='upload_form_cont'>
        <form method='POST' action='/update' enctype='multipart/form-data' id='upload_form'>
          <div>
            <div><label for='image_file'>Please select image file</label></div>
            <div><input type='file' name='update' id='image_file' onchange='fileSelected();' /></div>
          </div>
          <div><input type='button' value='Upload' onclick='startUploading()' /></div>
          <div id='fileinfo'>
            <div id='filename'></div>
            <div id='filesize'></div>
            <div id='filetype'></div>
            <div id='filedim'></div>
          </div>
          <div id='error'>You should select valid image files only!</div>
          <div id='error2'>An error occurred while uploading the file</div>
          <div id='abort'>The upload has been canceled by the user or the browser dropped the connection</div>
          <div id='warnsize'>Your file is very big. We can't accept it. Please select more small file</div>
          <div id='progress_info'>
            <div id='progress'></div>
            <div id='progress_percent'>&nbsp;</div>
            <div class='clear_both'></div>
            <div>
              <div id='speed'>&nbsp;</div>
              <div id='remaining'>&nbsp;</div>
              <div id='b_transfered'>&nbsp;</div>
              <div class='clear_both'></div>
            </div>
            <div id='upload_response'></div>
          </div>
        </form>
      </div>
    </div>
  </div>
</body>
)rawliteral";


/*** Index Page ***/
const char* index_html PROGMEM = R"rawliteral(<pre><strong>OpenFlightTX v1.0.10</strong>
)rawliteral";

// WebPages ----- END

void unload_webserver(void) {
    // Unload webserver....
    server.stop();

    // Unload WiFi...
    WiFi.disconnect();
}

void setup_webserver(void) {
    //************ TEMPORARY HARDCODE
    WiFi.begin("veeone", "trustno1");
    // WiFi.begin("Pi3_2G", "trustno1");

    // init and get the time
    // configTime(gmtOffset_sec, daylightOffset_sec, ntpServer);

    // Setting up OTA
    /*** Index Page ***/
    server.on("/", HTTP_GET, []() {
        server.sendHeader("Connection", "close");
        server.send(200, "text/html", firmware_upgrade_html);
    });

    /*** Firmware Upgrade Page ***/
    server.on("/firmware_upgrade", HTTP_GET, []() {
        server.sendHeader("Connection", "close");
        server.send(200, "text/html", firmware_upgrade_html);
    });

    // ################### HANDLING UPLOAD FIRMWARE HERE #########################
    /*** handling uploading firmware file ***/
    server.on("/update", HTTP_POST, []() {
        server.sendHeader("Connection", "close");
        server.send(200, "text/plain", (Update.hasError()) ? "FAIL" : "OK. Device is rebooting");

        // Try to delay a bit...
        delay(1000);
        ESP.restart();
    }, []() {     
        HTTPUpload& upload = server.upload();
        if(upload.status == UPLOAD_FILE_START) {
            // Serial.printf("Update: %s\n", upload.filename.c_str());
            if(!Update.begin(UPDATE_SIZE_UNKNOWN)) {        //start with max available size
                Update.printError(Serial);
            }     
        } else if(upload.status == UPLOAD_FILE_WRITE) {
            /*** flashing firmware to ESP ***/
            if(Update.write(upload.buf, upload.currentSize) != upload.currentSize) {
                Update.printError(Serial);
            }
        } else if(upload.status == UPLOAD_FILE_END) {
            if(Update.end(true)) { 
                //true to set the size to the current progress
                // Serial.printf("Update Success: %u\nRebooting...\n", upload.totalSize);
            } else {
                Update.printError(Serial);
            }
        }
    });

    // Starting up Server
    server.begin();

    // Just to debug...
    if(WiFi.status() == WL_CONNECTED) 
        Serial.println(WiFi.localIP());
}


BleGamepad bleGamepad;

/* create a hardware timer */
hw_timer_t * timer = NULL;

typedef enum {
    SCREEN_INTRO = 0,
    SCREEN_SELECTIONS,
    SCREEN_CONTROL
} OPENFLIGHT_SCREEN;


/*
  Icon configuration
  Width and height must match the icon font size
  GAP: Space between the icons
  BGAP: Gap between the display border and the cursor.
*/
#define ICON_WIDTH 16
#define ICON_HEIGHT 16
#define ICON_GAP 4
#define ICON_BGAP 16
#define ICON_Y 16+ ICON_GAP
#define ICONX_Y 32+ ICON_GAP

U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2_left(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);
// U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2_right(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);

struct menu_entry_type {
    const uint8_t *font;
    uint16_t icon;
    const char *name;
};

struct menu_state {
    int16_t menu_start;         /* in pixel */
    int16_t frame_position;     /* in pixel */
    uint8_t position;           /* position, array index */
};

struct menu_entry_type menu_entry_alpha_list[] = {
    { u8g2_font_open_iconic_all_2x_t, 224, "RC Controller"},
    { u8g2_font_open_iconic_all_2x_t, 94,  "Bluetooth"},
    { u8g2_font_open_iconic_all_2x_t, 155, "Stick Calibration"},
    { u8g2_font_open_iconic_all_2x_t, 242, "Stick Positions"},
    { u8g2_font_open_iconic_all_2x_t, 246, "Trims"},
    { u8g2_font_open_iconic_all_2x_t, 281, "WiFi"},
    { u8g2_font_open_iconic_all_2x_t, 198, "RX Binding"},
    { u8g2_font_open_iconic_all_2x_t, 253, "RF Settings"},
    { u8g2_font_open_iconic_all_2x_t, 207, "RF Scanners"},
    { u8g2_font_open_iconic_all_2x_t, 90,  "Battery"},
    { u8g2_font_open_iconic_all_2x_t, 142, "Save Settings"},
    { u8g2_font_open_iconic_all_2x_t, 205, "Reset Default"},
    { NULL, 0, NULL } 
};

struct menu_state current_alpha_state = { ICON_BGAP, ICON_BGAP, 0 };
struct menu_state destination_alpha_state = { ICON_BGAP, ICON_BGAP, 0 };

void draw_menu(struct menu_state *state) {
    int16_t x;
    uint8_t i;
    x = state->menu_start;
    i = 0;
    while( menu_entry_alpha_list[i].icon > 0 ) {
        if ( x >= -ICON_WIDTH && x < u8g2_left.getDisplayWidth()) {
            u8g2_left.setFont(menu_entry_alpha_list[i].font);
            u8g2_left.drawGlyph(x, ICON_Y, menu_entry_alpha_list[i].icon );
        }
        i++;
        x += ICON_WIDTH + ICON_GAP;
    }
    u8g2_left.drawFrame(state->frame_position-3, ICON_Y-ICON_HEIGHT-3, ICON_WIDTH+6, ICON_WIDTH+6);
    //u8g2_left.drawFrame(state->frame_position-2, ICON_Y-ICON_HEIGHT-2, ICON_WIDTH+4, ICON_WIDTH+4);
    //u8g2_left.drawFrame(state->frame_position-3, ICON_Y-ICON_HEIGHT-3, ICON_WIDTH+6, ICON_WIDTH+6);
}

void to_right(struct menu_state *state) {
    if(menu_entry_alpha_list[state->position+1].font != NULL) {
        if((int16_t)state->frame_position+ 2*(int16_t)ICON_WIDTH + (int16_t)ICON_BGAP < (int16_t)u8g2_left.getDisplayWidth()) {
            state->position++;
            state->frame_position += ICON_WIDTH + (int16_t)ICON_GAP;
        } else {
            state->position++;      
            state->frame_position = (int16_t)u8g2_left.getDisplayWidth() - (int16_t)ICON_WIDTH - (int16_t)ICON_BGAP;
            state->menu_start = state->frame_position - state->position*((int16_t)ICON_WIDTH + (int16_t)ICON_GAP);
        }
    }
}

void to_left(struct menu_state *state){
    if(state->position > 0){
        if ( (int16_t)state->frame_position >= (int16_t)ICON_BGAP+(int16_t)ICON_WIDTH+ (int16_t)ICON_GAP ){
            state->position--;
            state->frame_position -= ICON_WIDTH + (int16_t)ICON_GAP;
        } else {
            state->position--; 
            state->frame_position = ICON_BGAP;
            state->menu_start = state->frame_position - state->position*((int16_t)ICON_WIDTH + (int16_t)ICON_GAP);      
        }
    }
}


uint8_t towards_int16(int16_t *current, int16_t dest) {
    if ( *current < dest ){
        (*current)++;
        return 1;
    } else if ( *current > dest ) {
        (*current)--;
        return 1;
    }
    return 0;
}

uint8_t towards(struct menu_state *current, struct menu_state *destination) {
    uint8_t r = 0;
    uint8_t i;
    for( i = 0; i < 6; i++ ) {
        r |= towards_int16( &(current->frame_position), destination->frame_position);
        r |= towards_int16( &(current->menu_start), destination->menu_start);
    }
    return r;
}

// For Menu Navigation and Setup
#define MENU_STICK_ROLL_MAX     3000
#define MENU_STICK_ROLL_MIN     800
#define MENU_STICK_PITCH_MAX    3000
#define MENU_STICK_PITCH_MIN    800
#define MENU_STICK_YAW_MAX      3000
#define MENU_STICK_YAW_MIN      800

typedef enum {
    MENU_NONE = 0,
    MENU_LEFT,
    MENU_RIGHT,
    MENU_BACK,
    MENU_OK,
    MENU_EXTEND_LEFT,
    MENU_EXTEND_RIGHT
} MENU_NAVIGATION;

MENU_NAVIGATION stick_navigation_position;
bool menu_entry_alpha_selected = false;
bool require_unlock = false;

bool ble_begin = false;
uint32_t ble_profiling = 0;
uint32_t ble_profiling_raw = 0;
uint32_t nrf_profiling = 0;
uint32_t nrf_profiling_raw = 0;
uint32_t last_seconds = 0;
uint32_t last_nrf_scanned = 0;

//define the pins used by the transceiver module
#define SS      5
#define RST     14
#define DIO0    2

int counter = 0;

#define BUZZER_PIN      15
#define THROTTLE_PIN    36
#define YAW_PIN         39
#define PITCH_PIN       34
#define ROLL_PIN        35

#define VBAT_PIN        32
#define VCC_PIN         33

#define SW1_PIN         25
#define SW2_PIN         26
#define SW3_PIN         27
#define SW4_PIN         14
#define SW5_PIN         12
#define SW6_PIN         13

uint16_t throttle_raw   = 0;
uint16_t yaw_raw        = 0;
uint16_t pitch_raw      = 0;
uint16_t roll_raw       = 0;

uint16_t vbat_raw       = 0;
uint16_t vcc_raw        = 0;

uint16_t throttle_value = 0;
uint16_t yaw_value      = 0;
uint16_t pitch_value    = 0;
uint16_t roll_value     = 0;
/*
uint16_t tx_throttle    = 0;
uint16_t tx_yaw         = 0;
uint16_t tx_pitch       = 0;
uint16_t tx_roll        = 0;
*/
// For Center sticks trim
uint16_t throttle_max   = 0;
uint16_t throttle_min   = 4096;
uint16_t yaw_max        = 0;
uint16_t yaw_min        = 4096;
uint16_t pitch_max      = 0;
uint16_t pitch_min      = 4096;
uint16_t roll_max       = 0;
uint16_t roll_min       = 4096;

// For NRF24L01 frequency channels + TxPower
uint8_t freq_channel    = 0;
uint8_t freq_txpower    = 0;            // 0: LOW, 1: MIN, 2: HIGH, 3: MAX

uint64_t vbat_value     = 0;

bool sw1                = 0;
bool sw2                = 0;
bool sw3                = 0;
bool sw4                = 0;
bool sw5                = 0;
bool sw6                = 0;

uint8_t switches_value  = 0;

bool invert_throttle    = false;
bool invert_yaw         = false;
bool invert_pitch       = false;
bool invert_roll        = false;

int8_t trim_throttle   = 0;
int8_t trim_yaw        = 0;
int8_t trim_pitch      = 0;
int8_t trim_roll       = 0;

uint32_t frequency     = 919999000;
uint8_t  syncword      = 0xA0;
uint16_t packet_sent   = 0;
uint16_t packet_sent_count = 0;
uint32_t uptime        = 0;

#define MEDIAN_TOTAL    11
#define MEDIAN_POS      MEDIAN_TOTAL/2

// For function call used only
#define THROTTLE        0
#define YAW             1
#define PITCH           2
#define ROLL            3
#define VBAT            4

uint64_t last_update = 0;
uint64_t last_menu_stick_read = 0;
uint64_t last_menu_item_moved = 0;

uint16_t throttle_pool[MEDIAN_TOTAL];
uint16_t yaw_pool[MEDIAN_TOTAL];
uint16_t pitch_pool[MEDIAN_TOTAL];
uint16_t roll_pool[MEDIAN_TOTAL];
uint16_t vbat_pool[MEDIAN_TOTAL];

uint16_t tmp_median_store[MEDIAN_TOTAL];

void median_initialize() {
    // Prepare the storage engine for TYPR
    for(int i = 0; i < MEDIAN_TOTAL; i++) {
        throttle_pool[i] = 0;
        yaw_pool[i] = 0;
        pitch_pool[i] = 0;
        roll_pool[i] = 0;
    }
}


void IRAM_ATTR onTimer(){
    uptime++;
    packet_sent = packet_sent_count;
    packet_sent_count = 0;
}


/*
 * median_get:
 * To get the median from the data depending on the median_type
 *
 * E.g.:
 * retval = median_get(THROTTLE);
 */
uint16_t median_get(uint8_t median_type) {
    bool inserted = false;

    uint16_t median_tmp[MEDIAN_TOTAL];

    // Loop through the variable to determine position to insert
    //  [ 12, 32, 4 ,0, 50, 2, 10, 10, 5, 20, 0 ]
    /*
        [ 12 ]
        [ 12, 32 ]
        [ 4, 12, 32 ]
        [ 0, 4, 12, 32 ]
        [ 0, 4, 12, 32, 50 ]
        [ 0, 2, 4, 12, 32, 50 ]

     */

    // Initial
    uint16_t temp_val = 0;
    uint16_t total_insert = 1;

    // Search on type
    if(median_type == THROTTLE) {
        median_tmp[0] = throttle_pool[0];
    } else if(median_type == YAW) {
        median_tmp[0] = yaw_pool[0];
    } else if(median_type == PITCH) {
        median_tmp[0] = pitch_pool[0];
    } else if(median_type == ROLL) {
        median_tmp[0] = roll_pool[0];
    } else if(median_type == VBAT) {
        median_tmp[0] = vbat_pool[0];
    }

    // Loop insert and sort
    for(int raw_count = 1; raw_count < MEDIAN_TOTAL; raw_count++) {
        inserted = false;

        if(median_type == THROTTLE) {
            temp_val = throttle_pool[raw_count];
        } else if(median_type == YAW) {
            temp_val = yaw_pool[raw_count];
        } else if(median_type == PITCH) {
            temp_val = pitch_pool[raw_count];
        } else if(median_type == ROLL) {
            temp_val = roll_pool[raw_count];
        } else if(median_type == VBAT) {
            temp_val = vbat_pool[raw_count];
        }

        for(int median_count = 0; median_count < total_insert; median_count++) {
            if(!inserted) {

                if(temp_val < median_tmp[median_count]) {
                    inserted = true;

                    // Reverse copy and insert
                    for(int median_reverse = total_insert + 1; median_reverse > median_count; median_reverse--) {
                        median_tmp[median_reverse] = median_tmp[median_reverse - 1];
                    }

                    // Insert the detected
                    median_tmp[median_count] = temp_val;

                    // Increase total insertion
                    total_insert++;
                }
            }
        }

        // Nothing was inserted... so put at the last.
        if(!inserted) {
            median_tmp[total_insert++] = temp_val;
        }
    }

    // Return the result
    return median_tmp[MEDIAN_POS];
}

uint16_t throttle_insert(uint16_t val) {
    // Insert to the array...at the last 
    for(int i = 1; i < MEDIAN_TOTAL; i++) 
        // Shift to left
        throttle_pool[i - 1] = throttle_pool[i];
    
    throttle_pool[MEDIAN_TOTAL - 1] = val;

    // Find median....
    return median_get(THROTTLE);
}

uint16_t yaw_insert(uint16_t val) {
    // Insert to the array...at the last 
    for(int i = 1; i < MEDIAN_TOTAL; i++) 
        // Shift to left
        yaw_pool[i - 1] = yaw_pool[i];
    
    yaw_pool[MEDIAN_TOTAL - 1] = val;

    // Find median....
    return median_get(YAW);
}

uint16_t pitch_insert(uint16_t val) {
    // Insert to the array...at the last 
    for(int i = 1; i < MEDIAN_TOTAL; i++) 
        // Shift to left
        pitch_pool[i - 1] = pitch_pool[i];
    
    pitch_pool[MEDIAN_TOTAL - 1] = val;

    // Find median....
    return median_get(PITCH);
}

uint16_t roll_insert(uint16_t val) {
    // Insert to the array...at the last 
    for(int i = 1; i < MEDIAN_TOTAL; i++) 
        // Shift to left
        roll_pool[i - 1] = roll_pool[i];
    
    roll_pool[MEDIAN_TOTAL - 1] = val;

    // Find median....
    return median_get(ROLL);
}

/*
 * pool_insert:
 * To insert the data into the median pool depending on type of the variable
 *
 * E.g.:
 * throttle_value = pool_insert(THROTTLE);
 */
uint16_t pool_insert(uint8_t pool_type, uint16_t val) {
    // Insert to the array...at the last of the array
    for(int i = 1; i < MEDIAN_TOTAL; i++) {
        // Shift to left
        if(pool_type == THROTTLE) {
            throttle_pool[i - 1] = throttle_pool[i];
        } else if(pool_type == YAW) {
            yaw_pool[i - 1] = yaw_pool[i];
        } else if(pool_type == PITCH) {
            pitch_pool[i - 1] = pitch_pool[i];
        } else if(pool_type == ROLL) {
            roll_pool[i - 1] = roll_pool[i];
        } else if(pool_type == VBAT) {
            vbat_pool[i - 1] = vbat_pool[i];
        }
    }
  
    // Find median....
    if(pool_type == THROTTLE) {
        throttle_pool[MEDIAN_TOTAL - 1] = val;
    } else if(pool_type == YAW) {
        yaw_pool[MEDIAN_TOTAL - 1] = val;
    } else if(pool_type == ROLL) {
        roll_pool[MEDIAN_TOTAL - 1] = val;
    } else if(pool_type == PITCH) {
        pitch_pool[MEDIAN_TOTAL - 1] = val;
    } else if(pool_type == VBAT) {
        vbat_pool[MEDIAN_TOTAL - 1] = val;
    }
    return median_get(pool_type);
}

void intro() {
    for(int i = 0; i < 2; i++) {
        digitalWrite(BUZZER_PIN, HIGH);
        delay(10);
        digitalWrite(BUZZER_PIN, LOW);
        delay(1);
    }
}


void read_all_voltage() {
    throttle_raw = analogRead(THROTTLE_PIN);
    yaw_raw = analogRead(YAW_PIN);
    pitch_raw = analogRead(PITCH_PIN);
    // roll_raw = analogRead(ROLL_PIN);
    // Hardcode to increase the value a bit...
    roll_raw = analogRead(ROLL_PIN) + 150;

    vbat_raw = analogRead(VBAT_PIN);

    sw1 = !digitalRead(SW1_PIN);
    sw2 = !digitalRead(SW2_PIN);
    sw3 = !digitalRead(SW3_PIN);
    sw4 = !digitalRead(SW4_PIN);
    sw5 = !digitalRead(SW5_PIN);
    sw6 = !digitalRead(SW6_PIN);

    // Combine values to one byte...
    //                  NNssssss
    //                  IIwwwwww
    // switches_value = LL654321
    switches_value  = 0;
    switches_value |= (sw6) ? 1 << 5 : 0;
    switches_value |= (sw5) ? 1 << 4 : 0;
    switches_value |= (sw4) ? 1 << 3 : 0;
    switches_value |= (sw3) ? 1 << 2 : 0;
    switches_value |= (sw2) ? 1 << 1 : 0;
    switches_value |= (sw1) ? 1 << 0 : 0;

    throttle_value = pool_insert(THROTTLE, throttle_raw);
    if(!invert_throttle)
        throttle_value = throttle_value + (trim_throttle << 3);
    else
        throttle_value = throttle_value - (trim_throttle << 3);

    if(!invert_yaw) 
        yaw_value = pool_insert(YAW, yaw_raw) + (trim_yaw << 3);    
    else
        yaw_value = pool_insert(YAW, yaw_raw) - (trim_yaw << 3);    

    if(!invert_pitch) 
        pitch_value = pool_insert(PITCH, pitch_raw) + (trim_pitch << 3);    
    else
        pitch_value = pool_insert(PITCH, pitch_raw) - (trim_pitch << 3);    

    if(!invert_roll)
        roll_value = pool_insert(ROLL, roll_raw) + (trim_roll << 3);
    else
        roll_value = pool_insert(ROLL, roll_raw) - (trim_roll << 3);
    vbat_value = pool_insert(VBAT, vbat_raw);
}

void battery_management() {
    /*
    u8g2_left.clearBuffer();
    u8g2_left.firstPage();
    u8g2_left.setFont(menu_entry_alpha_huge_list[destination_alpha_state.position].font);
    u8g2_left.drawGlyph(40, ICONX_Y, menu_entry_alpha_huge_list[destination_alpha_state.position].icon);
    u8g2_left.nextPage();
    */

    u8g2_left.clearBuffer();
    u8g2_left.firstPage();
    u8g2_left.setFontMode(1);
    u8g2_left.setFont(u8g2_font_VCR_OSD_tr);  

    static char buf[6];

    u8g2_left.setFont(u8g2_font_ImpactBits_tr);  
    u8g2_left.setCursor(0,12);
    u8g2_left.print(F("Battery LEVEL"));
    // u8g2_left.drawStr(45, 15, buf);
    // strcpy(buf, ltoa(vbat_value, buf, 10));
    // u8g2_left.drawStr(0, 30, buf);

    float vbat_real_value = vbat_value * .0012142857; 
    //strcpy(buf, ltoa(vbat_real_value, buf, 10));
    dtostrf(vbat_real_value, 2, 1, buf);

    u8g2_left.setFont(u8g2_font_maniac_tf);  
    u8g2_left.drawStr(0, 64, buf);
    u8g2_left.setCursor(44, 64);
    u8g2_left.print(F("V"));

    // The battery frame
    u8g2_left.drawFrame(96, 0, 32, 64);

    // Bars Calculations
    uint8_t bars = map(vbat_value, 2650, 3460, 0, 31);
    for(int i = 0; i < bars; i++) {
        u8g2_left.drawHLine(98, 61 - (i*2), 28); 
    }

    u8g2_left.nextPage();
}

void setup() {
    Serial.begin(115200);

    // EEPROM initializing
    EEPROM.begin(512);

    // PIN Initialization
    pinMode(BUZZER_PIN, OUTPUT);
    pinMode(SW1_PIN, INPUT_PULLUP);
    pinMode(SW2_PIN, INPUT_PULLUP);
    pinMode(SW3_PIN, INPUT_PULLUP);
    pinMode(SW4_PIN, INPUT_PULLUP);
    pinMode(SW5_PIN, INPUT_PULLUP);
    pinMode(SW6_PIN, INPUT_PULLUP);

    // Display Setup
    // u8g2_SetI2CAddress(u8g2_right.getU8g2(), 0x3d*2);
    u8g2_left.begin();
    // u8g2_right.begin();

    //Serial.println(u8g2_left.u8g2_GetI2CAddress(u8g2_right));
    // u8g2_left.setI2CAddress(0x3C);
    //u8g2_left.setFont(u8g2_font_6x12_tr);
    u8g2_left.setFont(u8g2_font_6x12_tr);
    // u8g2_right.setFont(u8g2_font_6x12_tr);

    median_initialize();

    // Startup Sound
    // intro();

    // LoRA Setup
    // setup LoRa transceiver module
    // LoRa.setPins(SS, RST, DIO0);

    // Replace the LoRa.begin(---E-) argument with your location's frequency 
    // 433E6 for Asia
    // 866E6 for Europe
    // 915E6 for North America
    // while (!LoRa.begin(915000000)) {
    //     Serial.println(".");
    //     delay(500);
    // }

    // Change sync word (0xF3) to match the receiver
    // The sync word assures you don't get LoRa messages from other LoRa transceivers
    // ranges from 0-0xFF
    // LoRa.setSyncWord(0xA0);

    // Set to the highest PA Level
    // LoRa.setSignalBandwidth(20.8E3);
    // LoRa.setTxPower(20);
    // LoRa.setSpreadingFactor(7);
    // LoRa.setSignalBandwidth(125E3);
    // LoRa.setSignalBandwidth(250E3);
    // LoRa.setCodingRate4(5);
    // LoRa.setSignalBandwidth(15.6E3);
    // LoRa.setSpreadingFactor(6);

    // Serial.println("LoRa Initializing OK!");

    // Reading all settings
    read_settings();

    /* Use 1st timer of 4 */
    /* 1 tick take 1/(80MHZ/80) = 1us so we set divider 80 and count up */
    timer = timerBegin(0, 80, true);

    /* Attach onTimer function to our timer */
    timerAttachInterrupt(timer, &onTimer, true);

    /* Set alarm to call onTimer function every second 1 tick is 1us => 1 second is 1000000us */
    /* Repeat the alarm (third parameter) */
    timerAlarmWrite(timer, 1000000, true);

    /* Start an alarm */
    timerAlarmEnable(timer);

    // u8g2_right.firstPage();
    // u8g2_right.setFontMode(1);
    // u8g2_right.setFont(u8g2_font_helvB10_tr);  
    // u8g2_right.setFont(u8g2_font_VCR_OSD_tr);  
    // u8g2_right.setFont(u8g2_font_ImpactBits_tr);  
    // u8g2_right.setCursor(48, 40);
    // u8g2_right.print(F("READY"));
    // u8g2_right.nextPage();
    // setup_lora();
    /* Starting up 2nd core... */

    // Creating Lora Task on Core 0. This will keep sending over and over again...
    xTaskCreatePinnedToCore(
                                SignalHandler,        /* Task function. */
                                "SignalHandler",      /* name of task. */
                                10000,                  /* Stack size of task */
                                NULL,                   /* parameter of the task */
                                0,                      /* priority of the task */
                                &SignalTask,              /* Task handle to keep track of created task */
                                0);                     /* pin task to core 0 */

}

// Sending / Receive signals at Core 0
void SignalHandler(void *pvParameters) {
    // Forever loop in this loop :P
    for(;;) {
        if(nrf_scanner == true) {
            if(millis() > last_nrf_scanned) {
                Serial.println("Inside nrf_scanner loop...");

                // Collecting X number of samples on each channel
                for(int i = 0; i < MAX_CHANNELS; i++) {
                    channel_loads[i] = 0x00;
                    for(int j = 0; j < MAX_SAMPLES; j++) {

                        // Select a new channel...
                        radio.setChannel(i);

                        // Listening for a while
                        radio.startListening();

                        // Delay for a while...
                        delayMicroseconds(128);

                        // Stop Listening
                        radio.stopListening();


                        // Read out RPD flag... set to 1 if received power > -64dBm
                        // return ( read_register(RPD) & 1 ) ;
                        if(radio.testCarrier()) {
                            channel_loads[i]++;
                        }
                    }
                    // Debug
                    Serial.print("Channel: ");
                    Serial.println(i);
                }

                // Wait for next second...
                last_nrf_scanned = millis() + 100;
            }
        } else if(nrf_enable) {
            // Disable it first...
            if(nrf_scanner)
                nrf_scanner = false;

            // Assign values...
            throttle_value = (throttle_value > throttle_max) ? throttle_max : throttle_value;       // Play safe... before mapping
            throttle_value = (throttle_value < throttle_min) ? throttle_min : throttle_value;
            yaw_value = (yaw_value > yaw_max) ? yaw_max : yaw_value;                                // Play safe... before mapping
            yaw_value = (yaw_value < yaw_min) ? yaw_min : yaw_value;
            pitch_value = (pitch_value > pitch_max) ? pitch_max : pitch_value;                      // Play safe... before mapping
            pitch_value = (pitch_value < pitch_min) ? pitch_min : pitch_value;
            roll_value = (roll_value > roll_max) ? roll_max : roll_value;                           // Play safe... before mapping
            roll_value = (roll_value < roll_min) ? roll_min : roll_value;

            // Mapping before sending...
            txmessage.throttle = map(throttle_value, throttle_min, throttle_max, 1000, 2000);
            txmessage.yaw      = map(yaw_value, yaw_min, yaw_max, 2000, 1000);
            txmessage.pitch    = map(pitch_value, pitch_min, pitch_max, 2000, 1000);
            txmessage.roll     = map(roll_value, roll_min, roll_max, 1000, 2000);
            txmessage.switches = switches_value;

            // Sending Data over NRF
            radio.write(&txmessage, sizeof(txmessage));

            // Profiling..
            nrf_profiling_raw++;
            // delay(1);
        } else if(binding) {
            Serial.print(F("Sending Binding Channel:"));
            Serial.println(freq_channel);
            syncdata.freq_channel = (uint8_t) freq_channel;
            syncdata.dummy = 100;
            syncdata.fixed = 88;
            // syncdata.pipe = pipeIn;

            // Sending Data over NRF
            radio.write(&syncdata, sizeof(SyncData));

            nrf_profiling_raw++;
        } else {
            delay(1);
        } // End of if(nrf_scanner)

    } // End of for(;;)
}

void loop() {

    // Each seconds tick
    if(millis() > last_seconds) {
        ble_profiling = ble_profiling_raw;
        ble_profiling_raw = 0;

        nrf_profiling = nrf_profiling_raw;
        nrf_profiling_raw = 0;

        /*
        Serial.print(F("NRF:"));
        Serial.println(nrf_profiling);
         */

        last_seconds = millis() + 1000;


        // For maximize the RF
        /*
        if(delay_maxrf) {
            if(millis() > delay_maxrf_time) {
                radio.setPALevel(RF24_PA_MAX);          // Set the transmit power to Maximum.
                delay_maxrf = false;                    // Only once.
            }
        }
        */

        // This is the way to reduce CPU cycles. Only enable it when require
        if(wifi_enable) {
            if(!webserver_setup) {
                /* Setting up OTA */
                setup_webserver();
                webserver_setup = true;
            } else {
                if(WiFi.status() == WL_CONNECTED) {
                    server.handleClient();
                } else {
                    Serial.println(F("WiFi not connected"));
                }
            }
        } else {
            if(webserver_setup) {
                unload_webserver();
                webserver_setup = false;
            }
        }
    }

    read_all_voltage();

    // Serial.println(throttle_raw + " : " + yaw_raw);
    /*
    Serial.print("T [");
    Serial.print(throttle_raw);
    Serial.print("] Y[");
    Serial.print(yaw_raw);
    Serial.print("] P[");
    Serial.print(pitch_raw);
    Serial.print("] R[");
    Serial.print(roll_value);
    Serial.println("]");
    Serial.print(sw1);
    Serial.print(sw2);
    Serial.print(sw3);
    Serial.print(sw4);
    Serial.print(sw5);
    Serial.println(sw6);
    */

    if(millis() > last_update) {
        // Determine stick position
        if(millis() > last_menu_stick_read) {
            if(roll_value > MENU_STICK_ROLL_MAX) {
                stick_navigation_position = MENU_RIGHT;
            } else if(roll_value < MENU_STICK_ROLL_MIN) {
                stick_navigation_position = MENU_LEFT;
            } else if(pitch_value > MENU_STICK_PITCH_MAX) {
                stick_navigation_position = MENU_OK;
            } else if(pitch_value < MENU_STICK_PITCH_MIN) {
                stick_navigation_position = MENU_BACK;
            } else if(yaw_value > MENU_STICK_YAW_MAX) {
                if(!invert_yaw)
                    stick_navigation_position = MENU_EXTEND_RIGHT;
                else
                    stick_navigation_position = MENU_EXTEND_LEFT;
            } else if(yaw_value < MENU_STICK_YAW_MIN) {
                if(!invert_yaw)
                    stick_navigation_position = MENU_EXTEND_LEFT;
                else
                    stick_navigation_position = MENU_EXTEND_RIGHT;
            } else {
                stick_navigation_position = MENU_NONE;
            } 

            // Next reading...
            last_menu_stick_read = millis() + 200;
        } else {
            stick_navigation_position = MENU_NONE;
        }

        /*** DEBUG START **/
        // rf_scanners();
        // return;
        /*** DEBUG END   **/

        if(
            !menu_entry_alpha_selected
        ) {
            // Determine the menu position
            if(stick_navigation_position == MENU_RIGHT) {
                to_right(&destination_alpha_state);
            } else if(stick_navigation_position == MENU_LEFT) {
                to_left(&destination_alpha_state);
            } else if(stick_navigation_position == MENU_OK) {
                menu_entry_alpha_selected = true;
            }
            last_menu_item_moved = millis() + 80;

            u8g2_left.firstPage();
            draw_menu(&current_alpha_state);

            // u8g2_left.setFont(u8g2_font_helvB10_tr);  
            u8g2_left.setFont(u8g2_font_DigitalDiscoThin_tr);  
            u8g2_left.setCursor((u8g2_left.getDisplayWidth()-u8g2_left.getStrWidth(menu_entry_alpha_list[destination_alpha_state.position].name))/2, u8g2_left.getDisplayHeight()-25);
            u8g2_left.print(menu_entry_alpha_list[destination_alpha_state.position].name);

            // Title....
            // u8g2_left.setFont(u8g2_font_micro_tr);
            u8g2_left.setFont(u8g2_font_5x7_tf);
            u8g2_left.setCursor(0,62);
            u8g2_left.print(F("OpenFlightTX - 1.0.10"));

            // Battery
            show_battery_level(115, 56);

            // WiFi (if connected)
            if(wifi_enable) {
                u8g2_left.setFont(u8g2_font_open_iconic_all_1x_t);
                u8g2_left.drawGlyph(105, 64, 281);

                // If connected then print the IP
                if(WiFi.status() == WL_CONNECTED) {
                    u8g2_left.setFont(u8g2_font_5x7_tf);
                    u8g2_left.setCursor(0, 55);
                    u8g2_left.print(WiFi.localIP());
                }
            } else if(binding) {
                u8g2_left.setFont(u8g2_font_5x7_tf);
                u8g2_left.setCursor(0, 53);
                u8g2_left.print(F("RX Binding in Progress"));
            }

            u8g2_left.nextPage();

            towards(&current_alpha_state, &destination_alpha_state);
        } else {
            if(stick_navigation_position == MENU_BACK) {
                if(require_unlock) {
                    if(sw6) {
                        menu_entry_alpha_selected = false;
                        // u8g2_right.clearBuffer();
                        // u8g2_right.sendBuffer();
                    }
                } else {
                    menu_entry_alpha_selected = false;
                    // u8g2_right.clearBuffer();
                    // u8g2_right.sendBuffer();
                }
            } else {
                if(menu_entry_alpha_list[destination_alpha_state.position].name == "RC Controller") {
                    require_unlock = true;
                    rc_controller();
                } else if(menu_entry_alpha_list[destination_alpha_state.position].name == "Battery") {
                    require_unlock = false;
                    battery_management();
                } else if(menu_entry_alpha_list[destination_alpha_state.position].name == "Bluetooth") {
                    require_unlock = true;
                    bluetooth_gamepad();
                } else if(menu_entry_alpha_list[destination_alpha_state.position].name == "Stick Calibration") {
                    require_unlock = true;
                    stick_calibration();
                } else if(menu_entry_alpha_list[destination_alpha_state.position].name == "Save Settings") {
                    require_unlock = false;
                    save_settings();
                } else if(menu_entry_alpha_list[destination_alpha_state.position].name == "Reset Default") {
                    require_unlock = false;
                    reset_default();
                } else if(menu_entry_alpha_list[destination_alpha_state.position].name == "Stick Positions") {
                    require_unlock = true;
                    stick_position();
                } else if(menu_entry_alpha_list[destination_alpha_state.position].name == "Trims") {
                    require_unlock = true;
                    stick_trims();
                } else if(menu_entry_alpha_list[destination_alpha_state.position].name == "WiFi") {
                    require_unlock = false;
                    wifi_enabler();
                } else if(menu_entry_alpha_list[destination_alpha_state.position].name == "RF Settings") {
                    require_unlock = false;
                    rf_settings();
                } else if(menu_entry_alpha_list[destination_alpha_state.position].name == "RF Scanners") {
                    require_unlock = true;
                    rf_scanners();
                } else if(menu_entry_alpha_list[destination_alpha_state.position].name == "RX Binding") {
                    require_unlock = false;
                    rx_binding();
                }

                //// u8g2_right.clearBuffer();
                // u8g2_right.setFontMode(1);
                // u8g2_right.setFont(u8g2_font_helvB10_tr);  
                // u8g2_right.setCursor(0,15);
                // u8g2_right.print(F("Selected:"));
                // u8g2_right.setCursor(0,30);
                // u8g2_right.print(F(menu_entry_alpha_list[destination_alpha_state.position].name));
                //// u8g2_right.firstPage();
                
                // Based on the selection and do the task
                //// u8g2_right.setFont(menu_entry_alpha_huge_list[destination_alpha_state.position].font);
                //// u8g2_right.drawGlyph(35, ICONX_Y, menu_entry_alpha_huge_list[destination_alpha_state.position].icon);
                //// u8g2_right.nextPage();
                //u8g2_right.sendBuffer();
            }
        }


        last_update = millis() + 1;
    }

    /*
    //if(millis() - last_update >= 2) {
        // Send LoRa packet to receiver
        //while(true) {
            LoRa.setTxPower(20);
            LoRa.beginPacket();
            // LoRa.print("hello ");
            LoRa.print(counter);
            LoRa.endPacket();
            counter++;
            //delay(100);
            // delayMicroseconds(1500);
       // }


      last_update = millis();
    //}
    */
}

void rx_checker(void) {
}

void rc_controller(void) {
    static char buf[16];

    if(!nrf_enable) {
        setup_nrf_tx();
    }

    /*
    // Trying to send in this loop
    if(!lora_enable) {
        setup_lora();
        lora_enable = true;
    // } else {
        //sendFixedMessage( byte ADDH,byte ADDL, byte CHAN, const void *message, const uint8_t size){
    //    ResponseStatus rs = e32ttl100.sendFixedMessage(ADDR_H, ADDR_L, CHANNEL, "Welcome1");
    }
    */

    u8g2_left.clearBuffer();
    u8g2_left.firstPage();
    u8g2_left.setFont(u8g2_font_ImpactBits_tr);  
    u8g2_left.setCursor(0, 13);
    u8g2_left.print(F("OpenFlightTX RC"));

    show_battery_level(0, 56);

    draw_gimbalbox(53, 31, 32);
    draw_gimbalbox(90, 31, 32);

    uint16_t gimbal_y_raw = throttle_value;
    uint16_t gimbal_x_raw = yaw_value;
    if(gimbal_y_raw < throttle_min) gimbal_y_raw = throttle_min;
    if(gimbal_y_raw > throttle_max) gimbal_y_raw = throttle_max;
    if(gimbal_x_raw < yaw_min) gimbal_x_raw = yaw_min;
    if(gimbal_x_raw > yaw_max) gimbal_x_raw = yaw_max;

    uint8_t gimbal_y;
    if(!invert_throttle)
        gimbal_y = map(gimbal_y_raw, throttle_min, throttle_max, 0, 31);    // Throttle
    else
        gimbal_y = map(gimbal_y_raw, throttle_max, throttle_min, 0, 31);    // Throttle

    uint8_t gimbal_x;
    if(!invert_yaw)
        gimbal_x = map(gimbal_x_raw, yaw_min, yaw_max, 0, 31);              // Yaw
    else
        gimbal_x = map(gimbal_x_raw, yaw_max, yaw_min, 0, 31);              // Yaw

    // u8g2_left.drawFilledEllipse(64 + gimbal_x, gimbal_y, 2, 2, U8G2_DRAW_ALL);
    u8g2_left.drawFilledEllipse(53 + gimbal_x, 31 + gimbal_y, 2, 2, U8G2_DRAW_ALL);


    // Mapping based on the live values
    gimbal_y_raw = pitch_value;
    gimbal_x_raw = roll_value;
    if(gimbal_y_raw < pitch_min) gimbal_y_raw = pitch_min;
    if(gimbal_y_raw > pitch_max) gimbal_y_raw = pitch_max;
    if(gimbal_x_raw < roll_min) gimbal_x_raw = roll_min;
    if(gimbal_x_raw > roll_max) gimbal_x_raw = roll_max;

    if(!invert_pitch)
        gimbal_y = map(gimbal_y_raw, pitch_min, pitch_max, 0, 31);          // Pitch
    else
        gimbal_y = map(gimbal_y_raw, pitch_max, pitch_min, 0, 31);          // Pitch

    if(!invert_roll)
        gimbal_x = map(gimbal_x_raw, roll_min, roll_max, 0, 31);            // Roll
    else
        gimbal_x = map(gimbal_x_raw, roll_max, roll_min, 0, 31);            // Roll

    // u8g2_right.drawFilledEllipse(gimbal_x, gimbal_y, 3, 3, U8G2_DRAW_ALL);
    u8g2_left.drawFilledEllipse(90 + gimbal_x, 31 + gimbal_y, 2, 2, U8G2_DRAW_ALL);

    // Calculate uptime
    int hours = uptime / 3600;
    int minutes = (uptime / 60) % 60;
    int seconds = uptime % 60;
   
    u8g2_left.setFont(u8g2_font_5x7_tf);
    sprintf(buf, "%d packets/s", nrf_profiling);
    u8g2_left.drawStr(0, 24, buf);
    sprintf(buf, "%d MHz", freq_channel + 2400);
    u8g2_left.drawStr(0, 32, buf);

    sprintf(buf, "%02d:%02d:%02d", hours, minutes, seconds);
    u8g2_left.drawStr(84, 24, buf);

    switch(freq_txpower) {
        case 0:
            sprintf(buf, "TX MIN");
            break;
        case 1:
            sprintf(buf, "TX LOW");
            break;
        case 2:
            sprintf(buf, "TX HIGH");
            break;
        case 3:
            sprintf(buf, "TX MAX");
            break;
    }
    u8g2_left.drawStr(0, 40, buf);

    if(!sw6) {
        u8g2_left.setCursor(20, 63);
        u8g2_left.print(F("LOCKED"));
    }

    if(sw1) {
        u8g2_left.setCursor(0, 48);
        u8g2_left.print(F("A"));
    }
    
    if(sw2) {
        u8g2_left.setCursor(5, 48);
        u8g2_left.print(F("B"));
    }
    
    if(sw3) {
        u8g2_left.setCursor(10, 48);
        u8g2_left.print(F("C"));
    }
    
    if(sw4) {
        u8g2_left.setCursor(15, 48);
        u8g2_left.print(F("D"));
    }

    if(sw5) {
        u8g2_left.setCursor(20, 48);
        u8g2_left.print(F("E"));
    }
    
    
    u8g2_left.nextPage();
}

void stick_trims(void) {
    static int8_t stick_position_pointer = 0;

    // Data processing
    if(stick_navigation_position == MENU_LEFT) {
        if(stick_position_pointer == 0) {
            trim_throttle--;    
        } else if(stick_position_pointer == 1) {
            trim_yaw--;
        } else if(stick_position_pointer == 2) {
            trim_pitch--;
        } else if(stick_position_pointer == 3) {
            trim_roll--;
        }
    } else if(stick_navigation_position == MENU_RIGHT) {
        if(stick_position_pointer == 0) {
            trim_throttle++;    
        } else if(stick_position_pointer == 1) {
            trim_yaw++;
        } else if(stick_position_pointer == 2) {
            trim_pitch++;
        } else if(stick_position_pointer == 3) {
            trim_roll++;
        }
    } else if(stick_navigation_position == MENU_OK) {
        stick_position_pointer++;
    }

    // Done...Lets wait for next
    stick_navigation_position = MENU_NONE;

    // Limit counts
    if(stick_position_pointer > 3) stick_position_pointer = 0;

    if(trim_throttle < -15) trim_throttle = -15;
    if(trim_throttle > 15) trim_throttle = 15;
    if(trim_yaw < -15) trim_yaw = -15;
    if(trim_yaw > 15) trim_yaw = 15;
    if(trim_pitch < -15) trim_pitch = -15;
    if(trim_pitch > 15) trim_pitch = 15;
    if(trim_roll < -15) trim_roll = -15;
    if(trim_roll > 15) trim_roll = 15;

    // Displaying...
    u8g2_left.clearBuffer();
    u8g2_left.firstPage();
    u8g2_left.setFont(u8g2_font_ImpactBits_tr);  
    u8g2_left.setCursor(24, 11); 
    u8g2_left.print(F("Stick Trims"));
    u8g2_left.setFont(u8g2_font_5x7_tf);
    u8g2_left.setCursor(0, 25);
    u8g2_left.print(F("THR:"));
    u8g2_left.setCursor(0, 35);
    u8g2_left.print(F("YAW:"));
    u8g2_left.setCursor(0, 45);
    u8g2_left.print(F("PIT:"));
    u8g2_left.setCursor(0, 55);
    u8g2_left.print(F("ROL:"));

    // Drawing the Bars
    // Throttle Trims
    int8_t trim_values = trim_throttle;
    
    u8g2_left.drawFrame(20, 18, 34, 8);
    if(trim_throttle >= 0) {
        u8g2_left.drawBox(37, 20, trim_throttle, 4);
    } else {
        u8g2_left.drawBox(37 + trim_throttle, 20, -(trim_throttle), 4);
    }

    u8g2_left.drawFrame(20, 28, 34, 9);
    if(trim_yaw >= 0) {
        u8g2_left.drawBox(37, 30, trim_yaw, 4);
    } else {
        u8g2_left.drawBox(37 + trim_yaw, 30, -(trim_yaw), 4);
    }

    u8g2_left.drawFrame(20, 38, 34, 9);
    if(trim_pitch >= 0) {
        u8g2_left.drawBox(37, 40, trim_pitch, 4);
    } else {
        u8g2_left.drawBox(37 + trim_pitch, 40, -(trim_pitch), 4);
    }

    u8g2_left.drawFrame(20, 48, 34, 9);
    if(trim_roll >= 0) {
        u8g2_left.drawBox(37, 50, trim_roll, 4);
    } else {
        u8g2_left.drawBox(37 + trim_roll, 50, -(trim_roll), 4);
    }

    // Pointers
    // u8g2_left.setCursor(25, 25 + (stick_position_pointer * 10));
    // u8g2_left.print(F("<"));
    // u8g2_left.setFont(u8g2_font_m2icon_9_tf);
    // u8g2_left.drawGlyph(38, 27 + (stick_position_pointer * 10), 97);        // <- Pointer icon

    /*
    static char buf[6];
    strcpy(buf, ltoa(trim_throttle, buf, 10));
    u8g2_right.drawStr(100, 35,buf);

    if(reset_selections) {
        u8g2_left.drawFrame(26, 40, 30, 20);
    } else {
        u8g2_left.drawFrame(62, 40, 30, 20);
    }
    */
    draw_gimbalbox(60, 25, 33);
    draw_gimbalbox(95, 25, 33);

    uint16_t gimbal_y_raw = throttle_value;
    uint16_t gimbal_x_raw = yaw_value;
    if(gimbal_y_raw < throttle_min) gimbal_y_raw = throttle_min;
    if(gimbal_y_raw > throttle_max) gimbal_y_raw = throttle_max;
    if(gimbal_x_raw < yaw_min) gimbal_x_raw = yaw_min;
    if(gimbal_x_raw > yaw_max) gimbal_x_raw = yaw_max;

    uint8_t gimbal_y;
    if(!invert_throttle)
        gimbal_y = map(gimbal_y_raw, throttle_min, throttle_max, 0, 31);    // Throttle
    else
        gimbal_y = map(gimbal_y_raw, throttle_max, throttle_min, 0, 31);    // Throttle

    uint8_t gimbal_x;
    if(!invert_yaw)
        gimbal_x = map(gimbal_x_raw, yaw_min, yaw_max, 0, 31);              // Yaw
    else
        gimbal_x = map(gimbal_x_raw, yaw_max, yaw_min, 0, 31);              // Yaw

    // u8g2_left.drawFilledEllipse(64 + gimbal_x, gimbal_y, 2, 2, U8G2_DRAW_ALL);
    u8g2_left.drawFilledEllipse(60 + gimbal_x, 25 + gimbal_y, 2, 2, U8G2_DRAW_ALL);


    // Mapping based on the live values
    gimbal_y_raw = pitch_value;
    gimbal_x_raw = roll_value;
    if(gimbal_y_raw < pitch_min) gimbal_y_raw = pitch_min;
    if(gimbal_y_raw > pitch_max) gimbal_y_raw = pitch_max;
    if(gimbal_x_raw < roll_min) gimbal_x_raw = roll_min;
    if(gimbal_x_raw > roll_max) gimbal_x_raw = roll_max;

    if(!invert_pitch)
        gimbal_y = map(gimbal_y_raw, pitch_min, pitch_max, 0, 31);          // Pitch
    else
        gimbal_y = map(gimbal_y_raw, pitch_max, pitch_min, 0, 31);          // Pitch

    if(!invert_roll)
        gimbal_x = map(gimbal_x_raw, roll_min, roll_max, 0, 31);            // Roll
    else
        gimbal_x = map(gimbal_x_raw, roll_max, roll_min, 0, 31);            // Roll

    // u8g2_right.drawFilledEllipse(gimbal_x, gimbal_y, 3, 3, U8G2_DRAW_ALL);
    u8g2_left.drawFilledEllipse(95 + gimbal_x, 25 + gimbal_y, 2, 2, U8G2_DRAW_ALL);


    u8g2_left.setFont(u8g2_font_m2icon_9_tf);
    u8g2_left.drawGlyph(60, 27 + (stick_position_pointer * 10), 97);        // <- Pointer icon

    u8g2_left.nextPage();
}

void stick_position(void) {
    static int8_t stick_position_pointer = 0;

    // Data processing
    if(stick_navigation_position == MENU_LEFT) {
        if(stick_position_pointer == 0) {
            trim_throttle--;    
        } else if(stick_position_pointer == 1) {
            trim_yaw--;
        } else if(stick_position_pointer == 2) {
            trim_pitch--;
        } else if(stick_position_pointer == 3) {
            trim_roll--;
        }
    } else if(stick_navigation_position == MENU_RIGHT) {
        if(stick_position_pointer == 0) {
            trim_throttle++;    
        } else if(stick_position_pointer == 1) {
            trim_yaw++;
        } else if(stick_position_pointer == 2) {
            trim_pitch++;
        } else if(stick_position_pointer == 3) {
            trim_roll++;
        }
    } else if(stick_navigation_position == MENU_EXTEND_LEFT) {
        if(stick_position_pointer == 0) {
            invert_throttle = false;    
        } else if(stick_position_pointer == 1) {
            invert_yaw = false;
        } else if(stick_position_pointer == 2) {
            invert_pitch = false;
        } else if(stick_position_pointer == 3) {
            invert_roll = false;
        }
    } else if(stick_navigation_position == MENU_EXTEND_RIGHT) {
        if(stick_position_pointer == 0) {
            invert_throttle = true;    
        } else if(stick_position_pointer == 1) {
            invert_yaw = true;
        } else if(stick_position_pointer == 2) {
            invert_pitch = true;
        } else if(stick_position_pointer == 3) {
            invert_roll = true;
        }
    } else if(stick_navigation_position == MENU_OK) {
        stick_position_pointer++;
    }

    // Done...Lets wait for next
    stick_navigation_position = MENU_NONE;

    // Limit counts
    if(stick_position_pointer > 3) stick_position_pointer = 0;

    if(trim_throttle < -15) trim_throttle = -15;
    if(trim_throttle > 15) trim_throttle = 15;
    if(trim_yaw < -15) trim_yaw = -15;
    if(trim_yaw > 15) trim_yaw = 15;
    if(trim_pitch < -15) trim_pitch = -15;
    if(trim_pitch > 15) trim_pitch = 15;
    if(trim_roll < -15) trim_roll = -15;
    if(trim_roll > 15) trim_roll = 15;
    
    // Displaying...
    u8g2_left.clearBuffer();
    u8g2_left.firstPage();
    u8g2_left.setFont(u8g2_font_ImpactBits_tr);  
    u8g2_left.setCursor(10, 11); 
    u8g2_left.print(F("Stick Positions"));
    u8g2_left.setFont(u8g2_font_5x7_tf);
    u8g2_left.setCursor(0, 25);
    u8g2_left.print(F("THR:"));
    if(invert_throttle) {
        u8g2_left.print(F("INV"));
    } else {
        u8g2_left.print(F("NOR"));
    }
    u8g2_left.setCursor(0, 35);
    u8g2_left.print(F("YAW:"));
    if(invert_yaw) {
        u8g2_left.print(F("INV"));
    } else {
        u8g2_left.print(F("NOR"));
    }
    u8g2_left.setCursor(0, 45);
    u8g2_left.print(F("PIT:"));
    if(invert_pitch) {
        u8g2_left.print(F("INV"));
    } else {
        u8g2_left.print(F("NOR"));
    }
    u8g2_left.setCursor(0, 55);
    u8g2_left.print(F("ROL:"));
    if(invert_roll) {
        u8g2_left.print(F("INV"));
    } else {
        u8g2_left.print(F("NOR"));
    }

    // Pointers
    // u8g2_left.setCursor(25, 25 + (stick_position_pointer * 10));
    // u8g2_left.print(F("<"));
    u8g2_left.setFont(u8g2_font_m2icon_9_tf);
    u8g2_left.drawGlyph(38, 27 + (stick_position_pointer * 10), 97);        // <- Pointer icon

    /*
    static char buf[6];
    strcpy(buf, ltoa(trim_throttle, buf, 10));
    u8g2_right.drawStr(100, 35,buf);

    if(reset_selections) {
        u8g2_left.drawFrame(26, 40, 30, 20);
    } else {
        u8g2_left.drawFrame(62, 40, 30, 20);
    }
    */
    draw_gimbalbox(53, 25, 32);
    draw_gimbalbox(90, 25, 32);

    uint16_t gimbal_y_raw = throttle_value;
    uint16_t gimbal_x_raw = yaw_value;
    if(gimbal_y_raw < throttle_min) gimbal_y_raw = throttle_min;
    if(gimbal_y_raw > throttle_max) gimbal_y_raw = throttle_max;
    if(gimbal_x_raw < yaw_min) gimbal_x_raw = yaw_min;
    if(gimbal_x_raw > yaw_max) gimbal_x_raw = yaw_max;

    uint8_t gimbal_y;
    if(!invert_throttle)
        gimbal_y = map(gimbal_y_raw, throttle_min, throttle_max, 0, 31);    // Throttle
    else
        gimbal_y = map(gimbal_y_raw, throttle_max, throttle_min, 0, 31);    // Throttle

    uint8_t gimbal_x;
    if(!invert_yaw)
        gimbal_x = map(gimbal_x_raw, yaw_min, yaw_max, 0, 31);              // Yaw
    else
        gimbal_x = map(gimbal_x_raw, yaw_max, yaw_min, 0, 31);              // Yaw

    // u8g2_left.drawFilledEllipse(64 + gimbal_x, gimbal_y, 2, 2, U8G2_DRAW_ALL);
    u8g2_left.drawFilledEllipse(53 + gimbal_x, 25 + gimbal_y, 2, 2, U8G2_DRAW_ALL);


    // Mapping based on the live values
    gimbal_y_raw = pitch_value;
    gimbal_x_raw = roll_value;
    if(gimbal_y_raw < pitch_min) gimbal_y_raw = pitch_min;
    if(gimbal_y_raw > pitch_max) gimbal_y_raw = pitch_max;
    if(gimbal_x_raw < roll_min) gimbal_x_raw = roll_min;
    if(gimbal_x_raw > roll_max) gimbal_x_raw = roll_max;

    if(!invert_pitch)
        gimbal_y = map(gimbal_y_raw, pitch_min, pitch_max, 0, 31);          // Pitch
    else
        gimbal_y = map(gimbal_y_raw, pitch_max, pitch_min, 0, 31);          // Pitch

    if(!invert_roll)
        gimbal_x = map(gimbal_x_raw, roll_min, roll_max, 0, 31);            // Roll
    else
        gimbal_x = map(gimbal_x_raw, roll_max, roll_min, 0, 31);            // Roll

    // u8g2_right.drawFilledEllipse(gimbal_x, gimbal_y, 3, 3, U8G2_DRAW_ALL);
    u8g2_left.drawFilledEllipse(90 + gimbal_x, 25 + gimbal_y, 2, 2, U8G2_DRAW_ALL);

    u8g2_left.nextPage();

    // u8g2_right.clearBuffer();
    // u8g2_right.firstPage();
}

void read_settings(void) {
    uint16_t addr = 0x00;

    // Throttle Max
    throttle_max = EEPROM.read(addr) | EEPROM.read(addr + 1) << 8;

    // New unit / reset.... dont need to read....
    if(throttle_max == 0) return;

    // Throttle Min
    addr = 2;
    throttle_min = EEPROM.read(addr) | EEPROM.read(addr + 1) << 8;

    // Yaw Max
    addr = 4;
    yaw_max = EEPROM.read(addr) | EEPROM.read(addr + 1) << 8;

    // Yaw Min
    addr = 6;
    yaw_min = EEPROM.read(addr) | EEPROM.read(addr + 1) << 8;

    // Pitch Max
    addr = 8;
    pitch_max = EEPROM.read(addr) | EEPROM.read(addr + 1) << 8;

    // Pitch Min
    addr = 10;
    pitch_min = EEPROM.read(addr) | EEPROM.read(addr + 1) << 8;

    // Roll Max
    addr = 12;
    roll_max = EEPROM.read(addr) | EEPROM.read(addr + 1) << 8;

    // Roll Min
    addr = 14;
    roll_min = EEPROM.read(addr) | EEPROM.read(addr + 1) << 8;

    // Inverted Values
    addr = 16;
    int8_t val = EEPROM.read(addr);
    invert_throttle = ((val & 0b00001000) >> 3) ? true : false;
    invert_yaw      = ((val & 0b00000100) >> 2) ? true : false;
    invert_pitch    = ((val & 0b00000010) >> 1) ? true : false;
    invert_roll     = ((val & 0b00000001) >> 0) ? true : false;

    // Throttle Trim
    addr = 17;
    trim_throttle = EEPROM.read(addr);

    // Yaw Trim
    addr = 18;
    trim_yaw = EEPROM.read(addr);

    // Pitch Trim
    addr = 19;
    trim_pitch = EEPROM.read(addr);

    // Roll Trim
    addr = 20;
    trim_roll = EEPROM.read(addr);

    // Frequency Channel
    addr = 21;
    freq_channel = EEPROM.read(addr);

    // Frequency TxPower
    addr = 22;
    freq_txpower = EEPROM.read(addr);
}

void commit_all(void) {
/*
 * Memory Format:
 * 
 * TTttYYyyPPppRRrrPABCDFf
 *
 * TT: Throttle Max
 * tt: Throttle Min
 * YY: Yaw Max
 * yy: Yaw Min
 * PP: Pitch Max
 * pp: Pitch Min
 * RR: Roll Max
 * rr: Roll Min
 * P : Invert Gimbals (0000TYPR)
 * A : Throttle Trim
 * B : Yaw Trim
 * C : Pitch Trim
 * D : Roll Trim
 * F : NRF Channels (0 ~ 124) Total 125 channels
 * f : TX Power (MIN, LOW, HIGH and MAX)
 */
    uint16_t addr = 0x00;

    // Throttle Max
    write_data(addr++, throttle_max & 0xFF);            // Low bits
    write_data(addr++, throttle_max >> 8 & 0xFF);       // High bits

    // Throttle Min
    write_data(addr++, throttle_min & 0xFF);            // Low bits
    write_data(addr++, throttle_min >> 8 & 0xFF);       // High bits

    // Yaw Max
    write_data(addr++, yaw_max & 0xFF);                 // Low bits
    write_data(addr++, yaw_max >> 8 & 0xFF);            // High bits

    // Yaw Min
    write_data(addr++, yaw_min & 0xFF);                 // Low bits
    write_data(addr++, yaw_min >> 8 & 0xFF);            // High bits

    // Pitch Max
    write_data(addr++, pitch_max & 0xFF);               // Low bits
    write_data(addr++, pitch_max >> 8 & 0xFF);          // High bits

    // Pitch Min
    write_data(addr++, pitch_min & 0xFF);               // Low bits
    write_data(addr++, pitch_min >> 8 & 0xFF);          // High bits

    // Roll Max
    write_data(addr++, roll_max & 0xFF);                // Low bits
    write_data(addr++, roll_max >> 8 & 0xFF);           // High bits

    // Roll Min
    write_data(addr++, roll_min & 0xFF);                // Low bits
    write_data(addr++, roll_min >> 8 & 0xFF);           // High bits

    // Inverted Gimbals
    // uint8_t val = invert_throttle << 3 || invert_yaw << 2 || invert_pitch << 1 || invert_roll;
    uint8_t val = ((invert_throttle) ? 1 << 3 : 0) + ((invert_yaw) ? 1 << 2 : 0) + ((invert_pitch) ? 1 << 1 : 0) + ((invert_roll) ? 1 : 0);
    
    Serial.println(val);
    write_data(addr++, val);

    // Throttle Trim
    write_data(addr++, trim_throttle);

    // Yaw Trim
    write_data(addr++, trim_yaw);

    // Pitch Trim
    write_data(addr++, trim_pitch);

    // Roll Trim
    write_data(addr++, trim_roll);            

    // Frequency Channel
    write_data(addr++, freq_channel);

    // Frequency Tx Power
    write_data(addr++, freq_txpower);
}

void write_data(uint16_t addr, uint8_t val) {
    // timerAlarmDisable(timer);
    EEPROM.write(addr, val);
    EEPROM.commit();
    // timerAlarmEnable(timer);
}

void reset_settings(void) {
    for(int i = 0; i < 255; i++) {
        write_data(i, 0x00);
    }

    // Reboot
    reboot();
}

void reboot(void) {
    ESP.restart();
}

void reset_default(void) {
    static bool reset_selections = false;
    u8g2_left.clearBuffer();
    u8g2_left.firstPage();
    u8g2_left.setFont(u8g2_font_ImpactBits_tr);  
    u8g2_left.setCursor(10, 25); 
    u8g2_left.print(F("Reset Settings?"));
    u8g2_left.setCursor(30, 55); 
    u8g2_left.print(F("Yes   No"));

    if(stick_navigation_position == MENU_LEFT) {
        reset_selections = true;
    } else if(stick_navigation_position == MENU_RIGHT) {
        reset_selections = false;
    } else if(stick_navigation_position == MENU_OK) {
        // Reset Settings..
        if(reset_selections)
            reset_settings();

        // Simulate...so it will return back..
        menu_entry_alpha_selected = false;
        last_menu_stick_read = millis() + 200;
    }

    if(reset_selections) {
        u8g2_left.drawFrame(26, 40, 30, 20);
    } else {
        u8g2_left.drawFrame(62, 40, 30, 20);
    }
    u8g2_left.nextPage();

    // u8g2_right.clearBuffer();
    // u8g2_right.firstPage();
    // u8g2_right.nextPage();
}

void save_settings(void) {
    static bool save_selections = false;
    u8g2_left.clearBuffer();
    u8g2_left.firstPage();
    u8g2_left.setFont(u8g2_font_ImpactBits_tr);  
    u8g2_left.setCursor(10, 25);
    u8g2_left.print(F("Save Settings?"));
    u8g2_left.setCursor(30, 55);
    u8g2_left.print(F("Yes   No"));

    if(stick_navigation_position == MENU_LEFT) {
        save_selections = true;
    } else if(stick_navigation_position == MENU_RIGHT) {
        save_selections = false;
    } else if(stick_navigation_position == MENU_OK) {
        // Save settings
        if(save_selections) 
            commit_all();

        // Simulate...so it will return back..
        menu_entry_alpha_selected = false;
        last_menu_stick_read = millis() + 200;
    }

    if(save_selections) {
        u8g2_left.drawFrame(26, 40, 30, 20);
    } else {
        u8g2_left.drawFrame(62, 40, 30, 20);
    }
    u8g2_left.nextPage();

    // u8g2_right.clearBuffer();
    // u8g2_right.firstPage();
    // u8g2_right.nextPage();
}

// Draw the gimbal box
void draw_gimbalbox(uint8_t x, uint8_t y) {
    uint8_t w  = 63;
    uint8_t hm = 31;
    uint8_t vm = x + hm;

    // showing the gimbal range
    u8g2_left.drawFrame(x, y, w, w);

    // showing the crosshair
    // horizontal
    for(uint8_t i = x; i < (x+w); i+=5) {
        u8g2_left.drawPixel(i, hm);
    }
    
    // vertical
    for(uint8_t i = y; i < (y+w); i+=5) {
        u8g2_left.drawPixel(vm, i);
    }
}

void draw_gimbalbox(uint8_t x, uint8_t y, uint8_t size) {
    uint8_t w  = size;
    uint8_t hm = y + (size / 2);
    uint8_t vm = x + (size / 2);

    // showing the gimbal range
    u8g2_left.drawFrame(x, y, w, w);

    // showing the crosshair
    // horizontal
    for(uint8_t i = x; i < (x+w); i+=2) {
        u8g2_left.drawPixel(i, hm);
    }
    
    // vertical
    for(uint8_t i = y; i < (y+w); i+=2) {
        u8g2_left.drawPixel(vm, i);
    }
}

void stick_calibration() {
    static char buf[6];

    u8g2_left.clearBuffer();
    u8g2_left.firstPage();

    u8g2_left.setFont(u8g2_font_ImpactBits_tr);  
    u8g2_left.setCursor(10, 11); 
    u8g2_left.print(F("Stick Calibration"));

    // showing the gimbal range
    // draw_gimbalbox(64, 0);
    draw_gimbalbox(31, 32, 32);
    draw_gimbalbox(65, 32, 32);

    // Getting the throttle minimum
    if(throttle_value < throttle_min) {
        throttle_min = throttle_value;
    }
    if(throttle_value > throttle_max) {
        throttle_max = throttle_value;
    }
    if(yaw_value < yaw_min) {
        yaw_min = yaw_value;
    }
    if(yaw_value > yaw_max) {
        yaw_max = yaw_value;
    }
    if(pitch_value < pitch_min) pitch_min = pitch_value;
    if(pitch_value > pitch_max) pitch_max = pitch_value;
    if(roll_value < roll_min) roll_min = roll_value;
    if(roll_value > roll_max) roll_max = roll_value;

    // Mapping based on the live values
    uint16_t gimbal_y_raw = throttle_value;
    uint16_t gimbal_x_raw = yaw_value;
    if(gimbal_y_raw < throttle_min) gimbal_y_raw = throttle_min;
    if(gimbal_y_raw > throttle_max) gimbal_y_raw = throttle_max;
    if(gimbal_x_raw < yaw_min) gimbal_x_raw = yaw_min;
    if(gimbal_x_raw > yaw_max) gimbal_x_raw = yaw_max;

    uint8_t gimbal_y;
    if(!invert_throttle)
        gimbal_y = map(gimbal_y_raw, throttle_min, throttle_max, 0, 31);    // Throttle
    else
        gimbal_y = map(gimbal_y_raw, throttle_max, throttle_min, 0, 31);    // Throttle

    uint8_t gimbal_x;
    if(!invert_yaw)
        gimbal_x = map(gimbal_x_raw, yaw_min, yaw_max, 0, 31);              // Yaw
    else
        gimbal_x = map(gimbal_x_raw, yaw_max, yaw_min, 0, 31);              // Yaw

    // u8g2_left.drawFilledEllipse(64 + gimbal_x, gimbal_y, 2, 2, U8G2_DRAW_ALL);
    u8g2_left.drawFilledEllipse(32 + gimbal_x, 32 + gimbal_y, 2, 2, U8G2_DRAW_ALL);

    // Throttle
    u8g2_left.setFont(u8g2_font_5x7_tf);
    u8g2_left.setCursor(0, 24);
    u8g2_left.print(F("Throttle:"));
    strcpy(buf, ltoa(throttle_min, buf, 10));
    u8g2_left.drawStr(0, 32, buf);
    strcpy(buf, ltoa(throttle_max, buf, 10));
    u8g2_left.drawStr(0, 40, buf);

    // Yaw
    u8g2_left.setCursor(0, 48);
    u8g2_left.print(F("Yaw:"));
    strcpy(buf, ltoa(yaw_min, buf, 10));
    u8g2_left.drawStr(0, 56, buf);
    strcpy(buf, ltoa(yaw_max, buf, 10));
    u8g2_left.drawStr(0, 64, buf);

    // Getting the pitch minimum
    if(pitch_value < pitch_min) {
        pitch_min = pitch_value;
    }
    if(pitch_value > pitch_max) {
        pitch_max = pitch_value;
    }
    if(roll_value < roll_min) {
        roll_min = roll_value;
    }
    if(roll_value > roll_max) {
        roll_max = roll_value;
    }
    if(pitch_value < pitch_min) pitch_min = pitch_value;
    if(pitch_value > pitch_max) pitch_max = pitch_value;
    if(roll_value < roll_min) roll_min = roll_value;
    if(roll_value > roll_max) roll_max = roll_value;

    // Mapping based on the live values
    gimbal_y_raw = pitch_value;
    gimbal_x_raw = roll_value;
    if(gimbal_y_raw < pitch_min) gimbal_y_raw = pitch_min;
    if(gimbal_y_raw > pitch_max) gimbal_y_raw = pitch_max;
    if(gimbal_x_raw < roll_min) gimbal_x_raw = roll_min;
    if(gimbal_x_raw > roll_max) gimbal_x_raw = roll_max;

    if(!invert_pitch)
        gimbal_y = map(gimbal_y_raw, pitch_min, pitch_max, 0, 31);          // Pitch
    else
        gimbal_y = map(gimbal_y_raw, pitch_max, pitch_min, 0, 31);          // Pitch

    if(!invert_roll)
        gimbal_x = map(gimbal_x_raw, roll_min, roll_max, 0, 31);            // Roll
    else
        gimbal_x = map(gimbal_x_raw, roll_max, roll_min, 0, 31);            // Roll

    // u8g2_right.drawFilledEllipse(gimbal_x, gimbal_y, 3, 3, U8G2_DRAW_ALL);
    u8g2_left.drawFilledEllipse(66 + gimbal_x, 32 + gimbal_y, 2, 2, U8G2_DRAW_ALL);

    // Pitch
    u8g2_left.setCursor(99, 24);
    u8g2_left.print(F("Pitch:"));
    strcpy(buf, ltoa(pitch_min, buf, 10));
    u8g2_left.drawStr(99, 32, buf);
    strcpy(buf, ltoa(pitch_max, buf, 10));
    u8g2_left.drawStr(99, 40, buf);

    // Roll
    u8g2_left.setCursor(99, 48);
    u8g2_left.print(F("Roll:"));
    strcpy(buf, ltoa(roll_min, buf, 10));
    u8g2_left.drawStr(99, 56, buf);
    strcpy(buf, ltoa(roll_max, buf, 10));
    u8g2_left.drawStr(99, 64, buf);

    u8g2_left.nextPage();
}

void bluetooth_gamepad() {
    u8g2_left.clearBuffer();
    u8g2_left.firstPage();
    /*
    u8g2_left.setFont(menu_entry_alpha_huge_list[destination_alpha_state.position].font);
    u8g2_left.drawGlyph(40, ICONX_Y, menu_entry_alpha_huge_list[destination_alpha_state.position].icon);
    u8g2_left.nextPage();

    u8g2_right.clearBuffer();
    u8g2_right.firstPage();
    u8g2_right.setFontMode(1);
    u8g2_right.setFont(u8g2_font_ImpactBits_tr);  
    */

    static char buf[6];

    int throttle = map(throttle_value, 0, 4096, 127, -127); // Throttle
    int yaw = map(yaw_value, 0, 4096, -127, 127);           // Yaw
    int pitch = map(pitch_value, 0, 4096, 127, -127);       // Pitch
    int roll = map(roll_value, 0, 4096, -127, 127);         // Roll

    u8g2_left.setFontMode(1);
    // u8g2_left.setFont(u8g2_font_ImpactBits_tr);  
    u8g2_left.setFont(u8g2_font_tenfatguys_tf);
    u8g2_left.setCursor(10, 16);
    u8g2_left.print(F("BT GAMEPAD"));

    show_battery_level(0, 56);

    draw_gimbalbox(31, 32, 32);
    draw_gimbalbox(65, 32, 32);

    
    // Mapping based on the live values
    uint16_t gimbal_y_raw = throttle_value;
    uint16_t gimbal_x_raw = yaw_value;
    if(gimbal_y_raw < throttle_min) gimbal_y_raw = throttle_min;
    if(gimbal_y_raw > throttle_max) gimbal_y_raw = throttle_max;
    if(gimbal_x_raw < yaw_min) gimbal_x_raw = yaw_min;
    if(gimbal_x_raw > yaw_max) gimbal_x_raw = yaw_max;

    uint8_t gimbal_y;
    if(!invert_throttle)
        gimbal_y = map(gimbal_y_raw, throttle_min, throttle_max, 0, 31);    // Throttle
    else
        gimbal_y = map(gimbal_y_raw, throttle_max, throttle_min, 0, 31);    // Throttle

    uint8_t gimbal_x;
    if(!invert_yaw)
        gimbal_x = map(gimbal_x_raw, yaw_min, yaw_max, 0, 31);              // Yaw
    else
        gimbal_x = map(gimbal_x_raw, yaw_max, yaw_min, 0, 31);              // Yaw

    u8g2_left.drawFilledEllipse(32 + gimbal_x, 32 + gimbal_y, 2, 2, U8G2_DRAW_ALL);


    // Mapping based on the live values
    gimbal_y_raw = pitch_value;
    gimbal_x_raw = roll_value;
    if(gimbal_y_raw < pitch_min) gimbal_y_raw = pitch_min;
    if(gimbal_y_raw > pitch_max) gimbal_y_raw = pitch_max;
    if(gimbal_x_raw < roll_min) gimbal_x_raw = roll_min;
    if(gimbal_x_raw > roll_max) gimbal_x_raw = roll_max;

    if(!invert_pitch)
        gimbal_y = map(gimbal_y_raw, pitch_min, pitch_max, 0, 31);          // Pitch
    else
        gimbal_y = map(gimbal_y_raw, pitch_max, pitch_min, 0, 31);          // Pitch

    if(!invert_roll)
        gimbal_x = map(gimbal_x_raw, roll_min, roll_max, 0, 31);            // Roll
    else
        gimbal_x = map(gimbal_x_raw, roll_max, roll_min, 0, 31);            // Roll

    u8g2_left.drawFilledEllipse(66 + gimbal_x, 32 + gimbal_y, 2, 2, U8G2_DRAW_ALL);

    // u8g2_left.setFont(u8g2_font_TimesNewPixel_tr);  
    /*
    u8g2_left.setFont(u8g2_font_5x7_tf);
    u8g2_left.setCursor(0, 8);
    u8g2_left.print(F("T:"));
    strcpy(buf, ltoa(throttle, buf, 10));
    u8g2_left.drawStr(15, 8, buf);

    u8g2_left.setCursor(0,45);
    u8g2_left.print(F("Y:"));
    strcpy(buf, ltoa(yaw, buf, 10));
    u8g2_left.drawStr(15,45,buf);

    u8g2_left.setCursor(60,30);
    u8g2_left.print(F("P:"));
    strcpy(buf, ltoa(pitch, buf, 10));
    u8g2_left.drawStr(75,30,buf);


    u8g2_left.setCursor(60,45);
    u8g2_left.print(F("R:"));
    strcpy(buf, ltoa(roll, buf, 10));
    u8g2_left.drawStr(75,45,buf);
    */

    u8g2_left.setFont(u8g2_font_5x7_tf);
    
    u8g2_left.setCursor(99, 50);
    u8g2_left.print(F("Rate:"));
    strcpy(buf, ltoa(ble_profiling, buf, 10));
    u8g2_left.drawStr(99, 57,buf);

    if(!sw6) {
        u8g2_left.setCursor(99, 64);
        u8g2_left.print(F("LOCKED"));
    }
    
    u8g2_left.nextPage();

    if(!ble_begin) {
        bleGamepad.begin();
        ble_begin = true;
    }

    if(bleGamepad.isConnected()) {
        ble_profiling_raw++;

        //                [YAW, THROTTLE, ROLL, PITCH]
        bleGamepad.setAxes(yaw, throttle, roll, pitch, 0, 0, DPAD_CENTERED);
    }
}

void rf_settings(void) {
    // Data processing
    if(stick_navigation_position == MENU_LEFT) {
        if(freq_txpower > 0) {
            freq_txpower--;
            radio.setPALevel(freq_txpower);
        }
    } else if(stick_navigation_position == MENU_RIGHT) {
        if(freq_txpower < 3) {
            freq_txpower++;
            radio.setPALevel(freq_txpower);
        }
    } else if(stick_navigation_position == MENU_EXTEND_LEFT) {
        if(freq_channel > 0) {
            freq_channel--;
            radio.setChannel(freq_channel);
        }
    } else if(stick_navigation_position == MENU_EXTEND_RIGHT) {
        if(freq_channel < MAX_CHANNELS) {
            freq_channel++;
            radio.setChannel(freq_channel);
        }
    }

    u8g2_left.clearBuffer();
    u8g2_left.firstPage();
    u8g2_left.setFont(u8g2_font_ImpactBits_tr);  
    u8g2_left.setCursor(0, 15);
    u8g2_left.print(F("RF Settings"));

    // To List the current frequency... and also current power. Using the sticks to change it. Once back, it will commit the setting by pushing down the stick. If pulling up...nothing change.
    u8g2_left.setFont(u8g2_font_5x7_tf);
    u8g2_left.setCursor(0, 32);
    u8g2_left.print(F("Frequency:"));
    u8g2_left.setCursor(64, 32);
    u8g2_left.print(F("TX Power:"));
   
    // Getting the common variables
    static char buf[10];
    int frequency_mhz = 2400 + freq_channel;
    sprintf(buf, "%d MHz", frequency_mhz);
    u8g2_left.drawStr(0, 40, buf);

    u8g2_left.setCursor(0, 56);
    if(channel_used[freq_channel]) {
        u8g2_left.print(F("USED"));
    } else {
        u8g2_left.print(F("FREE"));
    }
     
    switch(freq_txpower) {
        case 0:
            sprintf(buf, "LOW");
            break;
        case 1:
            sprintf(buf, "MIN");
            break;
        case 2:
            sprintf(buf, "HIGH");
            break;
        case 3:
            sprintf(buf, "MAX");
            break;
        default: 
            sprintf(buf, "UNKNOWN");
            break;
    }
    u8g2_left.drawStr(64, 40, buf);

    u8g2_left.nextPage();
}

void show_battery_level(uint8_t x, uint8_t y) {
    u8g2_left.drawFrame(x, y, 12, 8);

    // Tips of the battery
    u8g2_left.drawLine(x+12, y+2, x+12, y+5);

    // Battery level indicator
    int vbat_bar_value = ((vbat_value * .0012142857) - 3.2) * 8;
    // int vbat_bar_value = vbat_value * 10 / 4096;
    uint8_t x1 = 0;
    for(int i = 0; i < vbat_bar_value; i++) {
        x1 = x + i + 2;
        u8g2_left.drawLine(x1, y + 2, x1, y + 5);
    }
}

void wifi_enabler(void) {
    /*
    u8g2_left.clearBuffer();
    u8g2_left.firstPage();
    u8g2_left.setFont(menu_entry_alpha_huge_list[destination_alpha_state.position].font);
    u8g2_left.drawGlyph(40, ICONX_Y, menu_entry_alpha_huge_list[destination_alpha_state.position].icon);
    u8g2_left.nextPage();
    */

    u8g2_left.clearBuffer();
    u8g2_left.firstPage();
    u8g2_left.setFontMode(1);
    u8g2_left.setFont(u8g2_font_VCR_OSD_tr);  

    static char buf[6];

    // u8g2_left.drawStr(45, 15, buf);
    // strcpy(buf, ltoa(vbat_value, buf, 10));
    // u8g2_left.drawStr(0, 30, buf);

    static bool wifi_selections = false;

    u8g2_left.setFont(u8g2_font_ImpactBits_tr);  
    u8g2_left.setCursor(15, 25); 
    u8g2_left.print(F("Enable WiFi?"));
    u8g2_left.setCursor(30, 55); 
    u8g2_left.print(F("Yes   No"));

    // TODO: Working on WiFi. Manually on...
    if(stick_navigation_position == MENU_LEFT) {
        wifi_selections = true;
    } else if(stick_navigation_position == MENU_RIGHT) {
        wifi_selections = false;
    } else if(stick_navigation_position == MENU_OK) {
        // Wifi...
        if(wifi_selections) {
            wifi_enable = true;
        } else {
            wifi_enable = false;
        }

        // Simulate...so it will return back..
        menu_entry_alpha_selected = false;
        last_menu_stick_read = millis() + 200;
    }

    if(wifi_selections) {
        u8g2_left.drawFrame(26, 40, 30, 20);
    } else {
        u8g2_left.drawFrame(62, 40, 30, 20);
    }
    u8g2_left.nextPage();
    
}

void setup_nrf_scanner() {
    // Setting NRF24L01
    // printf_begin();
    radio.begin();
    radio.setAutoAck(false);

    // Get into standby mode
    radio.startListening();
    radio.stopListening();
    // radio.printDetails();

    // Initialize variables...
    for(int i = 0; i < MAX_CHANNELS; i++) {
        channel_loads[i] = 0;
    }

    Serial.println("SET nrf_scanner = TRUE");
    nrf_scanner = true;
}

void setup_nrf_sync(void) {
    // Setting NRF24L01
    // printf_begin();
    radio.begin();

    // radio.setAutoAck(true);
    radio.setAutoAck(false);                        // Seems like i am doing this on the last version...
    radio.setPayloadSize(sizeof(SyncData));
    radio.setChannel(125);                            // 0: Sync channel (2525MHz)
    radio.setDataRate(RF24_250KBPS);                // Lowest.. at 250kbps
    radio.setPALevel(RF24_PA_MIN);
    // delay_maxrf_time = millis() + 2000;             // Delay 2 seconds later 
    // delay_maxrf = true;
    // radio.setPALevel(RF24_PA_MAX);               // Set the transmit power to Maximum... Later make it configurable.
    const uint64_t pipe = 0xE8E8F0F0E1LL;

    radio.openWritingPipe(pipe);
    // radio.printDetails();
}

void setup_nrf_tx(void) {
    // Setting NRF24L01
    // printf_begin();
    radio.begin();

    // radio.setAutoAck(true);
    radio.setAutoAck(false);                        // Seems like i am doing this on the last version...
    radio.setPayloadSize(sizeof(TxMessage));
    radio.setChannel(freq_channel);                 // This will need to set from EEPROM settings
    radio.setDataRate(RF24_250KBPS);                // Lowest.. at 250kbps
    switch(freq_txpower) {                          // Set the transmit power
        case 0:
            radio.setPALevel(RF24_PA_MIN);
            break;
        case 1:
            radio.setPALevel(RF24_PA_LOW);
            break;
        case 2:
            radio.setPALevel(RF24_PA_HIGH);
            break;
        case 3:
            radio.setPALevel(RF24_PA_MAX);
            break;
    }
    // delay_maxrf_time = millis() + 2000;             // Delay 2 seconds later 
    // delay_maxrf = true;
    // radio.setPALevel(RF24_PA_MAX);               // Set the transmit power to Maximum... Later make it configurable.


    const uint64_t pipe = 0xE8E8F0F0E1LL;
    radio.openWritingPipe(pipe);
    // radio.printDetails();

    Serial.println("SET nrf_scanner = TRUE");
    nrf_enable = true;
}

void restore_radio(void) {
    radio.setAutoAck(false);
    radio.setPayloadSize(sizeof(TxMessage));
    radio.setChannel(freq_channel);
    switch(freq_txpower) {                          // Set the transmit power
        case 0:
            radio.setPALevel(RF24_PA_MIN);
            break;
        case 1:
            radio.setPALevel(RF24_PA_LOW);
            break;
        case 2:
            radio.setPALevel(RF24_PA_HIGH);
            break;
        case 3:
            radio.setPALevel(RF24_PA_MAX);
            break;
    }

    const uint64_t pipe = 0xE8E8F0F0E1LL;
    radio.openWritingPipe(pipe);

    nrf_enable = true;
}

void setup_rxsync_server(void) {
    static char sync_html[128];

    sprintf(sync_html, "%d|%ld", freq_channel, pipeIn);
    server.on("/", HTTP_GET, []() {
        server.sendHeader("Connection", "close");
        server.send(200, "text/html", sync_html);
    });

    // Starting up Server
    server.begin();
}


void rx_binding(void) {
    u8g2_left.clearBuffer();
    u8g2_left.firstPage();
    static bool binding_selections = false;
    u8g2_left.setFont(u8g2_font_ImpactBits_tr);  
    u8g2_left.setCursor(15, 25); 
    u8g2_left.print(F("Enable Binding?"));
    u8g2_left.setCursor(30, 55); 
    u8g2_left.print(F("Yes   No"));

    // TODO: Working on WiFi. Manually on...
    if(stick_navigation_position == MENU_LEFT) {
        binding_selections = true;
    } else if(stick_navigation_position == MENU_RIGHT) {
        binding_selections = false;
    } else if(stick_navigation_position == MENU_OK) {
        // Wifi...
        if(binding_selections) {
            if(!binding) {
                // WiFi.softAP("OpenFlightTX", "12345678");
                setup_nrf_sync();
                // setup_rxsync_server();
                binding = true;
            }
        } else {
            binding = false;
            restore_radio();
        }

        // Simulate...so it will return back..
        menu_entry_alpha_selected = false;
        last_menu_stick_read = millis() + 200;
    }

    if(binding_selections) {
        u8g2_left.drawFrame(26, 40, 30, 20);
    } else {
        u8g2_left.drawFrame(62, 40, 30, 20);
    }
    // Just need to know the receiver's channel, encryption code
    u8g2_left.nextPage();
}

void rf_scanners(void) {
    int y = 0;
    static char buf[10];
    static int result_x = 0;

    if(!nrf_scanner) {
        setup_nrf_scanner();

        for(int i = 0; i < MAX_CHANNELS; i++) {
            channel_used[i] = false;
        }
    }

    // Storing...
    int channel_unused = 0;
    for(int i = 0; i < MAX_CHANNELS; i++) {
        if(channel_loads[i] > 0) {
            channel_used[i] = true;
        }
        if(!channel_used[i]) channel_unused++;
    }

    // Data processing
    if(stick_navigation_position == MENU_LEFT) {
        graph_type = 0;
    } else if(stick_navigation_position == MENU_RIGHT) {
        graph_type = 1;
    } else if(stick_navigation_position == MENU_EXTEND_LEFT) {
        result_x += 10;
    } else if(stick_navigation_position == MENU_EXTEND_RIGHT) {
        result_x -= 10;
    }

    u8g2_left.clearBuffer();
    u8g2_left.firstPage();
    u8g2_left.setFontMode(1);
    u8g2_left.setFont(u8g2_font_VCR_OSD_tr);  
    
    u8g2_left.setFont(u8g2_font_ImpactBits_tr);  
    u8g2_left.setCursor(0,12);
    u8g2_left.print(F("RF SCANNER"));

    u8g2_left.setFont(u8g2_font_5x7_tf);
    if(!sw6) {
        u8g2_left.setCursor(85, 15);
        u8g2_left.print(F("LOCKED"));
    }

    // List all free channels..
    bool continuemore = true;
    int c = 0;
    int col = 0;
    int frequency_mhz = 0;
    for(int i = 0; i < MAX_CHANNELS; i++) {
        if(channel_used[i]) {
            continue;
        }

        frequency_mhz = 2400 + i;
        sprintf(buf, "%d ", frequency_mhz);
        col = c / 4;
        int y = c % 4;
        u8g2_left.drawStr(0 + (col * 22) - result_x, 40 + (y * 8), buf);
        c++;

        if(c >= 28) break;
    }

    // u8g2_left.drawStr(45, 15, buf);
    // strcpy(buf, ltoa(vbat_value, buf, 10));
    // u8g2_left.drawStr(0, 30, buf);

    //strcpy(buf, ltoa(vbat_real_value, buf, 10));
    //sprintf(buf, "RATE:%i", lora_receive_profiling); 

    //u8g2_left.setFont(u8g2_font_5x7_tf);  
    //u8g2_left.drawStr(0, 32, buf);

    // Drawing from the memory...
    if(graph_type == 0) {
        for(int x = 0; x < MAX_CHANNELS; x++) {
            if(channel_loads[x] == 0) continue;
            // y = 63 - (channel_loads[x] * 32 / MAX_SAMPLES);
            y = 32 - rf24_min(0xF, channel_loads[x]);
            u8g2_left.drawLine(x, 32, x, y);
        }
    } else {
        for(int x = 0; x < MAX_CHANNELS; x++) {
            if(!channel_used[x]) continue;

            // Meaning ... already use..... then draw a line
            u8g2_left.drawLine(x, 16, x, 32);
        }
    }

    // Stats
    // u8g2_left.setFont(u8g2_font_5x7_tf);
    u8g2_left.setCursor(85, 7);
    u8g2_left.print(F("FREE:"));
    itoa(channel_unused, buf, 10);
    u8g2_left.drawStr(110, 7, buf);
    

    u8g2_left.nextPage();
}

/*
// set the value of a nRF24L01 register
void setRegister(byte r, byte v) {
    digitalWrite(NRF24_CE, 0);
    SPI.transfer((r&0x1F)|0x20);
    SPI.transfer(v);
    digitalWrite(NRF24_CE, 1);
}

// Get the value of a nRF24L01 register
byte getRegister(byte r) {
    byte c;
    digitalWrite(NRF24_CE, 0); 
    c = SPI.transfer(r&0x1F);
    c = SPI.transfer(0);  
    digitalWrite(NRF24_CE, 1); 
    return(c);
}

void disableRX(void) {
    digitalWrite(NRF24_CE, LOW);
}

// power up the nRF24L01p chip
void powerUp(void) {
    setRegister(_NRF24_CONFIG,getRegister(_NRF24_CONFIG)|0x02);
    delayMicroseconds(130);
}

// switch nRF24L01p off
void powerDown(void) {
    setRegister(_NRF24_CONFIG,getRegister(_NRF24_CONFIG)&~0x02);
}

// enable RX
void enableRX(void) {
    digitalWrite(NRF24_CE, HIGH);
}

// setup RX-Mode of nRF24L01p
void setRX(void) {
    setRegister(_NRF24_CONFIG,getRegister(_NRF24_CONFIG)|0x01);
    enableRX();
    // this is slightly shorter than
    // the recommended delay of 130 usec
    // - but it works for me and speeds things up a little...
    delayMicroseconds(100);
}
*/