#include "Var.h"

#include "Lcd.h"
#include "NotoSansBold15.h"
#include "NotoSansMonoSCB20.h"
#include "img.h"
void smooth_BMP(void)

{
  int currentSensorValue = analogRead(SENSOR_PIN);
  sl.add(currentSensorValue);
  int smoothedSensorValueExp = sl.get();
  int lastValueStoredExp = sl.getLast();
  if (smoothedSensorValueExp >= 550)
    smoothedSensorValueExp = 550;
  if (smoothedSensorValueExp <= 5)
    smoothedSensorValueExp = 5;
  slight = smoothedSensorValueExp;
  ledcWrite(channel, map(smoothedSensorValueExp, 5, 550, 18, 255)); // 输出PWM
  // ledcWrite(channel, map(smoothedSensorValueExp, 500, 4000, 200, 255)); // 输出PWM

  float currentSensorValueH = High;
  sh.add(currentSensorValueH);
  float smoothedSensorValueExpH = sh.get();
  sHigh = smoothedSensorValueExpH;
  float lastValueStoredExpH = sh.getLast();

  int currentSensorValueY = Yaw;
  sy.add(currentSensorValueY);
  int smoothedSensorValueExpY = sy.get();
  sYaw = smoothedSensorValueExpY;
  int lastValueStoredExpY = sy.getLast();

  float currentSensorValueT = bTemp;
  st.add(currentSensorValueT);
  float smoothedSensorValueExpT = st.get();
  sTemp = smoothedSensorValueExpT;
  float lastValueStoredExpT = st.getLast();

  float currentSensorValueHM = bm;
  sm.add(currentSensorValueHM);
  float smoothedSensorValueExpHM = sm.get();
  sM = smoothedSensorValueExpHM;
  float lastValueStoredExpHM = sm.getLast();
}

void Hmeter(int x)
{
  Hsprite.fillSprite(TFT_BLACK);
  //Hsprite.loadFont(NotoSansBold15);
  int temp = x;
  for (int i = 0; i < 80; i++)
  {
    if ((temp % 10) == 0)
    {
      Hsprite.drawLine(i * 4, 170 - 145, i * 4, 158 - 150, WHITE);
      Hsprite.drawLine((i * 4) + 1, 170 - 145, (i * 4) + 1, 158 - 150, WHITE);
      Hsprite.setTextColor(WHITE, TFT_BLACK);
      Hsprite.drawFloat(temp, 0, i * 4+8, 0,2);
    }
    else if ((temp % 5) == 0 && (temp % 10) != 0)
    {
      Hsprite.drawLine(i * 4, 170 - 145, i * 4, 162 - 145, WHITE);
      Hsprite.drawLine((i * 4) + 1, 170 - 145, (i * 4) + 1, 162 - 145, WHITE);
    }
    else
    {
      Hsprite.drawLine(i * 4, 170 - 145, i * 4, 164 - 145, WHITE);
    }
    temp = temp + 1;
  }
  // Hsprite.fillTriangle(160 - 160, 138, 156 - 160, 130, 164 - 160, 130, TFT_CYAN);
  Hsprite.pushSprite(35, 145);
}

void VTmeter(float x)
{
  VTsprite.fillSprite(TFT_BLACK);
  //VTsprite.loadFont(NotoSansBold15);
  int temp = x;
  for (int i = 0; i < 40; i++)
  {
    if ((temp % 10) == 0)
    {
      VTsprite.drawLine(33, i * 4 + 25, 33 - 20, i * 4 + 25, WHITE);
      VTsprite.drawLine(33, i * 4 + 26, 33 - 20, i * 4 + 26, WHITE);
      VTsprite.setTextColor(WHITE, TFT_BLACK);
      VTsprite.drawFloat(temp / 10, 0, 2, i * 4 + 8,2);
    }
    else if ((temp % 5) == 0 && (temp % 10) != 0)
    {
      VTsprite.drawLine(33, i * 4 + 25, 33 - 16, i * 4 + 25, WHITE);
      VTsprite.drawLine(33, i * 4 + 26, 33 - 16, i * 4 + 26,  WHITE);
    }
    else
    {
      VTsprite.drawLine(33, i * 4 + 25, 33 - 4, i * 4 + 25,WHITE);
    }
    temp = temp + 1;
  }
  VTsprite.pushSprite(0, 0);
}

