#include <Wire.h>
#include <SPI.h>
#include <SD.h>
#include <Adafruit_LIS3DH.h>
#include <Adafruit_Sensor.h>
#include <TimeLib.h>
#include <FlexCAN_T4.h> // Assuming FlexCAN_T4 is for CAN communication

#define LIS3DH_CS 10 // Chip select pin for the LIS3DH
#define SD_CS 4      // Chip select pin for the SD card

File dataFile;
Adafruit_LIS3DH lis = Adafruit_LIS3DH();
volatile bool dataReady = false;
char filename[15];
unsigned long lastTimeStamp = 0;
const unsigned long interval = 100; // Adjust interval as needed

FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> Can1; // Assuming FlexCAN_T4 setup for CAN communication

// Function to create a new file for each startup
void createNewFile() {
    int i = 0;
    do {
        snprintf(filename, sizeof(filename), "data_%03d.csv", i++);
    } while (SD.exists(filename));
    dataFile = SD.open(filename, FILE_WRITE);
    if (dataFile) {
        dataFile.println("Timestamp, X, Y");
        dataFile.close();
    } else {
        Serial.println("Error creating file");
    }
}

// Interrupt Service Routine to set dataReady flag
void isr() {
    dataReady = true;
}

void setup() {
    Serial.begin(9600);
    while (!Serial) delay(10); // Wait for serial monitor to open

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

    // Initialize LIS3DH
    if (!lis.begin(0x18)) { // Change to 0x19 for alternative I2C address
        Serial.println("Could not start LIS3DH");
        while (1);
    }
    lis.setRange(LIS3DH_RANGE_4_G); // 4G range for better resolution
    lis.setDataRate(LIS3DH_DATARATE_100_HZ); // Data rate

    // Attach interrupt to INT pin of LIS3DH
    pinMode(2, INPUT);
    attachInterrupt(digitalPinToInterrupt(2), isr, RISING);

    // Initialize time library
    setTime(0, 0, 0, 1, 1, 2022); // Set initial time (adjust as needed)

    // Initialize CAN communication
    Can1.begin();
    Can1.setBaudRate(500000);
}

void loop() {
    if (dataReady) {
        dataReady = false;

        sensors_event_t event;
        lis.getEvent(&event);

        float x = event.acceleration.x;
        float y = event.acceleration.y;

        unsigned long currentMillis = millis();
        if (currentMillis - lastTimeStamp >= interval) {
            lastTimeStamp = currentMillis;

            // Convert data from float to int (digital value)
            int xDigital = (int)(x * 100); // Scale as needed
            int yDigital = (int)(y * 100); // Scale as needed

            // Get timestamp
            String timeStamp = String(hour()) + ":" + String(minute()) + ":" + String(second());

            // Write data to SD card
            dataFile = SD.open(filename, FILE_WRITE);
            if (dataFile) {
                dataFile.print(timeStamp);
                dataFile.print(", ");
                dataFile.print(xDigital);
                dataFile.print(", ");
                dataFile.println(yDigital);
                dataFile.close();
            } else {
                Serial.println("Error opening file");
            }

            // Optionally send data to monitoring box via CAN
            CAN_message_t msg;
            msg.id = 0x100; // Arbitrary ID, adjust as needed
            msg.len = 8;
            msg.buf[0] = (xDigital >> 8) & 0xFF;
            msg.buf[1] = xDigital & 0xFF;
            msg.buf[2] = (yDigital >> 8) & 0xFF;
            msg.buf[3] = yDigital & 0xFF;
            // Add timestamp data if necessary
            Can1.write(msg);

            Serial.print("Time: ");
            Serial.print(timeStamp);
            Serial.print(" X: ");
            Serial.print(xDigital);
            Serial.print(" Y: ");
            Serial.println(yDigital);
        }
    }
}