#ifndef MOTION_H
#define MOTION_H
#include "States.h"

/*
 * ==========================================================
 * Focus Pebble V1.0
 * Motion Module
 *
 * Reads MPU6500 / MPU6050
 * Calculates movement
 * Filters sensor noise
 * Classifies motion
 * ==========================================================
 */

#include <Arduino.h>
#include <Wire.h>

#include "Config.h"

class Motion {
public:

  Motion();

  bool begin();

  void update();

  // Motion State
  MotionState getState();

  bool isStill();

  bool isPickup();

  bool isShake();

  bool isNormal();

  // Movement
  long getMovement();

  long getFilteredMovement();

  // Raw Accelerometer Values
  int16_t getX();

  int16_t getY();

  int16_t getZ();

private:

  bool readRawData();

  bool validReading(
    int16_t x,
    int16_t y,
    int16_t z);

  void calculateMovement();

  void classifyMotion();

private:

  //-------------------------------------------------
  // Raw Accelerometer Values
  //-------------------------------------------------

  int16_t ax;
  int16_t ay;
  int16_t az;

  //-------------------------------------------------
  // Previous Reading
  //-------------------------------------------------

  int16_t lastAx;
  int16_t lastAy;
  int16_t lastAz;

  //-------------------------------------------------
  // First Sample Flag
  //-------------------------------------------------

  bool firstReading;

  //-------------------------------------------------
  // Motion Calculation
  //-------------------------------------------------

  long movement;

  long filteredMovement;

  //-------------------------------------------------
  // Current Motion State
  //-------------------------------------------------

  MotionState currentState;
};

#endif