// Code for Accelerometer

#include <Adafruit_Sensor.h>
#include "Adafruit_LIS3DH.h"

// Used for software SPI
#define LIS3DH_CLK 13
#define LIS3DH_MISO 12
#define LIS3DH_MOSI 11
// Used for hardware & software SPI
#define LIS3DH_CS 10

int buttonPin = D3;

float ax, ay, az;
int counter = 0;
int bufferCounter = 0;
int step = 0;
int step_prev = 0;
float tempL = 0;
float temp2L = 0;

int press_num = 0;

// Parameters
int frameSize = 200;
const int bufferSize = 10;
// int minGlitch = 6;
int minGlitch = 8;
int maxGlitch = 300;


float last_az = 0.0;
int glitch_count = 0;
int loop_count = 0;
int level = 0;

float t;
float buffer[bufferSize];

Adafruit_LIS3DH lis = Adafruit_LIS3DH();

void setup(void) {

  Serial.begin(9600);
  Serial.println("LIS3DH test!");

  if (! lis.begin(0x19)) {
    Serial.println("Couldnt start");
    while (1);
  }
  Serial.println("LIS3DH found!");

  lis.setRange(LIS3DH_RANGE_4_G);

  Serial.print("Range = "); Serial.print(2 << lis.getRange());
  Serial.println("G");
  
  pinMode( buttonPin , INPUT_PULLUP);
  
}

void loop() {

  int buttonState = digitalRead( buttonPin );

  if( buttonState == LOW )
  {
    step = 0;
  }
    
    
  lis.read();
  sensors_event_t event;
  lis.getEvent(&event);
  
  ax = event.acceleration.x * 9.8;
  ay = event.acceleration.y * 9.8;
  az = event.acceleration.z * 9.8;

  bufferCounter++;
  if (bufferCounter <= bufferSize) {
    float value = (ax * ax + ay * ay + az * az - 96.04);
    buffer[bufferCounter - 1] = value;
  }
  else {
    float processed_data = 0;   
    
    // Mid Filter
    processed_data = buffer[bufferSize/2];
    
    counter++;
    
    if (temp2L < tempL && tempL > processed_data && tempL > minGlitch && tempL < maxGlitch) {
      step++;
    }
    temp2L = tempL;
    tempL = processed_data;
    memset(buffer, 0, sizeof(buffer));
    bufferCounter = 0;
  }


  if(abs(event.acceleration.z - 1.0) > 0.3) glitch_count++;
  
  int val = abs(last_az - event.acceleration.z) > 0.5 ? 30 : 60;
  
  int threshold = 192;
  
  if(loop_count > threshold) {
      if(glitch_count > threshold / 2) {
          level = 5;
      }
      else if(glitch_count > threshold / 8) {
          level = 3;
      }
      else if(glitch_count > 1) {
          level = 1;
      }
      else {
          level = 0;
      }
      
      if(step - step_prev > 0 && level == 0) {
          level = 1;
          step_prev = step;
      }
      
      loop_count = 0;
      glitch_count = 0;
      Particle.publish( "doPairedPublish", String::format("%d,%d", level, step));
  }

  Serial.print(step);
  Serial.println();
  
  last_az = event.acceleration.z;

  delay(5);
  loop_count++;
}