
/* SSD1306Wire.h functions, see https://github.com/ThingPulse/esp8266-oled-ssd1306

  display.clear()        - clear the local pixel buffer
  display.display()      - write the buffer to the display
  display.displayOff()   - turn the display OFF
  display.displayOn()    - turn the display ON
  display.drawCircle(int16_t x, int16_t y, int16_t radius)
  display.drawFastImage(int16_t x, int16_t y, int16_t width, int16_t height, const uint8_t *image)
  display.drawHorizontalLine(int16_t x, int16_t y, int16_t length)
  display.drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1)  - draw from x0k,y0 to x1,y1
  display.drawProgressBar(uint16_t x, uint16_t y, uint16_t width, uint16_t height, uint8_t progress)
  display.drawRect(int16_t x, int16_t y, int16_t width, int16_t height)
  display.drawString(int16_t x, int16_t y, String text)
  display.drawStringMaxWidth(int16_t x, int16_t y, int16_t maxLineWidth, String text)
  display.drawVerticalLine(int16_t x, int16_t y, int16_t length)
  display.drawXbm(int16_t x, int16_t y, int16_t width, int16_t height, const char* xbm)
  display.fillCircle(int16_t x, int16_t y, int16_t radius)
  display.fillRect(int16_t x, int16_t y, int16_t width, int16_t height)
  display.flipScreenVertically()   - flip the current screen upside down
  display.init()         - initialise the display
  display.invertDisplay()- inverted display mode
  display.end()          - free memory used by the display
  display.displayOn()    - turn the display ON
  display.mirrorScreen() - draw the screen mirrored
  display.normalDisplay()- normal display mode
  display.reconnect()    - connect again through I2C
  display.setBrightness(uint8_t)
  display.setColor(OLEDDISPLAY_COLOR color) - BLACK, WHITE, INVERSE
  display.setContrast(uint8_t contrast, uint8_t precharge = 241, uint8_t comdetect = 64)
  display.setFont(const uint8_t* fontData)
  display.setPixel(int16_t x, int16_t y)  - set a pixel at x,y
  display.setTextAlignment(OLEDDISPLAY_TEXT_ALIGNMENT textAlignment)  - TEXT_ALIGN_LEFT, TEXT_ALIGN_CENTER, TEXT_ALIGN_RIGHT, TEXT_ALIGN_CENTER_BOTH

  W = display.getStringWidth(const char* text, uint16_t length)
  W = display.getStringWidth(String text)
  
  Display size is 128 x 64 (w x h) pixels
  Default fonts are ArialMT at 10, 16 & 24 pixels high
  Text options include:
    2 x 24, + 1 x 10 pnt lines
    1 x 24, + 2 x 16 pnt lines
    1 x 24, + 3 x 10 pnt lines
    4 x 16 pnt lines, no line spaces, decending characters will merge
    3 x 16, + 1 x 10 pnt lines
    6 x 10 pnt lines, no line spaces, decending characters will merge
*/

// --------------------------------------------------------------------------------

void Display_40ms() {
  // called every 40ms (25Hz) from the main loop
  if (DispNOP) {return;}  // this function is aborted, no I2C activity
  if (DispDel > 0) {DispDel--; return;} // don't display if DispDel > 0

  if (DispClr) {Display_clear(); DispClr = false; return;}
  if (DispDisp) {Display_display(); DispDisp = false; DispClr = true; return;}

  // we refresh individual displays based on a counter, DispCnt
  if (DispCnt > 0) {DispCnt--;}
  else {
    // DispCnt is reset for display updates, so can be different for each
    if (DispBack) {
      // draw a back arrow on the screen
      Display_clear();
      Display_drawLine(48,32,80,16);
      Display_drawLine(48,32,80,48);
      Display_drawLine(80,16,80,48);
      Display_display();
      DispDel = 10;
    } else {
      if (DispNow > 0) {
        // special display modes used in calibration etc.
        switch(DispNow) {
          case 1: DispCnt = 8; Display_FlashTxt(); break;
        }
      } else {
        // if REC mode display is auto-switched to that
        if (REC) {
          Display_REC_Data();
        } else if (playRun) {
        // in 'play' mode display is auto-switched to that
          Display_REC_Data();
        } else if (APP > 0) {
        // in APP mode display is auto-switched to that
          Display_REC_Data();
        } else {
          // normal menu screens, display depends on menu modes
          switch(DispMenu) {
            case 0: Display_40ms_Sensors(); break;
          }
        }
      }
    }
    // the following line puts a rectangle around every display, showing the boundry region
    // display.drawRect(0,0,128,64);
  }
}

// --------------------------------------------------------------------------------

