#pragma once
#include <Wire.h>
namespace MotionSensor {
uint8_t address=0;
uint32_t polled=0,lastShake=0,lastProbe=0;
bool available=false;
bool fresh=false;
float axesG[3]={0,0,0};

bool read(uint8_t reg,uint8_t* bytes,uint8_t count) {
  Wire.beginTransmission(address);
  Wire.write(reg);

  if (Wire.endTransmission(false)!=0 || Wire.requestFrom(address,count)!=count) {

    return false;

  }

  for (uint8_t i=0;i<count;++i) bytes[i]=Wire.read();
  return true;
}

bool write(uint8_t reg,uint8_t value) {
  Wire.beginTransmission(address);Wire.write(reg);Wire.write(value);
  return Wire.endTransmission()==0;
}

void begin() {
  available=false;

  for (uint8_t candidate : {uint8_t(0x6A),uint8_t(0x6B)}) {

    address=candidate;uint8_t id=0;

    if (read(0x0F,&id,1) && id==0x69) {

      available=write(0x12,0x44) && write(0x10,0x40);
      return;

    }

  }
}

bool shake(uint32_t now) {
  fresh=false;

  if (!available) {

    if (now-lastProbe>=5000) {

      lastProbe=now;begin();

    }

    return false;

  }

  if (now-polled<20) {

    return false;

  }

  polled=now;uint8_t bytes[6];

  if (!read(0x28,bytes,6)) {

    available=false;
    return false;

  }

  float magnitude=0;

  for (uint8_t i=0;i<3;++i) {

    int16_t raw=int16_t(uint16_t(bytes[2*i]) | uint16_t(bytes[2*i+1])<<8);
    float g=raw*0.000061f;axesG[i]=g;magnitude+=g*g;

  }

  fresh=true;

  if (magnitude>2.56f && now-lastShake>=1500) {

    lastShake=now;
    return true;

  }

  return false;
}
}