#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include "TinyGPSPlus.h"
#include "SoftwareSerial.h"
#include "HardwareSerial.h"
#include "FS.h"
#include "SD.h"
#include "SPI.h"
#include <VL53L0X.h>

#define shutdown_pin (1 << 5)
#define CAMERA_MODEL_XIAO_ESP32S3  // Camera object

// must be defined after camera object
#include "camera_module.h"

Adafruit_MPU6050 mpu;  // Accelerometer object
TinyGPSPlus gps;       // GPS object
VL53L0X ltof;        // Laser Time of Flight depth sensor object

// Constants for Acelerometer
const float offset = 9.81;    // Offset accelerometer values to baseline 0
const float alpha_EMA = 0.3;  // Smaller = smoother = more delay, depends on your application
const float threshold0 = 5;

// Accelerometer thresholds
float mAV_EMA = 9.81;
float tilt_Deg = 0;
int previousThreshold = -1;

// Variables for double vs single tap detection
unsigned long lasttap = 0;  // Differentiates between single and double tap
unsigned long lasttype = 2;
unsigned long lasttilt = 0;
unsigned long delta = 100000;
unsigned long dTilt = 0;

// Constants for GPS
//SoftwareSerial serial_connection(44, 43);  // RX pin, TX pin
HardwareSerial GPS(0);
const int GPS_RX_PIN = D7;  // ESP32 RX = 44 (goes to GPS TX)
const int GPS_TX_PIN = D6;  // ESP32 TX = 43 (goes to GPS RX), or -1 if not connected
static unsigned long lastReport = 0;

// Camera constants
bool camera_sign = false;  // Check camera status
bool sd_sign = false;      // Check sd status
int imageCount = 1;        // File Counter

int total = 0;
int dollars = 0;
int newlines = 0;
int nonprint = 0;


// ********************* GET ACCELERATION (FOR TAP DETECTION)***********
float getAcceleration() {
  float aX;  // Raw x-axis acceleration value
  float aY;
  float aZ;

  float mAV;  // Magnitude of the acceleration vector ("total acceleration")

  // Fetch a sensor event
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  //Serial.println("Got event");

  // And read the x, y and z axis acceleration values
  aX = a.acceleration.x;
  aY = a.acceleration.y;
  aZ = a.acceleration.z;

  // Then calculate the magnitude of the acceleration vector
  mAV = sqrt(aX * aX + aY * aY + aZ * aZ) - offset;

  // Push insignificant negative values to 0
  if (mAV < 0) {
    mAV = 0;
  }

  // And finally apply an exponential moving average ("EMA")
  // filter to smooth the data
  mAV_EMA = (alpha_EMA * mAV) + ((1 - alpha_EMA) * mAV_EMA);

  return mAV_EMA;
}

// ********************** GET TILT (FOR SAMPLE DETECTION)***************
float getTilt() {

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

  // Convert to g's
  float axg = a.acceleration.x / 9.80665f;
  float ayg = a.acceleration.y / 9.80665f;
  float azg = a.acceleration.z / 9.80665f;

  // Total tilt from "upright along +Z" in degrees
  float tilt_deg = acosf(constrain(azg, -1.0f, 1.0f)) * 57.2958f;

  return tilt_deg;
  Serial.println(tilt_deg);
}


// ********************** ISO TIME TRACKING FOR GPS ********************
// set up time tracking for GPS
String isoTimeUTC() {
  if (gps.date.isValid() && gps.time.isValid()) {
    char buf[25];
    // YYYY-MM-DDThh:mm:ssZ
    snprintf(buf, sizeof(buf), "%04d-%02d-%02dT%02d:%02d:%02dZ",
             (int)gps.date.year(),
             (int)gps.date.month(),
             (int)gps.date.day(),
             (int)gps.time.hour(),
             (int)gps.time.minute(),
             (int)gps.time.second());
    return String(buf);
  }
  return String("");  // unknown yet
}

