#include <Adafruit_MPU6050.h>
#include <SPI.h>
#include "SdFat.h"
#include <ESP32Servo.h>

#define LED 2
#define Buzzer 4
#define SERVO_PIN 26          // ESP32 pin GPIO26 connected to servo motor
Servo servoMotor;

//Set time here
const float input_time = 2.2;      //Set the time in seconds for trigger the servo

//Set Acceleration value for timer start.
const float acceleration_timer_start = 15.00;

//Temporary varibale for time calculation
int set_time  = 0;
float temp_time = 0.000;

//Define SPI communication parameter of SD card.
#define SPI_SPEED SD_SCK_MHZ(4)
#define CS_PIN 5
SdFat sd;
File myFile;
bool data_record = false;
int enable = 0;

//For MPU6050 Accelerometer Sensor
Adafruit_MPU6050 mpu;
Adafruit_Sensor *mpu_temp, *mpu_accel, *mpu_gyro;

//This variables are used for timer interrupt
hw_timer_t *My_timer0 = NULL;
volatile bool flag = false;   // Initialize a flag variable to be toggled
bool temp_flag = false;
int count = -1;               //for time count
int z = 0;                    //for change detection
int y = 0;

//Application of Timer 0 Interrupt
void IRAM_ATTR Timer0_ISR() {
  temp_flag = flag;
  flag = !flag;
  count++;
  z = 0;
  y = 0;
}

//Set Timer 0 value 0.1 second
void Timer0_Control() {
  My_timer0 = timerBegin(0, 80, true);                    //For 1 sec, Timer No = 0, Prescaler = 80
  timerAttachInterrupt(My_timer0, &Timer0_ISR, true);
  timerAlarmWrite(My_timer0, 100000, true);              //Counter 100000 microseconds, 0.1 second
  //timerAlarmEnable(My_timer0);                           //Just Enable
}

//Initialize the SD card over SPI communication.
void initialize_sd_card() {
  Serial.println("\nInitializing SD card....................\n");
  if (!sd.begin(CS_PIN, SPI_SPEED)) {
    if (sd.card()->errorCode()) {
      Serial.println("SD card initialization failed.");
    } else if (sd.vol()->fatType() == 0) {
      Serial.println("Can't find a valid FAT16/FAT32 partition.");
    } else {
      Serial.println("Can't determine error type");
    }
    while (1) { //Added for stop here and continuously check.
      delay(10);
    }
    //return;
  } else {
    Serial.println("SD card found!\n");

    // Print SD card file details
    Serial.println("Files on SD card:");
    Serial.println("      Size    Name");
    sd.ls(LS_R | LS_SIZE);
    Serial.println();
  }
}

// Initialize the MPU6050 Sensor using I2C communication.
void initialize_MPU6050_Sensor() {
  Serial.println("\nInitializing MPU6050 Sensor..........\n");

  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 sensor");
    while (1) {	//Added for stop here and continuously check.
      delay(10);
    }
  } else {
    Serial.println("MPU6050 Found!");

    // Set Accelerometer range and print the details
    mpu.setAccelerometerRange(MPU6050_RANGE_16_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;
    }
  
    // Set Gyro range and print the details
    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;
    }

    // Set filter bandwidth and print the details
    mpu.setFilterBandwidth(MPU6050_BAND_21_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);

    // Read MPU6050 Specifications
    mpu_temp = mpu.getTemperatureSensor();
    mpu_temp->printSensorDetails();

    mpu_accel = mpu.getAccelerometerSensor();
    mpu_accel->printSensorDetails();

    mpu_gyro = mpu.getGyroSensor();
    mpu_gyro->printSensorDetails();
  }
}

sensors_event_t accel;
sensors_event_t gyro;
sensors_event_t temp;

