/*
* Sigfox
* Company: Dryp
* Model: Janaury 2019
* Hardware revision:
*/

// Includes
#include "Maxbotix.h"
#include "LowPower.h"
#include "Akeru.h"
#include <Wire.h>
#include "Adafruit_ADXL345_U.h"
#include "Adafruit_Sensor.h"

// Definitions
#define TX 9 // TD1208R Serial port
#define RX 6 // TD1208R Serial port
#define sensorPin 5 // MaxBotix MB7360 PWM pin
#define ENABLE_US 2 // MOSFET pin
#define TRANSMISSION_INTERVAL 75 // Transmission interval X * 8 seconds
#define DEBUG true // Turn on debug messages

// Variables
int temperature = 0; // TD1208R internal temperature
int roll = 0, pitch = 0;   //Roll & Pitch are the angles which rotate by the axis X and y
double x_Buff = 0.00;
double y_Buff = 0.00;
double z_Buff = 0.00;
unsigned long distance = 0; // MaxBotix MB7360 distance variable

// Sigfox instance management
Akeru akeru(RX, TX);

// Instantiate sensors
Adafruit_ADXL345_Unified accel = Adafruit_ADXL345_Unified(345);
Maxbotix rangeSensorPW(5, Maxbotix::PW, Maxbotix::HRLV,Maxbotix::MEDIAN, 10); // Sample size last

// Setup - runs once during bootup
void setup() {
  if(DEBUG) {
    // initialize serial communication at 9600 bits per second:
    Serial.begin(9600);
    Serial.println("[INFO] Dryp Unit Starting...");
  }


  // Set MOSFET pinMode to OUTPUT
  pinMode(ENABLE_US, OUTPUT);

  // Start communication with ADXL345
  if (!accel.begin()) {
    if(DEBUG) {
      Serial.println("[WARNING] Could not find ADXL345");
    }
  } else  {
    if(DEBUG) {
          Serial.println("[INFO] ADXL345 started");
    }
    // Set the range to whatever is appropriate for your project
    accel.setRange(ADXL345_RANGE_16_G);

    // Set output update rate to 6.25Hz with 3.125Hz bandwidth (40 μA consumption) TODO: Check if ADXL345_DATARATE_0_78_HZ exists and works
    accel.writeRegister(ADXL345_REG_BW_RATE, 0x06);
  }

  // Display additional sensor information
  if(DEBUG) {
    /* Display some basic information on this sensor */
    displaySensorDetails();

    /* Display additional settings (outside the scope of sensor_t) */
    displayDataRate();
    displayRange();
  }
}

// Loop runs continuously as long as system is up
void loop() {
  unsigned long start;
  // If debugging, examine how long it takes to do 1 measurement prior to transmission
  if (DEBUG)  {
    start = millis();
    akeru.echoOn();
  }

    // Start communication with TD1208R module
  if (!akeru.begin()) {
    if(DEBUG) {
      Serial.println("[WARNING] Could not find TD1208R");
    }
  } else if(DEBUG) {
    Serial.println("[INFO] TD1208R started");
  }

  /*
  * Set accelerometer in standby mode prior to execution of begin(), which sets the ADXL345 into measurement mode.
  * Per datasheet, it is recommends to put the unit into standby-mode after sleep, prior to going into measurement mode.
  */
  accel.writeRegister(ADXL345_REG_POWER_CTL, 0); // standby mode

  // Start communication with ADXL345
  if (!accel.begin()) {
    if(DEBUG) {
      Serial.println("[WARNING] Could not find ADXL345");
    }
  } else  {
    if(DEBUG) {
          Serial.println("[INFO] ADXL345 started");
    }
    // Set the range to whatever is appropriate for your project
    accel.setRange(ADXL345_RANGE_16_G);
  }

  // Set output power of TD1208R
  //akeru.setPower(14);

  // Enable distance sensor using MOSFET
  digitalWrite(ENABLE_US, LOW);

  // Get new ADXL345 sensor event while ultrasonic is warming up
  ADXL_measure();

  // Set ADXL345 sensor into sleep mode
  ADXL_sleep();

  // Get Ultra Sonic Sensor readings
  distance = (rangeSensorPW.getRange())*10;

  // Shut distance sensor off again
  digitalWrite(ENABLE_US, HIGH);

  // Get SigFox internal temparature
  SigFox_getTemperature();

  temperature = -5;
  // Prepare payload by hexing current variables
  String t = akeru.toHex(temperature);
  String d = akeru.toHex(distance);
  String r = akeru.toHex(roll);
  String p = akeru.toHex(pitch);

  // Concatenate all data into one single package
  String msg = t+d+r+p; // Put everything together

  // Show current sensor values, measurement time and hex'ed packages
  if(DEBUG) {
    Serial.print("Roll: "); Serial.println(roll);
    Serial.print("Pitch: "); Serial.println(pitch);
    Serial.print("Distance: "); Serial.println(distance);
    Serial.println ("Sigfox message");
    Serial.println (d);
    Serial.println (r);
    Serial.println (p);
    Serial.println (t);
    Serial.println("Measurement time: ");
    Serial.print(millis() - start);
    Serial.println("ms");
  }
  if (akeru.sendPayload(msg)) {
    if(DEBUG) {
      Serial.println("[INFO] message was sent");
    }
  } else if (DEBUG) {
    Serial.println("[ERROR] Message not sent");
  }


  delay(100);

  for (int second = 0; second < TRANSMISSION_INTERVAL; second++)
  {
    LowPower.powerDown(SLEEP_8S, ADC_OFF, BOD_OFF);
  }

}


