#include "MpuLcd.h"

void setup(void)
{
  Serial.begin(9600);
  
  Wire.begin(1,2);
  tft.init();
  
  pinMode(channel, INPUT_PULLUP);
  ledcSetup(channel, freq, resolution); // 设置通道
  ledcAttachPin(led, channel);          // 将通道与对应的引脚连接
  ledcWrite(channel, 21);               // 输出PWM

  // smoothed
  sl.begin(SMOOTHED_EXPONENTIAL, 1);
  sh.begin(SMOOTHED_EXPONENTIAL, 5);
  st.begin(SMOOTHED_EXPONENTIAL, 2);
  sy.begin(SMOOTHED_EXPONENTIAL, 5);
  sm.begin(SMOOTHED_EXPONENTIAL, 5);

  tft.setRotation(1);

  //  Wire.begin(36, 37, (uint32_t)350000);
  // pinMode(45, OUTPUT); // backlight
  // digitalWrite(45, HIGH);
  tft.fillScreen(TFT_BLACK);

  // tft.loadFont(NotoSansMonoSCB20);
  tft.setTextColor(rainbow(random(0, 127)));

  face.createSprite(244, 90);

  DashUp.createSprite(244, 25);
  DashDown.createSprite(244, 25);

  Hsprite.createSprite(244, 35);
  VTsprite.createSprite(35, 172);
  VHsprite.createSprite(45, 172);

  tft.setCursor(0, 15, 4);
  tft.println("-SYSTEM SELF-TEST...");
  // tft.setCursor(0, 30, 2);

  delay(200);
  tft.println("-I2C WIRE.BEGIN>>>");
  if (!mpu.setup(0x68))
  { // change to your own address
    while (1)
    {
      Serial.println("MPU connection failed. Please check your connection with `connection_check` example.");
      tft.println(" ... NG");
      delay(500);
    }
    tft.println(" ... OK");
  }

  tft.print("-GYRO CALIBRATION");
  Serial.println("Accel Gyro calibration will start in 5sec.");
  Serial.println("Please leave the device still on the flat plane.");
  mpu.verbose(true);
  delay(500);
  mpu.calibrateAccelGyro();
  tft.println(" ... OK");

  Serial.println("Mag calibration will start in 5sec.");
  Serial.println("Please Wave device in a figure eight until done.");
  delay(200);
  // mpu.calibrateMag();
  print_calibration();
  mpu.verbose(false);

  // AHT20
  tft.print("-AHT20 LINK VIA I2C");
  if (aht20.begin() == false)
  {
    Serial.println("AHT20 not detected. Please check wiring. Freezing.");
    while (1)
      ;
    tft.println(" ... NG");
  }
  tft.println(" ... OK");
  Serial.println("AHT20 acknowledged.");

  //
  unsigned status;
  // status = bmp.begin(BMP280_ADDRESS_ALT, BMP280_CHIPID);
  tft.print("-BMP280 LINK VIA I2C");
  status = bmp.begin();
  if (!status)
  {
    Serial.println(F("Could not find a valid BMP280 sensor, check wiring or "
                     "try a different address!"));
    Serial.print("SensorID was: 0x");
    Serial.println(bmp.sensorID(), 16);
    Serial.print("        ID of 0xFF probably means a bad address, a BMP 180 or BMP 085\n");
    Serial.print("   ID of 0x56-0x58 represents a BMP 280,\n");
    Serial.print("        ID of 0x60 represents a BME 280.\n");
    Serial.print("        ID of 0x61 represents a BME 680.\n");
    while (1)
      delay(10);
    tft.println(" ... NG");
  }
  tft.println(" ... OK");
  /* Default settings from datasheet. */
  bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,  /* Operating Mode. */
                  Adafruit_BMP280::SAMPLING_X2,  /* Temp. oversampling */
                  Adafruit_BMP280::SAMPLING_X16, /* Pressure oversampling */
                  Adafruit_BMP280::FILTER_X16,   /* Filtering. */
                  Adafruit_BMP280::STANDBY_MS_500);

  delay(200);
  tft.fillScreen(TFT_BLACK);
  face.fillRect(0, 0, X_TFT_width, YC, SKY_BLUE);
  face.fillRect(0, YC, X_TFT_width, Y_TFT_height, BROWN);
}
void loop()
{
  long now = millis();
  // fast task
  get_MPU_Angle_Readings();
  updateHorizon(roll, pitch);



  // slow task
   get_BMP_Readings();
   smooth_BMP();
   Hmeter(sYaw-110);
   VTmeter(sTemp * 100 -15);
   VHmeter((sHigh-HighOrg)*10-15); //abs high
   drawSenTOP();
   drawSenDown();

  fps = millis() - now;
  Serial.println(fps);

  // Serial.print("stemp=");
  // Serial.print(sTemp);
  // Serial.print("  sHigh=");
  // Serial.print(sHigh);
  // Serial.print("  Roll=");
  // Serial.print(Yaw);
  // Serial.print("  Yaw=");
  // Serial.println(bTemp);
  ////////////////////////////////////// TFT display //////////////////////////

  // if (millis() > redrawTime)
  // {

  //   redrawTime = millis() + REDRAW_DELAY;
  //   updateHorizon(roll, pitch);
  //   get_BMP_Readings();
  // }

  // brightness += brightStep;
  // if ( brightness == 0 || brightness == 255 ) {
  //   brightStep = -brightStep;
  // }

  // analogWrite(45, brightness);

  // delay(10);
}
