#include <Arduino.h>
#include "SparkFun_Ublox_Arduino_Library_Series_6_7.h"  //for GPS
#include <Wire.h>                                       //for screen
#include <Adafruit_GFX.h>                               //for screen
#include "Adafruit_LEDBackpack.h"                       //for screen
#include <stdlib.h>
#include <atomic>
#include <SolarCalculator.h>  //for determining startup brightness
#include <TimeLib.h>
#define DISPLAY_ADDRESS 0x70  //I2C address of screen
#define GPS_WAIT_TIME 1100
#define GPS_UPDATE_FREQUENCY 10  //GPS update frequency per sec
#define DEBOUNCE_DELAY 250       //time in ms
#define MIDDLE_SCREEN_BRIGHTNESS 5
//#define PROFILE true 

//GPS
SFE_UBLOX_GPS myGPS;

//threading
std::atomic<float> speed(0);
std::atomic_int gpsFixType(0);

//screen
Adafruit_AlphaNum4 disp = Adafruit_AlphaNum4();
unsigned long timeOfLastFix = 0UL;
unsigned long timer = millis();
bool setInitBrightness = false;
unsigned long brightnessTimer = millis();

//button
volatile bool buttonPushed = false;
volatile int ScreenBrightnessUser = 15;
volatile int screenBrightnessCurrent = ScreenBrightnessUser;
volatile unsigned long lastButtonPush = 0UL;
volatile bool manualBrightness = false;

//profiler
int runCounter = 0;
unsigned long profileTimer = millis();


void chageBrightness() {
  if (millis() - lastButtonPush > DEBOUNCE_DELAY) {
    buttonPushed = true;
    lastButtonPush = millis();
    manualBrightness = true;
  }
}

void connectGPS() {  //need to do this everytime the GPS turns back on
  do {               //set gps to 115200 baudrate code comes from sparkfun examples
   // Serial.println("GNSS: trying 115200 baud");
    Serial1.begin(115200);
    if (myGPS.begin(Serial1) == true) break;
    delay(100);
    //Serial.println("GNSS: trying 9600 baud");
    Serial1.begin(9600);
    if (myGPS.begin(Serial1) == true) {
      //Serial.println("GNSS: connected at 9600 baud, switching to 115200");
      myGPS.setSerialRate(115200);
      delay(100);
    } else {
      delay(2000);  //Wait a bit before trying again to limit the Serial output
    }
  } while (1);
  myGPS.setAutoPVT(true);
  myGPS.setUART1Output(COM_TYPE_UBX);                  //Set the UART1 port to output UBX only (turn off NMEA noise)
  myGPS.setNavigationFrequency(GPS_UPDATE_FREQUENCY);  //set frequency of updates to 10/sec
  //myGPS.setDynamicModel(DYN_MODEL_AUTOMOTIVE);         //set dynamic model of GPS to automotive to more accutate results
}

bool setStartBrightness() {
  double az = 0;
  double el = 0;
  int hour = myGPS.getHour();
  int min = myGPS.getMinute();
  int sec = myGPS.getSecond();
  int day = myGPS.getDay();
  int month = myGPS.getMonth();
  int year = myGPS.getYear();
  setTime(hour, min, sec, day, month, year);
  time_t timeUtc = now();
  double lat = myGPS.getLatitude();
  lat = lat / 10000000;
  double lng = myGPS.getLongitude();
  lng = lng / 10000000;
  calcHorizontalCoordinates(timeUtc, lat, lng, az, el);

  //el = el - 180;

  if (el > SUNRISESET_STD_ALTITUDE) {
    screenBrightnessCurrent = 15;
  } else if ((el < SUNRISESET_STD_ALTITUDE) && (el > CIVIL_DAWNDUSK_STD_ALTITUDE)) {
    screenBrightnessCurrent = MIDDLE_SCREEN_BRIGHTNESS;
  } else if (el < CIVIL_DAWNDUSK_STD_ALTITUDE) {
    screenBrightnessCurrent = 0;
  }
  ScreenBrightnessUser = screenBrightnessCurrent;
  disp.setBrightness(screenBrightnessCurrent);
  return true;
}

