#include "ssd1306.h"
#include "nano_gfx.h"
#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include "splash.h"
#include <SdFat.h>

#define LED_HB 3
#define PB_UP 3 
#define PB_DWN 2
#define PB_SEL 8 
#define RXPin 5
#define TXPin 4 
#define GPSBaud 9600
#define SD_CS_PIN 6 
#define SerialBaud 9600
#define MAX_FILES 5
#define MAX_FILENAME_LENGTH 8 //includes null termination, not .csv extension 
#define MAX_HOLES 18

SAppMenu menu; 
char *menuItems[MAX_FILES];
char fileNames[MAX_FILES][MAX_FILENAME_LENGTH];

// Use SdFat init
SdFat SD;

SoftwareSerial ss(RXPin, TXPin);
TinyGPSPlus gps;


struct gpsPacket {
  float lat = 0;
  float lon = 0;
};

struct Hole {
  uint8_t par = 0;
  float latitude = 0;
  float longitude = 0;
};

// Array to store the holes
Hole thisCourse[MAX_HOLES];

uint8_t hbval = 27;
int8_t hbdelta = 4;
unsigned long pbRefresh = 0; 
int menuFlag = -1; 


void setup() {

  pinSetup();
  SSD1306_Setup();

  bitmapDemo();
  delay(4000);
  ssd1306_clearScreen();
  
  int fileNo = 0; 
  if (!SD.begin(SD_CS_PIN, SD_SCK_MHZ(25))) {
    ssd1306_setFixedFont(ssd1306xled_font6x8);
    ssd1306_printFixed(0,  8, "SD Card Failed!", STYLE_NORMAL);
    while (1); //add fail screen
  }
  fileNo = SD_menu();
 
  SSD1306Menu_Setup(fileNo);

  //wait for menu selection
  while(menuFlag == -1){
    heartbeat(&hbval,&hbdelta);                  
    buttonRefreshMenu(&menu, &pbRefresh, &menuFlag);
    delay(1);
  }
  
  //construct file path with old title array
  strncpy(fileNames[MAX_FILES-1], fileNames[menuFlag], MAX_FILENAME_LENGTH - 1);
  strncpy(fileNames[0], "courses/",9);
  strcat(fileNames[0],fileNames[MAX_FILES-1]);
  strcat(fileNames[0],".csv");
  

  if (!loadCourseData(fileNames[0])) {
    ssd1306_clearScreen();
    ssd1306_printFixed(0,  8, "Course Data Failed!", STYLE_NORMAL);
    while (1);
  }
}

void loop() {
  //Secondary init deconflicting lots of memory from SD
  //Serial.begin(SerialBaud);
  
  ss.begin(GPSBaud);
  bool gpsValid = 0; 
  float dis = 0; 
  int holeNo = 0;

  //for (int i = 0; i < MAX_HOLES; i++) {
  //  Serial.print("Hole ");
  //  Serial.print(i+1);
  //  Serial.print(" (Par ");
  //  Serial.print(thisCourse[i].par);
  //  Serial.print("): ");
  //  Serial.print(thisCourse[i].latitude, 7);
  //  Serial.print(", ");
  // Serial.println(thisCourse[i].longitude, 7);
  //}

  ssd1306_clearScreen();

  
  while(1){
    heartbeat(&hbval,&hbdelta);                  
    //buttonRefreshMenu(&menu, &pbRefresh, &menuFlag);
    //Course Selection Loop
    while(!(menuFlag == -1)){
      buttonRefreshCourse(&pbRefresh, &holeNo);
      heartbeat(&hbval,&hbdelta); 
      if(ss.available() > 0){
        if(gps.encode(ss.read())){
          if(gps.location.isValid()){
            if(!gpsValid){
              ssd1306_clearScreen();
              gpsValid = 1; 
            }
            buttonRefreshCourse(&pbRefresh, &holeNo);
            dis = calcDistance(gps.location.lat(),gps.location.lng(),
                        thisCourse[holeNo].latitude,thisCourse[holeNo].longitude);
            courseYardageDisplay(dis,holeNo+1,thisCourse[holeNo].par);
          }
          else{
            //GPS invalid Screen
            ssd1306_printFixedN(0,  8, "GPS Loading", STYLE_NORMAL, FONT_SIZE_NORMAL);
          }
        }
      }
    }
  }
}

/////////////////////// FUNCTIONS ////////////////////////

void pinSetup(){
  pinMode(LED_HB, OUTPUT);
  pinMode(PB_UP, INPUT_PULLUP);
  pinMode(PB_DWN, INPUT_PULLUP);
  pinMode(PB_SEL, INPUT_PULLUP);
}

void SSD1306_Setup(){
  ssd1306_128x64_i2c_init();
  ssd1306_clearScreen();
}

void SSD1306Menu_Setup(int fileNo){
  ssd1306_clearScreen();
  ssd1306_setFixedFont(ssd1306xled_font6x8);
  ssd1306_createMenu(&menu, menuItems, fileNo);
  ssd1306_showMenu(&menu);
}

static void bitmapDemo()
{
    ssd1306_drawBitmap(0, 0, 128, 48, Splash);
    ssd1306_setFixedFont(ssd1306xled_font6x8);
    ssd1306_printFixed(0, 48, "GPS GOLF RANGEFINDER", STYLE_BOLD);
    ssd1306_printFixed(12, 56, "C Lindberg 2025", STYLE_NORMAL);
}

void heartbeat(uint8_t* val, int8_t* delta) {
  static unsigned long last_time = 0;
  unsigned long now = millis();
  if ((now - last_time) < 40) {
    return;
  }
  last_time = now;
  if (*val > 100) {
    *delta = -*delta;
  }
  if (*val < 8) {
    *delta = -*delta;
  }
  *val += *delta;
  //analogWrite(LED_HB, *val);
}

