#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>
#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

//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;

//speed calc
//float averageSpeed = 0;
//int speedSlot = 0;  //for placing the speed in the array for smoothing
//float *speedArray;

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

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

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
}

void setup() {
  //setup screen
  Wire.setSCL(17);
  Wire.setSDA(16);
  //Wire.setClock(100000);
  disp.begin(DISPLAY_ADDRESS);
  disp.setBrightness(screenBrightnessButton);
  disp.clear();
  disp.writeDigitAscii(0, 71);  //"G"
  disp.writeDigitAscii(1, 80);  //"P"
  disp.writeDigitAscii(2, 83);  //"S"
  disp.writeDisplay();

  //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();
}

void loop() {


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

  disp.clear();
  if (gpsFixType.load() > 0) {
    if (screenBrightnessCurrent != screenBrightnessButton){
      disp.setBrightness(screenBrightnessButton);
      screenBrightnessCurrent=screenBrightnessButton;
    }
    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
}

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.
}