void Display_40ms_Sensors() {
  // DispMenu == 0 so display sensor screens
  // DispPnt determines which group of screens are displayed
  switch (DispPnt) {
    case 0: // Battery screens
      switch(DispMode) {
        case 0: Display_Battery(); break;
        case 1: Display_BatData(); break;
        default: DispMode = 0; break; // over-run, this should not be necessary
      } break;
    case 1: // Servo screens
      switch(DispMode) {
        case 0: Display_Servo_PWM(); break;
        case 1: Display_Servo_Home(); break;
        default: DispMode = 0; break; // over-run, this should not be necessary
      } break;
    case 2: // Wii data
      Display_Wii_Ctrl(); break;
  }
  DispChng = false;
}

// --------------------------------------------------------------------------------

void Display_APP_Data() {
  // display APP mode data whilst in APP mode
  // this is sent directly to the display and not the mirror app
  String zCmd$;
  display.setFont(ArialMT_Plain_16);
  display.setTextAlignment(TEXT_ALIGN_RIGHT);
  if (APP_Disp < 1) {
    display.drawString(127, 0, String(servoVal[3]));  // Servo 3 Claw
    display.drawString(127,15, String(servoVal[2]));  // Servo 2 Vertical arm
    display.drawString(127,30, String(servoVal[1]));  // Servo 1 Forward arm
    display.drawString(127,45, String(servoVal[0]));  // Servo 0 Turntable
  } else {
    display.drawString(127, 0, String(servoVal[3] - Home3));  // Servo 3 Claw
    display.drawString(127,15, String(servoVal[2] - Home2));  // Servo 2 Vertical arm
    display.drawString(127,30, String(servoVal[1] - Home1));  // Servo 1 Forward arm
    display.drawString(127,45, String(servoVal[0] - Home0));  // Servo 0 Turntable
  }

  display.setTextAlignment(TEXT_ALIGN_LEFT);
  display.drawString(12,15, "APP");
  display.drawString( 8,30, "MODE");
}

// --------------------------------------------------------------------------------

void Display_Battery() {
  // display battery information
  Display_clear();            // remove current display content
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //       1111111111111111     2222222222222222     3333333333333333
  Display_setFont(16);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 06, "Battery");
  if (BatAvg > BatCritical) {
    // battery powered mode
    Display_setTextAlignment(TEXT_ALIGN_LEFT);
    Display_drawString(0, 27, String(BatAvg));
    Display_setTextAlignment(TEXT_ALIGN_CENTER);
    Display_drawString(64, 27, String(float(BatAvg)/BatCal) + "v");
    Display_setTextAlignment(TEXT_ALIGN_RIGHT);
    AnyLong = (BatAvg - BatCritical)*100/(BatMax - BatCritical);
    if (AnyLong > 100) {AnyLong = 100;} // limit max to 100%
    if (AnyLong < 0) {AnyLong = 0;}     // limit min to 0%
    Display_drawString(127, 27, String(AnyLong) + "%");
    // display the time
    Display_setTextAlignment(TEXT_ALIGN_LEFT);
    Display_drawString(0, 48, getHmsTime(millis()));
    Display_setTextAlignment(TEXT_ALIGN_RIGHT);
    Display_drawString(127, 48, BatTime$);
  } else {
    // USB powered mode
    Display_setTextAlignment(TEXT_ALIGN_CENTER);
    Display_drawString(64, 27, "USB Pwr");
    // display the time
    Display_drawString(64, 48, getHmsTime(millis()));
  }
  Display_display();  // display immediately
  DispDel = 10;       // wait 2s before displaying next count
}

// --------------------------------------------------------------------------------

void Display_BatData() {
  // plot the BatData[] array as a rolling average
  Display_clear();            // remove current display content
  DispCX = 4;
  for (int zB = 0; zB < 60; zB++) {
    if (BatData[zB] > 0) {Display_drawRect(DispCX,63 - BatData[zB],2,BatData[zB]);}
    DispCX += 2;
  }
  // draw axice
  DispCX = 4;
  Display_drawLine(DispCX,0,DispCX,63);
  Display_drawLine(DispCX,63,DispCX + 118,63);
  Display_display();  // display immediately
  DispDel = 10;       // wait 2s before displaying next count
}

// --------------------------------------------------------------------------------

void Display_Count(int zNum) {
  // displays a number in the centre of the screen, normally a countdown
  // locks display updates to prevent display task from overwriting count
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //                     111111111111111111111111
  Display_setFont(24);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 20, String(zNum));
  Display_display();
  DispNOP = true; // prevent display updates
}

// --------------------------------------------------------------------------------