//Read MPU 6050 data and store on SD card
void MPU_data() {
  mpu_temp->getEvent(&temp);
  mpu_accel->getEvent(&accel);
  mpu_gyro->getEvent(&gyro);

  Serial.println("Accelerometer Sensor data:");
  Serial.print("\t\tTemperature:  ");
  Serial.print(temp.temperature);
  Serial.println(" deg C");

  Serial.print("\t\tAccel X: ");
  Serial.print(accel.acceleration.x);
  Serial.print(" \tY: ");
  Serial.print(accel.acceleration.y);
  Serial.print(" \tZ: ");
  Serial.print(accel.acceleration.z);
  Serial.println(" m/s^2 ");

  Serial.print("\t\tGyro X: ");
  Serial.print(gyro.gyro.x);
  Serial.print(" \tY: ");
  Serial.print(gyro.gyro.y);
  Serial.print(" \tZ: ");
  Serial.print(gyro.gyro.z);
  Serial.println(" radians/s ");
  Serial.println();

  if (data_record == true) {
    // Data Logging to SD Card
    myFile = sd.open("Data.csv", FILE_WRITE);

    //Print Data.csv file size of SD card. (Every time)
    Serial.print("Data File size: ");
    Serial.print(myFile.size());
    Serial.println(" bytes");
    Serial.println();

    if (myFile) {
      if (myFile.size() == 0) {
        // If the file is empty, write the headers
        myFile.println("Time (sec), Temperature (C), Accel X, Accel Y, Accel Z, Gyro X, Gyro Y, Gyro Z");
        myFile.println("__________________________________________________________________");
        Serial.println("Data label written to sd card -------------------------------");
      }

      myFile.print(temp_time);                myFile.print(", ");
      myFile.print(temp.temperature);         myFile.print(", ");
      myFile.print(accel.acceleration.x);     myFile.print(", ");
      myFile.print(accel.acceleration.y);     myFile.print(", ");
      myFile.print(accel.acceleration.z);     myFile.print(", ");
      myFile.print(gyro.gyro.x);              myFile.print(", ");
      myFile.print(gyro.gyro.y);              myFile.print(", ");
      myFile.print(gyro.gyro.z);              myFile.print(", ");
      myFile.println();

      myFile.close();
      Serial.println("Data written to sd card -------------------------------------");
      Serial.println();
    } else {
      Serial.println("Error opening Data.csv for writing");
    }
  }
}

//This is set up code
void setup(void) {
  Serial.begin(115200);
  while (!Serial) { //Disbale this while loop when not connected with computer.
    delay(10); // will pause Zero, etc until serial console opens
  }

  pinMode(LED, OUTPUT);
  pinMode(Buzzer, OUTPUT);
  servoMotor.attach(SERVO_PIN);            // Attaches the servo on ESP32 pin

  // Initialize SD card
  initialize_sd_card();

  // Initialize MPU 6050 Sensor
  initialize_MPU6050_Sensor();

  // Set servo start position
  Serial.printf("Servo set default position..............................\n");
  servoMotor.write(0);
  Serial.printf("Waiting for trigger.........................\n");

  set_time = (input_time * 10);
  Timer0_Control();
}

//This is main loop
void loop() {
  if (enable == 0) {
    //Read MPU data continiously.
    data_record = false;
    MPU_data();
    Serial.printf("Waiting for trigger.........................\n");
  }

  if (accel.acceleration.x > acceleration_timer_start) {
    enable++;

    if (enable == 1) {
      Serial.printf("The trigger acceleration value = ");
      Serial.println(accel.acceleration.x);
      Serial.printf("Timer started.........................\n");
      timerAlarmEnable(My_timer0);    //Enable timer 0 here
    }
  }

  //This below part for timer interrupt operation (each 100ms)
  if (temp_flag != flag) {
    //Read and write data to sd card as per timer interrupt
    if (z == 0) {
      //Added for read MPU data and print on terminal
      //Serial.printf("Time = %u seconds\n", count);
      temp_time = count * 0.1;

      //Print live time on the serial monitor.
      Serial.printf("************************** Time = %.2f Second%s ******************************\n", temp_time, (count <= 10) ? "" : "s");

      //Read sensor data and store on SD card.
      data_record = true;
      MPU_data();

      // Print SD card all files details (every time)
      Serial.println("Files on SD card:");
      Serial.println("      Size    Name");
      sd.ls(LS_R | LS_SIZE);
      Serial.println();
    }

    if (z == 0 && count <= set_time) {
      digitalWrite(LED, !digitalRead(LED));
      digitalWrite(Buzzer, !digitalRead(Buzzer));
      //Serial.printf("Time = %u seconds\n", count);
      //Serial.printf("flag =  %u\n", flag);
      //Serial.printf("temp_flag =  %u\n", temp_flag);
    }
    z++;
  }

  //Perform the operation when the set time is matched with timer 0 time
  if (count == set_time && y == 0) {
    Serial.println("Servo rotate..............................................................\n");
    servoMotor.write(90);
    digitalWrite(Buzzer, LOW);
    digitalWrite(LED, LOW);
    y++;
  }
}