#include <Arduino.h>
#include <wakaama-client.h>

#include <Sodaq_wdt.h>
#include <Sodaq_nbIOT.h>

#define MODEM_ON_OFF_PIN 7
#define MODEM_STREAM Serial1

#define DEBUG_STREAM SerialUSB
//#define DEBUG_STREAM_BAUD 115200
#define DEBUG_STREAM_BAUD 9600

Sodaq_nbIOT nbIOT;

int pingSocket;
int pending;
char * buffer;

// constructor only sets uri, init() called later so we can debug print
/**
 * 40.115.26.19 - ethinnoday-lwm2m-server.westeurope.cloudapp.azure.com
 * 5.39.83.206  - leshan.eclipse.org
 */
ArduinoClient wakaamaClient("yvan_arduino_client", "coap://5.39.83.206:5683");

void setup() {
  buffer = static_cast<char*>(malloc(5));
  memset(buffer, 0, 5);
  DEBUG_STREAM.begin(DEBUG_STREAM_BAUD);
  while(!DEBUG_STREAM) {
    ;
  }
  MODEM_STREAM.begin(nbIOT.getDefaultBaudrate());
  while(!MODEM_STREAM) {
    ;
  }

  DEBUG_STREAM.println(F("nbIOT init(): start"));
  nbIOT.init(MODEM_STREAM, MODEM_ON_OFF_PIN);
  nbIOT.setDiag(DEBUG_STREAM);
  DEBUG_STREAM.println(F("nbIOT init(): done"));

  // initialize the client
  nbIOT.connectSocket();

  pingSocket = nbIOT.createSocket(1313);

  wakaamaClient.init(&nbIOT);
}

void loop() {
  DEBUG_STREAM.println(F("Checking modem state ... "));

  nbIOT.isConnected();

  if(!nbIOT.isRegistered()) {
    DEBUG_STREAM.println(F("Modem is deregistered, trying to reconnect."));
    if(!nbIOT.reconnectSocket()) {
//      delay(1000);
      return;
    }
  }

  nbIOT.sendSocket(pingSocket, "87.229.111.223", 1111, (uint8_t*)"pong", 4);
  pending = nbIOT.socketBytesPending(pingSocket);
  if(pending > 0) {
    nbIOT.socketReceive(pingSocket, buffer, pending);
    DEBUG_STREAM.print(F(__FILE__":loop() ping:"));
    DEBUG_STREAM.println(buffer);
  }

  // our worker loop
  wakaamaClient.doWorkStep();

  // sleep a bit?
//  delay(1000);
}