#include <SPI.h>
#include "Adafruit_GFX.h"
#include "Adafruit_ILI9341.h"

// Analog meter display demo on ILI9341 3.5" TFT
// Adapted from
/*
  An example analogue meter using a ILI9341 TFT LCD screen
  Alan Senior 23/2/2015
*/

// These are 'flexible' lines that can be changed
#define TFT_CS 10
#define TFT_DC 9
#define TFT_RST 8 // RST can be set to -1 if you tie it to Arduino's reset

// Use hardware SPI (on Uno, #13, #12, #11) and the above for CS/DC
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

// analog and digital meter globals
float ltx = 0;    // Saved x coord of bottom of needle
int16_t osx = 120, osy = 120; // Saved x & y coords
uint32_t updateTime = 0;       // time for next update

#define GAUGES 4
#define GAUGE_HEIGHT
#define GAUGE_WIDTH

#define ILI9341_GREY 0x5AEB

#define BACKGROUND ILI9341_BLACK
#define FOREGROUND ILI9341_WHITE
#define BORDER ILI9341_GREY
#define GOOD ILI9341_GREEN
#define WARNING ILI9341_YELLOW
#define DANGER ILI9341_RED

typedef struct {
  int x;
  int y;
  int width;
  int height;
  int next_value;
  int last_value;
  int needleLength;
} gauge_t;

gauge_t gauges[GAUGES];

void setup() {
  gauges[0] = {
    0, 0, 160, 120, 0, 0, 80
  };
  gauges[1] = {
    160, 0, 160, 120, 0, 0, 80
  };
  gauges[2] = {
    0, 120, 160, 120, 0, 0, 80
  };
  gauges[3] = {
    160, 120, 160, 120, 0, 0, 80
  };

  Serial.begin(115200);
  while (!Serial) delay(10);
  Serial.println("ILI9341D Analog Meter!");

  tft.begin();
  tft.fillScreen(ILI9341_GREY);

  //check display bounds
  int h = tft.height();
  int w = tft.width();
  char line[40];
  snprintf(line, sizeof line, "default h %d w %d", h, w);
  Serial.println(line);

  // initialize screen
  tft.setRotation(1); // horizontal wide screen
  h = tft.height();  //check if properly reassigned
  w = tft.width();
  for (byte i = 0; i < GAUGES; i++) {
    analogMeter(&gauges[i]);
  }
}

int increment = 5;

void loop() {
  if (updateTime <= millis()) {
    updateTime = millis() + 50;

    for (byte i = 0; i < GAUGES; i++) {
      plotNeedle(&gauges[i]); // Update analogue meter, 8ms delay per needle increment
      gauges[i].next_value += increment;
      if (gauges[i].next_value == 100 || gauges[i].next_value == 0) {
        increment *= -1;
      }
    }
  }
}

