#include <Arduino.h>
#include <SoftwareSerial.h>
#include <ArduinoJson.h>
SoftwareSerial mySerial(10, 11);

char chrIMEI[16];
char serialdata[256]; //Array to store the chars before parsing, size???
const int kLedPin = 13;

bool socketIsSet = false;
bool messagesInQueue = false;
unsigned int interval = 1000 * 30 * 1;
unsigned long time_alive_message;

void sendMessage(String payload) {
  payload.concat("0");
  char buf[256];
  int payloadLen = payload.length();
  payload.toCharArray(buf, payloadLen);
  String myString = "AT+NSOST=0,172.27.131.100,15683,";
  myString.concat(String(payloadLen - 1));
  myString.concat(",");
  for (int i = 0; i < payloadLen - 1; i++) {
    myString.concat(String(buf[i], HEX));
  }
  myString.concat("\r\n");
  myString.toCharArray(buf, 256);
  mySerial.write(buf);
}

int readline(int readch, char *buffer, int len) {
  static int pos = 0;
  int rpos;
  if (readch > 0) {
    switch (readch) {
      case '\r': // Ignore CR
        break;
      case '\n': // Return on new-line
        rpos = pos;
        pos = 0;  // Reset position index ready for next time
        return rpos;
      default:
        if (pos < len - 1) {
          buffer[pos++] = readch;
          buffer[pos] = 0;
        }
        break;
    }
  }
  return 0;
}

void setup()  {
  //Serial.begin(9600);
  //Configure the LED for output and sets the intial state to off
  pinMode(kLedPin, OUTPUT);
  digitalWrite(kLedPin, LOW);
  //Serial.println("Testing up- and downlink");
  mySerial.begin(9600);
  mySerial.write("AT+NSOCR=DGRAM,17,7000,1\r\n");
  mySerial.flush();
}

void loop() {
  if (!socketIsSet) {
    //Serial.println("Setting SOCKET");
    mySerial.write("AT+NSOCR=DGRAM,17,7000,1\r\n");
    mySerial.flush();
    socketIsSet = true;
  }

  unsigned long time = millis();
  if (time_alive_message + interval < millis()) {
    //Serial.println("Sending ALIVE message");
    String payload = "{\"sensor\":\"gps\",\"time\":1351824120,\"data\":[48.75608,2.302038]}";
    sendMessage(payload);
    mySerial.flush();
    time_alive_message = millis();
  }

  if (mySerial.available()) {
    if (readline(mySerial.read(), serialdata, 512)) {
      //Serial.println("rtn: " + String(serialdata));
      if (strncmp(serialdata, "OK", 2) == 0) {
        //don't do anything
      } else if (strncmp(serialdata, "+CGSN:", 6) == 0) {
        for (int i = 0; i < 16; i++) {
          chrIMEI[i] = serialdata[6 + i];
        }
        chrIMEI[15] = '\0';
      } else if (strncmp(serialdata, "+NSONMI:", 8) == 0) {
        //Serial.println("Message available for download");
        messagesInQueue = true;
        char nsorf[10] = "AT+NSORF=";
        char *csocket = strtok(serialdata + 8, ":");
        //Serial.println("char *csocket: " + String(csocket)); //0,5
        char buf[256] = "";
        strcat(buf, nsorf);
        strcat(buf, csocket);
        char cr[2] = "\r";
        strcat(buf, cr);
        mySerial.write(buf);
      } else {
        //then it's a comma separated string like //0,172.27.131.100,15683,5,48656C6C6F,0
        if (messagesInQueue) {
          char *soc = strtok(serialdata, ",");
          unsigned int l1 = strlen(soc);
          char *ip = strtok(serialdata + l1 + 1, ",");
          unsigned int l2 = strlen(ip);
          char *port = strtok(serialdata + l1 + 1 + l2 + 1, ",");
          unsigned int l3 = strlen(port);
          char *length = strtok(serialdata + l1 + 1 + l2 + 1 + l3 + 1, ",");
          unsigned int l4 = strlen(length);
          char *data = strtok(serialdata + l1 + 1 + l2 + 1 + l3 + 1 + l4 + 1, ",");
          unsigned int l5 = strlen(data);
          char *remaining_length = strtok(serialdata + l1 + 1 + l2 + 1 + l3 + 1 + l4 + 1 + l5 + 1, ",");
          unsigned int l6 = strlen(remaining_length);
          messagesInQueue = false;
          //Hex to ASCII A (the network always convertes to hex)
          char output[256];
          for (size_t i = 0; i < l5; i++) {
            char temp[3] = {0};  // temp array for hex data
            strncpy(temp, &data[i * 2], 2);  // copy to temp array, two characters only
            output[i] = (char)strtol(temp, NULL, 16);  // convert hex (base 16) from temp to character in output
          }
          //Hex to ASCII B (necessary when orginal message is in hex as well as recommended)
          char output2[128];
          for (size_t i = 0; i < l5; i++) {
            char temp2[3] = {0};  // temp array for hex data
            strncpy(temp2, &output[i * 2], 2);  // copy to temp array, two characters only
            output2[i] = (char)strtol(temp2, NULL, 16);  // convert hex (base 16) from temp to character in output
          }
          //Serial.println("Message: " + String(output2));
          //Deserialize JSON
          StaticJsonDocument<16> doc;
          DeserializationError error = deserializeJson(doc, output2, 128);
          if (error) {
            //Serial.print(F("deserializeJson() failed: "));
            //Serial.println(error.f_str());
            return;
          }
          //Process message..
          const char* command = doc["command"]; // "led_on"
          if (strcmp(command, "led_on") == 0) {
            digitalWrite(kLedPin, HIGH);
            //Serial.println("led is ON");
          } else if (strcmp(command, "led_off") == 0) {
            digitalWrite(kLedPin, LOW);
            //Serial.println("led is OFF");
          }
        }
      }
    }
  }
}