#include <lmic.h>
#include <hal/hal.h>
#include <SPI.h>

#include <PString.h>

#include <Base64.h>

#include <Arduino.h>
#include <NMEAGPS.h>

#include <Wire.h>
#include <Adafruit_LSM303_U.h>
#include <Adafruit_BMP085_U.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_Sensor_Set.h>
#include <Adafruit_Simple_AHRS.h>
#include <Madgwick.h>
#include <Mahony.h>

#define USE_SPI   1
SPIClass mySPI(USE_SPI);
extern SPIClass *SPIp;

// Accelerometer + Giro + ...

// Create sensor instances.
Adafruit_LSM303_Accel_Unified accel(30301);
Adafruit_LSM303_Mag_Unified   mag(30302);
Adafruit_BMP085_Unified       bmp(18001);

// Create simple AHRS algorithm using the above sensors.
Adafruit_Simple_AHRS          ahrs(&accel, &mag);

// Update this with the correct SLP for accurate altitude measurements
float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;

// GPS

HardwareSerial & gps_port = Serial1;
#define USING_GPS_PORT "Serial1"

#include "Streamers.h"
#ifdef NeoHWSerial_h
  #define DEBUG_PORT NeoSerial
#else
  #define DEBUG_PORT Serial3
#endif

// Check configuration

// #ifndef NMEAGPS_INTERRUPT_PROCESSING
//  #error You must define NMEAGPS_INTERRUPT_PROCESSING in NMEAGPS_cfg.h!
// #endif

static NMEAGPS   gps;

// LMIC

// This EUI must be in little-endian format, so least-significant-byte
// first. When copying an EUI from ttnctl output, this means to reverse
// the bytes. For TTN issued EUIs the last bytes should be 0xD5, 0xB3,
// 0x70.
static const u1_t PROGMEM APPEUI[8]={ 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
void os_getArtEui (u1_t* buf) { memcpy_P(buf, APPEUI, 8);}

// This should also be in little endian format, see above.
static const u1_t PROGMEM DEVEUI[8]={ 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
void os_getDevEui (u1_t* buf) { memcpy_P(buf, DEVEUI, 8);}

// This key should be in big endian format (or, since it is not really a
// number but a block of memory, endianness does not really apply). In
// practice, a key taken from ttnctl can be copied as-is.
// The key shown here is the semtech default key.
static const u1_t PROGMEM APPKEY[16] = { 0x2B, 0x7E, 0x15, 0x16, 0x28, 0xAE, 0xD2, 0xA6, 0xAB, 0xF7, 0x15, 0x88, 0x09, 0xCF, 0x4F, 0x3C };
void os_getDevKey (u1_t* buf) {  memcpy_P(buf, APPKEY, 16);}

static osjob_t sendjob;
static uint8_t* encodedString;

// Schedule TX every this many seconds (might become longer due to duty
// cycle limitations).
const unsigned TX_INTERVAL = 60;

// Pin mapping
const lmic_pinmap lmic_pins = {
    .nss = PA4,
    .rxtx = LMIC_UNUSED_PIN,
    .rst = PB0,
    .dio = {PA0, PA1, PA8},
};

static void GPSisr( uint8_t c )
{
  gps.handle( c );
}

void onEvent (ev_t ev) {
    switch(ev) {
        case EV_TXCOMPLETE:
            // Schedule next transmission
            os_setTimedCallback(&sendjob, os_getTime()+sec2osticks(TX_INTERVAL), do_send);
            free(encodedString);
            break;
         default:
            break;
    }
}

void do_send(osjob_t* j){
    // Check if there is not a current TX/RX job running
    if (LMIC.opmode & OP_TXRXPEND) {
        // Serial.println(F("OP_TXRXPEND, not sending"));
    } else {
        // Prepare upstream data transmission at the next possible time.
        LMIC_setTxData2(1, encodedString, sizeof(encodedString)-1, 0);
        // Serial.println(F("Packet queued"));
    }
    // Next TX is scheduled after TX_COMPLETE event.
}

void setup()
{
  // SPI
  SPIp = &mySPI;

  // Initialize the sensors.
  Serial3.begin(115200);
  accel.begin();
  mag.begin();
  bmp.begin();

  // GPS

  // Start the UART for the GPS device
//  gps_port.attachInterrupt( GPSisr );
  gps_port.begin( 9600 );

  // LMIC init
  os_init();
  // Reset the MAC state. Session and pending data transfers will be discarded.
  LMIC_reset();

  // Start job
  do_send(&sendjob);
}

void loop(void)
{
  sensors_vec_t   orientation;
  char buffer_gps[128];
  PString result_gps(buffer_gps, sizeof(buffer_gps), "");

  char buffer_ahrs[16];
  PString result_ahrs(buffer_ahrs, sizeof(buffer_ahrs), "");

  char buffer_bmp[16];
  PString result_bmp(buffer_bmp, sizeof(buffer_bmp), "");

  char buffer[160];
  PString result(buffer, sizeof(buffer), "");

  if (gps.available()) {
    // Print all the things!
    trace_all( result, gps, gps.read() );
  }

  if (gps.overrun()) {
    gps.overrun( false );
    //DEBUG_PORT.println( F("DATA OVERRUN: took too long to use gps.read() data!") );
  }

  // Use the simple AHRS function to get the current orientation.
  if (ahrs.getOrientation(&orientation))
  {
    /* 'orientation' should have valid .roll and .pitch fields */
    result_ahrs.format("%f, %f, %f", orientation.roll, orientation.pitch, orientation.heading);
  }

  // Calculate the altitude using the barometric pressure sensor
  sensors_event_t bmp_event;
  bmp.getEvent(&bmp_event);
  if (bmp_event.pressure)
  {
    /* Get ambient temperature in C */
    float temperature;
    bmp.getTemperature(&temperature);
    /* Convert atmospheric pressure, SLP and temp to altitude */
    result_bmp.format("%f, %f", bmp.pressureToAltitude(seaLevelPressure,
                                        bmp_event.pressure,
                                        temperature),
                      result.print(temperature));
  }

  // Collect all the data in ; separated string
  result << result_gps << ";" << result_ahrs << ";" << result_bmp;

  // Encode to Base64
  int encodedLength = Base64.encodedLength(result.length());
  encodedString = (uint8_t*)malloc(encodedLength);
  Base64.encode(encodedString, buffer, result.length());

  // Serial.print(buffer);

  // Send over LMIC
  os_runloop_once();

  //delay(100);
}