void VHmeter(int x)
{
  VHsprite.fillSprite(TFT_BLACK);
  //VHsprite.loadFont(NotoSansBold15);
  int temp = x;
  for (int i = 0; i < 40; i++)
  {
    if ((temp % 10) == 0)
    {
      VHsprite.drawLine(0, i * 4 + 25, 17+8, i * 4 + 25, WHITE);
      VHsprite.drawLine(0, i * 4 + 26, 17+8, i * 4 + 26, WHITE);
      VHsprite.setTextColor(WHITE, TFT_BLACK);
      VHsprite.drawFloat(temp/10, 0, 11, i * 4 + 8,2);
    }
    else if ((temp % 5) == 0 && (temp % 10) != 0)
    {
      VHsprite.drawLine(0, i * 4 + 25, 11+8, i * 4 + 25, WHITE);
      VHsprite.drawLine(0, i * 4 + 26, 11+8, i * 4 + 26, WHITE);
    }
    else
    {
      VHsprite.drawLine(0, i * 4 + 25, 7, i * 4 + 25, WHITE);
    }
    temp = temp + 1;
  }

  // Vsprite.fillTriangle(0, 112, 8, 108, 8, 116, TFT_CYAN);

  // Vsprite.drawWideLine(0, 73, 15, 73,3,RED);

  VHsprite.fillTriangle(0, 83, 0, 89, 7, 86, RED);

  VHsprite.pushSprite(320 - 40, 0);
}

unsigned int rainbow(byte value)
{
  // Value is expected to be in range 0-127
  // The value is converted to a spectrum colour from 0 = blue through to 127 = red

  byte red = 0;   // Red is the top 5 bits of a 16 bit colour value
  byte green = 0; // Green is the middle 6 bits
  byte blue = 0;  // Blue is the bottom 5 bits

  byte quadrant = value / 32;

  if (quadrant == 0)
  {
    blue = 31;
    green = 2 * (value % 32);
    red = 0;
  }
  if (quadrant == 1)
  {
    blue = 31 - (value % 32);
    green = 63;
    red = 0;
  }
  if (quadrant == 2)
  {
    blue = 0;
    green = 63;
    red = value % 32;
  }
  if (quadrant == 3)
  {
    blue = 0;
    green = 63 - 2 * (value % 32);
    red = 31;
  }
  return (red << 11) + (green << 5) + blue;
}