void Display_FlashTxt() {
  // displays two lines of text, centred on a flashing screen, in font 16
  Display_clear();  // clear screen before setting mode
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //               1111111111111111    2222222222222222
  if (DispInv) {
    // inverted display mode
    DispInv = false; display.invertDisplay();
  } else {
    DispInv = true; display.normalDisplay();
    // count down the number of flashes
    DispFlash--; if (DispFlash < 1) {DispNow = 0;}  // turn off flashing display
  }
  Display_setFont(16);  // fonts are ArialMT_Plain_10,16,24
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 14, DispTx1$);
  Display_drawString(64, 34, DispTx2$);
  Display_display();
  DispDel = 8;
}

// --------------------------------------------------------------------------------

void Display_Init() {
  // initialise the OLED display
  display.init();
  display.clear();
  display.setContrast(255);
  display.display();
  DispClr = false; DispDisp = false;
}

// --------------------------------------------------------------------------------

void Display_Intro() {
  // first screen to be displayed
  // default fonts are ArialMT_Plain_10, ArialMT_Plain_16 and ArialMT_Plain_24
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //     1111111111111111    2222222222222222    3333333333333333
  Display_setFont(16);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 4, "REACH");
  Display_drawString(64,24, "ROBOT");
  Display_drawString(64,44, "MK1");
  Display_display();
  DispDel = 50;       // wait 2s before displaying next count
}

// --------------------------------------------------------------------------------

void Display_Mirrored() {
  // display robot type when DispMon is first connected
  // we clear current flags so that this is displayed immediately
  DispClr = false; DispDisp = false; DispCnt = 1;
  
  Display_clear();            // remove current display content
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //   1111111111111111 2222222222222222 3333333333333333 4444444444
  Display_display();
  Display_setBrightness(255);
  Display_setFont(16);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 2, "REACH");
  Display_drawString(64, 19, "ROBOT");
  Display_drawString(64, 36, "MK1");
  Display_setFont(10);
  Display_drawString(64,53, "Display Mirror");
  Display_display();  // display immediately
  DispDel = 50;       // wait 2s before displaying next count
}

// --------------------------------------------------------------------------------

void Display_QText2x16(String zT0$,String zT1$) {
  // called to display two lines of text + <NO OK>, centre screen in font 16
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //     1111111111111111    2222222222222222    3333333333333333
  Display_setFont(16);  // fonts are ArialMT_Plain_10,16,24
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 4, zT0$);
  Display_drawString(64, 24, zT1$);
  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  Display_drawString(0, 44, "< NO");
  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_drawString(127, 44, "OK >");
  Display_display();
  DispNOP = true;
}

// --------------------------------------------------------------------------------

void Display_Reboot() {
  // called when a soft reset is about to be initiated
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  //      111111111111111111111111      222222222222222222222222
  Display_setFont(24);  // fonts are ArialMT_Plain_10, ArilMT_Plain_24
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64,  5, "RESET");
  Display_setFont(16);
  Display_drawString(64, 35, "Pending...");
  Display_display();
  DispNOP = true;
}

// --------------------------------------------------------------------------------