void displaySensorDetails(void)
{
  sensor_t sensor;
  accel.getSensor(&sensor);
    Serial.println("------------------------------------");
    Serial.print  ("Sensor:       "); Serial.println(sensor.name);
    Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
    Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
    Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" m/s^2");
    Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" m/s^2");
    Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" m/s^2");
    Serial.println("------------------------------------");
    Serial.println("");
}

void displayDataRate(void)
{
  Serial.print  ("Data Rate:    ");

  switch(accel.getDataRate())
  {
    case ADXL345_DATARATE_3200_HZ:
      Serial.print  ("3200 ");
      break;
    case ADXL345_DATARATE_1600_HZ:
      Serial.print  ("1600 ");
      break;
    case ADXL345_DATARATE_800_HZ:
      Serial.print  ("800 ");
      break;
    case ADXL345_DATARATE_400_HZ:
      Serial.print  ("400 ");
      break;
    case ADXL345_DATARATE_200_HZ:
      Serial.print  ("200 ");
      break;
    case ADXL345_DATARATE_100_HZ:
      Serial.print  ("100 ");
      break;
    case ADXL345_DATARATE_50_HZ:
      Serial.print  ("50 ");
      break;
    case ADXL345_DATARATE_25_HZ:
      Serial.print  ("25 ");
      break;
    case ADXL345_DATARATE_12_5_HZ:
      Serial.print  ("12.5 ");
      break;
    case ADXL345_DATARATE_6_25HZ:
      Serial.print  ("6.25 ");
      break;
    case ADXL345_DATARATE_3_13_HZ:
      Serial.print  ("3.13 ");
      break;
    case ADXL345_DATARATE_1_56_HZ:
      Serial.print  ("1.56 ");
      break;
    case ADXL345_DATARATE_0_78_HZ:
      Serial.print  ("0.78 ");
      break;
    case ADXL345_DATARATE_0_39_HZ:
      Serial.print  ("0.39 ");
      break;
    case ADXL345_DATARATE_0_20_HZ:
      Serial.print  ("0.20 ");
      break;
    case ADXL345_DATARATE_0_10_HZ:
      Serial.print  ("0.10 ");
      break;
    default:
      Serial.print  ("???? ");
      break;
  }
  Serial.println(" Hz");
}

void displayRange(void)
{
  Serial.print  ("Range:         +/- ");

  switch(accel.getRange())
  {
    case ADXL345_RANGE_16_G:
      Serial.print  ("16 ");
      break;
    case ADXL345_RANGE_8_G:
      Serial.print  ("8 ");
      break;
    case ADXL345_RANGE_4_G:
      Serial.print  ("4 ");
      break;
    case ADXL345_RANGE_2_G:
      Serial.print  ("2 ");
      break;
    default:
      Serial.print  ("?? ");
      break;
  }
  Serial.println(" g");
}

void ADXL_measure(void) {

  delay(1000);
  sensors_event_t event;
  accel.getEvent(&event);

  if(DEBUG) {
  Serial.print("X: "); Serial.print(event.acceleration.x); Serial.print("  ");
  Serial.print("Y: "); Serial.print(event.acceleration.y); Serial.print("  ");
  Serial.print("Z: "); Serial.print(event.acceleration.z); Serial.print("  ");Serial.println("m/s^2 ");
  }

  x_Buff = event.acceleration.x;
  y_Buff = event.acceleration.y;
  z_Buff = event.acceleration.z;

  roll = atan2(y_Buff , z_Buff) * 57.3;
  pitch = atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3;
 }

void ADXL_sleep(void) {
  accel.writeRegister(ADXL345_REG_POWER_CTL, 0);
}

void SigFox_getTemperature(void)
{


  if (akeru.getTemperature(&temperature))
  {
    if(DEBUG)
    {
    Serial.print("Temperature = ");
    Serial.print(temperature);
    Serial.println(" C");
    }
  }
  else
  {
    temperature=99;
    Serial.println("Temperature KO");
  }
}