void drawHorizon(int roll, int pitch)
{
  // face.fillScreen(TFT_BLACK);
  // face.fillRect(0, 0, X_TFT_width, YC, SKY_BLUE);
  // face.fillRect(0, YC, X_TFT_width, Y_TFT_height, BROWN);
  // Calculate coordinates for line start
  float sx = cos(roll * DEG2RAD);
  float sy = sin(roll * DEG2RAD);

  int16_t x0 = sx * HOR;
  int16_t y0 = sy * HOR;
  int16_t xd = 0;
  int16_t yd = 1;
  int16_t xdn = 0;
  int16_t ydn = 0;

  if (roll > 45 && roll < 135)
  {
    xd = -1;
    yd = 0;
  }
  if (roll >= 135)
  {
    xd = 0;
    yd = -1;
  }
  if (roll < -45 && roll > -135)
  {
    xd = 1;
    yd = 0;
  }
  if (roll <= -135)
  {
    xd = 0;
    yd = -1;
  }

  if ((roll != last_roll) || (pitch != last_pitch))
  {
    xdn = 6 * xd;
    ydn = 6 * yd;
    // roll meter

    face.drawLine(XC - x0 - xdn, YC - y0 - ydn - pitch, XC + x0 - xdn, YC + y0 - ydn - pitch, SKY_BLUE);
    face.drawLine(XC - x0 + xdn, YC - y0 + ydn - pitch, XC + x0 + xdn, YC + y0 + ydn - pitch, BROWN);
    xdn = 5 * xd;
    ydn = 5 * yd;
    face.drawLine(XC - x0 - xdn, YC - y0 - ydn - pitch, XC + x0 - xdn, YC + y0 - ydn - pitch, SKY_BLUE);
    face.drawLine(XC - x0 + xdn, YC - y0 + ydn - pitch, XC + x0 + xdn, YC + y0 + ydn - pitch, BROWN);
    xdn = 4 * xd;
    ydn = 4 * yd;
    face.drawLine(XC - x0 - xdn, YC - y0 - ydn - pitch, XC + x0 - xdn, YC + y0 - ydn - pitch, SKY_BLUE);
    face.drawLine(XC - x0 + xdn, YC - y0 + ydn - pitch, XC + x0 + xdn, YC + y0 + ydn - pitch, BROWN);

    xdn = 3 * xd;
    ydn = 3 * yd;
    face.drawLine(XC - x0 - xdn, YC - y0 - ydn - pitch, XC + x0 - xdn, YC + y0 - ydn - pitch, SKY_BLUE);
    face.drawLine(XC - x0 + xdn, YC - y0 + ydn - pitch, XC + x0 + xdn, YC + y0 + ydn - pitch, BROWN);
  }
  xdn = 2 * xd;
  ydn = 2 * yd;
  face.drawLine(XC - x0 - xdn, YC - y0 - ydn - pitch, XC + x0 - xdn, YC + y0 - ydn - pitch, SKY_BLUE);
  face.drawLine(XC - x0 + xdn, YC - y0 + ydn - pitch, XC + x0 + xdn, YC + y0 + ydn - pitch, BROWN);

  face.drawLine(XC - x0 - xd, YC - y0 - yd - pitch, XC + x0 - xd, YC + y0 - yd - pitch, SKY_BLUE);
  face.drawLine(XC - x0 + xd, YC - y0 + yd - pitch, XC + x0 + xd, YC + y0 + yd - pitch, BROWN);

  face.drawLine(XC - x0, YC - y0 - pitch, XC + x0, YC + y0 - pitch, GREY);

  last_roll = roll;
  last_pitch = pitch;
  // face.setTextColor(TFT_WHITE, SKY_BLUE);
  // face.setCursor(2, 0, 1);
  // face.print("RUNHOURS");

  // face.setTextColor(TFT_WHITE, SKY_BLUE);
  // face.setCursor(225, 0, 1);
  // face.print("CLIMB");
}

void drawSenTOP(void)
{
  DashUp.fillSprite(TFT_BLACK);
  // DashUp.loadFont(NotoSansMonoSCB20);

  int SS = millis() / 1000;
  int mm = (SS % 3600) / 60;
  int hh = (SS / 3600);

  DashUp.setTextColor(Fubo, TFT_BLACK);
  DashUp.setCursor(2, 2, 4);
  // DashUp.print("Timer");
  // DashUp.setCursor(0, 8, 4);
  DashUp.print(hh);
  DashUp.print(":");
  DashUp.print(mm);
  DashUp.print(":");
  DashUp.print(SS % 60);

  // HIGH

  DashUp.setTextColor(0x6e5a, TFT_BLACK);
  DashUp.setCursor(100, 2, 4);
  // DashUp.print("Altitude");
  // DashUp.setCursor(180, 8, 4);
  DashUp.print(sHigh,0);
  // DashUp.setTextSize(1);
  DashUp.print("m");
  // Abs high



  DashUp.setTextDatum(BR_DATUM);
  DashUp.setCursor(180, 2, 4);
  if (sHigh - HighOrg < 0)
  {
    DashUp.setTextColor(GREEN, TFT_BLACK);
    DashUp.print("-");
  }
    
  if (sHigh - HighOrg >= 0)
  {
    DashUp.setTextColor(ORANGE, TFT_BLACK);
    DashUp.print("+");
  }

  // DashUp.print("Altitude");
  // DashUp.setCursor(180, 8, 4);
  DashUp.print(abs(sHigh - HighOrg),1);//HighOrg
  //DashUp.print("m");
  // DashUp.setTextSize(1);

  DashUp.pushSprite(35, 0);
}

