#include <Arduino.h>
#include <U8g2lib.h>

#ifdef U8X8_HAVE_HW_SPI
#include <SPI.h>
#endif
#ifdef U8X8_HAVE_HW_I2C
#include <Wire.h>
#endif

U8G2_SSD1306_128X64_ALT0_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);   // same as the NONAME variant, but may solve the "every 2nd line skipped" problem

uint32_t t;

int Roll = -64;
int Pitch;
int spd = 399.9;
int alt = 399.9;

//// BMP 画面
static const unsigned char open_bits[] U8X8_PROGMEM =
    {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x38,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0x80, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1F, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x1B, 0xC7, 0xF0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09,
0xF0, 0x3F, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x7C, 0x01, 0xFC, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x3F, 0x00, 0x0F, 0xE0, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x04, 0x0D, 0xC0, 0x00, 0xFE, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06,
0x07, 0x70, 0x00, 0x07, 0xF0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x01, 0x9C, 0x00, 0x00,
0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0xE7, 0x80, 0x00, 0xE0, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x03, 0x00, 0x31, 0xE0, 0x03, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
0x00, 0x1C, 0x78, 0x0E, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x80, 0x06, 0x1E, 0x38,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x03, 0x87, 0xF0, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0xC0, 0x01, 0xC1, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0xC0, 0x00, 0x61, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x38, 0x80,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x60, 0x00, 0x1C, 0xC0, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x3C, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x30, 0x00, 0x26, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x66, 0x40,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0xC3, 0x60, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x18, 0x01, 0xE3, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x08, 0x03, 0x31, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x06, 0x19, 0xA0,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x06, 0x0C, 0xA0, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x04, 0x0C, 0x06, 0xE0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x06, 0x18, 0x03, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x30, 0x01, 0xF0,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x60, 0x00, 0xF0, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x03, 0xC0, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x01, 0x80, 0x00, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x10,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x3F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC,
0x7F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xF0, 0x4E, 0x4E, 0x40, 0xC1, 0xFE, 0x08, 0x09,
0xE6, 0x07, 0x3C, 0x1F, 0xE0, 0x4E, 0x4E, 0x40, 0xC0, 0xFC, 0x08, 0x09, 0xE6, 0x03, 0x38, 0x0F,
0xE7, 0xCC, 0xE4, 0xCF, 0xCC, 0xFC, 0xFE, 0x79, 0xE6, 0x73, 0x33, 0xEF, 0xE7, 0xC9, 0xE4, 0xCF,
0xCC, 0xFC, 0x7E, 0x79, 0xE6, 0x79, 0x33, 0xE7, 0xF1, 0xC3, 0xF0, 0xC0, 0xC0, 0xFE, 0x1E, 0x79,
0xE6, 0x79, 0x33, 0xE7, 0xF8, 0x41, 0xF1, 0xC1, 0xC1, 0xFF, 0x0E, 0x79, 0xE6, 0x79, 0x33, 0xE7,
0xFE, 0x48, 0xF9, 0xCF, 0xCC, 0xFF, 0xCE, 0x79, 0xE6, 0x7B, 0x33, 0xE7, 0xFE, 0x4C, 0xF9, 0xCF,
0xCC, 0xFF, 0xCE, 0x79, 0xE6, 0x73, 0x33, 0xEF, 0xE0, 0x4E, 0x79, 0xC0, 0xCC, 0x7C, 0x0E, 0x7C,
0x0E, 0x03, 0x38, 0x0F, 0xE1, 0xCE, 0x79, 0xC0, 0xCE, 0x7C, 0x1E, 0x7E, 0x1E, 0x07, 0x3C, 0x1F,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x7F, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE, 0x3F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

void setup(void) {
  u8g2.begin();

  u8g2.clearBuffer();
  // u8g2.setBitmapMode(false /* solid */);
  u8g2.drawBitmap(16, 4, 12, 56, open_bits);
  u8g2.sendBuffer();
  delay(2000);


  u8g2.enableUTF8Print();  // 显示中文

  u8g2.clearBuffer();
  u8g2.setFont(u8g2_font_unifont_t_chinese3);
  u8g2.setCursor(37,16);
  u8g2.print("坡度校准");
  u8g2.setFont(u8g2_font_helvB08_tf);
  u8g2.setCursor(9,34);
  u8g2.printf("gxof: %d", 123);
  u8g2.setCursor(9,48);
  u8g2.printf("gyof: %d", 123);
  u8g2.setCursor(9,62);
  u8g2.printf("gzof: %d", 123);

  u8g2.setCursor(73,34);
  u8g2.printf("axof: %d", 123);
  u8g2.setCursor(73,48);
  u8g2.printf("ayof: %d", 123);
  u8g2.setCursor(73,62);
  u8g2.printf("azof: %d", 123);
  u8g2.sendBuffer();
  delay(2000);

  u8g2.clearBuffer();
  u8g2.setFont(u8g2_font_unifont_t_chinese3);
  u8g2.setCursor(37,16);
  u8g2.print("方向校准");
  u8g2.setFont(u8g2_font_helvB08_tf);
  u8g2.setCursor(37,34);
  u8g2.printf("mxof:   %d", 123);
  u8g2.setCursor(37,48);
  u8g2.printf("myof:   %d", 123);
  u8g2.setCursor(37,62);
  u8g2.printf("mzof:   %d", 123);
  u8g2.sendBuffer();
  delay(5000);

  u8g2.clearBuffer();
  u8g2.setFont(u8g2_font_unifont_t_chinese3);
  u8g2.setCursor(37,24);
  u8g2.print("页面通道");
  u8g2.setFont(u8g2_font_helvB10_tf);
  u8g2.setCursor(60,53);
  u8g2.printf("%.d", 12);
  u8g2.sendBuffer();
  delay(2000);
}

void loop(void) {
  long start = micros();
  Roll+=3;
  Pitch = 4;
  
  u8g2.clearBuffer();
  //////////////////////////////////////
  // Pitch 中位37 + - 26
  int y[5];

  if (Pitch > 40) Pitch = 40;
  else if (Pitch < -39) Pitch = -39;

  y[2] = 37 + Pitch * 0.65;
  y[0] = y[2] - 26;
  y[1] = y[2] - 13;
  y[3] = y[2] + 13;
  y[4] = y[2] + 26;

  if (y[0]<11) y[0] = 64 + 13 + (y[0] - 11);
  if (y[1]<11) y[1] = 64 + 13 + (y[1] - 11);
  if (y[3]>76) y[3] = 10 + (y[3] - 76);
  if (y[4]>76) y[4] = 10 + (y[4] - 76);

  u8g2.drawLine(41, y[0], 50, y[0]);
  u8g2.drawLine(41, y[1], 50, y[1]);
  u8g2.drawLine(41, y[2], 57, y[2]);
  u8g2.drawLine(41, y[3], 50, y[3]);
  u8g2.drawLine(41, y[4], 50, y[4]);

  u8g2.drawLine(78, y[0], 87, y[0]);
  u8g2.drawLine(78, y[1], 87, y[1]);
  u8g2.drawLine(71, y[2], 87, y[2]);
  u8g2.drawLine(78, y[3], 87, y[3]);
  u8g2.drawLine(78, y[4], 87, y[4]);

  // Roll
  int xR;
  if (Roll<0) xR = 64+Roll;
  else xR = 64;
  u8g2.drawBox(xR, 3, abs(Roll), 6);
  u8g2.drawBox(63, 0, 2, 9);

  // Frame_L
  u8g2.drawLine(0, 11, 41, 11);
  u8g2.drawLine(41, 11, 41, 63);
  // Frame_R
  u8g2.drawLine(87, 11, 127, 11);
  u8g2.drawLine(87, 11, 87, 63);

  // Jet
  u8g2.drawLine(59, 37, 69, 37);
  u8g2.drawLine(59, 37, 59, 41);
  u8g2.drawLine(69, 37, 69, 41);

  // SPD
  u8g2.setFont(u8g2_font_helvR14_tf); // R\B, 08\10\12\14\18\24
  u8g2.setCursor(2,43);
  u8g2.print((int)spd);
  u8g2.setCursor(95,43);
  u8g2.print((int)alt);
  // ALT
  u8g2.setFont(u8g2_font_helvR10_tf);
  u8g2.setCursor(6,60);
  u8g2.print("kmh");
  u8g2.setCursor(104,60);
  u8g2.print("M");
  //////////////////////////////////////
  u8g2.sendBuffer();
  t = micros() - start;

  delay(200);
}






void RTH_PAGE(){
  u8g2.clearBuffer();

  u8g2.drawFrame(47,1,33,18);
  u8g2.setFont(u8g2_font_helvB12_tf); // R\B, 08\10\12\14\18\24
  u8g2.setCursor(51,16);
  u8g2.print("270");
  
  u8g2.setFont(u8g2_font_helvB10_tf);
  u8g2.setCursor(0,16);
  u8g2.print("D2.4");

  u8g2.setCursor(100,16);
  u8g2.print("S12");

  u8g2.setFont(u8g2_font_helvR18_tf);
  u8g2.setCursor(44,58);
  u8g2.print("124");

  u8g2.setFont(u8g2_font_open_iconic_embedded_4x_t);
  u8g2.setCursor(48,64);
  u8g2.print("\u0044");

  /*
  u8g2.setFont(u8g2_font_open_iconic_arrow_4x_t);
  u8g2.setCursor(4,64);
  u8g2.print("\u0049");
  u8g2.setCursor(89,64);
  u8g2.print("\u004a");
  */

  u8g2.sendBuffer();
}