data_collection.cpp

#include <iostream>
#include <fstream>
#include <vector>
#include <chrono>
#include <thread>
#include <csignal>
#include <ctime>
#include <string>
#include <wiringPiI2C.h>
#include <unistd.h>
#include "bno055.h"
#include "rpi_tca9548a.h"
#include <nlohmann/json.hpp>

using json = nlohmann::json;
using namespace std::chrono;
using namespace std::this_thread;

std::ofstream logFile;
bool running = true;
rpi_tca9548a tca;
int fd;

const std::vector<int> sensorChannels = {0x02, 0x04, 0x08, 0x10, 0x20};
const std::vector<int> sensorChannelsBackup = {1, 2, 3, 4, 5};
const std::vector<std::string> sensorLocations = {"Left Knee", "Left Hip", "Right Ankle", "Right Knee", "Right Hip"};

void signal_handler(int signal) {
    std::cout << "\nStopping data logging..." << std::endl;
    running = false;
}

bool initI2C() {
    fd = wiringPiI2CSetup(0x29);
    if (fd == -1) {
        std::cerr << "Failed to initialize I2C connection!" << std::endl;
        return false;
    }
    return true;
}

s8 I2C_bus_write_with_tca(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt) {
    if (fd == -1) return BNO055_ERROR;
    for (int i = 0; i < cnt; i++) {
        if (wiringPiI2CWriteReg8(fd, reg_addr + i, reg_data[i]) == -1) {
            return BNO055_ERROR;
        }
    }
    return BNO055_SUCCESS;
}

s8 I2C_bus_read(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt) {
    if (fd == -1) return BNO055_ERROR;
    for (int i = 0; i < cnt; i++) {
        reg_data[i  ] = wiringPiI2CReadReg8(fd, reg_addr + i);
        if (reg_data[i] == -1) return BNO055_ERROR;
    }
    return BNO055_SUCCESS;
}

void delay_msec(u32 msec) {
    usleep(msec * 1000);
}

std::string get_human_readable_timestamp() {
    auto now = system_clock::now();
    auto ms = duration_cast<milliseconds>(now.time_since_epoch()) % 1000;
    std::time_t now_c = system_clock::to_time_t(now);
    std::tm local_tm = *std::localtime(&now_c);
    char buffer[20];
    std::strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", &local_tm);
    return std::string(buffer) + "." + std::to_string(ms.count());
}

