#include <Wire.h>
#include <MPU6050.h>
#include <math.h>

#define ULTRA_TRIGGER_PIN 7
#define ULTRA_ECHO_PIN 8
#define LED_DISTANCE_PIN 9
#define LED_ACCELERATION_PIN 10

MPU6050 mpu;

const float DISTANCE_THRESHOLD = 20.0;
const int ACCELERATION_THRESHOLD = 1500; 

void setup() {
  *(&DDRD) |= (1 << ULTRA_TRIGGER_PIN);
  
  *(&DDRB) &= ~(1 << ULTRA_ECHO_PIN);
  
  *(&DDRB) |= (1 << LED_DISTANCE_PIN) | (1 << LED_ACCELERATION_PIN);
  
  Serial.begin(9600);

  Wire.begin();
  mpu.initialize(); 
}

void loop() {
  *(&PORTD) &= ~(1 << ULTRA_TRIGGER_PIN);
  delayMicroseconds(2);
  *(&PORTD) |= (1 << ULTRA_TRIGGER_PIN);
  delayMicroseconds(10);
  *(&PORTD) &= ~(1 << ULTRA_TRIGGER_PIN);
  
  long duration = pulseIn(ULTRA_ECHO_PIN, HIGH);
  float distance = duration * 0.0343 / 2.0;
  
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);

 
  float totalAcc = sqrt(pow(ax, 2) + pow(ay, 2) + pow(az, 2));

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  Serial.print("Total Acceleration: ");
  Serial.println(totalAcc);
  if (distance < DISTANCE_THRESHOLD) {
    digitalWrite(LED_DISTANCE_PIN,HIGH);
    Serial.println("Distance LED ON");
  } else {
    digitalWrite(LED_DISTANCE_PIN,LOW);
    Serial.println("Distance LED OFF");
  }

  if (totalAcc > ACCELERATION_THRESHOLD) {
    digitalWrite(LED_ACCELERATION_PIN,HIGH);
    Serial.println("Acceleration LED ON");
  } else {
    digitalWrite(LED_ACCELERATION_PIN,LOW);
    Serial.println("Acceleration LED OFF");
  }
  
  delay(1000); 
}