void Display_REC_Data() {
  // display REC/APP mode data whilst in REC pr APP mode
  String zCmd$;
  Display_clear();
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  // 1111111111111111222222222222222233333333333333334444444444444444
  Display_setFont(16);
  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  if (moves[playPnt] < 5000) {
    if (REC_Disp < 1) {
      Display_drawString(127, 0, String(servoVal[3]));  // Servo 3 Claw
      Display_drawString(127,16, String(servoVal[2]));  // Servo 2 Vertical arm
      Display_drawString(127,32, String(servoVal[1]));  // Servo 1 Forward arm
      Display_drawString(127,48, String(servoVal[0]));  // Servo 0 Turntable
    } else {
      Display_drawString(127, 0, String(servoVal[3] - Home3));  // Servo 3 Claw
      Display_drawString(127,16, String(servoVal[2] - Home2));  // Servo 2 Vertical arm
      Display_drawString(127,32, String(servoVal[1] - Home1));  // Servo 1 Forward arm
      Display_drawString(127,48, String(servoVal[0] - Home0));  // Servo 0 Turntable
    }
  } else {
      // command line so don't display servo values
      Display_drawString(127, 0, "-");  // Servo 3 Claw
      Display_drawString(127,16, "-");  // Servo 2 Vertical arm
      Display_drawString(127,32, "-");  // Servo 1 Forward arm
      Display_drawString(127,48, "-");  // Servo 0 Turntable
  }
  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  Display_drawString(  0, 0, "Pnt: " + String(1 + (playPnt/4)));
  Display_drawString(  0,16, "Cnt: " + String(playCnt/4));
  AnyLong = (playCnt * 100)/moveArraySize;
  Display_drawString(  0,32, "Mem: " + String(AnyLong) + "%");
  
  if (moves[playPnt] < 5000) {
    Display_drawString(  0,48, "Tgt Pos");
  } else {
    // display command information
    anyAny = moves[playPnt + 1];
    switch(moves[playPnt]) {
      case cmdPlayAddPnt:
        Display_drawString(  0,48, "Add: " + String(moves[playPnt + 1])); break;
      case cmdPlayDelay:
        if (anyAny > 90) {zCmd$ = String(float(anyAny)/100.0) + " sec";
        } else {zCmd$ = String(anyAny) + "0 ms";
        } Display_drawString(  0,48, "Del: " + zCmd$); break;
      case cmdPlayDelRnd:
        Display_drawString(  0,48, "DelRnd: " + String(moves[playPnt + 1])); break;
      case cmdPlayClap:
        Display_drawString(  0,48, "Clap: " + String(moves[playPnt + 1])); break;
      case cmdPlayClapRnd:
        Display_drawString(  0,48, "ClapRnd: " + String(moves[playPnt + 1])); break;
      case cmdPlayFor:
        Display_drawString(  0,48, "For: " + String(moves[playPnt + 1])); break;
      case cmdPlayForRnd:
        Display_drawString(  0,48, "ForRnd: " + String(moves[playPnt + 1])); break;
      case cmdPlayGosub:
        Display_drawString(  0,48, "Gosub: " + String(moves[playPnt + 1])); break;
      case cmdPlayGoto:
        Display_drawString(  0,48, "Goto: " + String(moves[playPnt + 1])); break;
      case cmdPlayGsbRnd:
        Display_drawString(  0,48, "GsbRnd: " + String(moves[playPnt + 1])); break;
      case cmdPlayGtoRnd:
        Display_drawString(  0,48, "GtoRnd: " + String(moves[playPnt + 1])); break;
      case cmdPlayNext:
        Display_drawString(  0,48, "Next"); break;
      case cmdPlayPause:
        if (anyAny > 90) {zCmd$ = String(float(anyAny)/100.0) + " sec";
        } else {zCmd$ = String(anyAny) + "0 ms";
        } Display_drawString(  0,48, "Pause:" + zCmd$); break;
      case cmdPlayReturn:
        Display_drawString(  0,48, "Return"); break;
      case cmdPlaySpeed:
        Display_drawString(  0,48, "Spd:" + String(moves[playPnt + 1]) + "%"); break;
      case cmdPlaySpeedRnd:
        Display_drawString(  0,48, "SpdRnd:" + String(moves[playPnt + 1]) + "%"); break;
    }
  }
  Display_display();
  DispDel = 5;
}

// --------------------------------------------------------------------------------

void Display_Servo_Home() {
  // display servo PWM values
  Display_clear();            // remove current display content
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  // 1111111111111111        2222222222222222        3333333333333333
  Display_setFont(16);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 24, "HOME");

  Display_drawString(64, 0, String(servoVal[3] - Home3));  // Servo 3 Claw
  Display_drawString(64,48, String(servoVal[0] - Home0));  // Servo 0 Turntable
  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  Display_drawString( 0,24, String(servoVal[1] - Home1));  // Servo 1 Forward arm
  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_drawString(127,24, String(servoVal[2] - Home2)); // Servo 2 Vertical arm
  Display_display();  // display immediately
  DispDel = 4;       // wait 2s before displaying next count
}

// --------------------------------------------------------------------------------

void Display_Servo_PWM() {
  // display servo PWM values
  Display_clear();            // remove current display content
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  // 1111111111111111        2222222222222222        3333333333333333
  Display_setFont(16);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 24, "PWM");

  Display_drawString(64, 0, String(servoVal[3]));  // Servo 3 Claw
  Display_drawString(64,48, String(servoVal[0]));  // Servo 0 Turntable
  Display_setTextAlignment(TEXT_ALIGN_LEFT);
  Display_drawString( 0,24, String(servoVal[1]));  // Servo 1 Forward arm
  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_drawString(127,24, String(servoVal[2])); // Servo 2 Vertical arm
  Display_display();  // display immediately
  DispDel = 4;       // wait 2s before displaying next count
}

// --------------------------------------------------------------------------------

void Display_Text2x16(String zT0$,String zT1$) {
  // called to display a two lines of text centre screen in font 16
  Display_clear();
  Display_setFont(16);  // fonts are ArialMT_Plain_10,16,24
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 14, zT0$);
  Display_drawString(64, 34, zT1$);
  Display_display();
  DispNOP = true;
}

