#pragma once

#include <string>
#include <vector>

#include <Eigen/Dense>

double get_unix_time(int week, double seconds);

#define IE_SPLIT_SEC_LENGTH 1000

///////////////////////////////////////////////////////////
// Gps File

struct IERawData
{
    int week;
    double seconds;
    double unix_time;
    double latitude, longitude, height;
    double n_velocity, e_velocity, u_velocity;
    double roll, pitch, yaw;                    // radius
    int status;

    // the orientation is described in ros frame: front - left - up
    Eigen::Matrix3d getOrientation() const {
        return (Eigen::AngleAxisd(-yaw, Eigen::Vector3d::UnitZ()) *
                    Eigen::AngleAxisd(-pitch, Eigen::Vector3d::UnitY()) *
                    Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()))
                .toRotationMatrix();
    }

    static bool parseFromString(std::string line, IERawData &ie_data);
};

class IEFileReader
{
public:
    IEFileReader(){}
    ~IEFileReader(){}

    int loadIEFile(std::string ieFile, double start_time, double end_time,
                   bool split = false, std::string outputPath = "");
    int loadIEStartFile(std::string ieStartFile);

    IERawData getIEData(double unix_time);
    inline size_t cursor() { return cursor_; }

    std::vector<IERawData> raw_data;
    IERawData start_data;

private:
    size_t cursor_ = 0;
};

///////////////////////////////////////////////////////////
// Imu File

struct IMURawData
{
    int week;
    double seconds;
    double unix_time;
    double x_acc, y_acc, z_acc;
    double x_angular, y_angular, z_angular; // radius

    static bool parseFromString(std::string line, int type, IMURawData &ie_data);
};

class IMUFileReader
{
public:
    IMUFileReader() {}
    ~IMUFileReader() {}

    int loadIMUFile(std::string imuFile, int type, double start_time, double end_time);
    void guessIMUBias(int count, Eigen::Vector3d &acc_bias, Eigen::Vector3d &gyro_bias);

    std::vector<IMURawData> raw_data;

private:
    int makeFakeIMU(double start_time, double end_time);
};

///////////////////////////////////////////////////////////
// Wheel File

struct WheelRawData
{
    int week;
    double seconds;
    double unix_time;
    double movement, total_movement;
};

class WheelFileReader
{
public:
    WheelFileReader() : total_movement(0) {}
    ~WheelFileReader() {}

    int loadWheelFile(std::string wheelFile, double start_time, double end_time);
    std::vector<WheelRawData> raw_data;

private:
    bool parseFromString(std::string line, WheelRawData &wheel_data);
    double total_movement;
};

///////////////////////////////////////////////////////////
// Odometry File

struct OdometryRawData
{
    double unix_time;
    double x, y, z;
    double qw, qx, qy, qz;

    static bool parseFromString(std::string line, OdometryRawData &odom_data);
};

class OdometryFileReader
{
public:
    OdometryFileReader() {}
    ~OdometryFileReader() {}

    int loadOdometryFile(std::string odomFile, double start_time, double end_time);
    std::vector<OdometryRawData> raw_data;
};

///////////////////////////////////////////////////////////
// Combine File

enum RawDataType {
    GNSS,
    IMU,
    WHEEL,
    ODOM
};

struct TimedRawData {
    double unix_time;
    RawDataType type;
    union {
        IERawData gnss;
        IMURawData imu;
        WheelRawData wheel;
        OdometryRawData odom;
    };

    TimedRawData(const IERawData &gnss) : unix_time(gnss.unix_time), type(RawDataType::GNSS), gnss(gnss) {}
    TimedRawData(const IMURawData &imu) : unix_time(imu.unix_time), type(RawDataType::IMU), imu(imu) {}
    TimedRawData(const WheelRawData &wheel) : unix_time(wheel.unix_time), type(RawDataType::WHEEL), wheel(wheel) {}
    TimedRawData(const OdometryRawData &odom) : unix_time(odom.unix_time), type(RawDataType::ODOM), odom(odom) {}

    bool operator<(const TimedRawData &other) const { return unix_time < other.unix_time; }
    bool operator>(const TimedRawData &other) const { return unix_time > other.unix_time; }
};