// *********************** RECORD GPS COORDINATES ***********************
void recordLocation(fs::FS &fs, const char *impath, int capacitive, int depth, int sampleflag) {
  File walk = fs.open("/walk.csv", FILE_APPEND);

  if (gps.location.isValid()) {
    // print full message when button is down
    //Serial.println("Latitude:");
    //Serial.println(gps.location.lat(), 6);

    //Serial.println("Longitude");
    //Serial.println(gps.location.lat(), 6);

    //Serial.println("Altitude:");
    //Serial.println(gps.location.lat(), 6);

    double lat = gps.location.lat();
    double lon = gps.location.lng();
    double alt = gps.altitude.feet();
    String tiso = isoTimeUTC();

    if (!walk) {
      Serial.println("Failed to open csv for GPS record");
      return;
    }

    if (walk) {
      // Print timestamp, GPS coordinates
      walk.print(tiso);
      walk.print(", ");

      walk.print(lat, 6);
      walk.print(", ");

      walk.print(lon, 6);
      walk.print(", ");

      walk.print(alt, 6);
      walk.print(", ");

      // if there is an image path given, record it
      if (impath) {
        walk.print(impath);
        walk.print(", ");
      } else {
        walk.print(" , ");
      }
     
      // if there is a capacitive measurement given, record it
      if (capacitive) {
        walk.print(capacitive);
        walk.print(", ");
      } else {
        walk.print(" , ");
      }

      // if there is a depth measurement given, record it
      if (depth) {
        walk.print(depth);
        walk.print(", ");
      } else {
        walk.print(" , ");
      }

      // always record sample flag
      walk.print(sampleflag);
      walk.print(", ");
      walk.println();

      Serial.println("GPS location recorded");

    } else {
      Serial.println("Append failed");
    }
    walk.close();
  }

  else {
    //Serial.println("GPS location invalid, printing 0s");

    double lat = 0.0;
    double lon = 0.0;
    double alt = 0.0;
    String tiso = isoTimeUTC();

    if (walk) {
      // Print timestamp, GPS coordinates
      walk.print(tiso);
      walk.print(", ");

      walk.print(lat, 6);
      walk.print(", ");

      walk.print(lon, 6);
      walk.print(", ");

      walk.print(alt, 6);
      walk.print(", ");

      // if there is an image path given, record it
      if (impath) {
        walk.print(impath);
        walk.print(", ");
      } else {
        walk.print(" , ");
      }
      // if there is a capacitive measurement given, record it
      if (capacitive) {
        walk.print(capacitive);
        walk.print(", ");
      } else {
        walk.print(" , ");
      }

      // if there is a depth measurement given, record it
      if (depth) {
        walk.print(depth);
        walk.print(", ");
      } else {
        walk.print(" , ");
      }

      // always record sample flag
      walk.print(sampleflag);
      walk.print(", ");
      walk.println();

      Serial.println("GPS location recorded");

    } else {
      Serial.println("Append failed");
    }
    walk.close();
  }
}

// ************************* RECORD CAMERA IMAGE TO SD AND WRITE TO CSV ***********************
void recordImage(fs::FS &fs, char *fileName, int capacitive, int depth, int sampleflag) {
  // Check that camera and SD are working; take a picture and record it
  if (!(camera_sign && sd_sign)) {
    Serial.println("you have a problem with either the camera or the SD card");
    return;
  }

  // 1) Create a filename (no leading slash)
  snprintf(fileName, 32, "image%d.jpg", imageCount);  // make sure fileName points to >=32 bytes

  // 2) Capture JPEG from camera module (no esp_camera types here)
  uint8_t *buf = nullptr;
  size_t len = 0;
  if (!cameraCaptureToJpeg(&buf, &len)) {
    Serial.println("Failed to capture camera frame");
    return;
  }

  // 3) Build full path
  char path[64];
  snprintf(path, sizeof(path), "/walkimgs/%s", fileName);

  // 4) Write image to SD
  File image = fs.open(path, FILE_WRITE);
  if (!image) {
    Serial.println("Failed to open file for writing");
    cameraReturnFrame();  // IMPORTANT: don’t leak the frame buffer
    return;
  }

  size_t written = image.write(buf, len);
  image.close();

  // 5) Release image buffer (via module)
  cameraReturnFrame();

  if (written == len) {
    Serial.printf("Wrote image: %s (%u bytes)\n", path, (unsigned)len);
  } else {
    Serial.printf("Write failed: %s (wrote %u of %u)\n", path, (unsigned)written, (unsigned)len);
    return;  // optional: bail if the write failed
  }

  // 6) Record the location + file path in the CSV
  recordLocation(fs, path, capacitive, depth, sampleflag);  // use the passed-in FS, not SD directly
}


