/**
 * Wave file writing, with or without splitting by length.
 *
 * Author: Jan Schlüter <jan.schluter@lis-lab.fr>
 * Author: Maxence Ferrari <maxence.ferrari@lis-lab.fr>
 */

#include "filewriter.h"
#include "macros.h"
#include <stdexcept>
#include <vector>
#include <iostream>
#include <chrono>
#include <sstream>
#include <iomanip>
#include <cstring>
#include <cmath>

FileWriter::FileWriter(std::string &filename_template, size_t num_channels, size_t sample_rate, size_t depth) :
        filename_template(filename_template), num_channels(num_channels), sample_rate(sample_rate), depth(depth) {
    // nothing
}

FileWriter::~FileWriter() {
    // nothing
}

std::string FileWriter::generate_filename() {
    // this has of course nothing to do with file writing and could be pulled
    // out, but I doubt anybody will ever care
    using namespace std::chrono;
    auto miliseconds = duration_cast<milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
    long seconds = miliseconds/1000;
    struct tm *timeinfo = localtime(&seconds);

    size_t found = filename_template.find("%z");
    while(found != std::string::npos){
      std::stringstream ss;
      ss << std::setw(3) << std::setfill('0') << miliseconds%1000;
      std::string s = ss.str();
      filename_template.replace(found, 2, s);
      found = filename_template.find("%z", found+3);
    }

    size_t length = 0;
    size_t space = 0;
    std::vector<char> buffer;
    while (!length) {
        space += 100;
        buffer.resize(filename_template.size() + space);
        length = strftime(buffer.data(), buffer.size(),
                filename_template.c_str(), timeinfo);
    }
    return std::string(buffer.begin(), buffer.begin() + length);
}

void FileWriter::write(std::vector<uint8_t> &samples, std::vector<uint8_t> &imu_data) {
    write(samples.data(), samples.size(), imu_data.data());
}


void store_little_endian(uint8_t (&target)[2], uint16_t value) {
    target[0] = value & 0xFF;
    target[1] = (value >> 8) & 0xFF;
}

void store_little_endian(uint8_t (&target)[4], uint32_t value) {
    target[0] = value & 0xFF;
    target[1] = (value >> 8) & 0xFF;
    target[2] = (value >> 16) & 0xFF;
    target[3] = (value >> 24) & 0xFF;
}

uint32_t read_little_endian(uint8_t (&source)[4]) {
    return (source[0] + source[1] << 8 + source[2] << 16 + source[3] << 24);
}

WavFileWriter::WavFileWriter(std::string &filename_template, size_t num_channels, size_t sample_rate, size_t depth) :
        WavFileWriter(filename_template, num_channels, sample_rate, depth, 0) {
    // nothing
}

WavFileWriter::WavFileWriter(std::string &filename_template, size_t num_channels, size_t sample_rate, size_t depth,
                             size_t expected_num_samples) :
        FileWriter(filename_template, num_channels, sample_rate, depth),
        outfile(generate_filename(), std::ios::out | std::ios::binary | std::ios::trunc),
        samples_written(0) {
    // check if we could open the file
    if (!outfile.is_open()) {
        throw std::runtime_error("could not create output file");
    }
    // write header
    store_little_endian(header.fmt_channels, num_channels);
    store_little_endian(header.fmt_sample_rate, sample_rate);
    store_little_endian(header.fmt_bits_per_sample, 8 * depth);
    store_little_endian(header.fmt_byte_rate, num_channels * sample_rate * depth);
    store_little_endian(header.fmt_frame_size, num_channels * depth);
    if (expected_num_samples) {
        size_t expected_data_size = expected_num_samples * num_channels * depth;
        store_little_endian(header.data_size, expected_data_size);
        store_little_endian(header.chunk_size, expected_data_size + 36);
        // TODO: on linux, we could use fallocate to reserve the final size
    }
    // TODO: on posix, we could use posix_fadvice to indicate sequential access
    outfile.seekp(0);
    outfile.write((char*) &header, sizeof(header));
}

WavFileWriter::~WavFileWriter() {
    // finalize header, if needed
    size_t data_size = samples_written * num_channels * depth;
    if (data_size != read_little_endian(header.data_size)) {
        store_little_endian(header.data_size, data_size);
        store_little_endian(header.chunk_size, data_size + 36);
        outfile.seekp(0);
        outfile.write((char*) &header, sizeof(header));
    }
}

