void screens::mainMenu(uint8_t menu_id) {
    reset(); // start from fresh screen.
    drawTitleBox(PSTR2("MODE SELECTION"));

    display.fillRect(0, 10*menu_id+12, display.width(), 10,WHITE);

    display.setTextColor(menu_id == 0 ? BLACK : WHITE);
    display.setCursor(5,10*0+13);
    display.print(PSTR2("AUTO SEARCH"));
    display.setTextColor(menu_id == 1 ? BLACK : WHITE);
    display.setCursor(5,10*1+13);
    display.print(PSTR2("BAND SCANNER"));
    display.setTextColor(menu_id == 2 ? BLACK : WHITE);
    display.setCursor(5,10*2+13);
    display.print(PSTR2("MANUAL MODE"));

#ifdef USE_DIVERSITY
    if(isDiversity())
    {
        display.setTextColor(menu_id == 3 ? BLACK : WHITE);
        display.setCursor(5,10*3+13);
        display.print(PSTR2("DIVERSITY"));
    }
#endif
    display.setTextColor(menu_id == 4 ? BLACK : WHITE);
    display.setCursor(5,10*4+13);
    display.print(PSTR2("SETUP MENU"));

    display.display();
}
char screens::begin(const char *call_sign) {
    // Set the address of your OLED Display.
    // 128x64 ONLY!!
#ifdef SH1106
    display.begin(SH1106_SWITCHCAPVCC, 0x3C);  // initialize with the I2C addr 0x3D or 0x3C (for the 128x64)
#else
    display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  // initialize with the I2C addr 0x3D or 0x3C (for the 128x64)
#endif


#ifdef USE_FLIP_SCREEN
    flip();
#endif

#ifdef USE_BOOT_LOGO
    display.display(); // show splash screen
    delay(3000);
#endif
    // init done
    reset();

#ifdef USE_BOOT_CHECK
    display.fillRect(0, 0, display.width(), 11,WHITE);
    display.setTextColor(BLACK);
    display.setCursor(((display.width() - (10*6)) / 2),2);
    display.print(PSTR2("Boot Check"));

    display.setTextColor(WHITE);
    display.setCursor(0,8*1+4);
    display.print(PSTR2("Power:"));
    display.setCursor(display.width()-6*3,8*1+4);
    display.print(PSTR2("OK"));
    display.setCursor(0,8*2+4);

    display.display();
#ifdef USE_DIVERSITY
    display.print(PSTR2("Diversity:"));
    display.display();
    delay(250);
    display.setCursor(display.width()-6*9,8*2+4);
    if(isDiversity()) {
        display.print(PSTR2(" ENABLED"));
    }
    else {
        display.print(PSTR2("DISABLED"));
    }
#endif
    display.setCursor(((display.width() - (strlen(call_sign)*12)) / 2),8*4+4);
    display.setTextSize(2);
    display.print(call_sign);
    display.display();
    delay(1250);
#endif

    return 0; // no errors
}
void screens::screenSaver(uint8_t diversity_mode, uint8_t channelName, uint16_t channelFrequency, const char *call_sign) {
    reset();
#ifdef USE_DIM_ON_SCREENSAVER
    display.dim(true);
#endif
    display.setTextSize(6);
    display.setTextColor(WHITE);
    display.setCursor(0,0);
    display.print(channelName, HEX);
    display.setTextSize(1);
    display.setCursor(70,0);
    display.print(call_sign);
    display.setTextSize(2);
    display.setCursor(70,28);
    display.setTextColor(WHITE);
    display.print(channelFrequency);
    display.setTextSize(1);
#ifdef USE_DIVERSITY
    if(isDiversity()) {
        display.setCursor(70,18);
        switch(diversity_mode) {
            case useReceiverAuto:
                display.print(PSTR2("AUTO"));
                break;
            case useReceiverA:
                display.print(PSTR2("ANTENNA A"));
                break;
            case useReceiverB:
                display.print(PSTR2("ANTENNA B"));
                break;
        }
        display.setTextColor(BLACK,WHITE);
        display.fillRect(0, display.height()-19, 7, 9, WHITE);
        display.setCursor(1,display.height()-18);
        display.print("A");
        display.setTextColor(BLACK,WHITE);
        display.fillRect(0, display.height()-9, 7, 9, WHITE);
        display.setCursor(1,display.height()-8);
        display.print("B");
    }
#endif
    display.display();
}
void screens::updateScreenSaver(char active_receiver, uint8_t rssi, uint8_t rssiA, uint8_t rssiB) {
#ifdef USE_DIVERSITY
    if(isDiversity()) {
        // read rssi A
        #define RSSI_BAR_SIZE 119
        uint8_t rssi_scaled=map(rssiA, 1, 100, 3, RSSI_BAR_SIZE);
        display.fillRect(7 + rssi_scaled, display.height()-19, (RSSI_BAR_SIZE-rssi_scaled), 9, BLACK);
        if(active_receiver == useReceiverA)
        {
            display.fillRect(7, display.height()-19, rssi_scaled, 9, WHITE);
        }
        else
        {
            display.fillRect(7, display.height()-19, (RSSI_BAR_SIZE), 9, BLACK);
            display.drawRect(7, display.height()-19, rssi_scaled, 9, WHITE);
        }

        // read rssi B
        rssi_scaled=map(rssiB, 1, 100, 3, RSSI_BAR_SIZE);
        display.fillRect(7 + rssi_scaled, display.height()-9, (RSSI_BAR_SIZE-rssi_scaled), 9, BLACK);
        if(active_receiver == useReceiverB)
        {
            display.fillRect(7, display.height()-9, rssi_scaled, 9, WHITE);
        }
        else
        {
            display.fillRect(7, display.height()-9, (RSSI_BAR_SIZE), 9, BLACK);
            display.drawRect(7, display.height()-9, rssi_scaled, 9, WHITE);
        }
    }
    else {
        display.setTextColor(BLACK);
        display.fillRect(0, display.height()-19, 25, 19, WHITE);
        display.setCursor(1,display.height()-13);
        display.print(PSTR2("RSSI"));
        #define RSSI_BAR_SIZE 101
        uint8_t rssi_scaled=map(rssi, 1, 100, 1, RSSI_BAR_SIZE);
        display.fillRect(25 + rssi_scaled, display.height()-19, (RSSI_BAR_SIZE-rssi_scaled), 19, BLACK);
        display.fillRect(25, display.height()-19, rssi_scaled, 19, WHITE);
    }
#else
    display.setTextColor(BLACK);
    display.fillRect(0, display.height()-19, 25, 19, WHITE);
    display.setCursor(1,display.height()-13);
    display.print(PSTR2("RSSI"));
    #define RSSI_BAR_SIZE 101
    uint8_t rssi_scaled=map(rssi, 1, 100, 1, RSSI_BAR_SIZE);
    display.fillRect(25 + rssi_scaled, display.height()-19, (RSSI_BAR_SIZE-rssi_scaled), 19, BLACK);
    display.fillRect(25, display.height()-19, rssi_scaled, 19, WHITE);
#endif
    if(rssi < 20)
    {
        display.setTextColor((millis()%250 < 125) ? WHITE : BLACK, BLACK);
        display.setCursor(50,display.height()-13);
        display.print(PSTR2("LOW SIGNAL"));
    }
#ifdef USE_DIVERSITY
    else if(isDiversity()) {
        display.drawLine(50,display.height()-10,110,display.height()-10,BLACK);
    }
#endif
    display.display();
}