#ifndef COMMON_BASE_H
#define COMMON_BASE_H

#include <time.h>

#include <array>
#include <cmath>
#include <cstdint>
#include <limits>
#include <map>
#include <string>

#include "basic_type.h"

namespace jf {
namespace common {

struct TimeStamp {
    uint32_t sec;
    uint32_t nanosec;

    TimeStamp() : sec(0), nanosec(0) {}
    TimeStamp(uint32_t _sec, uint32_t _nsec) : sec(_sec), nanosec(_nsec) {}

    double ToSec(void) const { return (double)sec + 1e-9 * (double)nanosec; }

    uint64_t ToTicks(void) const {
        const uint64_t TicksPerSecond = 10000 * 1000, NanosecondsPerTick = 100;
        uint64_t ticks = (sec * TicksPerSecond) + (nanosec / NanosecondsPerTick);
        return ticks;
    }
};

//----------------------------
struct Point {
    double x;
    double y;
    double z;
    Point() : x(0.0), y(0.0), z(0.0) {}
};

struct Point2D {
    float32_t x;
    float32_t y;
    Point2D() : x(0.f), y(0.f) {}
};

struct Point3D {
    float32_t x;
    float32_t y;
    float32_t z;

    Point3D() : x(0.f), y(0.f), z(0.f) {}
};
//----------------------------
struct Point32 {
    float32_t x;
    float32_t y;
    float32_t z;

    Point32() : x(0.0f), y(0.0f), z(0.0f) {}
};
//----------------------------
// For ROS motion planning node communication test usage, to be removed
struct DynamicBorders {
    float32_t dx;
};
//----------------------------
// For ROS motion planning node communication test usage, to be removed
struct MotionPlanning {
    float32_t trajectery_test;
};

//----------------------------
//----------------------------
struct TrajectoryPoints {
    TrajectoryPoints() : ds(0.0f), dx(0.0f), dy(0.0f), vx(0.0f), vy(0.0f), ax(0.0f), ay(0.0f), t(0.0f), valid(false) {}

    float32_t ds;
    float32_t dx;
    float32_t dy;
    float32_t vx;
    float32_t vy;
    float32_t ax;
    float32_t ay;
    float32_t t;
    bool valid;
};
//----------------------------
struct TarTrajectory {
    TarTrajectory() : trajectory() {}

    std::array<TrajectoryPoints, 100> trajectory;  // target trajecotry consists of a set of points
};
//----------------------------
struct VehicleControl {
    VehicleControl() : steering_angle_lat(0.0f), steering_torque_lat(0.0f), engine_torque(0.0f), engine_pedal(0.0f), engine_a_des(0), brake_torque(0.0f), brake_pedal(0.0f), brake_a_des(0.0f), steering_status(0), long_control_status(0) {}

    float32_t steering_angle_lat;   // Lateral steering angle control command
    float32_t steering_torque_lat;  // Lateral steering torque control command

    float32_t engine_torque;  // Longitudinal control command to EMS/VCU
    float32_t engine_pedal;
    float32_t engine_a_des;  // Longitudinal control command to brake system
    float32_t brake_torque;
    float32_t brake_pedal;
    float32_t brake_a_des;

    uint8_t steering_status;      // Lateral control status
    uint8_t long_control_status;  // Longitudinal control command status to EMS/VCU
    uint8_t lat_control_status;
};

//----------------------------
struct Vector3 {
    double x;
    double y;
    double z;
    Vector3() : x(0.0), y(0.0), z(0.0) {}
};

struct Quaternion {
    double x;
    double y;
    double z;
    double w;
    Quaternion() : x(0.0), y(0.0), z(0.0), w(0.0) {}
};

struct Pose {
    Point position;
    Quaternion orientation;
};
struct Twist {
    Vector3 linear;
    Vector3 angular;
};

//----------------------------
struct EgoMotion {
    Pose pose;                       // Pose information
    Twist twist;                     // Twist information
    Vector3 linear_acceleration;     // Linear Acceleration in 3 dimensions
    double velocity_covar[3][3];     // Only X velocity measured, so currently only
                                     // covariance of this
    double angular_vel_covar[3][3];  // Angular velocity covariance
    double accel_covar[3][3];        // Acceleration covariance
    double orient_covar[3][3];       // Orientation covariance
};

struct ClothoidModel {
    uint8_t model_degree;
    float32_t c0_position;
    float32_t c1_heading_angle;
    float32_t c2_curvature;
    float32_t c3_curvature_derivative;

    ClothoidModel() : model_degree(3), c0_position(0.f), c1_heading_angle(0.f), c2_curvature(0.f), c3_curvature_derivative(0.f) {}
};

enum {
    CATEGORY_UNKNOWN = 0,
    CATEGORY_PEDESTRIAN = 1,
    CATEGORY_CYCLIST = 2,
    CATEGORY_BUS = 3,
    CATEGORY_SMALL_MEDIUM_CAR = 4,
    CATEGORY_TRUCKS = 5,
    CATEGORY_TRI_MOTOR = 6,
    CATEGORY_SPECIAL_VEHICLE = 7,
    CATEGORY_TINY_CAR = 8,
    CATEGORY_AMBULANCE = 9,
    CATEGORY_UNCERTAIN_VEHICLE = 10
};
typedef uint8_t ObjCategoryType;

enum { NEAR_SIDE_REAR = 0, NEAR_SIDE_FRONT = 1, NEAR_SIDE_RIGHT = 2, NEAR_SIDE_LEFT = 3, NEAR_SIDE_UNKNOWN = 4 };
typedef uint8_t ObjNearSideType;

enum { DEBUG = 0, INFO = 1, WARN = 2, ERROR = 3, FATAL = 4 };
typedef uint8_t LogLevel;

}  // namespace common
}  // namespace jf

//----------------------------

#endif  // COMMON_BASE_H
