#include <TinyWireM.h>
#include <ssd1306xled.h>

const int mpuAddress = 0x68;
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;

void mpuBegin(){
  TinyWireM.begin();
  TinyWireM.beginTransmission(mpuAddress);
  TinyWireM.write(mpuAddress);
  TinyWireM.write(0);
  auto error = TinyWireM.endTransmission();
}

void mpuMeasure(){
  TinyWireM.beginTransmission(mpuAddress);
  TinyWireM.write(0x3B);                   // Starting with register 0x3B (ACCEL_XOUT_H)
  TinyWireM.endTransmission(false);

  //int16_t GyX,AcX, GyY, GyZ, AcY, AcZ, Tmp;

  TinyWireM.requestFrom( mpuAddress, 14);   // request a total of 14 bytes
  AcX = TinyWireM.read()<<8 | TinyWireM.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY = TinyWireM.read()<<8 | TinyWireM.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = TinyWireM.read()<<8 | TinyWireM.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = TinyWireM.read()<<8 | TinyWireM.read();  // 0x41 (TEMP_OUT_H)   & 0x42 (TEMP_OUT_L)
  GyX = TinyWireM.read()<<8 | TinyWireM.read();  // 0x43 (GYRO_XOUT_H)  & 0x44 (GYRO_XOUT_L)
  GyY = TinyWireM.read()<<8 | TinyWireM.read();  // 0x45 (GYRO_YOUT_H)  & 0x46 (GYRO_YOUT_L)
  GyZ = TinyWireM.read()<<8 | TinyWireM.read();  // 0x47 (GYRO_ZOUT_H)  & 0x48 (GYRO_ZOUT_L)
}
/*
char int_to_char(int16_t val){
  int int_val = constrain(map(val, -32768, 32767, -100, 100), -100, 100);
  String val_string = String(int_val);
  char val_char[val_string.length() + 1];
  strcpy(val_char, val_string.c_str());
  return val_char;
}
*/
char* int_to_char(int16_t val){
  int int_val = constrain(map(val, -32768, 32767, -100, 100), -100, 100);
  String val_string = String(int_val);
  char* val_char = (char*)malloc(val_string.length() + 1);
  if (val_char != NULL) {
    strcpy(val_char, val_string.c_str());
  }
  return val_char;
}

char string_to_char(String label){
  char label_char[label.length() + 1];
  strcpy(label_char, label.c_str());
  return label_char;
}

void display(int row, int col, String label, int val) {
  SSD1306.ssd1306_setpos(row, col);
  SSD1306.ssd1306_string_font6x8(string_to_char(label));
  SSD1306.ssd1306_string_font6x8(int_to_char(val));
}

void setup() {
  mpuBegin();
  SSD1306.ssd1306_init();
  SSD1306.ssd1306_fillscreen(0);
}

void loop() {
  SSD1306.ssd1306_fillscreen(0);
  mpuMeasure();
  
  SSD1306.ssd1306_setpos(0, 0);
  String val_string = String(AcX);
  char val_char[val_string.length() + 1];
  strcpy(val_char, val_string.c_str());
  SSD1306.ssd1306_string_font6x8(val_char);
  /*
  display(0, 0, "AcX : ", AcX);
  display(0, 2, "AcY : ", AcY);
  display(0, 4, "AcZ : ", AcZ);
  display(64, 0, "GyX : ", GyX);
  display(64, 2, "GyY : ", GyY);
  display(64, 4, "GyZ : ", GyZ);
  */
  delay(100);
}