void drawSenDown(void)
{
  DashDown.fillSprite(TFT_BLACK);
  // DashUp.loadFont(NotoSansMonoSCB20);
  DashDown.setTextColor(WHITE, TFT_BLACK);
  DashDown.setCursor(2, 2, 4);
  DashDown.print(temperature, 1);
  // DashUp.setTextSize(1);
  DashDown.print("`c");
  // DashUp.setTextSize(2);

  // Humi

  // DashDown.setCursor(210, 120, 0);
  // DashDown.print("humidity");
  DashDown.setCursor(168, 2, 4);
  DashDown.print(sM, 1);
  // DashUp.setTextSize(1);
  DashDown.print("%");
  DashDown.pushSprite(35, 115);
}

void drawInfo(void)
{

  // // Yaw
  // face.setTextColor(WHITE, BROWN);
  // face.setCursor(120, 125, 0);
  // face.print("Yaw");
  // face.setCursor(120, 135, 2);
  // face.print(Yaw);
  // face.print("`");
  // face.fillScreen(TFT_BLACK);
  //  Update things near middle of screen first (most likely to get obscured)

  // Level wings graphic

  face.fillRect(XC - 2, YC - 2, 5, 5, PINK);
  face.drawFastHLine(XC - 55, YC, 50, TFT_RED);
  face.drawFastHLine(XC + 55 - 50, YC, 50, TFT_RED);
  face.drawFastVLine(XC - 45 + 40, YC, 5, TFT_RED);
  face.drawFastVLine(XC + 45 - 40, YC, 5, TFT_RED);

  face.drawFastHLine(XC - 60, YC - 40, 120, GREY); // 20 deg horizontal
  face.drawFastHLine(XC - 45, YC - 30, 90, GREY);  // 15 deg horizontal
  face.drawFastHLine(XC - 60, YC - 20, 120, GREY); // 10 deg horizontal
  face.drawFastHLine(XC - 45, YC - 10, 90, GREY);  // 5 deg horizontal

  face.drawFastHLine(XC - 45, YC + 10, 90, GREY);  //-5 deg horizontal
  face.drawFastHLine(XC - 60, YC + 20, 120, GREY); //-10 deg horizontal
  face.drawFastHLine(XC - 45, YC + 30, 90, GREY);  //-15 deg horizontal
  face.drawFastHLine(XC - 60, YC + 40, 120, GREY); //-20 deg horizontal
                                                        //   face.loadFont(NotoSansBold15);
                                                        // Pitch numbers  upper 10....lower 10 left and right
  face.setTextColor(GREY);
  face.setCursor(XC - 60 - 20, YC - 20 - 3, 1); // left +10
  face.print("10");                             // left +10
  face.setCursor(XC + 60 + 3, YC - 20 - 3);     // Right +10
  face.print("10");                             // left -10
  face.setCursor(XC - 60 - 20, YC + 20 - 3);    // left -10
  face.print("10");                             // Right +10
  face.setCursor(XC + 60 + 3, YC + 20 - 3);     // Right -10
  face.print("10");                             // Right -10

  // Pitch numbers  lower 20....upper 20 left and right
  face.setCursor(XC - 60 - 20, YC - 40 - 3); //+left 20
  face.print("20");                          // left 20
  face.setCursor(XC + 60 + 3, YC - 40 - 3);  // +Right 20
  face.print("20");                          // left 20
  face.setCursor(XC - 60 - 20, YC + 40 - 3); //-left 20
  face.print("20");                          // Right 20
  face.setCursor(XC + 60 + 3, YC + 40 - 3);  //-Right 20
  face.print("20");                          // Right 20
}

