#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_MPU6050.h>
#include <esp_now.h>
#include <WiFi.h>

Adafruit_MPU6050 mpu;

int16_t ax, ay, az;
float wave_height, sendWave;

//ESP NOW
uint8_t broadcastAddress[] = {0x24, 0x0A, 0xC4, 0x00, 0x01, 0x00};

typedef struct struct_message {
  float waveHeight;
} struct_message;

struct_message myData;

esp_now_peer_info_t peerInfo;




void setup() {
  Serial.begin(115200);

  WiFi.mode(WIFI_STA);

  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }

  //esp_now_register_send_cb(OnDataSent);

  // Register peer
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;

  // Add peer
  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add peer");
    return;
  }
  while (!Serial)
    delay(10);

  Serial.println("Adafruit MPU6050 test!");

  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
    case MPU6050_RANGE_2_G:
      Serial.println("+-2G");
      break;
    case MPU6050_RANGE_4_G:
      Serial.println("+-4G");
      break;
    case MPU6050_RANGE_8_G:
      Serial.println("+-8G");
      break;
    case MPU6050_RANGE_16_G:
      Serial.println("+-16G");
      break;
  }
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
    case MPU6050_RANGE_250_DEG:
      Serial.println("+- 250 deg/s");
      break;
    case MPU6050_RANGE_500_DEG:
      Serial.println("+- 500 deg/s");
      break;
    case MPU6050_RANGE_1000_DEG:
      Serial.println("+- 1000 deg/s");
      break;
    case MPU6050_RANGE_2000_DEG:
      Serial.println("+- 2000 deg/s");
      break;
  }

  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) {
    case MPU6050_BAND_260_HZ:
      Serial.println("260 Hz");
      break;
    case MPU6050_BAND_184_HZ:
      Serial.println("184 Hz");
      break;
    case MPU6050_BAND_94_HZ:
      Serial.println("94 Hz");
      break;
    case MPU6050_BAND_44_HZ:
      Serial.println("44 Hz");
      break;
    case MPU6050_BAND_21_HZ:
      Serial.println("21 Hz");
      break;
    case MPU6050_BAND_10_HZ:
      Serial.println("10 Hz");
      break;
    case MPU6050_BAND_5_HZ:
      Serial.println("5 Hz");
      break;
  }

  Serial.println("");
  delay(100);
}

void loop() {
  /* Get new sensor events with the readings */

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  float ax = a.acceleration.x ; 
  float ay = a.acceleration.y ;
  float az = a.acceleration.z ;

  float gyX = g.gyro.x ; 
  float gyY = g.gyro.y ;
  float gyZ = g.gyro.z ;

  // Hitung magnitudo akselerasi
  double acceleration_magnitude = sqrt(ax * ax + ay * ay + az * az);

  double PGA = acceleration_magnitude * 8; 

  // Kecepatan sudut
  double kecepatanSudut = sqrt(gyX * gyX + gyY * gyY + gyZ * gyZ) * 0.0175; // Konversi ke rad/sec

  // Tinggi gelombang
  double tinggiGelombang = acceleration_magnitude / kecepatanSudut * kecepatanSudut ;

  Serial.print("akselerasi = ");
  Serial.println(acceleration_magnitude);
  Serial.print("Kecepatan sudut = ");
  Serial.println(kecepatanSudut);
  Serial.print("tinggi gelombang = ");
  Serial.println(tinggiGelombang);
  Serial.print("PGA = ");
  Serial.println(PGA);

  Serial.println( __TIME__;);

  // // Perkiraan akselerasi vertikal (akibat gelombang)
  // double vertical_acceleration_estimate = acceleration_magnitude - abs(az);

  // // Tentukan puncak dan lembah gelombang
  // static double last_acceleration = 0.0;
  // static bool is_crest = false;
  // static bool is_trough = false;

  // if (vertical_acceleration_estimate > last_acceleration && !is_crest) {
  //   is_crest = true;
  //   Serial.println("Crest detected");
  // } else if (vertical_acceleration_estimate < last_acceleration && is_crest) {
  //   is_trough = true;
  //   Serial.println("Trough detected");

  //   // Hitung tinggi gelombang
  //   if (is_trough && is_crest) {
  //     wave_height = vertical_acceleration_estimate - last_acceleration;
  //     Serial.print("Wave Height: ");
  //     Serial.print(abs(wave_height));
  //     Serial.println(" m");
  //     is_trough = false;
  //     is_crest = false;
  //   }
  // }

  // last_acceleration = vertical_acceleration_estimate;

  // Serial.print("Acceleration X: ");
  // Serial.print(a.acceleration.x);
  // Serial.print(", Y: ");
  // Serial.print(a.acceleration.y);
  // Serial.print(", Z: ");
  // Serial.print(a.acceleration.z);
  // Serial.println(" m/s^2");

  delay(2000);
}