#include <SPI.h>
#include <TFT_eSPI.h>
#include "User_Setup.h"

#define M_SIZE 1.3333
TFT_eSPI tft = TFT_eSPI();

#define TFT_GREY 0x5AEB
// Display updates every 35 ms
#define LOOP_PERIOD 35
// Saved x coord of bottom of needle
float ltx = 0;
// Saved x & y coords
uint16_t osx = M_SIZE * 120, osy = M_SIZE * 120;
// time for next update
uint32_t updateTime = 0;
// Value last displayed
int old_analog =  -999;
// Value last displayed
int old_digital = -999;
int value[6] = {0, 0, 0, 0, 0, 0};
int old_value[6] = { -1, -1, -1, -1, -1, -1};
int d = 0;
///////////
int MWX = 239;  //  Meter rectangle width
int MHY = 126;  //  Meter rectangle height
int RHM_x0 = 0;
int RHM_y0 = 0;
int RHM_xl = 140;
int RHM_yl = 20;
// int x_del = 0;  // define number of right shift x - pixels
int x_del = 20;  // define number of right shift x - pixels
int y_del = 0;  // define number of down shift y - pixels
int LHM_x0 = x_del;  // define meter rectangle upper LH corner x - coordinant in pixels plus x_del
int LHM_y0 = y_del;  // define meter rectangle upper LH corner y - coordinant in pixels plus x_del
int LHM_xL = LHM_x0 + MWX;  // define meter rectangle far corner x coordinant pixel location
int LHM_yL = LHM_y0 + MHY;  // define meter rectangle far corner y coordinant pixel location
int p_x0 = ( MWX / 2 + 1) + x_del;
int p_x0_del = 20;
int p_y0 = MHY + 14 + y_del;
int p_y0_del = 20;
//////////
void setup(void) {
  tft.init();
  tft.setRotation(1);
  Serial.begin(115200); // For debug 57600
  tft.fillScreen(TFT_BLACK);
  // Draw analogue meter
  analogMeter();
  //////
  byte d = 40;
  /////
  // Next update time
  updateTime = millis();
}

