#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <NewPing.h>
#include <SD.h>
#include <SPI.h>

// Constants for the ultrasonic sensor
#define TRIG_PIN 7
#define ECHO_PIN 6
#define MAX_DISTANCE 400  // Maximum distance we want to ping for (in centimeters)
#define SD_CS_PIN 10

// Create an instance of the MPU6050 and ultrasonic sensor
Adafruit_MPU6050 mpu;
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);

// Calibration factor for the ultrasonic sensor
const float calibrationFactor = 1.0;  // Adjust this value based on your calibration

// Moving average filter parameters
const int numReadings = 10;
float readings[numReadings];
int readIndex = 0;
float total = 0;
float average = 0;

float accelerationX, accelerationY, accelerationZ;
float velocityX, velocityY, velocityZ;
float positionX, positionY, positionZ;
float lastAccelerationX, lastAccelerationY, lastAccelerationZ;
float lastVelocityX, lastVelocityY, lastVelocityZ;
float angleX, angleY, angleZ;
unsigned long lastTime, startTime;

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

  // Initialize SD card
  if (!SD.begin(SD_CS_PIN)) {
    Serial.println("Card failed, or not present");
    while (1);
  }
  Serial.println("Card initialized.");

  // Initialize variables
  accelerationX = accelerationY = accelerationZ = 0.0;
  velocityX = velocityY = velocityZ = 0.0;
  positionX = positionY = positionZ = 0.0;
  lastAccelerationX = lastAccelerationY = lastAccelerationZ = 0.0;
  lastVelocityX = lastVelocityY = lastVelocityZ = 0.0;
  angleX = angleY = angleZ = 0.0;
  lastTime = startTime = millis();

  // Initialize filter readings
  for (int thisReading = 0; thisReading < numReadings; thisReading++) {
    readings[thisReading] = 0;
  }
}

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

  // Convert raw acceleration to m/s^2
  accelerationX = a.acceleration.x;
  accelerationY = a.acceleration.y;
  accelerationZ = a.acceleration.z;

  // Convert gyroscope data to degrees per second
  float gyroX = g.gyro.x * 180 / PI;
  float gyroY = g.gyro.y * 180 / PI;
  float gyroZ = g.gyro.z * 180 / PI;

  // Get current time and calculate deltaTime
  unsigned long currentTime = millis();
  float deltaTime = (currentTime - lastTime) / 1000.0; // convert to seconds
  lastTime = currentTime;

  // Numerical integration using trapezoidal rule to get velocity
  velocityX += ((accelerationX + lastAccelerationX) / 2) * deltaTime;
  velocityY += ((accelerationY + lastAccelerationY) / 2) * deltaTime;
  velocityZ += ((accelerationZ + lastAccelerationZ) / 2) * deltaTime;

  // Numerical integration using trapezoidal rule to get position
  positionX += ((velocityX + lastVelocityX) / 2) * deltaTime;
  positionY += ((velocityY + lastVelocityY) / 2) * deltaTime;
  positionZ += ((velocityZ + lastVelocityZ) / 2) * deltaTime;

  // Numerical integration to get angles in degrees
  angleX += gyroX * deltaTime;
  angleY += gyroY * deltaTime;
  angleZ += gyroZ * deltaTime;

  // Update last acceleration and velocity
  lastAccelerationX = accelerationX;
  lastAccelerationY = accelerationY;
  lastAccelerationZ = accelerationZ;
  lastVelocityX = velocityX;
  lastVelocityY = velocityY;
  lastVelocityZ = velocityZ;

  // Get the temperature from the MPU6050 (in degrees Celsius)
  float temperature = temp.temperature;

  // Calculate the speed of sound based on the temperature
  float speedOfSound = 331.4 + 0.6 * temperature; // in m/s

  // Get the distance reading from the ultrasonic sensor
  unsigned int duration = sonar.ping();
  float distance = (duration / 2.0) * (speedOfSound / 10000.0); // convert to cm

  // Apply calibration factor
  distance *= calibrationFactor;

  // Apply moving average filter
  total = total - readings[readIndex];
  readings[readIndex] = distance;
  total = total + readings[readIndex];
  readIndex = (readIndex + 1) % numReadings;
  average = total / numReadings;

  // Calculate total elapsed time
  float elapsedTime = (millis() - startTime) / 1000.0;

  // Print position and angle data along with ultrasonic sensor reading
  Serial.print("Elapsed Time: ");
  Serial.print(elapsedTime);
  Serial.print(" s, Position X: ");
  Serial.print(positionX);
  Serial.print(", Position Y: ");
  Serial.print(positionY);
  Serial.print(", Position Z: ");
  Serial.print(positionZ);
  Serial.print(", Angle X: ");
  Serial.print(angleX);
  Serial.print(", Angle Y: ");
  Serial.print(angleY);
  Serial.print(", Angle Z: ");
  Serial.print(angleZ);
  Serial.print(", Ultrasonic Distance: ");
  Serial.print(average);
  Serial.println(" cm");

  // Save the point to the SD card
  File dataFile = SD.open("pointcloud.csv", FILE_WRITE);
  if (dataFile) {
    dataFile.print(positionX);
    dataFile.print(",");
    dataFile.print(positionY);
    dataFile.print(",");
    dataFile.print(positionZ);
    dataFile.print(",");
    dataFile.print(average);
    dataFile.println();
    dataFile.close();
  } else {
    Serial.println("Error opening pointcloud.csv");
  }

  delay(100); // Adjust this delay based on your sample rate
}