#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h> // 320x240
#include <Fonts/FreeSansBold24pt7b.h>

// #define TFT_BL 21 // Backlight
#define TFT_SCLK 18 // 14
#define TFT_MOSI 23 // 13
#define TFT_MISO 15 // 12 
#define TFT_DC    2
#define TFT_CS    5 // 15
#define TFT_RST  -1 // 4 // -1 // No reset defined
#define ILI9341_LIGHTGREY 0xC618  // 198, 195, 198
#define ILI9341_DARKGREY  0x7BEF  // 123, 125, 123

Adafruit_ILI9341 display = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_MOSI, TFT_SCLK, TFT_RST);
GFXcanvas16 canvas(320 / 2, 240); // Half width (320) full height(170)

unsigned int charWidth = 6, charHeight = 8, charSize = 4;
unsigned int hdg = 30; //int(random(360));

unsigned int RotX(int a, int b, float S, float C)  // Rotate and offset x coordinate
{
  return (unsigned int)(a * C - b * S + display.width() / 4 - 1); // Centred on right half
}

unsigned int RotY(int a, int b, float S, float C)  // Rotate and offset y coordinate
{
  return (unsigned int)(a * S + b * C + display.height() / 2 - 1); // Half way down display
}

// Select a sector on the compass rose that contains the heading
static char Sector[4] = {'\0'};
char* getSector(float heading)
{
  char ordinalHeading[][16] = {
    "N  ", "NNE", "NE ", "ENE",
    "E  ", "ESE", "SE ", "SSE",
    "S  ", "SSW", "SW ", "WSW",
    "W  ", "WNW", "NW ", "NNW"
  };

  for (int i = 0; i < 16; i++) {
    if (heading > (348.5 + i * 11.5) && heading <= (11.5 + i * 11.5)) {
      strcpy(Sector, ordinalHeading[i]);
    }
  }
  return Sector;
}

void updateDisplay(int Heading)
{
  const int NeedleHeight = canvas.height() / 2 - 6;
  const int NeedleWidth = NeedleHeight / 5;
  const unsigned int Radius = canvas.width() / 2 - 1;
  const unsigned int Xoffset = Radius;
  const unsigned int Yoffset = Radius;

  const int a1 = 0, b1 = NeedleHeight;
  const int a2 = -NeedleWidth, b2 = 0;
  const int a3 = 0, b3 = -NeedleHeight;
  const int a4 = NeedleWidth, b4 = 0;
  unsigned int x1, y1, x2, y2, x3, y3, x4, y4;  // Vertices of needle to be drawn
  unsigned int Offset;

  //Draw heading
  float H = Heading * PI / 180;  // Convert to radians for rotation
  char* SectorStr;
  // charSize = 2;
  charSize = 5;
  canvas.setTextWrap(false);
  canvas.fillScreen(ILI9341_WHITE); // Clear canvas
  canvas.setTextSize(charSize);
  canvas.setTextColor(ILI9341_BLACK);
  SectorStr = getSector(Heading);
  // Centred one and a half characters in. Assumes 32 high x 24 wide characters
  Offset = 3 * charWidth * charSize / 2 - ((strlen(SectorStr) * charSize * charWidth) / 2);
  canvas.setCursor(Offset + 4, charHeight * charSize / 2);        // Top line
  // canvas.setCursor(Offset + 4, 40 );        // Kludge for font
  canvas.println(SectorStr);
  // canvas.setCursor(4 + charWidth * charSize,
  //   display.height() / 2 - charSize * charHeight / 2 );  // Middle line
  canvas.setCursor(canvas.getCursorX() + charWidth * charSize, canvas.getCursorY()); // Middle line
  canvas.println('\\');  // backslash
  // canvas.setCursor(4, canvas.height() - 3 * charHeight * charSize / 2); // Bottom line
  // canvas.setCursor(4, 160); // Bottom line
  canvas.setCursor(canvas.getCursorX() + 4, canvas.getCursorY()); // Bottom line
  if (Heading < 100) canvas.print(0); // Add leading zeroes as required
  if (Heading < 10) canvas.print(0);
  canvas.print(Heading);
  display.drawRGBBitmap(0, 0, canvas.getBuffer(), canvas.width(), canvas.height());
  // display.drawBitmap(0, 0, canvas.getBuffer(), canvas.width(), canvas.height(), ILI9341_WHITE, ILI9341_BLACK);
  // Draw the compass
  if (H > 0.0) H = 2 * PI - H; // Flip to always point north
  float S = sin(H);
  float C = cos(H);
  // Rotate needle away from north by Heading degrees
  x1 = RotX(a1, b1, S, C);
  y1 = RotY(a1, b1, S, C);
  x2 = RotX(a2, b2, S, C);
  y2 = RotY(a2, b2, S, C);
  x3 = RotX(a3, b3, S, C);
  y3 = RotY(a3, b3, S, C);
  x4 = RotX(a4, b4, S, C);
  y4 = RotY(a4, b4, S, C);
  canvas.fillScreen(ILI9341_WHITE); // Clear canvas
  canvas.drawTriangle(x1, y1, x2, y2, x4, y4, ILI9341_BLACK); // South half outlined
  canvas.fillTriangle(x1, y1, x2, y2, Xoffset, Yoffset, ILI9341_LIGHTGREY); // South half shaded
  canvas.fillTriangle(x2, y2, x3, y3, x4, y4, ILI9341_BLACK); // North half filled
  canvas.fillTriangle(x2, y2, x3, y3, Xoffset, Yoffset, ILI9341_DARKGREY); // North half shaded
  canvas.drawCircle(Xoffset, Yoffset + 2, Radius, ILI9341_BLACK); // x,y,r
  canvas.drawCircle(Xoffset, Yoffset + 2, Radius - 1, ILI9341_BLACK); // Double thickness circle
  display.drawRGBBitmap(canvas.width(), 0, canvas.getBuffer(), canvas.width(), canvas.height());
  // display.drawBitmap(canvas.width(), 0, canvas.getBuffer(), canvas.width(), canvas.height(), ILI9341_WHITE, ILI9341_BLACK);
}


void setup()
{
  display.begin(); // screenWidth(), screenHeight()
  display.setRotation(3); // Landscape with USB on right
  // canvas.setFont(&FreeSerifBold24pt7b); // Use custom font
  // canvas.setFont(&FreeSansBold24pt7b); // Use custom font
  canvas.setTextWrap(false);            // Clip text within canvas
  Serial.begin(115200);
  Serial.println("ESP32TFTcompassDisplayTestCanvas.ino");
  Serial.println("Initialising...");
  // pinMode(TFT_BL, OUTPUT); // Turn on backlight
  // digitalWrite(TFT_BL, HIGH);
  Serial.println("Displaying last fix");
  display.fillScreen(ILI9341_WHITE); // Clear screen
  display.setTextColor(ILI9341_BLACK);
  display.setTextSize(charSize);
  display.setCursor(0, 0);
  display.println("Last fix:");
  display.println(-33.70244, 5);  // 9 characters
  display.println(151.09931, 5);  // 10 characters
  display.println(12.43, 2);  // 5 characters
  delay(2000);
  display.fillScreen(ILI9341_WHITE);
}

void loop()
{
  static bool Inverted = false;

  // updateDisplay(random(10)-random(5) + hdg);
  // updateDisplay(hdg + random(10 - random(5)));
  // frabble();
  // delay(200);
  display.invertDisplay(Inverted);
  Inverted = !Inverted;
  delay(5000);
}