void loop() {
  if (updateTime <= millis()) {
    // Update meter every 35 milliseconds
    //updateTime = millis() + LOOP_PERIOD;
    updateTime = millis() + 35;
    // Create a Sine wave for testing
    d += 4;
    if (d >= 360)
      d = 0;
    value[0] = 50 + 50 * sin((d + 0) * 0.0174532925);
    //value[0] = map(analogRead(A0), 0, 1023, 0, 100); // Test with value from Analogue 0
    // Create a Sine wave for testing - these are y-coord values for the top end of the pointer line
    /*
      value[0] = 50 + 50 * sin((d + 0) * 0.0174532925);
      value[1] = 50 + 50 * sin((d + 60) * 0.0174532925);
      value[2] = 50 + 50 * sin((d + 120) * 0.0174532925);
      value[3] = 50 + 50 * sin((d + 180) * 0.0174532925);
      value[4] = 50 + 50 * sin((d + 240) * 0.0174532925);
      value[5] = 50 + 50 * sin((d + 300) * 0.0174532925);*/
    // unsigned long t = millis();
    // plotPointer();
    // It takes between 2 and 12ms to replot the needle with zero delay
    plotNeedle(value[0], 0);
    // Serial.println(millis()-t); // Print time taken for meter update
  }
}
//  Draw the analogue meter on the screen
/*
  void analogMeter() {
  // Meter rectangle outline
  // Left Meter
  tft.fillRect(LHM_x0, LHM_y0, MWX, LHM_yL, TFT_YELLOW);
  tft.fillRect(LHM_x0 + 5, LHM_y0 + 3, MWX - 9, LHM_yL - 7, TFT_WHITE);
  // Right Meter
  // tft.fillRect(155, 0, 300, 126 + mydely, TFT_GREY);
  // tft.fillRect(160, 3, 295, 119 + mydely, TFT_WHITE);
  tft.setTextColor(TFT_BLACK);  // Text color
  // Draw ticks every 5 degrees from -50 to +50 degrees (100 deg. FSD swing)
  for (int i = -50; i < 51; i += 5) {
    // Long scale tick length
    int tl = 15;
    // Coodinates of tick to draw
    float sx = cos((i - 90) * 0.0174532925);
    float sy = sin((i - 90) * 0.0174532925);
    uint16_t x0 = sx * (100 + tl) + p_x0;     // p_x0 = 120
    uint16_t y0 = sy * (100 + tl) + p_y0;     // p_y0 = 140
    uint16_t x1 = sx * 100 + p_x0;   // p_x0 = 120
    uint16_t y1 = sy * 100 + p_y0;   // p_y0 = 140
    // Coordinates of next tick for zone fill these are linked
    // to variable specified locations, for p_x0 and p_y0
    // including any specified x_del or y_del shift values
    float sx2 = cos((i + 5 - 90) * 0.0174532925);
    float sy2 = sin((i + 5 - 90) * 0.0174532925);
    int x2 = sx2 * (100 + tl) + p_x0;  // p_x0 = 120
    int y2 = sy2 * (100 + tl) + p_y0;  // p_y0 = 140
    int x3 = sx2 * 100 + p_x0;  // p_x0 = 120
    int y3 = sy2 * 100 + p_y0;  // p_y0 = 140
    // Yellow zone limits
    if (i >= -50 && i < 0) {
      tft.fillTriangle(x0, y0, x1, y1, x2, y2, TFT_YELLOW);
      tft.fillTriangle(x1, y1, x2, y2, x3, y3, TFT_YELLOW);
    }
    // Green zone limits
    if (i >= 0 && i < 25) {
      tft.fillTriangle(x0, y0, x1, y1, x2, y2, TFT_GREEN);
      tft.fillTriangle(x1, y1, x2, y2, x3, y3, TFT_GREEN);
    }
    // Orange zone limits
    if (i >= 25 && i < 50) {
      tft.fillTriangle(x0, y0, x1, y1, x2, y2, TFT_ORANGE);
      tft.fillTriangle(x1, y1, x2, y2, x3, y3, TFT_ORANGE);
    }
    // Short scale tick length
    if (i % 25 != 0)
      tl = 8;
    // Recalculate coords in case tick length changed
    x0 = sx * (100 + tl) + p_x0;  // p_x0 = 120
    y0 = sy * (100 + tl) + p_y0;  // p_y0 = 140
    x1 = sx * 100 + p_x0;  // p_x0 = 120
    y1 = sy * 100 + p_y0;  // p_y0 = 140
    // Draw tick, either short or long
    tft.drawLine(x0, y0, x1, y1, TFT_BLACK);
    // Check if labels on the meter face arc should be drawn,
    // with position tweaks
    if (i % 25 == 0) {
      // Calculate scale label positions
      x0 = sx * (100 + tl + 10) + p_x0;
      y0 = sy * (100 + tl + 10) + p_y0;
      switch (i / 25) {
        case -2: tft.drawCentreString("0", x0, y0 - 12, 2); break;
        case -1: tft.drawCentreString("25", x0, y0 - 9, 2); break;
        case 0: tft.drawCentreString("50", x0, y0 - 6, 2); break;
        case 1: tft.drawCentreString("75", x0, y0 - 9, 2); break;
        case 2: tft.drawCentreString("100", x0, y0 - 12, 2); break;
      }
    }
    // Now draw the arc of the scale
    sx = cos((i + 5 - 90) * 0.0174532925);
    sy = sin((i + 5 - 90) * 0.0174532925);
    x0 = sx * 100 + p_x0;
    y0 = sy * 100 + p_y0;
    // Draw scale arc, don't draw the last part
    if (i < 50)
      tft.drawLine(x0, y0, x1, y1, TFT_BLACK);
  }
  // %RH at bottom right
  tft.drawString("%RH", 5 + x_del + MWX -1 - 40, MHY - 7 - 20, 2);
  //tft.drawRoundRect(5 + x_del + MWX -1 - 40, MHY - 7 - 20)
  // Comment out to avoid font 4
  tft.drawCentreString("%RH", p_x0 + x_del, 70 + y_del, 4);
  // Draw bezel line Bezel - OK
  tft.drawRect(LHM_x0 + 0, LHM_y0 + 0, LHM_x0 + MWX - x_del, LHM_y0 + MHY, TFT_RED);
  // Put meter needle at 0
  plotNeedle(0, 0);

  }*/