void setup() {
  //setup screen
  Wire.setSCL(17);
  Wire.setSDA(16);
  //Wire.setClock(100000);
  disp.begin(DISPLAY_ADDRESS);
  disp.setBrightness(ScreenBrightnessUser);

  //screen brightness
  pinMode(20, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(20), chageBrightness, FALLING);
  rp2040.wdt_begin(2000);  //start hardware watchdog
}

void setup1() {
  //setup GPS
  Serial1.setRX(1);
  Serial1.setTX(0);
  connectGPS();
  //setStartBrightness();
}

void loop() {
  //set init brightness
  if ((!setInitBrightness && gpsFixType.load() > 0) || ((!manualBrightness && millis() > brightnessTimer + 6000) && gpsFixType.load() > 0)) {  //fire every minute
    setStartBrightness();
    setInitBrightness = true;
    brightnessTimer = millis();
  }

  //change brightness
  if (buttonPushed == true) {
    if (ScreenBrightnessUser == 15) {
      ScreenBrightnessUser = MIDDLE_SCREEN_BRIGHTNESS;
    } else if (ScreenBrightnessUser == MIDDLE_SCREEN_BRIGHTNESS) {
      ScreenBrightnessUser = 0;
    } else {
      ScreenBrightnessUser = 15;
    }
    disp.setBrightness(ScreenBrightnessUser);
    screenBrightnessCurrent = ScreenBrightnessUser;
    buttonPushed = false;
  }

  disp.clear();
  if (gpsFixType.load() > 0) {
    if (screenBrightnessCurrent != ScreenBrightnessUser) {
      disp.setBrightness(ScreenBrightnessUser);
      screenBrightnessCurrent = ScreenBrightnessUser;
    }
    timeOfLastFix = millis();  //reset no gps screen display
    int temp = speed.load() * 10;
    //int temp = rand();
    if (temp < 10) { temp = 0; }  // show 0 when stopped and not noise.
    int digit0 = temp / 1000;
    int digit1 = (temp / 100) - (digit0 * 10);
    int digit2 = (temp / 10) - (digit0 * 100) - (digit1 * 10);
    int digit3 = temp - (digit0 * 1000) - (digit1 * 100) - (digit2 * 10);
    if (speed > 100) {  //dont show leading 0 if going under 100km/h
      disp.writeDigitAscii(0, 48 + digit0);
    }
    if (speed > 10) {  //dont show 10s digit if less then 10km/h
      disp.writeDigitAscii(1, 48 + digit1);
    }
    disp.writeDigitAscii(2, 48 + digit2, true);
    disp.writeDigitAscii(3, 48 + digit3);
  } else {
    if (screenBrightnessCurrent != 0) {
      disp.setBrightness(0);
      screenBrightnessCurrent = 0;
    }
    unsigned long currentTime = millis();
    unsigned long timeSinceLastFIx = timeOfLastFix - currentTime;
    int flashStage = timeSinceLastFIx % 3000;
    for (int x = 0; x < 4; x++) {
      if (flashStage < 1000) {
        //disp.writeDigitAscii(x, 95);

        disp.writeDigitAscii(x, 3U);  //bottom
      } else if (flashStage < 2000 && flashStage > 1000) {
        disp.writeDigitRaw(x, 192u);  //middle
      } else if (flashStage < 3000 && flashStage > 2000) {
        disp.writeDigitRaw(x, 1);  //top
      }
    }
  }
  disp.writeDisplay();
  rp2040.wdt_reset();  //rest watchdog timer

  #if PROFILE 
  //profiling
  runCounter++;

  if(millis() > profileTimer + 1000){
    Serial.println(runCounter);
    runCounter=0;
    profileTimer=millis();
  }
  #endif

}


void loop1() {
  //myGPS.getPVT(GPS_WAIT_TIME);
  speed.store(myGPS.getGroundSpeed() * 0.0036);       //get speed in mm/s and convert to Km/h
  gpsFixType.store(myGPS.getFixType(GPS_WAIT_TIME));  //getFixType() will return >1 for fix type and 0 for no fix.
}