#include "logger.h"
#include <Arduino.h>
#include <WiFi.h>
#include <TinyGPS++.h>
#include <XPowersLib.h>
#include "display.h"
#include "pins.h"
#include "RadioLib.h"

#define RADIO_SCLK_PIN 5
#define RADIO_MISO_PIN 19
#define RADIO_MOSI_PIN 27
#define RADIO_CS_PIN 18
#define RADIO_DIO0_PIN 26
#define RADIO_RST_PIN 23
#define RADIO_DIO1_PIN 33
#define RADIO_BUSY_PIN 32
SX1278 radio = new Module(RADIO_CS_PIN, RADIO_DIO1_PIN, RADIO_RST_PIN, RADIO_BUSY_PIN);

logging::Logger logger;
TinyGPSPlus    gps;
char nmea[512];
HardwareSerial ss(1);
char trameg[128] = "#G";


XPowersAXP2101 power;
#define XPOWERS_CHIP_AXP2101
#define PMU_IRQ 35
#define PMU_SDA 21
#define PMU_SCK 22

void setup() {
  Serial.begin(115200);

  /*
  logger.log(logging::LoggerLevel::LOGGER_LEVEL_ERROR, "MAIN", "This is a error");
  logger.log(logging::LoggerLevel::LOGGER_LEVEL_WARN, "MAIN", "This is a warning");
  logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "MAIN", "This is a info");
  logger.log(logging::LoggerLevel::LOGGER_LEVEL_DEBUG, "MAIN", "This is a debug");
  */

  setup_display();
  show_display("PIOTest", "GPS and other stuffs", 2000);

  /* Scan for WiFi networks
   * On commence par scanner les réseaux WiFi disponibles
   * On attend que le scan soit terminé et on affiche les réseaux trouvés  
  show_display("WiFi", "Scanning around", "Please wait...", 2000);
  WiFi.scanNetworks();
  int n = WiFi.scanComplete();
  if (n == -2) {
	WiFi.scanNetworks();
  }
  while (n == -1) {
	delay(100);
	n = WiFi.scanComplete();
  }
  if (n == 0) {
	logger.log(logging::LoggerLevel::LOGGER_LEVEL_ERROR, "WIFI", "No networks found");
	show_display("WiFi", "No networks found", 2000);
  } else {
	logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "WIFI", "Found %d networks", n);
	show_display("WiFi", "Found networks", "Please wait...", 2000);
	for (int i = 0; i < n; ++i) {
		logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "WIFI", "Network: %s", WiFi.SSID(i).c_str());
		show_display("WiFi", WiFi.SSID(i).c_str(), 2000);
	}
  }
  */

  /*
   * Initialize the PMU
   */
  bool result = power.begin(Wire, AXP2101_SLAVE_ADDRESS, PMU_SDA, PMU_SCK);
  if (result == false) {
	logger.log(logging::LoggerLevel::LOGGER_LEVEL_ERROR, "PMU", "Failed to initialize PMU");
  }
  logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "PMU", "PMU initialized");


  /* Initialize the GPS
   */
  logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "Initializing GPS");
  show_display("GPS", "Initializing GPS", "Please wait...", 2000);

  ss.begin(9600, SERIAL_8N1, GPS_TX, GPS_RX);
  // setup GPS for only GPRMC sentences, but don't work anyway...
  ss.println("PMTK314,0,0,0,1,0,0,0,0,0,0,0,0,0,0");

  /* Initialize the LoRa radio
   */
  logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "RADIO", "Initializing LoRa radio");
  radio.beginFSK(435);
}


void loop() {

  int size = 0;
  char * pch;

  while (ss.available() > 0) { 
	char c = ss.read();
	gps.encode(c);
  	// add char to trame
	nmea[size] = c;
	//
	size++;
  }
  nmea[size] = '\0';

  if (gps.location.isValid()) {
	logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "Full NMEA:"); 
	pch = strtok (nmea,"\n");
	while (pch != NULL) {
	  logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "\t%s", pch);
	  pch = strtok (NULL, "\n");
	}
	logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "Parsing NMEA:");
	logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "\tLatitude: %f", gps.location.lat());
	logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "\tLongitude: %f", gps.location.lng());
	logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "\tNumber of satellites : %d", gps.satellites.value());
	logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "\tAltitude: %f", gps.altitude.meters());
	logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "\tSpeed: %f", gps.speed.kmph());
	logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "\tCourse: %f", gps.course.deg());
	logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "\tDate: %d/%d/%d", gps.date.day(), gps.date.month(), gps.date.year());
	logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "\tTime: %d:%d:%d", gps.time.hour(), gps.time.minute(), gps.time.second());
	logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "\tHDOP: %f", gps.hdop.hdop());
	}
    else {
		logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "No GPS data available");
  	}
	logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "GPS", "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-");

	// erase trameg with \0
    memset(trameg, 0, sizeof(trameg));
	strcat(trameg, "#G");

	char strday[4];
	char strmonth[4];
	char stryear[6];
	char stryear2[4];
	char strhour[4];
	char strminute[4];
	char strsecond[4];
	sprintf(strday, "%u", gps.date.day());
	sprintf(strmonth, "%u", gps.date.month());
	sprintf(stryear, "%u", gps.date.year());
	sprintf(strhour, "%u", gps.time.hour());
	sprintf(strminute, "%u", gps.time.minute());
	sprintf(strsecond, "%u", gps.time.second());

	// Trame = "#GDDMMYY;HHMMSS;LATITUDE;LONGITUDE;ALTITUDE;SATELLITES;*RSSI%\r\n"
    strcat(trameg, strday);
    strcat(trameg, strmonth);
	strcat(trameg, stryear);
//    stryear2[0] = stryear[2];
//    stryear2[1] = stryear[3];
    strcat(trameg, stryear2);
    strcat(trameg, ";");
    strcat(trameg, strhour);
    strcat(trameg, strminute);
    strcat(trameg, strsecond);
	strcat(trameg, ";");
	char strlat[16];
	char strlng[16];
	dtostrf(gps.location.lat(), 8, 6, strlat);
	dtostrf(gps.location.lng(), 8, 6, strlng);
	strcat(trameg, strlat);
	strcat(trameg, ";");
	strcat(trameg, strlng);
	strcat(trameg, ";");
	char stralt[16];
	dtostrf(gps.altitude.meters(), 8, 2, stralt);
	strcat(trameg, stralt);
	strcat(trameg, ";");
	char strsat[4];
	sprintf(strsat, "%u", gps.satellites.value());
	strcat(trameg, strsat);
	strcat(trameg, "*");
	strcat(trameg, "RSSSSSSSSI");
	strcat(trameg, "%");
	strcat(trameg, "\r\n");

	strcat(trameg, "\0");

	// count bytes until \0
	int sizestr = 0;
	while (trameg[sizestr] != '\0') {
	  sizestr++;
	}
    logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "TRAME_BUILD", "Trame: %s", trameg);

  int state = radio.transmit(trameg, sizestr);
  //int state = radio.transmit(nmea, strlen(nmea));
  if (state == RADIOLIB_ERR_NONE) {
	logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "RADIO", "Trame sent. Size: %d bytes", strlen(trameg));
	// print trame
    logger.log(logging::LoggerLevel::LOGGER_LEVEL_INFO, "RADIO", "Trame: %s", trameg);
  } else {
	logger.log(logging::LoggerLevel::LOGGER_LEVEL_ERROR, "RADIO", "Failed to send trame");
  }
  delay(1000);

}