void analogMeter() {
  // Meter outline
  tft.fillRect(0, 0, M_SIZE * 239, M_SIZE * 126, TFT_GREY);
  tft.fillRect(5, 3, M_SIZE * 230, M_SIZE * 119, TFT_WHITE);
  // Text colour
  tft.setTextColor(TFT_BLACK);
  // Draw ticks every 5 degrees from -50 to +50 degrees (100 deg. FSD swing)
  for (int i = -50; i < 51; i += 5) {
    // Long scale tick length
    int tl = 15;
    // Coordinates of tick to draw
    float sx = cos((i - 90) * 0.0174532925);
    float sy = sin((i - 90) * 0.0174532925);
    uint16_t x0 = sx * (M_SIZE * 100 + tl) + M_SIZE * 120;
    uint16_t y0 = sy * (M_SIZE * 100 + tl) + M_SIZE * 140;
    uint16_t x1 = sx * M_SIZE * 100 + M_SIZE * 120;
    uint16_t y1 = sy * M_SIZE * 100 + M_SIZE * 140;
    // Coordinates of next tick for zone fill
    float sx2 = cos((i + 5 - 90) * 0.0174532925);
    float sy2 = sin((i + 5 - 90) * 0.0174532925);
    int x2 = sx2 * (M_SIZE * 100 + tl) + M_SIZE * 120;
    int y2 = sy2 * (M_SIZE * 100 + tl) + M_SIZE * 140;
    int x3 = sx2 * M_SIZE * 100 + M_SIZE * 120;
    int y3 = sy2 * M_SIZE * 100 + M_SIZE * 140;
    // Yellow zone limits
    //if (i >= -50 && i < 0) {
    //  tft.fillTriangle(x0, y0, x1, y1, x2, y2, TFT_YELLOW);
    //  tft.fillTriangle(x1, y1, x2, y2, x3, y3, TFT_YELLOW);
    //}
    // Green zone limits
    if (i >= 0 && i < 25) {
      tft.fillTriangle(x0, y0, x1, y1, x2, y2, TFT_GREEN);
      tft.fillTriangle(x1, y1, x2, y2, x3, y3, TFT_GREEN);
    }
    // Orange zone limits
    if (i >= 25 && i < 50) {
      tft.fillTriangle(x0, y0, x1, y1, x2, y2, TFT_ORANGE);
      tft.fillTriangle(x1, y1, x2, y2, x3, y3, TFT_ORANGE);
    }
    // Short scale tick length
    if (i % 25 != 0) tl = 8;
    // Recalculate coords incase tick lenght changed
    x0 = sx * (M_SIZE * 100 + tl) + M_SIZE * 120;
    y0 = sy * (M_SIZE * 100 + tl) + M_SIZE * 140;
    x1 = sx * M_SIZE * 100 + M_SIZE * 120;
    y1 = sy * M_SIZE * 100 + M_SIZE * 140;
    // Draw tick
    tft.drawLine(x0, y0, x1, y1, TFT_BLACK);
    // Check if labels should be drawn, with position tweaks
    if (i % 25 == 0) {
      // Calculate label positions
      x0 = sx * (M_SIZE * 100 + tl + 10) + M_SIZE * 120;
      y0 = sy * (M_SIZE * 100 + tl + 10) + M_SIZE * 140;
      switch (i / 25) {
        case -2: tft.drawCentreString("0", x0, y0 - 12, 2); break;
        case -1: tft.drawCentreString("25", x0, y0 - 9, 2); break;
        case 0: tft.drawCentreString("50", x0, y0 - 7, 2); break;
        case 1: tft.drawCentreString("75", x0, y0 - 9, 2); break;
        case 2: tft.drawCentreString("100", x0, y0 - 12, 2); break;
      }
    }
    // Now draw the arc of the scale
    sx = cos((i + 5 - 90) * 0.0174532925);
    sy = sin((i + 5 - 90) * 0.0174532925);
    x0 = sx * M_SIZE * 100 + M_SIZE * 120;
    y0 = sy * M_SIZE * 100 + M_SIZE * 140;
    // Draw scale arc, don't draw the last part
    if (i < 50) tft.drawLine(x0, y0, x1, y1, TFT_BLACK);
  }
  // Units at bottom right
  tft.drawString("%RH", M_SIZE * (5 + 230 - 40), M_SIZE * (119 - 20), 2);
  // Comment out to avoid font 4
  tft.drawCentreString("%RH", M_SIZE * 120, M_SIZE * 70, 4);
  // Draw bezel line
  tft.drawRect(5, 3, M_SIZE * 230, M_SIZE * 119, TFT_BLACK);
  // Put meter needle at 0
  plotNeedle(0, 0); 
}
// Update needle position
// This function is blocking while needle moves, time depends on ms_delay
// 10ms minimises needle flicker if text is drawn within needle sweep area
// Smaller values OK if text not in sweep area, zero for instant movement but
// does not look realistic... (note: 100 increments for full scale deflection)
/*void plotNeedle(int value, byte ms_delay) {
  tft.setTextColor(TFT_BLACK, TFT_WHITE);
  // Select the font
  tft.setFreeFont(FF18);
  char buf[8]; dtostrf(value, 4, 0, buf);
  tft.drawRightString(buf, LHM_x0 + 40, MHY - 27, GFXFF);
  // Limit value to emulate needle end stops
  if (value < -10)
    value = -10;
  if (value > 110)
    value = 110;
  // Move the needle until new value reached
  while (!(value == old_analog)) {
    if (old_analog < value)
      old_analog++;
    else
      old_analog--;
    // Update immediately if delay is 0
    if (ms_delay == 0)
      old_analog = value;
    // Map value to angle
    float sdeg = map(old_analog, -10, 110, -150, -30);
    // Calculate tip of needle cords
    float sx = cos(sdeg * 0.0174532925);
    float sy = sin(sdeg * 0.0174532925);
    // Calculate x delta of needle start (does not start at pivot point)
    float tx = tan((sdeg + 90) * 0.0174532925);
    // Erase old needle image
    tft.drawLine(p_x0 + p_x0_del * ltx - 1, p_y0 - p_y0_del, osx - 1, osy, TFT_WHITE);
    tft.drawLine(p_x0 + p_x0_del * ltx, p_y0 - p_y0_del, osx, osy, TFT_WHITE);
    tft.drawLine(p_x0 + p_x0_del * ltx + 1, p_y0 - p_y0_del, osx + 1, osy, TFT_WHITE);
    // Re-plot text under needle
    int xrr = (MWX / 2) + 1 - 20;
    int yrr = MHY - 60;
    tft.fillRoundRect(xrr, yrr, 95, 50, 20, TFT_WHITE);
    tft.setTextColor(TFT_BLACK);
    // Comment out to avoid font 4
    tft.drawCentreString("%RH", p_x0, 70, 4);
    // Store new needle end coords for next erase
    ltx = tx;
    osx = sx * 98 + p_x0;
    osy = sy * 98 + p_y0;
    // Draw the needle in the new postion, magenta makes needle a bit bolder
    // draw 3 lines to thicken needle
    tft.drawLine(p_x0 + p_x0_del * ltx - 1, p_y0 - p_y0_del, osx - 1, osy, TFT_RED);
    tft.drawLine(p_x0 + p_x0_del * ltx, p_y0 - p_y0_del, osx, osy, TFT_MAGENTA);
    tft.drawLine(p_x0 + p_x0_del * ltx + 1, p_y0 - p_y0_del, osx + 1, osy, TFT_RED);
    // Slow needle down slightly as it approaches new postion
    if (abs(old_analog - value) < 10)
      ms_delay += ms_delay / 5;
    // Wait before next update
    delay(ms_delay);
  }
}*/
void plotNeedle(int value, byte ms_delay) {
  tft.setTextColor(TFT_BLACK, TFT_WHITE);
  char buf[8]; dtostrf(value, 4, 0, buf);
  tft.drawRightString(buf, M_SIZE * 40, M_SIZE * (119 - 20), 2);
  // Limit value to emulate needle end stops
  if (value < -10) value = -10;
  if (value > 110) value = 110;
  // Move the needle until new value reached
  while (!(value == old_analog)) {
    if (old_analog < value) old_analog++;
    else old_analog--;
    // Update immediately if delay is 0
    if (ms_delay == 0) old_analog = value;
    // Map value to angle
    float sdeg = map(old_analog, -10, 110, -150, -30);
    // Calculate tip of needle coords
    float sx = cos(sdeg * 0.0174532925);
    float sy = sin(sdeg * 0.0174532925);
    // Calculate x delta of needle start (does not start at pivot point)
    float tx = tan((sdeg + 90) * 0.0174532925);
    // Erase old needle image
    tft.drawLine(M_SIZE * (120 + 20 * ltx - 1), M_SIZE * (140 - 20), osx - 1, osy, TFT_WHITE);
    tft.drawLine(M_SIZE * (120 + 20 * ltx), M_SIZE * (140 - 20), osx, osy, TFT_WHITE);
    tft.drawLine(M_SIZE * (120 + 20 * ltx + 1), M_SIZE * (140 - 20), osx + 1, osy, TFT_WHITE);
    // Re-plot text under needle
    tft.setTextColor(TFT_BLACK);
    // Comment out to avoid font 4
    tft.drawCentreString("%RH", M_SIZE * 120, M_SIZE * 70, 4);
    // Store new needle end coords for next erase
    ltx = tx;
    osx = M_SIZE * (sx * 98 + 120);
    osy = M_SIZE * (sy * 98 + 140);
    // Draw the needle in the new postion, magenta makes needle a bit bolder
    // draws 3 lines to thicken needle
    tft.drawLine(M_SIZE * (120 + 20 * ltx - 1), M_SIZE * (140 - 20), osx - 1, osy, TFT_RED);
    tft.drawLine(M_SIZE * (120 + 20 * ltx), M_SIZE * (140 - 20), osx, osy, TFT_MAGENTA);
    tft.drawLine(M_SIZE * (120 + 20 * ltx + 1), M_SIZE * (140 - 20), osx + 1, osy, TFT_RED);
    // Slow needle down slightly as it approaches new postion
    if (abs(old_analog - value) < 10) ms_delay += ms_delay / 5;
    // Wait before next update
    delay(ms_delay);
  }
}