#include <Arduino.h>
#include <avr/sleep.h>
#include <SparkFun_ADXL345.h>                // SparkFun ADXL345 Library
#include <pins_arduino.h>

#define LED_BUILTIN               13
#define VBATPIN                   A9
#define ADXL345_MG2G_MULTIPLIER   (0.004)      // 4mg per lsb
#define SENSORS_GRAVITY_STANDARD  (9.80665F)
volatile bool is_asleep = false;
volatile int  isr_counter;

ADXL345 adxl = ADXL345();             // USE FOR I2C COMMUNICATION

/// INTERRUPT
int     interruptPin    =   1;      // Setup pin X to be the interrupt pin

float   ab_accY;

// Offset due to gravity/sensor
float gYoffset;

/* Declare functions */
void sleepNow();
void wake();
int  digPinToInterrupt();


void setup()
{
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);
  delay(2000);
  Serial.begin(9600);

  // ***************** SETUP ADXL 345 ****************************
  adxl.powerOn();                     // Power on the ADXL345

  adxl.setRangeSetting(16);           // Give the range settings
                                      // Accepted values are 2g, 4g, 8g or 16g
                                      // Higher Values = Wider Measurement Range
                                      // Lower Values = Greater Sensitivity

  adxl.setRate(100);                  // Sampling rate to 100 Hz
  adxl.setActivityXYZ(0, 1, 0);       // Set to activate movement detection in the axes "adxl.setActivityXYZ(X, Y, Z);" (1 == ON, 0 == OFF)
  adxl.setActivityThreshold(20);      // 62.5mg per increment   // Set activity   // Inactivity thresholds (0-255)// (default 75)
  adxl.setInactivityXYZ(0, 0, 0);     // Set to detect inactivity in all the axes "adxl.setInactivityXYZ(X, Y, Z);" (1 == ON, 0 == OFF)
  adxl.setTapDetectionOnXYZ(0, 0, 0); // Detect taps in the directions turned ON "adxl.setTapDetectionOnX(X, Y, Z);" (1 == ON, 0 == OFF)
  adxl.setInterruptMapping(ADXL345_INT_ACTIVITY_BIT,ADXL345_INT2_PIN);
  adxl.InactivityINT(0);
  adxl.ActivityINT(1);
  adxl.FreeFallINT(0);
  adxl.doubleTapINT(0);
  adxl.singleTapINT(0);


  float offYsum = 0;

  // 10 seconds worth of measurements (assuming 100 Hz)
  for (int j = 0; j <= 1000; j++)
  {
    // Accelerometer Readings
    int x,y,z;
    adxl.readAccel(&x, &y, &z);         // Read the accelerometer values and store them in variables declared above x,y,z
    float accY = 0.3065 * y;
    offYsum += accY;
  }

  gYoffset = offYsum/1000;
  pinMode(interruptPin,INPUT_PULLUP);//Set pin d2 to input using the builtin pullup resistor
  Serial.println("End of setup");

}

void loop()
{
  int x,y,z;
  adxl.getInterruptSource();
  adxl.readAccel(&x, &y, &z);         // Read the accelerometer values and store them in variables declared above x,y,z

  float accY = 0.3065 * y;
  ab_accY = abs(abs(accY) - abs(gYoffset));
  float measThresh = 3;

  if ( ab_accY <= measThresh)
  {
          sleepNow();
  }
  else
  {
    /* Continue */
  }

}

/* --------------------------HELPER FUNCTION----------------------------- */

int digPinToInterrupt(int digPin) {
  int intPin;
  if (digPin == 1){
      intPin = 3;
      // return intPin;
  }
  else if (digPin == 0)
  {
      intPin = 2;
      // return intPin;
  }
  else {

  }
  return intPin;
}
// ------------------------ INTERRUPT ROUTINES -------------------- //


void wakeUp(){
  if (is_asleep)
  {
    sleep_disable();
    detachInterrupt(digPinToInterrupt(interruptPin));
    digitalWrite(LED_BUILTIN, HIGH);
    isr_counter++;
    is_asleep = false;
  }
}

void sleepNow()
{
  delay(10);
  if (!is_asleep)
  {
    set_sleep_mode(SLEEP_MODE_PWR_DOWN);
    noInterrupts();
    sleep_enable();
    digitalWrite(LED_BUILTIN, LOW);
    is_asleep = true;
    attachInterrupt(digPinToInterrupt(interruptPin),wakeUp,HIGH);
    interrupts();
    sleep_cpu();
  }
}