#include "Wire.h"

const int MPU_addr = 0x68; // I2C address of the MPU6050
int16_t data[7];  // [accX, accY, accZ, temp, gyrX, gyrY, gyrZ]
const int ledPin = 6;      // LED pin used to simulate vibration feedback

void setup() {
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B); // Power management register
  Wire.write(0);    // Wake up sensor
  Wire.endTransmission(true);

  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, LOW);  // Ensure LED is off initially

  Serial.begin(9600);
}

void loop() {
  getData();

  float accZ_g = data[2] / 16384.0;

  // Print raw sensor data
  for (byte i = 0; i < 7; i++) {
    Serial.print(data[i]);
    Serial.print('\t');
  }

  // Print acceleration in g
  Serial.print(" | accZ (g): ");
  Serial.print(accZ_g);

  // Jump detection (WILL CALIBRATE)
  if (accZ_g > 1.8) {
    Serial.print(" <-- Jump detected!");
  }

  // Block contact detection (strong impact upward)
  // WILL RECEIVE MARIO's CONTACT INFO FROM PC
  if (accZ_g > 3.0) {
    Serial.print(" <-- Block hit! LED ON");
    digitalWrite(ledPin, HIGH);  // Turn LED on (simulate vibration)
    delay(300);                  // Duration of feedback
    digitalWrite(ledPin, LOW);   // Turn LED off
  }

  Serial.println();
  delay(200);
}

void getData() {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // Starting register for accel data
  Wire.endTransmission(false);  // Repeated start
  Wire.requestFrom(MPU_addr, 14, true);  // Request 14 bytes

  for (byte i = 0; i < 7; i++) {
    data[i] = Wire.read() << 8 | Wire.read();  // Combine high and low byte
  }
}