#include <U8g2lib.h>
#include <TinyGPSMinus.h>
#include <Wire.h>
#include <QMC5883LCompass.h>
#include "wifi_save.h"
#include <HTTPClient.h>
#include <Arduino_JSON.h>


U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/U8X8_PIN_NONE);  // if you use Hardware I2C port, full framebuffer, size = 1024 bytes
QMC5883LCompass compass;

#define RXD2 16
#define TXD2 17

// ID 1 = Julia; ID 2 = Tim 
#define OWN_ID 1
#define REQ_ID 2

#define GPS_BAUD 9600

String webserviceAddress = "http://192.168.179.2:65133/updateGPS";
bool connected = false;

HardwareSerial gpsSerial(2);
TinyGPSMinus gps;

byte triangles[][6] = {
  { 10, 48, 30, 48, 20, 16 },
  { 5, 43, 23, 51, 26, 17 },
  { 2, 36, 16, 50, 31, 21 },
  { 1, 29, 9, 47, 35, 26 },
  { 4, 22, 4, 42, 36, 32 },
  { 9, 17, 1, 35, 35, 38 },
  { 16, 14, 2, 28, 31, 43 },
  { 23, 13, 5, 21, 26, 47 },
  { 30, 16, 10, 16, 20, 48 },
  { 35, 21, 17, 13, 14, 47 },
  { 38, 28, 24, 14, 9, 43 },
  { 39, 35, 31, 17, 5, 38 },
  { 36, 42, 36, 22, 4, 32 },
  { 31, 47, 39, 29, 5, 26 },
  { 24, 50, 38, 36, 9, 21 },
  { 17, 51, 35, 43, 14, 17 },
  { 10, 48, 30, 48, 20, 16 }
};

void setup(void) {

  Serial.begin(115200);
  
  if (wifi_set_main())
  {
      Serial.println("Connect WIFI SUCCESS");
      connected = true;
  }
  else
  {
      Serial.println("Connect WIFI FAULT");
  }

    compass.init();
  
  u8g2.begin();  // start a display.
  gpsSerial.begin(GPS_BAUD, SERIAL_8N1, RXD2, TXD2);
  Serial.println("Serial 2 started at 9600 baud rate");

}

void drawTriangles(byte i, char* time, char* distanceString) {
  u8g2.clearBuffer();
  u8g2.drawTriangle(triangles[i][0], triangles[i][1], triangles[i][2], triangles[i][3], triangles[i][4], triangles[i][5]);
  u8g2.sendBuffer();
  u8g2.setFont(u8g2_font_helvR08_tr);
  u8g2.drawButtonUTF8(48, 64-40, U8G2_BTN_BW0, 0, 0, 0, "Last Update:");
  u8g2.drawButtonUTF8(48, 64-30, U8G2_BTN_BW0, 0, 0, 0, time);
  u8g2.drawButtonUTF8(48, 64-16, U8G2_BTN_BW0, 0, 0, 0, distanceString);
  u8g2.sendBuffer();
  delay(2000);

}

bool doHttpRequest(const char* payload, char* latitude, char* longitude, char* gpsTime) {
   HTTPClient http;
      // Change space to 0
      *(latitude + 6)  = 0;

      String serverPath = webserviceAddress + "?long=" + *longitude + "&lat=" + *latitude + "&id=" + OWN_ID + "&req_id=" + REQ_ID + "&gpsDateTime=" + *gpsTime;

      Serial.println(serverPath);
      
      // Your Domain name with URL path or IP address with path
      http.begin(serverPath.c_str());
      
      // If you need Node-RED/server authentication, insert user and password below
      //http.setAuthorization("REPLACE_WITH_SERVER_USERNAME", "REPLACE_WITH_SERVER_PASSWORD");
      
      // Send HTTP GET request
      int httpResponseCode = http.GET();
      
      if (httpResponseCode>0) {
        Serial.print("HTTP Response code: ");
        Serial.println(httpResponseCode);
        payload = http.getString().c_str();
        Serial.println(payload);
        return true;
      }
      else {
        Serial.print("Error code: ");
        Serial.println(httpResponseCode);
        return false;
      }
}

byte calculateBearing(float bearing) {
  int relativeAngle = (int)(bearing - (360 - compass.getAzimuth()));
  relativeAngle += 360;
  relativeAngle %= 360;
  return compass.getBearing(relativeAngle);

}
void setDistanceBearingString(char *distance, byte *bearing, const char *payload) {
  JSONVar jsonPayload = JSON.parse(payload);
  *distance = jsonPayload["dist"];
  *bearing = calculateBearing(String(jsonPayload["bearing"]).toFloat());
}



void loop(void) {
  while (gpsSerial.available() > 0){
    // get the byte data from the GPS
    char gpsData = gpsSerial.read();
    
    if (gps.encode(gpsData)) {
      Serial.print(gpsData);
      break;
    }
  }

  long unsigned date, time, age;
  char *longitude, *latitude, *dateTime;
  longitude = gps.get_longitude(); 
  latitude = gps.get_latitude();
  gps.get_datetime(&date, &time, &age);

  Serial.println(latitude);
  Serial.println(longitude);

  const char *myPayloadResponse;

  char timeString[10], dateString[8], dateTimeString[18];
  ltoa(time, timeString, 10);
  ltoa(date, dateString, 8);
      for (int i = 0; i < 18 ;i++){
        if(i < 8)
            dateTimeString[i] = dateString[i];
        else
            dateTimeString[i] = timeString[i-8];
    }

    byte triangleIndex = 0;
    char distanceArray[] =  { 'N', 'o', ' ', 'S', 'e', 'r', 'v', 'e', 'r', '\0'};
    char *distanceString = &(distanceArray[0]);

  if(doHttpRequest(myPayloadResponse, latitude, longitude, dateTimeString))
    setDistanceBearingString(distanceString, &triangleIndex, myPayloadResponse);
 
  Serial.println();
  Serial.println("Time: ");
  Serial.println(time);
  Serial.println("Date: ");
  Serial.println(date);
  
  drawTriangles(triangleIndex, timeString, distanceString);

  Serial.println("-------------------------------");
    int a;
  compass.read();

  //Calculate heading
  a = compass.getAzimuth();
  float heading =  compass.getBearing(a);
  char myArray[3];
  compass.getDirection(myArray, a);

  Serial.print("A: ");
  Serial.print(a);
  Serial.print("Heading: ");
  Serial.print(heading);
  Serial.print(" - ");
  Serial.print(myArray);
  Serial.println();
  Serial.println("-------------------------------");

  delay(1000);
}