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

#define FREQUENCY_HZ 100
#define INTERVAL_MS (1000 / (FREQUENCY_HZ + 1))
#define NUM_SAMPLES 100 // จำนวนตัวอย่างที่ต้องการ (อันนี้ไม่ใช้ เวลารัน mcu อ่าน โค้ตแล้วมันดีเล 3 มิลิวินาที)
const int buttonPin = 18; // กำหนดหมายเลขขาของปุ่ม ต่อแบบ pull down (ไม่ต่อไฟเป็น 0 กดสวิทเป็น 1)
const int ledPin = 5;   // กำหนดหมายเลขขาของ LED เอาไว้ดูค่าตอน ขา สวิทย์ เป็น 1 หรือ 0
int buttonState = 0;    // เก็บสถานะปุ่ม

// สร้างอ็อบเจ็กต์ของคลาส Adafruit_MPU6050
Adafruit_MPU6050 mpu;
static unsigned long last_interval_ms = 0;
unsigned long start_time_ms = 0;
int count_samples = 0;

void setup() {
  pinMode(buttonPin, INPUT); // ตั้งค่าขาปุ่มเป็น INPUT
  pinMode(ledPin, OUTPUT);
  Serial.begin(115200);

  // Try to initialize!
  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_10_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;
  }
  count_samples = 0;
  Serial.println("");
  delay(100);
}

void loop() {
  // อ่านสถานะปุ่ม
  buttonState = digitalRead(buttonPin);

  if (buttonState == HIGH && start_time_ms == 0) { // ถ้าปุ่มถูกกดและยังไม่เริ่มจับเวลา
    digitalWrite(ledPin, HIGH); // เปิด LED
    Serial.println("timestamp,accX,accY,accZ"); // ตั้ง label ให้ โปรแกรมอ่าน
    start_time_ms = millis(); // เริ่มจับเวลา
  }

  if (start_time_ms != 0) {
    unsigned long current_time_ms = millis();
    if (count_samples < 101 ) { // ถ้าผ่านไปไม่เกิน 1 วินาที และยังไม่ครบ 100 ตัวอย่าง
      sensors_event_t a, g, temp;
      mpu.getEvent(&a, &g, &temp);
      int time = count_samples*10;

      //Serial.print(current_time_ms - start_time_ms); // พิมพ์ timestamp ใช้ปรับให้ตรง
      Serial.print(time);
      Serial.print(",");
      Serial.print(a.acceleration.x);
      Serial.print(",");
      Serial.print(a.acceleration.y);
      Serial.print(",");
      Serial.println(a.acceleration.z);
      //Serial.print(",");
      //Serial.println(count_samples);
      count_samples++;
      
      //Serial.println(time);
      delay(7);
    } else {
      start_time_ms = 0; // หยุดจับเวลาเมื่อผ่านไป 1 วินาที หรือครบ 200 ตัวอย่าง
      digitalWrite(ledPin, LOW); // ปิด LED
      Serial.println();
      count_samples = 0; // รีเซ็ตตัวนับตัวอย่าง
    }
  }
}