void buttonRefreshMenu(SAppMenu* menuLocal, unsigned long* pbClock, int* menuFlag){
  const int debounceDelay = 125;
  if((millis()-*pbClock) > debounceDelay){
    if(!digitalRead(PB_DWN)){
      ssd1306_menuDown(menuLocal);
      ssd1306_updateMenu(menuLocal);
   }
    else if(!digitalRead(PB_UP)){
      ssd1306_menuUp(menuLocal);
      ssd1306_updateMenu(menuLocal);
   }
    else if(!digitalRead(PB_SEL)){
      *menuFlag = ssd1306_menuSelection(menuLocal);
      ssd1306_clearScreen();
   }
   *pbClock = millis();
 }
}


void buttonRefreshCourse(unsigned long* pbClock, int* holeIndex){
  const int debounceDelay = 300;
  if((millis()-*pbClock) > debounceDelay){
    if(!digitalRead(PB_DWN)){
      //decrement hole #
      *holeIndex -= 1; 
      if(*holeIndex == -1){
        *holeIndex = 17; 
      }
   }
    else if(!digitalRead(PB_UP)){
      //increment hole #
      *holeIndex += 1; 
      if(*holeIndex == 18){
        *holeIndex = 0;
      }
   }
   *pbClock = millis();
  }
}

void courseYardageDisplay(float yardage, int holeNo, int par){

  char yardageStr[5];
  char holeNoStr[2];
  char parStr[1];
  int decimals = 0;

  if(yardage > 99999){
    strncpy(yardageStr,"2 FAR",5);
  }
  else if (yardage < 1000){
    decimals = 1; 
    dtostrf(yardage, 5,decimals, yardageStr);
  }
  else{
    dtostrf(yardage, 5,decimals, yardageStr);
  }
  

  dtostrf(holeNo, 2,0, holeNoStr);
  dtostrf(par, 1,0, parStr);

  ssd1306_setFixedFont(ssd1306xled_font6x8);
  ssd1306_printFixedN(0, 0, "Distance to Green", STYLE_NORMAL, FONT_SIZE_NORMAL);

  ssd1306_printFixedN(0,16,yardageStr, STYLE_NORMAL, FONT_SIZE_4X);

  ssd1306_printFixedN(36, 48, holeNoStr, STYLE_NORMAL, FONT_SIZE_NORMAL);
  ssd1306_printFixedN(0, 48, "Hole: ", STYLE_NORMAL, FONT_SIZE_NORMAL);

  ssd1306_printFixedN(86, 48, "YARDS", STYLE_BOLD, FONT_SIZE_NORMAL);
  ssd1306_printFixedN(0, 56, "Par: ", STYLE_NORMAL, FONT_SIZE_NORMAL);
  ssd1306_printFixedN(30, 56, parStr, STYLE_NORMAL, FONT_SIZE_NORMAL);
}


int SD_menu() {
  SdFile dir;
  SdFile entry;
  if (!dir.open("/courses")) {
    return;
  }
  if (!dir.isDir()) {
    dir.close();
    return;
  }  
  int fileIndex = 0;
  while (entry.openNext(&dir, O_RDONLY)) {
    if (!entry.isDir()) {
      char filename[MAX_FILENAME_LENGTH + 4]; //+4 for .csv extension
      entry.getName(filename, sizeof(filename));

      int len = strlen(filename);
      if (len >= 4) {
        // Make the last 4 characters lowercase
        for (int i = len - 4; filename[i]; i++) {
          filename[i] = tolower(filename[i]);
        }

        // Check if it ends with ".csv"
        if (strcmp(&filename[len - 4], ".csv") == 0) {
          filename[len - 4] = '\0'; // Remove ".csv"

          // Only add files not starting with '.'
          if (fileIndex < MAX_FILES && filename[0] != '.') {
            strncpy(fileNames[fileIndex], filename, MAX_FILENAME_LENGTH - 1);
            fileNames[fileIndex][MAX_FILENAME_LENGTH - 1] = '\0'; // Ensure null-termination
            fileIndex++;
          }
        }
      }
    }
    entry.close();
  }
  dir.close();
  // point menu items to new filenames 
  for (int i = 0; i < fileIndex; i++) {
    menuItems[i] = fileNames[i];
  }
  return fileIndex; 
}

bool loadCourseData(const char* filePath) {
  SdFile file;

  if (!file.open(filePath, O_READ)) {
    //Serial.println("Failed to open course file!");
    return false;
  }
  char line[32];
  uint8_t holeIndex = 0;

  while (file.fgets(line, sizeof(line)) && holeIndex < MAX_HOLES) {
    parseLine(line, thisCourse[holeIndex]);
    holeIndex++;
  }
  file.close();

  return true;
}

// Helper to parse a CSV line
void parseLine(char* line, Hole &hole) {
  char *token;

  token = strtok(line, ","); //wash hole number

  token = strtok(NULL, ",");
  if (token != NULL) hole.par = atof(token);

  token = strtok(NULL, ",");
  if (token != NULL) hole.latitude = atof(token);

  token = strtok(NULL, ",");
  if (token != NULL) hole.longitude = atof(token);
}


float calcDistance(double lat1,double lng1, double lat2, double lng2) {
  
 const float R = 6.975E+6; // yards
 const float pi = 3.1415; 

 float phiO = lat1 * pi/180;
 float phiT = lat2 * pi/180;
 float dPhi = phiO - phiT; 
 float dLam = (lng2 - lng1) * pi/180;

 float x; 
 float y; 
 float distance; 

 x = dLam * cos((phiO + phiT)/2)*1E+4;
 y = dPhi*1E+4;

 distance = (sqrt(x*x + y*y) * R)/(1E+4);

 return distance;
}