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

// --- HC-SR04 Pins ---
const int trigPin = 9;
const int echoPin = 10;
float duration, distance;

// --- PCA9546A Multiplexer ---
#define PCA9546A_ADDR 0x70
#define CH_HAPTIC     0x02  // Channel 1
#define CH_MPU6050    0x04  // Channel 2

MPU6050 mpu;
Adafruit_DRV2605 drv;

void selectChannel(uint8_t channel) {
  Wire.beginTransmission(PCA9546A_ADDR);
  Wire.write(channel);
  Wire.endTransmission();
  delay(10);
}

void resetEchoPin() {
  pinMode(echoPin, OUTPUT);
  digitalWrite(echoPin, LOW);
  delay(1);
  pinMode(echoPin, INPUT);
}

void setup() {
  Serial.begin(9600);
  while (!Serial);

  Wire.begin();

  // HC-SR04 setup
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // MPU6050 Init
  selectChannel(CH_MPU6050);
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("❌ MPU6050 not found.");
    while (1);
  }
  Serial.println("✅ MPU6050 connected.");

  // DRV2605 Init
  selectChannel(CH_HAPTIC);
  if (!drv.begin()) {
    Serial.println("❌ DRV2605 not found.");
    while (1);
  }
  drv.selectLibrary(1);
  drv.setMode(DRV2605_MODE_INTTRIG);
  Serial.println("✅ DRV2605 ready.");
}

void vibrateClicks(int clicks, int delayBetween = 150) {
  selectChannel(CH_HAPTIC);
  for (int i = 0; i < clicks; i++) {
    drv.setWaveform(0, 1); // "Strong click" preset
    drv.setWaveform(1, 0); // end sequence
    drv.go();
    delay(100);             // korte klik duur
    delay(delayBetween);    // pauze tussen klikjes
  }
}

void loop() {
  // Lees MPU6050 data
  selectChannel(CH_MPU6050);
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);

  float ax_f = ax / 16384.0;
  float ay_f = ay / 16384.0;
  float az_f = az / 16384.0;

  float pitch = atan2(ax_f, sqrt(ay_f * ay_f + az_f * az_f)) * 180 / PI;
  float roll  = atan2(ay_f, sqrt(ax_f * ax_f + az_f * az_f)) * 180 / PI;

  Serial.print("📐 Pitch: ");
  Serial.print(pitch, 2);
  Serial.print("°, Roll: ");
  Serial.print(roll, 2);
  Serial.println("°");

  // Lees afstand HC-SR04
  resetEchoPin();
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH, 30000);
  if (duration == 0) {
    Serial.println("⚠️ No echo received");
    distance = -1;
  } else {
    distance = duration * 0.0343 / 2.0;
    Serial.print("📏 Distance: ");
    Serial.print(distance);
    Serial.println(" cm");
  }

  // Trigger haptische feedback voor afstand (zoals voorheen)
  if (distance > 0 && distance < 8) {
    Serial.println("💥 Warning: adjust depth!");
    vibrateClicks(1);  // 1 klik voor afstand waarschuwing
  }

  // Trigger haptische feedback voor hoek met meerdere clicks afhankelijk van foutgrootte
  if (abs(roll) > 15 || abs(pitch) > 15) {
    int clicks = 1;
    if (abs(roll) > 45 || abs(pitch) > 45) {
      clicks = 3;
    } else if (abs(roll) > 30 || abs(pitch) > 30) {
      clicks = 2;
    }
    Serial.print("💥 Warning: adjust angle! Clicks: ");
    Serial.println(clicks);
    vibrateClicks(clicks);
  }

  delay(1000);
}