#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32

/* Assign a unique ID to this sensor at the same time */
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
int iswCompass = 0; // Check Switch to display compass 0 - Moving, 1 - Round
const int BUTTON_PIN = A1;       // the number of the pushbutton pin
int iCount = 0;
int LED = 13;
int POTMETER = A0;

void setup(void)
{
  pinMode(LED, OUTPUT); // LED
  pinMode(POTMETER, INPUT); // reading potentiometer
  pinMode(BUTTON_PIN, INPUT_PULLUP); // reading switch
  mag.begin(); //  CompassStart
  display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS); // OLEDStart
}

void loop(void)
{
  digitalWrite(LED, HIGH);   // turn the LED on (HIGH is the voltage level)
 // delay(1000);                       // wait for a second
  // checkBattery(); // Check Battery
  iswCompass = digitalRead(BUTTON_PIN); // Check Switch to display compass: 0 - Moving, 1 - Round
  ReadCompass(); // Compass Dial
  StartCounter(); // Counter
  display.display();
}

void ReadCompass() {
  /* Get a new sensor event */
  sensors_event_t event;
  mag.getEvent(&event);

  // Calculate heading when the magnetometer is level, then correct for signs of axis.
  float heading = atan2(event.magnetic.y, event.magnetic.x);
  //float declinationAngle = -0.33405602; // SA -19.14*
  float declinationAngle = map(analogRead(A0), 0, 1023, -20, 20) * PI / 180 ;
  heading += declinationAngle;

  // Correct for when signs are reversed.
  if (heading < 0)
    heading += 2 * PI;

  // Check for wrap due to addition of declination.
  if (heading > 2 * PI)
    heading -= 2 * PI;

  // Convert radians to degrees for readability.
  float headingDegrees = heading * 180 / M_PI;

  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);

  // Showing Moving or Round Compass: 0 - Moving, 1 - Round
  if (iswCompass < 1) {
    // Show Moving Compass
    display.setTextSize(4);
    display.setCursor(25, 15);
    display.print(headingDegrees, 0);
    display.print((char)247);

    // Draw moving horizon with compass directions
    display.setTextSize(2);
    int center = SCREEN_WIDTH / 2;
    int directionIndex = (int)(headingDegrees / 22.5); // 22.5 degrees per step for 16 directions
    int offset = (int)(headingDegrees) % 22;

    //const char* directions[] = {"N", ".", "NE", ".", "E", ".", "SE", ".", "S", ".", "SW", ".", "W", ".", "NW", "."};
    const char* directions[] = {"N", "", "NE", "", "E", "", "SE", "", "S", "", "SW", "", "W", "", "NW", ""};

    for (int i = -2; i <= 2; i++) {
      int idx = (directionIndex + i + 16) % 16;
      int pos = center + (i * 24) - (offset * 24 / 22.5); // Smooth transition
      if (pos >= 0 && pos <= SCREEN_WIDTH - 24) {
        display.setCursor(pos, 50);
        display.print(directions[idx]);
      }
    }
    delay(1000); // Update every 0.1 seconds
  } else {
    // Show Round Compass
    display.setTextSize(2);
    display.setCursor(20, 3);

    if (headingDegrees >= 337.5 || headingDegrees < 22.5)
      display.print("N");
    else if (headingDegrees >= 22.5 && headingDegrees < 67.5)
      display.print("NE");
    else if (headingDegrees >= 67.5 && headingDegrees < 112.5)
      display.print("E");
    else if (headingDegrees >= 112.5 && headingDegrees < 157.5)
      display.print("SE");
    else if (headingDegrees >= 157.5 && headingDegrees < 202.5)
      display.print("S");
    else if (headingDegrees >= 202.5 && headingDegrees < 247.5)
      display.print("SW");
    else if (headingDegrees >= 247.5 && headingDegrees < 292.5)
      display.print("W");
    else if (headingDegrees >= 292.5 && headingDegrees < 337.5)
      display.print("NW");

    // Display angle in degrees
    display.setTextSize(3);
    display.setCursor(5, 25);
    display.print(headingDegrees, 0);
    display.print((char)247);

    // Draw compass
    int centerX = SCREEN_WIDTH - 25;
    int centerY = SCREEN_HEIGHT / 2;
    int radius = min(centerX, centerY) - 8; // Adjust radius for circle

    display.drawCircle(centerX, centerY, radius, SSD1306_WHITE);
    display.fillCircle(centerX, centerY, 1, SSD1306_WHITE); // Small dot at the center
    display.drawLine(centerX, centerY, centerX + radius * cos(heading - PI / 2), centerY + radius * sin(heading - PI / 2), SSD1306_WHITE);

    // Draw degree markings
    for (int angle = 0; angle < 360; angle += 30) {
      int x1 = centerX + (radius - 2) * cos(angle * PI / 180);
      int y1 = centerY + (radius - 2) * sin(angle * PI / 180);
      int x2 = centerX + radius * cos(angle * PI / 180);
      int y2 = centerY + radius * sin(angle * PI / 180);
      display.drawLine(x1, y1, x2, y2, SSD1306_WHITE);

      // Display degree numbers at every 90 degrees
      if (angle % 90 == 0) {
        int xText = centerX + (radius + 8) * cos(angle * PI / 180);
        int yText = centerY + (radius + 8) * sin(angle * PI / 180);
        display.setCursor(xText - 6, yText - 6); // Adjust position for better readability
        display.setTextSize(1);
        //display.print(angle);
      }
    }
    delay(1000);
  }
}

int value = 0;
float voltage;
float perc;

void checkBattery() {
  value = analogRead(A0);
  voltage = value * 5.0 / 1023;
  perc = map(voltage, 3.6, 4.2, 0, 100);

  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(80, 0);
  display.print("Batt:");
  display.print(perc, 0);
  display.print("%");
  delay(1000);
}


void StartCounter() {
  iCount++; // Counter
  int tme = 0;
  tme = iCount;
  int hr = tme / 3600; // Number of seconds in an hour
  int mins = (tme - hr * 3600) / 60; // Remove the number of hours and calculate the minutes.
  int sec = tme - hr * 3600 - mins * 60; // Remove the number of hours and minutes, leaving only seconds.
  sec %= 60;
  mins %= 60;
  hr %= 24;

  String Strhr = String(hr);
  String Strmins = String(mins);
  String Strsec = String(sec);

  if (Strhr.length() < 2)
    Strhr = "0" + Strhr;

  if (Strmins.length() < 2)
    Strmins = "0" + Strmins;

  if (Strsec.length() < 2)
    Strsec = "0" + Strsec;

  String hrMinSec = Strhr + ":" + Strmins + ":" + Strsec;
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  // Showing Moving or Round Compass: 0 - Moving, 1 - Round
  if (iswCompass < 1) {
    display.setCursor(40, 0);
  } else {
    display.setCursor(10, 55);
  }
  display.print(hrMinSec);
}