void print_calibration()
{
  Serial.println("< calibration parameters >");
  Serial.println("accel bias [g]: ");
  Serial.print(mpu.getAccBiasX() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
  Serial.print(", ");
  Serial.print(mpu.getAccBiasY() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
  Serial.print(", ");
  Serial.print(mpu.getAccBiasZ() * 1000.f / (float)MPU9250::CALIB_ACCEL_SENSITIVITY);
  Serial.println();
  Serial.println("gyro bias [deg/s]: ");
  Serial.print(mpu.getGyroBiasX() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
  Serial.print(", ");
  Serial.print(mpu.getGyroBiasY() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
  Serial.print(", ");
  Serial.print(mpu.getGyroBiasZ() / (float)MPU9250::CALIB_GYRO_SENSITIVITY);
  Serial.println();
  Serial.println("mag bias [mG]: ");
  Serial.print(mpu.getMagBiasX());
  Serial.print(", ");
  Serial.print(mpu.getMagBiasY());
  Serial.print(", ");
  Serial.print(mpu.getMagBiasZ());
  Serial.println();
  Serial.println("mag scale []: ");
  Serial.print(mpu.getMagScaleX());
  Serial.print(", ");
  Serial.print(mpu.getMagScaleY());
  Serial.print(", ");
  Serial.print(mpu.getMagScaleZ());
  Serial.println();
}
void print_roll_pitch_yaw()
{
  // Serial.print("Yaw, Pitch, Roll: ");
  // Serial.print(mpu.getYaw(), 2);
  // Serial.print(", ");
  // Serial.print(mpu.getPitch(), 2);
  // Serial.print(", ");
  // Serial.println(mpu.getRoll(), 2);
}
void get_MPU_Angle_Readings()
{
  // MPU9250

  if (mpu.update())
  {
    static uint32_t prev_ms = millis();
    if (millis() > prev_ms + 50)
    {
      print_roll_pitch_yaw();
      prev_ms = millis();
    }
  }

  Pitch = (mpu.getPitch());
  pitch = Pitch;
  alt = Pitch; // getting values to display
  roll = (mpu.getRoll());
  climb = roll;
  Yaw = (mpu.getYaw());
}
void updateHorizon(int roll, int pitch)
{
  bool draw = 1;
  int delta_pitch = 0;
  int pitch_error = 0;
  int delta_roll = 0;
  while ((last_pitch != pitch) || (last_roll != roll))
  {
    delta_pitch = 0;
    delta_roll = 0;

    if (last_pitch < pitch)
    {
      delta_pitch = 1;
      pitch_error = pitch - last_pitch;
    }

    if (last_pitch > pitch)
    {
      delta_pitch = -1;
      pitch_error = last_pitch - pitch;
    }

    if (last_roll < roll)
      delta_roll = 1;
    if (last_roll > roll)
      delta_roll = -1;

    if (delta_roll == 0)
    {
      if (pitch_error > 1)
        delta_pitch *= 2;
    }
    drawHorizon(last_roll + delta_roll, last_pitch + delta_pitch);
    drawInfo();
    face.pushSprite(35, 25);
  }
}

void get_BMP_Readings()
{

  // bmp280
  //  Serial.print(F("Temperature = "));
  //  Serial.print(bmp.readTemperature());
  //  Serial.println(" *C");

  // Serial.print(F("Pressure = "));
  // Serial.print(bmp.readPressure());
  // Serial.println(" Pa");

  // Serial.print(F("Approx altitude = "));
  // Serial.print(bmp.readAltitude(1013.25)); /* Adjusted to local forecast! */
  // Serial.println(" m");

  // Serial.println();
  // delay(2000);

  temperature = bmp.readTemperature();
  bTemp = temperature;
  pressure = (bmp.readPressure() / 100.0F);

  alt = (bmp.readAltitude(1013.25) - startAltitude);
  High = alt;

  if (initHighFlag == 0)
  {
    if (millis() > 10000)
    {
      HighOrg = sHigh;
      initHighFlag = 1;
    }
  }

  // AHT20
  if (aht20.available() == true)
  {
    // Get the new temperature and humidity value
    float temperature = aht20.getTemperature();
    float humidity = aht20.getHumidity();
    bm = humidity;

    // Print the results
    //  Serial.print("Temperature: ");
    //  Serial.print(temperature, 2);
    //  Serial.print(" C\t");
    //  Serial.print("Humidity: ");
    //  Serial.print(humidity, 2);
    //  Serial.print("% RH");

    // Serial.println();
  }
}