int main(int argc, char* argv[]) {
    signal(SIGINT, signal_handler);
    if (!initI2C()) return -1;
    tca.init(0x70);
    
    std::string filename;
    if (argc > 1) {
        filename = argv[1];  // Take filename from command-line argument
    } else {
        std::cout << "Enter filename for logging (e.g., imu_data.json): ";
        std::cin >> filename;
    }

    logFile.open(filename, std::ios::app);
    if (!logFile.is_open()) {
        std::cerr << "Failed to open imu_data.json for writing" << std::endl;
        return -1;
    }
    
    std::vector<bno055_t> sensors(5);
    for (size_t i = 0; i < sensorChannels.size(); i++) {
        tca.set_channel(sensorChannelsBackup[i]);
        usleep(10000);
        sensors[i].bus_write = I2C_bus_write_with_tca;
        sensors[i].bus_read = I2C_bus_read;
        sensors[i].delay_msec = delay_msec;
        sensors[i].dev_addr = 0x29;
        if (bno055_init(&sensors[i]) != BNO055_SUCCESS || bno055_set_operation_mode(BNO055_OPERATION_MODE_NDOF) != BNO055_SUCCESS) {
            std::cerr << "Failed to initialize sensor at " << sensorLocations[i] << std::endl;
        }
    }

    int sampleIntervalMs = 100;
    while (running) {
        auto start_time = steady_clock::now();
        
        auto t1 = steady_clock::now();
        json logEntry;
        logEntry["timestamp"] = get_human_readable_timestamp();
        logEntry["sensors"] = json::array();
        auto t2 = steady_clock::now();

        for (size_t i = 0; i < sensors.size(); i++) {
            auto sensor_start = steady_clock::now();
            
            tca.set_channel(sensorChannelsBackup[i]);
            auto t3 = steady_clock::now();
            
            bno055_euler_double_t euler;
            bno055_accel_double_t accel;
            bno055_linear_accel_double_t linear_accel;
            bno055_gravity_double_t gravity;
            bno055_gyro_double_t angular_velocity;
            bno055_mag_double_t mag_field;
            bno055_quaternion_t quater;

            if (bno055_convert_double_euler_hpr_deg(&euler) != BNO055_SUCCESS) {
                std::cerr << "Failed to read euler from: " << sensorLocations[i] << std::endl;
            }
            std::cout << sensorLocations[i] << ": " << euler.h << " Roll: " << euler.r << " Pitch: " << euler.p << std::endl;
            if (euler.h == 0.0 && euler.r == 0.0 && euler.p == 0.0){
                std::cerr << "BAD READING ALL ZEROS " << std::endl;
            }
            //bno055_convert_double_accel_xyz_msq(&accel);
            //bno055_convert_double_linear_accel_xyz_msq(&linear_accel);
            //bno055_convert_double_gravity_xyz_msq(&gravity);
            //bno055_convert_double_gyro_xyz_dps(&angular_velocity);
            //bno055_convert_double_mag_xyz_uT(&mag_field);
            //bno055_read_quaternion_wxyz(&quater);

            auto t4 = steady_clock::now();

            json sensorData;
            sensorData["location"] = sensorLocations[i];
            sensorData["euler"] = {{"heading", euler.h}, {"roll", euler.r}, {"pitch", euler.p}};
            // sensorData["acceleration"] = {{"x", accel.x}, {"y", accel.y}, {"z", accel.z}};
            // sensorData["linear_acceleration"] = {{"x", linear_accel.x}, {"y", linear_accel.y}, {"z", linear_accel.z}};
            // sensorData["gravity"] = {{"x", gravity.x}, {"y", gravity.y}, {"z", gravity.z}};
            // sensorData["angular_velocity"] = {{"x", angular_velocity.x}, {"y", angular_velocity.y}, {"z", angular_velocity.z}};
            // sensorData["magnetic_field"] = {{"x", mag_field.x}, {"y", mag_field.y}, {"z", mag_field.z}};
            //sensorData["quaternion"] = {{"w", quater.w}, {"x", quater.x}, {"y", quater.y}, {"z", quater.z}};
            
            logEntry["sensors"].push_back(sensorData);
            
            auto sensor_end = steady_clock::now();

            // std::cout << "Sensor " << i << " switch time: " 
            //         << duration_cast<milliseconds>(t3 - sensor_start).count() << " ms" << std::endl;

            std::cout << "Sensor " << i << " read time: " 
                    << duration_cast<milliseconds>(t4 - t3).count() << " ms" << std::endl;

            // std::cout << "Sensor " << i << " JSON prep time: " 
            //         << duration_cast<milliseconds>(sensor_end - t4).count() << " ms" << std::endl;
        }

        auto t5 = steady_clock::now();
        logFile << logEntry.dump() << std::endl;
        logFile.flush();
        auto t6 = steady_clock::now();

        // std::cout << "File write & flush time: " 
        //         << duration_cast<milliseconds>(t6 - t5).count() << " ms" << std::endl;

        auto end_time = steady_clock::now();
        // std::cout << "Total loop time: " 
        //         << duration_cast<milliseconds>(end_time - start_time).count() << " ms" << std::endl;

        milliseconds sleep_time = milliseconds(sampleIntervalMs) - duration_cast<milliseconds>(end_time - start_time);
        if (sleep_time.count() > 0) sleep_for(sleep_time);
        std::cout << " ------------- " << std::endl;
    }

    logFile.close();

    return 0;
}