/*
  test code for Accl Gyro Temp Mag sensor
  SparkFun IMU Breakout - MPU-9250(+AK8963)
  I2C connected to A4(SDA)&A5(SCL)

  need 5.0V-3.3V voltage matching 
*/

#include <Wire.h>

#define PinLED 13
#define I2CAG 0x68
#define I2CM  0x0c
#define I2CTIMEOUTms  20
#define I2CTIMEOUTnum  5

volatile int led=0;
unsigned long t;

#define BUFSZ 32
volatile uint8_t buf[32];

void HexPrint(int v, int bw, int s){
  char line[16];
  int x,l=0;
  if (s) {
    if ((1<<(bw-1))&v) {
      line[l++]='-';
      v=-v;
    } else {
      line[l++]='+';
    }
    bw=bw-1;
  } else {
    line[l++]=' ';
  }
  if (bw%4) bw=bw-(bw%4)+4;
  for (int i=bw-4;i>=0;i-=4) {
    x=(v>>i)&15;
    line[l++]=(x<10)?x+'0':x+'a'-10;
  }
  line[l]=0;
  Serial.print(line);
}

uint8_t I2CRead(int dadr, int radr){
  int t=I2CTIMEOUTnum;
  Wire.beginTransmission(dadr);
  delayMicroseconds(50);
  Wire.write(radr);
  Wire.endTransmission(false);
  Wire.requestFrom(dadr,1);
  while (!Wire.available()) {
    Serial.print(".");
    delay(I2CTIMEOUTms);
    if (t--<=0) return 0xff;
  }
  return Wire.read();
}

int I2CReadN(int dadr, int radr, int n){
  int i=0,t=I2CTIMEOUTnum;
  if (n<=0) goto _RETURN;
  if (n>=BUFSZ) goto _RETURN;
  Wire.beginTransmission(dadr);
  Wire.write(radr);
  Wire.endTransmission(false);
  Wire.requestFrom(dadr,n);
  while (i<n) {
    while (!Wire.available()) {
      Serial.print(".");
      delay(I2CTIMEOUTms);
      if (t--<=0) goto _RETURN;
    }
    buf[i]=Wire.read();
    i++;
  }
  _RETURN:
  buf[i]=0;
  return i;
}

void I2CWrite(int dadr, int radr, uint8_t val){
  Wire.beginTransmission(dadr);
  Wire.write(radr);
  Wire.write(val);
  Wire.endTransmission();
}

void I2CPrint(int dadr, int radr, char *msg){
  int v;
  Serial.print(msg);
  v=I2CRead(dadr,radr);
  Serial.println(v,HEX);
}

void setup() {
  pinMode(PinLED, OUTPUT);
  Serial.begin(115200);
  Serial.println("Hello, world!");
  
  Wire.begin();

  I2CPrint(I2CAG,0x75,"75 Who am I  ");
  I2CPrint(I2CAG,0x6A,"6A User Ctrl ");
  I2CPrint(I2CAG,0x6B,"6B Pwr Mgmt1 ");
  I2CPrint(I2CAG,0x6C,"6C Pwr Mgmt2 ");
  I2CPrint(I2CAG,0x1A,"1A Config    ");
  I2CPrint(I2CAG,0x1B,"1B Gyro Cnf  ");
  I2CPrint(I2CAG,0x1C,"1C Accl Cnf  ");
  I2CPrint(I2CAG,0x1D,"1D Accl Cnf2 ");
  I2CPrint(I2CAG,0x37,"37 IntPinCfg ");
  I2CPrint(I2CM, 0x0a,"0A Mag Ctrl1 ");

  I2CWrite(I2CAG,0x6B,0); // PwrMgmt1:wakeup
  I2CWrite(I2CAG,0x1B,0x18); // GyroCnf:2000dps,F=0
  I2CWrite(I2CAG,0x1C,0x18); // AcclCnf:16G
  I2CWrite(I2CAG,0x37,0x02); // IntPinCfg:Bypass for Magnetic Sensor
  I2CWrite(I2CM, 0x0a,0x16); // MagCtrl1:100Hz
  Serial.println("end setup");
}

void loop() {
  int v,i,n;
  int16_t w;

  led=~led;
  digitalWrite(PinLED,led);
  delay(2000);

  t=millis();
  Serial.print("time=");
  Serial.print(t);
  Serial.print(" ");
  
  // Acceleration(XYZ), Temperature, AngularVelocity(XYZ)
  n=I2CReadN(I2CAG,0x3B,14);
  Serial.print("Sens ");
  for (i=0;i<n;i+=2) {
    w=(buf[i]<<8)|buf[i+1];
    HexPrint(w,16,1);
    Serial.print(" ");
  }
  // Magnetic(XYZ)
  v=I2CRead(I2CM,0x02);//trig for mag sensor
  n=I2CReadN(I2CM,0x03,7);
  Serial.print("| ");
  for (i=0;i<n;i+=2) {
    w=(buf[i+1]<<8)|buf[i];
    HexPrint(w,16,1);
    Serial.print(" ");
  }
  v=analogRead(A0);
  HexPrint(v,10,0);
  Serial.println();

}