// #########################################################################
//  Draw the analogue meter on the screen
// #########################################################################
void analogMeter(gauge_t* gauge)
{
  float spaceOnSides = gauge->width * 0.292;
  float spaceOnLeft = spaceOnSides * 0.333;
  float spaceOnRight = spaceOnSides - spaceOnLeft;

  int centerX = gauge->x - spaceOnLeft + gauge->width / 2;
  int centerY = gauge->y + gauge->height + 20;

  // Meter outline
  tft.fillRect(gauge->x, gauge->y, gauge->width, gauge->height, BACKGROUND);

  tft.setTextColor(FOREGROUND);  // Text colour

  int incDeg = 5;

  // Draw ticks every 5 degrees from -50 to +50 degrees (100 deg. FSD swing)
  for (int i = -50; i < 51; i += incDeg) {
    // Long scale tick length
    int tickLength = 15;

    float dtr = PI / 180.0;

    // Coodinates of tick to draw
    float sx = cos((i - 90) * dtr);
    float sy = sin((i - 90) * dtr);
    int16_t x0 = sx * (gauge->needleLength + tickLength) + centerX;
    int16_t y0 = sy * (gauge->needleLength + tickLength) + centerY;
    int16_t x1 = sx * gauge->needleLength + centerX;
    int16_t y1 = sy * gauge->needleLength + centerY;

    // Coordinates of next tick for zone fill
    float sx2 = cos((i + incDeg - 90) * dtr);
    float sy2 = sin((i + incDeg - 90) * dtr);
    int x2 = sx2 * (gauge->needleLength + tickLength) + centerX;
    int y2 = sy2 * (gauge->needleLength + tickLength) + centerY;
    int x3 = sx2 * gauge->needleLength + centerX;
    int y3 = sy2 * gauge->needleLength + centerY;

    // Green zone limits
    if (i >= -50 && i < 0) {  //center to 3/4
      tft.fillTriangle(x0, y0, x1, y1, x2, y2, GOOD);
      tft.fillTriangle(x1, y1, x2, y2, x3, y3, GOOD);
    }

    // Yellow zone limits
    if (i >= 0 && i < 25) { //3/4 to full
      tft.fillTriangle(x0, y0, x1, y1, x2, y2, WARNING);
      tft.fillTriangle(x1, y1, x2, y2, x3, y3, WARNING);
    }

    // Red zone limits
    if (i >= 25 && i < 50) {  //center to 3/4
      tft.fillTriangle(x0, y0, x1, y1, x2, y2, DANGER);
      tft.fillTriangle(x1, y1, x2, y2, x3, y3, DANGER);
    }

    // Short scale tick length
    if (i % 25 != 0) tickLength = 8;

    // Recalculate coords incase tick length changed
    x0 = sx * (gauge->needleLength + tickLength) + centerX;
    y0 = sy * (gauge->needleLength + tickLength) + centerY;
    x1 = sx * gauge->needleLength + centerX;
    y1 = sy * gauge->needleLength + centerY;

    // Draw tick
    tft.drawLine(x0, y0, x1, y1, FOREGROUND);

    // Check if labels should be drawn, with position tweaks
    if (i % 25 == 0) {
      // Calculate label positions
      x0 = sx * (gauge->needleLength + tickLength + 10) + centerX;
      y0 = sy * (gauge->needleLength + tickLength + 10) + centerY;
      tft.setTextSize(2);
      switch (i / 25) {
        case -2: tft.setCursor(x0, y0 - 12); tft.print("0"); break;
        case -1: tft.setCursor(x0, y0 - 9); tft.print("25"); break;
        case 0: tft.setCursor(x0, y0 - 6); tft.print("50"); break;
        case 1: tft.setCursor(x0, y0 - 9); tft.print("75"); break;
        case 2: tft.setCursor(x0 - 5, y0 - 12); tft.print("100"); break;
      }
    }

    // Now draw the arc of the scale
    sx = cos((i + incDeg - 90) * dtr);
    sy = sin((i + incDeg - 90) * dtr);
    x0 = sx * gauge->needleLength + centerX;
    y0 = sy * gauge->needleLength + centerY;
    // Draw scale arc, don't draw the last part
    if (i < 50) tft.drawLine(x0, y0, x1, y1, FOREGROUND);
  }

  // tft.drawString("%RH", 5 + 230 - 40, 119 - 20, 2); // Units at bottom right
  // tft.drawCentreString("%RH", 120, 70, 4); // Comment out to avoid font 4
  // tft.drawRect(5, 3, 250, 119, FOREGROUND); // Draw bezel line (230)

  plotNeedle(gauge); // Put meter needle at 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(gauge_t* gauge)
{
  float spaceOnSides = gauge->width * 0.292;
  float spaceOnLeft = spaceOnSides * 0.333;
  float spaceOnRight = spaceOnSides - spaceOnLeft;

  int centerX = gauge->x - spaceOnLeft + gauge->width / 2;
  int centerY = gauge->y + gauge->height + 20;

  float dtr = PI / 180.0;
  tft.setTextColor(FOREGROUND, BACKGROUND);

  int value = gauge->next_value;
  if (value < 0) value = 0; // Limit value to emulate needle end stops
  if (value > 100) value = 100;

  gauge->last_value = value; // Update immediately id delay is 0

  float sdeg = map(gauge->last_value, 0, 100, -140, -40); // Map value to angle
  // Calculate tip of needle coords
  float sx = cos(sdeg * dtr);
  float sy = sin(sdeg * dtr);

  // Calculate x delta of needle start (does not start at pivot point)
  float tx = tan((sdeg + 90) * dtr);

  // Erase old needle image
  tft.drawLine(centerX + 20 * ltx - 1, centerY - 20, osx - 1, osy, BACKGROUND);
  tft.drawLine(centerX + 20 * ltx, centerY - 20, osx, osy, BACKGROUND);
  tft.drawLine(centerX + 20 * ltx + 1, centerY - 20, osx + 1, osy, BACKGROUND);

  // Store new needle end coords for next erase
  ltx = tx;
  osx = sx * gauge->needleLength - 2 + centerX;
  osy = sy * gauge->needleLength - 2 + centerY;

  // Draw the needle in the new postion, magenta makes needle a bit bolder
  // draws 3 lines to thicken needle
  tft.drawLine(centerX + 20 * ltx - 1, centerY - 20, osx - 1, osy, ILI9341_RED);
  tft.drawLine(centerX + 20 * ltx, centerY - 20, osx, osy, ILI9341_MAGENTA);
  tft.drawLine(centerX + 20 * ltx + 1, centerY - 20, osx + 1, osy, ILI9341_RED);
}