void WavFileWriter::write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) {
    outfile.write((char*) samples, num_samples * sizeof(*samples));
    samples_written += num_samples /(num_channels * depth);
}

IMUFileWriter::IMUFileWriter(std::string &filename_template, size_t num_channels, size_t sample_rate,size_t depth, size_t timestamp) :
        FileWriter(filename_template, num_channels, sample_rate, depth),
        outfile(generate_filename(),
        std::ios::out | std::ios::trunc),
        last_timestamp(0),
        rcvState(StateReception::Waiting),
        msgDecodedFunction(0),
        msgDecodedPayloadLength(0),
        msgDecodedPayload(nullptr),
        msgDecodedPayloadIndex(0),
        msgDecoded(0) {
        outfile << header;
}

inline float le16tof(uint8_t *array){
    return static_cast<float>(static_cast<int16_t>(__builtin_bswap16(*reinterpret_cast<uint16_t*>(array))));
}

float IMUFileWriter::GetFloatSafe(const unsigned char *p, int index) {
    unsigned char tmp[4];
    std::memcpy(tmp, p + index, 4); // Copy 4 bytes from p + index into tmp

    float result;
    std::memcpy(&result, tmp, sizeof(result)); // Copy bytes from tmp into result

    return result;
}

unsigned char IMUFileWriter::CalculateChecksum(int msgFunction,
                int msgPayloadLength, const unsigned char msgPayload[])
{
    unsigned char checksum = 0;
    checksum ^= static_cast<unsigned char>(0xFE);
    checksum ^= static_cast<unsigned char>(msgFunction >> 8);
    checksum ^= static_cast<unsigned char>(msgFunction >> 0);
    checksum ^= static_cast<unsigned char>(msgPayloadLength >> 8);
    checksum ^= static_cast<unsigned char>(msgPayloadLength >> 0);
    for (int i = 0; i < msgPayloadLength; i++) {
        checksum ^= msgPayload[i];
    }
    return checksum;
}

void IMUFileWriter::DecodeMessage(unsigned char c) {
    switch (rcvState) {
        case StateReception::Waiting:
            if (c == 0xFE)
                rcvState = StateReception::FunctionMSB;
            break;
        case StateReception::FunctionMSB:
            msgDecodedFunction = static_cast<int16_t>(c << 8);
            rcvState = StateReception::FunctionLSB;
            break;
        case StateReception::FunctionLSB:
            msgDecodedFunction += static_cast<int16_t>(c << 0);
            rcvState = StateReception::PayloadLengthMSB;
            break;
        case StateReception::PayloadLengthMSB:
            msgDecodedPayloadLength = static_cast<uint16_t>(c << 8);
            std::cerr << "Message function : " << std::hex << static_cast<uint16_t>(msgDecodedFunction) << std::endl;
            rcvState = StateReception::PayloadLengthLSB;
            break;
        case StateReception::PayloadLengthLSB:
            msgDecodedPayloadLength += static_cast<uint16_t>(c << 0);
            if (msgDecodedPayloadLength > 0) {
                if (msgDecodedPayloadLength < 1024) {
                    msgDecodedPayloadIndex = 0;
                    msgDecodedPayload = static_cast<unsigned char*>(malloc(msgDecodedPayloadLength));
                    if (msgDecodedPayload == nullptr) {
                        throw std::bad_alloc(); // Handle memory allocation failure
                    }
                    rcvState = StateReception::Payload;
                } else {
                    rcvState = StateReception::Waiting;
                }
            } else
                rcvState = StateReception::CheckSum;
            break;
        case StateReception::Payload:
            if (msgDecodedPayloadIndex > msgDecodedPayloadLength)
            {
                //Erreur
                msgDecodedPayloadIndex = 0;
                rcvState = StateReception::Waiting;
            }
            msgDecodedPayload[msgDecodedPayloadIndex++] = c;
            if (msgDecodedPayloadIndex >= msgDecodedPayloadLength)
            {
                rcvState = StateReception::CheckSum;
                msgDecodedPayloadIndex = 0;
            }
            break;
        case StateReception::CheckSum:
        {
            unsigned char calculatedChecksum = CalculateChecksum(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload);
            unsigned char receivedChecksum = c;
            if (calculatedChecksum == receivedChecksum) {
                //Lance l'event de fin de decodage
                ProcessDecodedMessage(msgDecodedFunction, msgDecodedPayloadLength, msgDecodedPayload);
                msgDecoded++;
            } else {
                std::cerr << "Checksum error" << std::endl;
            }
            rcvState = StateReception::Waiting;
        }
        break;
        default:
            rcvState = StateReception::Waiting;
            break;
    }
    if(rcvState != StateReception::Waiting) {
        std::cerr << "State : " << static_cast<int>(rcvState) << std::endl;
    }
}