//******************** SETUP **************************
void setup() {
  Serial.begin(115200);
  while (!Serial)
    delay(10);  // will pause Zero, Leonardo, etc until serial console opens

  // Accelerometer init
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");
  // Set the G sensitivity and sampling rate of accelerometer
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);  // Valid values are 2, 4, 8, 16

  // GPS init
  GPS.setRxBufferSize(2048);
  GPS.begin(9600, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN);
  Serial.println("GPS Start");

  // SD Card init
  const int SD_CS   = 21;
  const int SD_SCK  = 7;
  const int SD_MISO = 8;
  const int SD_MOSI = 9;

  SPI.begin(SD_SCK, SD_MISO, SD_MOSI, SD_CS);
  if (!SD.begin(SD_CS, SPI)) {
    Serial.println("Card Mount Failed");
    return;
  }
  uint8_t cardType = SD.cardType();

  if (cardType == CARD_NONE) {
    Serial.println("No SD card attached");
    return;
  }

  Serial.print("SD Card Type: ");
  if (cardType == CARD_MMC) {
    Serial.println("MMC");
  } else if (cardType == CARD_SD) {
    Serial.println("SDSC");
  } else if (cardType == CARD_SDHC) {
    Serial.println("SDHC");
  } else {
    Serial.println("UNKNOWN");
  }

  sd_sign = true;  // sd initialization check passes

  // SD card CSV file init
  File walk = SD.open("/walk.csv", FILE_WRITE);
  if (!walk) {
    Serial.println("Failed to open walk.csv for writing");
    return;
  }
  if (walk.println("timestamp, lat, lon, alt, impath, capacitive, depth, sampleflag")) {
    Serial.println("walk.csv header written");
  } else {
    Serial.println("walk.csv init failed");
  }
  walk.close();

  // SD card image directory init
  if (SD.mkdir("/walkimgs")) {
    Serial.println("Dir created");
  } else {
    Serial.println("mkdir failed");
  }

  // Camera init
  if (!cameraInit()) {
    Serial.println("Camera init failed");
  } else {
    Serial.println("Camera ready");
    camera_sign = true;  // Camera initialization check passes
  }

  // Analog init
  analogReadResolution(12);

  // Laser Time of Flight init
  Wire.begin(D4, D5); 
   if (!ltof.init()) {
      Serial.println("Laser TOF init failed");
    }

  // GPS RE-INIT because Camera steals its pins
  GPS.end();
  GPS.setRxBufferSize(2048);
  GPS.begin(9600, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN);
  Serial.println("GPS re-begin after peripherals");

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

// ********************************** MAIN LOOP ******************************************
void loop() {

  // Timestamp that updates every loop() iteration
  unsigned long timeNow = millis();

  // Capacitive and LTOF sensing that update every iteration
  int analogVolts = analogReadMilliVolts(2);
  int heightMm = ltof.readRangeSingleMillimeters();

  // track Acceleration
  mAV_EMA = getAcceleration();
  //Serial.println(mAV_EMA);

  //static unsigned long lastPrint = 0;
  //if (millis() - lastPrint > 100) {
    //lastPrint = millis();
    //Serial.print("mAV_EMA= ");
    //Serial.println(mAV_EMA, 3);
    //Serial.print(" threshold0=");
    //Serial.println(threshold0, 3);
  //}

  // track Tilt
  tilt_Deg = getTilt();

  // signal if GPS is unavailable
  while (!GPS.available()) {
    Serial.println("No GPS Data");
  }

  // encode GPS feedback
  while (GPS.available()) {
    char c = GPS.read();
    gps.encode(c);
    //Serial.write(c);
  }

  // if some kind of tap detected..........
  if (mAV_EMA >= threshold0) {
    delta = timeNow - lasttap;

    // give a 100 millisecond error range to avoid tailgating taps
    if (delta > 250) {
      // 50 second minimum to avoid false double taps

      // DOUBLE TAP TRIGGER: CAMERA
      if ((delta > 500) && (delta < 2000) && (lasttype == 0)) {
        Serial.println("__________DOUBLE TAP DETECTED____________");
        Serial.println(delta);

        delay(2000);

        char filename[32];
        sprintf(filename, "/image%d.jpg", imageCount);
        recordImage(SD, filename, analogVolts, heightMm, 0);
        imageCount++;
        lasttype = 1;
      }

      // SINGLE TAP TRIGGER: GPS
      else {
        Serial.println("____________TAP DETECTED___________");
        recordLocation(SD, "", analogVolts, heightMm, 0);
        lasttype = 0;
      }
      lasttap = millis();
    }
  }

  // TIP TRIGGER: SENSOR RECORDING
  dTilt = timeNow - lasttilt;
  //Serial.println(tilt_Deg);
  if ((tilt_Deg < 70.0f) &&
      (dTilt > 10000) &&
      (mAV_EMA < threshold0)) {  
      //gesture : down
      Serial.println("________gesture DOWN: SAMPLE DETECTED_______");
      Serial.print("Analog volts: ");
      Serial.println(analogVolts);
      Serial.print("Height: ");
      Serial.println(heightMm);

      // wait for the sampling process to end, then record a flag
      recordLocation(SD, "", analogVolts, heightMm, 1);

      lasttilt = millis();
  }

  delay(10);
}