// --------------------------------------------------------------------------------

void Display_Title24(String zT$) {
  // called to display a title centre screen
  Display_clear();
  Display_setFont(24);  // fonts are ArialMT_Plain_10,16,24
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 20, zT$);
  Display_display();
  DispNOP = true;
}

// --------------------------------------------------------------------------------

void Display_WiFiStatus() {
  // displays WiFi link data  
  Display_clear();            // remove current display content
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  // 1111111111111111  2222222222  3333333333  4444444444  5555555555
  Display_setFont(16);
  Display_setTextAlignment(TEXT_ALIGN_CENTER);
  Display_drawString(64, 0, "WiFi STATUS");

  if (WiFiConnected) {
    if (WiFiRxBytes > 0) {
      // some data has been received
      Display_setFont(10);
      Display_setTextAlignment(TEXT_ALIGN_RIGHT);
      Display_drawString(61, 18, "Rx Bytes:");
      Display_setTextAlignment(TEXT_ALIGN_LEFT);
      Display_drawString(64, 18, getNumCSV(WiFiRxBytes));
      Display_setTextAlignment(TEXT_ALIGN_RIGHT);
      Display_drawString(61, 30, "Rx Error:");
      Display_setTextAlignment(TEXT_ALIGN_LEFT);
      Display_drawString(64, 30, String(WiFiRxErr));
  
      Display_setTextAlignment(TEXT_ALIGN_RIGHT);
      Display_drawString(61, 42, "Tx Bytes:");
      Display_setTextAlignment(TEXT_ALIGN_LEFT);
      Display_drawString(64, 42, getNumCSV(WiFiTxBytes));
      Display_setTextAlignment(TEXT_ALIGN_RIGHT);
      Display_drawString(61, 54, "Tx Error:");
      Display_setTextAlignment(TEXT_ALIGN_LEFT);
      Display_drawString(64, 54, String(WiFiTxErr));
    } else {
      // transmission errors so display disconnected
      Display_setTextAlignment(TEXT_ALIGN_CENTER);
      Display_drawString(64, 22, "NO LINK");
      Display_setFont(10);
      Display_drawString(64, 48, String(myMAC));
    }
  } else {
    Display_drawString(64, 22, "Disconnected");
    Display_drawString(64, 44, "Trying " + String(WiFiTryNum));
  }
  Display_display();  // display immediately
  DispDel = 4;       // wait 2s before displaying next count
}

// --------------------------------------------------------------------------------

void Display_Wii_Ctrl() {
  // display 6 bytes received from Wii over ESP-NOW
  Display_clear();            // remove current display content
  // Y64 vertical text spacing, from Y=0 to Y=63
  // 0123456789012345678901234567890123456789012345678901234567890123
  // 111111111122222222223333333333444444444455555555556666666666
  // 1111111111  2222222222
  // display I2C values as decimal 0 - 255
  Display_setTextAlignment(TEXT_ALIGN_RIGHT);
  Display_setFont(10); DispCX = 58;
  Display_drawString(DispCX,  0, String(RxWiFi[0]));
  Display_drawString(DispCX, 10, String(RxWiFi[1]));
  Display_drawString(DispCX, 20, String(RxWiFi[2]));
  Display_drawString(DispCX, 30, String(RxWiFi[3]));
  Display_drawString(DispCX, 40, String(RxWiFi[4]));
  Display_drawString(DispCX, 50, String(RxWiFi[5]));
  // display I2C values as binary 00000000 - 11111111
  Display_setTextAlignment(TEXT_ALIGN_LEFT); DispCX = 66;
  Display_drawString(DispCX,  0, GetBIN(RxWiFi[0]));
  Display_drawString(DispCX, 10, GetBIN(RxWiFi[1]));
  Display_drawString(DispCX, 20, GetBIN(RxWiFi[2]));
  Display_drawString(DispCX, 30, GetBIN(RxWiFi[3]));
  Display_drawString(DispCX, 40, GetBIN(RxWiFi[4]));
  Display_drawString(DispCX, 50, GetBIN(RxWiFi[5]));
  // display 'I2C' in top left corner
  Display_drawString(2, 0, "Wii");
       if (WiiType == 'C') {Display_drawString(2, 12, "Classic");}
  else if (WiiType == 'N') {Display_drawString(2, 12, "Nunchuk");}
  else if (WiiType == 'P') {Display_drawString(2, 12, "Classic Pro");}
  Display_display();  // display immediately
  DispDel = 4;       // wait 2s before displaying next count
}

// --------------------------------------------------------------------------------