void IMUFileWriter::ProcessDecodedMessage(int msgFunction, int msgPayloadLength, const unsigned char* msgPayload) {
    unsigned int timeStamp = 0;
    switch(static_cast<short>(msgFunction)) {
        case static_cast<short>(HS_DATA_PACKET_FULL_TIMESTAMP): {
            /*
            IMUFileWriter::SensorType sensorType = static_cast<SensorType>(msgPayload[0]);
            unsigned char id = msgPayload[1];
            unsigned char nbChannels = msgPayload[2];
            unsigned char range = msgPayload[3];
            unsigned char resolutionBits = msgPayload[4];
            unsigned short samplingFrequency = BUILD_UINT16(msgPayload[6], msgPayload[5]);
            unsigned short nbSamples = BUILD_UINT16(msgPayload[8], msgPayload[7]);

            int lengthPerSample = nbChannels * resolutionBits / 8 + 4;
            double accelMaxValue = pow(2, resolutionBits)/2;
            double gyroMaxValue = pow(2, resolutionBits) / 2;
            double gyroRange = 250.0;       //Hardcode pour le moment
            double magRange = 4900.0;       //Fixe

            for(int i=0; i < nbSamples && msgPayloadLength >= lengthPerSample * i + 9; i++) {
                timeStamp = BUILD_UINT32(9 + i * lengthPerSample, 9 + i * 9 + i * lengthPerSample+1, 9 + i * lengthPerSample+2, 9 + i * lengthPerSample+3);

                if(timeStamp > lastTimeStamp): {
                    lastTimeStamp = timeStamp;
                    switch(sensorType) {
                        case IMUFileWriter::SensorType::IMU: {
                            outfile << timeStamp;
                            outfile << ", " << BUILD_UINT16(13 + i * lengthPerSample,13 + i * lengthPerSample+1); // AccelX
                            outfile << ", " << BUILD_UINT16(15 + i * lengthPerSample,15 + i * lengthPerSample+1); // AccelY
                            outfile << ", " << BUILD_UINT16(17 + i * lengthPerSample,17 + i * lengthPerSample+1); // AccelZ
                            outfile << ", " << BUILD_UINT16(19 + i * lengthPerSample,19 + i * lengthPerSample+1); // GyroX
                            outfile << ", " << BUILD_UINT16(21 + i * lengthPerSample,21 + i * lengthPerSample+1); // GyroY
                            outfile << ", " << BUILD_UINT16(23 + i * lengthPerSample,23 + i * lengthPerSample+1); // GyroZ
                            outfile << ", " << BUILD_UINT16(25 + i * lengthPerSample,25 + i * lengthPerSample+1); // MagX
                            outfile << ", " << BUILD_UINT16(27 + i * lengthPerSample,27 + i * lengthPerSample+1); // MagY
                            outfile << ", " << BUILD_UINT16(29 + i * lengthPerSample,29 + i * lengthPerSample+1); // MagZ
                            outfile << std::endl;
                        }
                        break;
                        case IMUFileWriter::SensorType::Accel:
                            break;
                        case IMUFileWriter::SensorType::Gyro:
                            break;
                        case IMUFileWriter::SensorType::Mag:
                            break;
                        case IMUFileWriter::SensorType::Temperature:
                            break;
                        case IMUFileWriter::SensorType::Pressure:
                            break;
                        case IMUFileWriter::SensorType::Light:
                            break;
                        default:
                            break;
                    }
                } else {
                    outfile << "TS IMU Error" << std::endl;
                }
            }*/
           outfile << "Not implemented yet. Deprecated version?" << std::endl; // Commented block added by Philémon Prévot 29/08/2024
        }
        break;
        case static_cast<short>(HS_DATA_PACKET_FULL_TIMESTAMP_V2): {
            IMUFileWriter::SensorType sensorType = static_cast<SensorType>(msgPayload[0]);
            unsigned char id = msgPayload[1];
            unsigned char nbChannels = msgPayload[2];
            float rangeScale = GetFloatSafe(msgPayload, 3);
            unsigned char resolutionBits = msgPayload[7];
            float samplingFrequency = GetFloatSafe(msgPayload, 8);
            unsigned short nbSamples = msgPayload[12];
            unsigned char dataSize = (resolutionBits / 8);

            int lengthPerSample = nbChannels * resolutionBits / 8 + 4;
            double dataMaxValue = std::pow(2, resolutionBits) / 2.0;
            
            for(int i = 0; i < nbSamples && msgPayloadLength >= static_cast<size_t>(lengthPerSample * i + 13); i++) {
                uint32_t timeStamp = BUILD_UINT32(msgPayload[13 + i * lengthPerSample+3],msgPayload[13 + i * lengthPerSample+2],msgPayload[13 + i * lengthPerSample+1],msgPayload[13 + i * lengthPerSample]);
                switch(sensorType) {
                    case IMUFileWriter::SensorType::Accel:
                            if (lastAccelTimeStamp >= 500000000)
                                lastAccelTimeStamp = 0;
                            if (timeStamp > lastAccelTimeStamp) {
                                lastAccelTimeStamp = timeStamp;
                                outfile << "ACCEL, " << timeStamp / 1000.0;
                                outfile << ", " << BUILD_INT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue );
                                outfile << ", " << BUILD_INT16(msgPayload[17 + dataSize + i * lengthPerSample],msgPayload[17 +dataSize + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue );
                                outfile << ", " << BUILD_INT16(msgPayload[17 + 2*dataSize + i * lengthPerSample],msgPayload[17 +2*dataSize + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue );
                                outfile << std::endl;
                            }
                            else {
                                //printf("TS Accel Error\n");
                            }
                        break;
                    case IMUFileWriter::SensorType::Gyro:
                            if (lastGyroTimeStamp >= 500000000)
                                lastGyroTimeStamp = 0;
                            if (timeStamp > lastGyroTimeStamp) {
                                lastGyroTimeStamp = timeStamp;
                                outfile << "GYRO, " << timeStamp;
                                outfile << ", " << BUILD_INT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue);
                                outfile << ", " << BUILD_INT16(msgPayload[17 + dataSize + i * lengthPerSample],msgPayload[17 +dataSize + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue);
                                outfile << ", " << BUILD_INT16(msgPayload[17 + 2*dataSize + i * lengthPerSample],msgPayload[17 +2*dataSize + i * lengthPerSample+1]) * ( rangeScale / dataMaxValue);
                                outfile << std::endl;
                            }
                            else {
                                //printf("TS Gyro Error\n");
                            }
                        break;
                    case IMUFileWriter::SensorType::Mag:
                            if (lastMagTimeStamp >= 500000000)
                                lastMagTimeStamp = 0;
                            if (timeStamp > lastMagTimeStamp) {
                                lastMagTimeStamp = timeStamp;
                                outfile << "MAG, " << timeStamp;
                                outfile << ", " << BUILD_INT16(msgPayload[17 + i * lengthPerSample+1],msgPayload[17 + i * lengthPerSample]) * ( rangeScale / dataMaxValue);
                                outfile << ", " << BUILD_INT16(msgPayload[17 + dataSize + i * lengthPerSample+1],msgPayload[17 +dataSize + i * lengthPerSample]) * ( rangeScale / dataMaxValue);
                                outfile << ", " << BUILD_INT16(msgPayload[17 + 2*dataSize + i * lengthPerSample+1],msgPayload[17 +2*dataSize + i * lengthPerSample]) * ( rangeScale / dataMaxValue);
                                outfile << std::endl;
                            }
                            else {
                                //printf("TS Mag Error\n");
                            }
                        break;
                    case IMUFileWriter::SensorType::Temperature:
                        if (lastTemperatureTimeStamp >= 500000000)
                            lastTemperatureTimeStamp = 0;
                        if (timeStamp > lastTemperatureTimeStamp) {
                            lastTemperatureTimeStamp = timeStamp;
                            outfile << "TEMP, " << timeStamp;
                            outfile << ", " << GetFloatSafe(msgPayload,17 + i * lengthPerSample);
                            outfile << std::endl;
                        }
                        else {
                            //printf("TS Temperature Error\n");
                        }
                        break;
                    case IMUFileWriter::SensorType::Pressure:
                        if (lastPressureTimeStamp >= 500000000)
                            lastPressureTimeStamp = 0;
                        if (timeStamp > lastPressureTimeStamp) {
                            lastPressureTimeStamp = timeStamp;
                            outfile << "PRESSURE, " << timeStamp;
                            outfile << ", " << IMUFileWriter::GetFloatSafe(msgPayload,17 + i * lengthPerSample);
                            outfile << std::endl;
                        }
                        else {

                        }
                        break;
                    case IMUFileWriter::SensorType::Light:
                        if (lastLightTimeStamp >= 500000000)
                            lastLightTimeStamp = 0;
                        if (timeStamp > lastLightTimeStamp) {
                            lastLightTimeStamp = timeStamp;
                            outfile << "LIGHT, " << timeStamp;
                            outfile << ", " << BUILD_UINT16(msgPayload[17 + i * lengthPerSample],msgPayload[17 + i * lengthPerSample+1]);
                            outfile << ", " << BUILD_UINT16(msgPayload[17 + dataSize+i * lengthPerSample],msgPayload[17 +dataSize+ i * lengthPerSample+1]);
                            outfile << std::endl;
                        }
                        else {
                            //printf("TS Light Error\n");
                        }
                        break;
                    case IMUFileWriter::SensorType::Unknown:
                        outfile << "UNKNOWN, " << timeStamp;
                        outfile << std::endl;
                    default:
                        break;
                }
            }
        }
        break;
        case static_cast<short>(GPS_DATA_PACKET): {
            IMUFileWriter::GPSDatas gpsDatas;
            unsigned short ms = BUILD_UINT16(msgPayload[3], msgPayload[4]);
            if(ms > 999) {
                ms = 0;
            }

            gpsDatas.dateOfFix.year = msgPayload[7];
            gpsDatas.dateOfFix.month = msgPayload[6];
            gpsDatas.dateOfFix.day = msgPayload[5];
            gpsDatas.dateOfFix.hour = msgPayload[0];
            gpsDatas.dateOfFix.minute = msgPayload[1];
            gpsDatas.dateOfFix.second = msgPayload[2];

            gpsDatas.fix = msgPayload[8] != 0;
            gpsDatas.fixQuality = msgPayload[9];
            gpsDatas.latitude = GetFloatSafe(msgPayload, 10);
            gpsDatas.latitudeDirection = static_cast<char>(msgPayload[14]);
            gpsDatas.longitude = GetFloatSafe(msgPayload, 15);
            gpsDatas.longitudeDirection = static_cast<char>(msgPayload[19]);
            gpsDatas.speed = GetFloatSafe(msgPayload, 20);
            gpsDatas.angle = GetFloatSafe(msgPayload, 24);
            gpsDatas.altitude = GetFloatSafe(msgPayload, 28);
            gpsDatas.satellites = msgPayload[32];
            gpsDatas.antenna = msgPayload[33];

            if (lastGPSDate.year != gpsDatas.dateOfFix.year || lastGPSDate.month != gpsDatas.dateOfFix.month ||lastGPSDate.day != gpsDatas.dateOfFix.day || 
                lastGPSDate.hour!=gpsDatas.dateOfFix.hour || lastGPSDate.minute!=gpsDatas.dateOfFix.minute || lastGPSDate.second!=gpsDatas.dateOfFix.second) {
                lastGPSDate = gpsDatas.dateOfFix;
                outfile << "GPS, " << gpsDatas.dateOfFix.year;
                outfile << "/" << gpsDatas.dateOfFix.month;
                outfile << "/" << gpsDatas.dateOfFix.day;
                outfile << " " << gpsDatas.dateOfFix.hour;
                outfile << ":" << gpsDatas.dateOfFix.minute;
                outfile << ":" << gpsDatas.dateOfFix.second;
                outfile << " fix:" << gpsDatas.fix;
                outfile << ", fixQual:" << gpsDatas.fixQuality;
                outfile << ", Lat:" << gpsDatas.latitude;
                outfile << " " << gpsDatas.latitudeDirection;
                outfile << ", Lon:" << gpsDatas.longitude;
                outfile << " " << gpsDatas.longitudeDirection;
                outfile << ", speed:" << gpsDatas.speed;
                outfile << ", ang:" << gpsDatas.angle;
                outfile << ", alt:" << gpsDatas.altitude;
                outfile << ", sat:" << gpsDatas.satellites;
                outfile << std::endl;
            }
        }
        break;
        case static_cast<short>(GPS_PPS_PACKET): {
            uint64_t PPSTimeStamp = BUILD_UINT64(msgPayload[7], msgPayload[6], msgPayload[5], msgPayload[4],
                                          msgPayload[3], msgPayload[2], msgPayload[1], msgPayload[0]);

            PPSTimeStamp *= 10; // Convert to nanoseconds (assuming internal clock frequency is 100MHz)

            if(PPSTimeStamp>lastPPSTimeStampNS) {
                lastPPSTimeStampNS = PPSTimeStamp;
                outfile << "PPS: " << PPSTimeStamp;
                outfile << std::endl;
            }
        }
        break;
        default: break;
    }
}

void IMUFileWriter::write(uint8_t *sample, size_t size, uint8_t *imu_data) {
    uint8_t *imu_data_cur(imu_data);

    uint8_t softwareMajorRev=sample[5];
    uint8_t softwareMinorRev=sample[6];
    std::cerr << "sMR" << static_cast<int>(sample[10]) << std::endl;
    std::cerr << "Size : " << size << std::endl;
    
    uint64_t timeStamp100MHzCurrentPacket=0;
    if(true) {
        //On recupere l'instant de fin du paquet courant
        timeStamp100MHzCurrentPacket=BUILD_UINT64(sample[14],
                    sample[13],
                    sample[12],
                    sample[11],
                    sample[10],
                    sample[9],
                    sample[8],
                    sample[7]);
        timeStamp100MHzCurrentPacket*=10;     //To put the ts in ns
        uint8_t enteteSize=16;

        outfile << "PACKET TIMESTAMP: " << static_cast<int>(timeStamp100MHzCurrentPacket) << std::endl;
        for(int i=enteteSize;i<size-enteteSize; i++)
        {
            DecodeMessage(sample[i]);
        }
    }
    else {
        uint8_t *imu_data_cur(imu_data);
        while(imu_data_cur + frame_size + 5 < imu_data + additional_data_size){
            if(!(imu_data_cur[0]==0xFE && imu_data_cur[1]==0x0A && imu_data_cur[2]==0x0A && imu_data_cur[5]==0x08)) {
                // skip trame if header is incorrect
                imu_data_cur += frame_size + 5;
                continue;
            }
            imu_data_cur += 5; // skip frame header
            auto timestamp = static_cast<int32_t>(__builtin_bswap32(*reinterpret_cast<uint32_t*>(imu_data_cur + 9)));
            if (timestamp > last_timestamp) {
                last_timestamp = timestamp;
                outfile << timestamp;
                outfile << "," << le16tof(imu_data_cur + 13) / 32756 * 19.62;   // ax resolution +- 2g
                outfile << "," << le16tof(imu_data_cur + 15) / 32756 * 19.62;   // ay resolution +- 2g
                outfile << "," << le16tof(imu_data_cur + 17) / 32756 * 19.62;   // az resolution +- 2g
                outfile << "," << le16tof(imu_data_cur + 19) / 32756 * 250;     // gx resolution +- 255deg/sec
                outfile << "," << le16tof(imu_data_cur + 21) / 32756 * 250;     // gy resolution +- 255deg/sec
                outfile << "," << le16tof(imu_data_cur + 23) / 32756 * 250;     // gz resolution +- 255deg/sec
                outfile << "," << le16tof(imu_data_cur + 25) / 32756 * 4900.;    // mx +- 4900µTesla
                outfile << "," << le16tof(imu_data_cur + 27) / 32756 * (-4900.); // my +- 4900µTesla
                outfile << "," << le16tof(imu_data_cur + 29) / 32756 * (-4900.); // mz +- 4900µTesla
                outfile << std::endl;
            }
            imu_data_cur += frame_size;
        }
    }
}

size_t IMUFileWriter::get_last_timestamp(){
    return last_timestamp;
}


SplitWavFileWriter::SplitWavFileWriter(std::string &filename_template, size_t num_channels, size_t sample_rate,
                                       size_t depth, size_t samples_per_file) :
        FileWriter(filename_template, num_channels, sample_rate, depth),
        samples_per_file(samples_per_file),
        current_file(nullptr),
        current_file_samples_written(0) {
    // nothing
}

SplitWavFileWriter::~SplitWavFileWriter() {
    if (current_file) {
        delete current_file;
    }
}

void SplitWavFileWriter::write(uint8_t *samples, size_t num_samples, uint8_t *imu_data) {
    size_t available = num_samples /(num_channels * depth);
    // start as many new files as required to write out the samples
    while (current_file_samples_written + available >= samples_per_file) {
        if (!current_file) {
            current_file = new WavFileWriter(filename_template, num_channels, sample_rate, depth, samples_per_file);
        }
        // write out as much as fits into the current file and move the pointer
        size_t missing = samples_per_file - current_file_samples_written;
        current_file->write(samples, missing * num_channels * depth, imu_data);
        samples += missing * num_channels * depth;
        available -= missing;
        // start a new file
        delete current_file;
        current_file = nullptr;
        current_file_samples_written = 0;
    }
    // if there are samples left, write them to the current file
    if (available) {
        if (!current_file) {
            current_file = new WavFileWriter(filename_template, num_channels, sample_rate, depth, samples_per_file);
        }
        current_file->write(samples, available * num_channels * depth, imu_data);
        current_file_samples_written += available;
    }
}


SplitIMUWavFileWriter::SplitIMUWavFileWriter(std::string &filename_template, std::string &imu_name_template,
                                             size_t num_channels, size_t sample_rate, size_t depth, size_t samples_per_file):
        FileWriter(filename_template, num_channels, sample_rate, depth),
        imu_name_template(imu_name_template),
        samples_per_file(samples_per_file),
        current_file(nullptr),
        imu_file(nullptr),
        current_file_samples_written(0),
        max_timestamp(0) {
    // nothing
}

SplitIMUWavFileWriter::~SplitIMUWavFileWriter() {
    if (current_file) {
        delete current_file;
    }
    if (imu_file) {
        delete imu_file;
    }
}

void SplitIMUWavFileWriter::write(uint8_t *samples, size_t num_samples, uint8_t* imu_data) {
    size_t available = num_samples /(num_channels * depth);
    // start as many new files as required to write out the samples
    while (current_file_samples_written + available >= samples_per_file) {
        if (!current_file) {
            current_file = new WavFileWriter(filename_template, num_channels, sample_rate, depth, samples_per_file);
            if (imu_file) {
                max_timestamp = imu_file->get_last_timestamp();
                delete imu_file;
                imu_file = new IMUFileWriter(imu_name_template, num_channels, sample_rate, depth, max_timestamp);
            }
        }
        if (!imu_file) imu_file = new IMUFileWriter(imu_name_template, num_channels, sample_rate, depth, max_timestamp);
        // write out as much as fits into the current file and move the pointer
        size_t missing = samples_per_file - current_file_samples_written;
        current_file->write(samples, missing * num_channels * depth, imu_data);
        if (imu_data) imu_file->write(samples, missing * num_channels * depth, imu_data);
        imu_data = nullptr; // prevent multiple writing
        samples += missing * num_channels * depth;
        available -= missing;
        // start a new file
        delete current_file;
        max_timestamp = imu_file->get_last_timestamp();
        delete imu_file;
        current_file = nullptr;
        imu_file = nullptr;
        current_file_samples_written = 0;
    }
    // if there are samples left, write them to the current file
    if (available) {
        if (!current_file) {
            current_file = new WavFileWriter(filename_template, num_channels, sample_rate, depth, samples_per_file);
            if (imu_file) {
                max_timestamp = imu_file->get_last_timestamp();
                delete imu_file;
                imu_file = new IMUFileWriter(imu_name_template, num_channels, sample_rate, depth, max_timestamp);
            }
        }
        if (!imu_file) imu_file = new IMUFileWriter(imu_name_template, num_channels, sample_rate, depth, max_timestamp);
        current_file->write(samples, available * num_channels * depth, imu_data);
        if (imu_data) imu_file->write(samples, available * num_channels * depth, imu_data);
        imu_data = nullptr;
        current_file_samples_written += available;
    }

}