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

#include "Motion.h"

Motion::Motion() {
  ax = ay = az = 0;

  lastAx = lastAy = lastAz = 0;

  movement = 0;
  filteredMovement = 0;

  firstReading = true;

  currentState = MOTION_STILL;
}

////////////////////////////////////////////////////////////

bool Motion::begin() {
  Wire.begin(SDA_PIN, SCL_PIN);

  // More stable for MPU6500 clones
  Wire.setClock(100000);

  // Wake sensor
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);
  Wire.write(0x00);

  if (Wire.endTransmission() != 0) {
    LOG("MOTION", "Sensor Not Found");
    return false;
  }

  LOG("MOTION", "Sensor Ready");

  return true;
}

////////////////////////////////////////////////////////////

void Motion::update() {
  if (!readRawData())
    return;

  calculateMovement();

  classifyMotion();
}

////////////////////////////////////////////////////////////

bool Motion::readRawData() {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);

  if (Wire.endTransmission(false) != 0) {
    LOG("MOTION", "I2C Write Error");
    return false;
  }

  if (Wire.requestFrom((uint8_t)MPU_ADDR, (uint8_t)6) != 6) {
    LOG("MOTION", "I2C Read Error");
    return false;
  }

  int16_t x =
    (Wire.read() << 8) | Wire.read();

  int16_t y =
    (Wire.read() << 8) | Wire.read();

  int16_t z =
    (Wire.read() << 8) | Wire.read();

  if (!validReading(x, y, z)) {
    LOG("MOTION", "Invalid Reading Ignored");
    return false;
  }

  ax = x;
  ay = y;
  az = z;

  return true;
}

////////////////////////////////////////////////////////////

bool Motion::validReading(
  int16_t x,
  int16_t y,
  int16_t z) {
  if (abs(x) >= INVALID_LIMIT)
    return false;

  if (abs(y) >= INVALID_LIMIT)
    return false;

  if (abs(z) >= INVALID_LIMIT)
    return false;

  return true;
}

////////////////////////////////////////////////////////////

void Motion::calculateMovement() {
  if (firstReading) {
    lastAx = ax;
    lastAy = ay;
    lastAz = az;

    firstReading = false;

    movement = 0;
    filteredMovement = 0;

    return;
  }

  long rawMovement =
    abs(ax - lastAx) + abs(ay - lastAy) + abs(az - lastAz);

  // Exponential Moving Average
  filteredMovement =
    (filteredMovement * 3 + rawMovement) / 4;

  movement = filteredMovement;

  lastAx = ax;
  lastAy = ay;
  lastAz = az;
}

////////////////////////////////////////////////////////////

void Motion::classifyMotion() {
  MotionState previousState = currentState;

  if (movement < STILL_THRESHOLD) {
    currentState = MOTION_STILL;
  } else if (movement < NORMAL_THRESHOLD) {
    currentState = MOTION_NORMAL;
  } else if (movement < PICKUP_THRESHOLD) {
    currentState = MOTION_NORMAL;
  } else if (movement < SHAKE_THRESHOLD) {
    currentState = MOTION_PICKUP;
  } else {
    currentState = MOTION_SHAKE;
  }

  // Only log when state changes
  if (previousState != currentState) {
    LOG_VALUE("MOTION", "Movement", movement);

    switch (currentState) {
      case MOTION_STILL:
        LOG("MOTION", "STILL");
        break;

      case MOTION_NORMAL:
        LOG("MOTION", "NORMAL");
        break;

      case MOTION_PICKUP:
        LOG("MOTION", "PICKUP");
        break;

      case MOTION_SHAKE:
        LOG("MOTION", "SHAKE");
        break;
    }
  }
}

////////////////////////////////////////////////////////////

MotionState Motion::getState() {
  return currentState;
}

////////////////////////////////////////////////////////////

long Motion::getMovement() {
  return movement;
}

////////////////////////////////////////////////////////////

long Motion::getFilteredMovement() {
  return filteredMovement;
}

////////////////////////////////////////////////////////////

bool Motion::isStill() {
  return currentState == MOTION_STILL;
}

////////////////////////////////////////////////////////////

bool Motion::isNormal() {
  return currentState == MOTION_NORMAL;
}

////////////////////////////////////////////////////////////

bool Motion::isPickup() {
  return currentState == MOTION_PICKUP;
}

////////////////////////////////////////////////////////////

bool Motion::isShake() {
  return currentState == MOTION_SHAKE;
}

////////////////////////////////////////////////////////////

int16_t Motion::getX() {
  return ax;
}

////////////////////////////////////////////////////////////

int16_t Motion::getY() {
  return ay;
}

////////////////////////////////////////////////////////////

int16_t Motion::getZ() {
  return az;
}