carlet.hpp

#include <iostream>
#if 0 // example

#include <ctime>

#define CARLET_IMPLEMENTATION
#include "carlet.hpp"

void naive_plan(const carlet::Veh::SensorData& sensor_data, const carlet::Veh::State& ego_state,
    carlet::Veh::Control& ctrl)
{
    (void)sensor_data;
    (void)ego_state;
    
    ctrl.steer = 0.0f;
    ctrl.accel = 1.0f;
}

int main(int argc, char** argv)
{
    (void)argc;
    (void)argv;

    srand(time(NULL));

    const auto straight_road{carlet::Road::gen_straight(
        Vector3{.x=0.0f, .y=0.0f, .z=0.0f},
        Vector3{.x=2000.0f, .y=0.0f, .z=0.0f},
        2, 3.7f)};

    auto sim{carlet::Simulator::instance()};
    sim->map().road_net.push_back(straight_road);
    sim->create_ctrl_veh(carlet::veh_model::tesla, 1);
    sim->gen_random_vehs(80,
        carlet::kmph_to_mps(40.0),
        carlet::kmph_to_mps(120.0));

    carlet::Veh* v{};
    carlet::Veh::Control ctrl{};

    while (sim->is_running()) {
        if ((v = sim->get_ctrl_veh()) != nullptr) {
            naive_plan(v->sensor_data(), v->state(), ctrl);
            v->act(ctrl);
        }
        sim->step(0.02f);
        sim->render();
    }
    return 0;
}

#endif // example

#define CARLET_IMPLEMENTATION

#ifndef CARLET_HPP_
#define CARLET_HPP_

#include <mutex>
#include <cmath>
#include <vector>
#include <unordered_map>
#include <raylib.h>

#define CARLET_DEF_SINGLETON(classname)                           \
    public:                                                       \
        static inline classname* instance()                       \
        {                                                         \
            static classname *instance_ = nullptr;                \
            static std::once_flag flag;                           \
            if (!instance_) {                                     \
                std::call_once(flag, [&](){                       \
                    instance_ = new (std::nothrow) classname();   \
                });                                               \
            }                                                     \
            return instance_;                                     \
        }                                                         \
    private:                                                      \
        classname(const classname&) = delete;                     \
        classname& operator=(const classname&) = delete;          \
        classname(const classname&&) = delete;                    \
        classname& operator=(const classname&&) = delete;


namespace carlet {

class Simulator;
struct Veh;

struct Road
{
    struct Strip {
        Vector3 l;  // left line edge
        Vector3 r;  // right line edge
    }; // struct Strip

    struct LaneSample {
        Vector3 c;  // lane center sample point
        float width;
    }; // struct LaneSample

    using Lane = std::vector<LaneSample>;
    using Lanelet = std::vector<Strip>;
    using RoadEdge = std::vector<Strip>;

    std::vector<Lane> lanes;
    std::vector<Lanelet> lanelets;
    RoadEdge left_edge;
    RoadEdge right_edge;

    static Road gen_straight(const Vector3& start_position, const Vector3& end_position, int num_lane, float lane_width);
}; // struct Road

using RoadNet = std::vector<Road>;

struct Map
{
    RoadNet road_net;
    Mesh road_mesh;
    Model road_model;
    Mesh edge_mesh;
    Model edge_model;
    Mesh lanelet_mesh;
    Model lanelet_model;
}; // struct Map

struct VehModel
{
    float wheel_base;
    float wheel_radius;
    float gc_to_back_axle;  // gc: gravity center
    float max_eps_angle;
    float min_eps_angle;
    float eps_to_wheel_angle;   // 1 deg wheel_angle => eps_to_wheel_angle eps_angle
    float max_accel;
    float Cd;              // Coefficient of Drag
    float mass;
    float max_hp;          // max horse power
    float hp_loss;         // hp loss
    float gear_friction;   // gear friction
    Vector3 shape;
}; // struct VehModel

struct Object
{
    Object() : Object({.x=0.0f, .y=0.0f, .z=0.0f}) {}
    explicit Object(const Vector3& shp);

    const int id;
    Vector3 shape;
    Model model;
    Color color;
    friend class Simulator;
private:
    virtual bool step(float dt) { return true; }
}; // struct Object

struct Veh: public Object
{
    struct Obs
    {
        std::vector<Veh*>& vehs;
        Map& map;
    }; // struct Obs

    struct TorqControl
    {
        float eps_torq;
        float accel;
    }; // struct TorqControl

    struct AngleControl
    {
        float eps_angle;
        float accel;
    }; // struct AngleControl

    struct Obstacle
    {
        Vector3 center;
        Vector3 shape;
        float heading;
        float vel;
    }; // struct Obstacle

    struct SensorData
    {
        std::vector<Obstacle> obsts;
        std::unordered_map<int, std::vector<Vector3>> lanelets;
    }; // struct SensorData

    struct Eps
    {
        float angle;    // rad
        float spd;      // rad/s 
        float torq;     // N/m
    }; // struct Eps

    struct State
    {
        double x;
        double y;
        double z;
        double vel;
        double accel;
        double jerk;
        double yaw;
        double yaw_rate;
        double steer_angle;

        Vector3 position() const { return Vector3{.x=static_cast<float>(x),
                                                  .y=static_cast<float>(y),
                                                  .z=static_cast<float>(z)}; }
        double distance(const Vector3& other) const;
        double distance(const State& other) const;
        static State init_with(float init_x, float init_y, float init_z, float init_vel);
    }; // struct State

    Veh(float init_vel, const VehModel& model)
        : Veh(0.0, 0.0f, init_vel, model, false)
    {}

    Veh(float init_x, float init_y, float init_vel, const VehModel& model, bool controllable)
        : Object(model.shape)
        , front_wheel_steer_(0.0f)
        , accelerator_ratio_(0.0f)
        , brake_ratio_(0.0f)
        , state_(State::init_with(init_x, init_y, model.shape.z / 2.0f, init_vel))
        , vm(model)
        , idm_cruise_vel_(init_vel)
        , controllable_(controllable)
        , valid_(true)
    {}

    void act(const TorqControl& control);
    void act(const AngleControl& control);
    inline const State& state() const { return state_; }
    inline const SensorData& sensor_data() const { return sensor_data_; }
    inline bool valid() const { return valid_; }
    inline const Eps& eps() const { return eps_; }

    const VehModel vm;
private:
    friend class Simulator;
    float front_wheel_steer_;
    float accelerator_ratio_;
    float brake_ratio_;
    Eps eps_;
    State state_;
    SensorData sensor_data_;
    const float idm_cruise_vel_;
    const bool controllable_;
    bool valid_;

    float wheel_torque(float accelerator_ratio) const;
    float wheel_friction() const;
    float air_resistance() const;
    float wheel_rot_spd() const;    // r/min
    void lat_control(const TorqControl& control);
    void lat_control(const AngleControl& control);
    void lon_control(float accel);
    void dynamic_act(float steer, float accel, float dt);
    void idm_act(const AngleControl& control);
    bool idm_plan(const Veh::Obs& full_obs, AngleControl& control);
    bool idm_reset_position(const Vector3& ref_position, const Simulator* simulator);
    bool step(float dt);
}; // struct Veh

class Simulator
{
    CARLET_DEF_SINGLETON(Simulator)
public:
    ~Simulator();
    bool is_running();
    bool step(float dt);
    void render();

    int create_ctrl_veh(const VehModel& model, int lane_idx);
    void gen_random_vehs(int n, float min_vel, float max_vel);
    Veh* get_ctrl_veh(int idx) const;
    bool collision_with_any_veh(const Veh* veh) const;

    inline Veh::Obs full_obs() { return Veh::Obs{.vehs=vehs_, .map=map_}; }
    inline Veh* get_ctrl_veh() const { return get_ctrl_veh(first_ctrl_idx_); };
    inline Map& map() { return map_; }
    inline const Map& map() const { return map_; }
private:
    Simulator();
    void map_to_mesh_model();
    void update_camera();
    void draw_mini_map();
    void draw_control_info();
    void sensing(Veh* ego, float dt);
    void sensing_obsts(Veh* ego);
    void sensing_lanelets(Veh* ego);

    std::vector<Veh*> vehs_;
    Camera3D camera_;
    Map map_;
    int first_ctrl_idx_;
}; // class Simulator

#define CARLET_PI   3.14159265358979323846
#define CARLET_2_PI 6.283185307179586

#define CARLET_G    9.8                     // m/s^2

template<typename T>
struct AlwaysFalse { enum {value = false}; };

template<typename T>
inline constexpr T pow2(T v) { return v * v; }

template<typename T>
inline constexpr T pow3(T v) { return v * v * v; }

template<typename T>
inline constexpr T min(T a, T b) { return a > b ? b : a; }

template<typename T>
inline constexpr T max(T a, T b) { return a > b ? a : b; }

template<typename T>
inline constexpr T abs(T a) { return a > 0 ? a : -a; }

template<typename T>
inline constexpr T clamp(T v, T low, T high) { return v > high ? high : (v < low ? low : v); }

template<typename T>
inline constexpr T mps_to_kmph(T mps) { return mps * static_cast<T>(3.6); }

template<typename T>
inline constexpr T kmph_to_mps(T kmph) { return kmph / static_cast<T>(3.6); }

template<typename T>
inline constexpr T hp_to_kw(T hp) { return hp * static_cast<T>(0.735); }

template<typename T>
inline constexpr T kw_to_hp(T kw) { return kw * static_cast<T>(1.36); }

template<typename T>
inline constexpr T rad_to_deg(T rad) { return rad / CARLET_PI * 180.0; }

template<typename T>
inline constexpr T deg_to_rad(T deg) { return deg / 180.0 * CARLET_PI; }

template<typename T>
inline float distance(const T& a, const T& b)
{
    static_assert(AlwaysFalse<T>::value, "Not supportted");
    return 0.0f;    // supress compiler warning
}

template<>
inline float distance<Vector3>(const Vector3& a, const Vector3& b)
{
    return std::sqrt(pow2(a.x - b.x) + pow2(a.y - b.y) + pow2(a.z - b.z));
}

template<>
inline float distance<Vector2>(const Vector2& a, const Vector2& b)
{
    return std::sqrt(pow2(a.x - b.x) + pow2(a.y - b.y));
}

template<>
inline float distance<Veh>(const Veh& a, const Veh& b)
{
    return std::sqrt(pow2(a.state().x - b.state().x) +
                     pow2(a.state().y - b.state().y) +
                     pow2(a.state().z - b.state().z));
}

template<typename T>
inline constexpr T normalize_heading(T rad)
{
    while (rad > CARLET_PI) {
        rad -= CARLET_2_PI;
    }
    while (rad < -CARLET_PI) {
        rad += CARLET_2_PI;
    }
    return rad;
}

namespace veh_model {

extern const VehModel tesla;

}; // namespace veh_model

} // namespace carlet

#ifdef _GLIBCXX_OSTREAM
std::ostream& operator<<(std::ostream& os, const Vector2& vec);
std::ostream& operator<<(std::ostream& os, const Vector3& vec);
std::ostream& operator<<(std::ostream& os, const carlet::Veh::State& state);
#endif // _GLIBCXX_OSTREAM

#endif // CARLET_HPP_


#ifdef CARLET_IMPLEMENTATION
#ifndef CARLET_CPP_
#define CARLET_CPP_

#include <cassert>
#include <rlgl.h>
#include <raymath.h>

#ifdef _GLIBCXX_OSTREAM
#   include <iomanip>
#endif // _GLIBCXX_OSTREAM


#ifndef CARLET_EPSf
#   include <limits>
#   define CARLET_EPSf              std::numeric_limits<float>::epsilon()
#endif // CARLET_EPSf

#ifndef CARLET_EPS
#   include <limits>
#   define CARLET_EPS               std::numeric_limits<double>::epsilon()
#endif // CARLET_EPS

#ifndef CARLET_MAX_ID
#   define CARLET_MAX_ID            100000
#endif // CARLET_MAX_ID

#ifndef CARLET_TARGET_FPS
#   define CARLET_TARGET_FPS        50
#endif // CARLET_TARGET_FPS

#ifndef CARLET_MAX_SENSOR_RANGE
#   define CARLET_MAX_SENSOR_RANGE  100.0   // meter
#endif // CARLET_MAX_SENSOR_RANGE

#ifndef CARLET_WIN_WIDTH
#   define CARLET_WIN_WIDTH         1000
#endif // CARLET_WIN_WIDTH

#ifndef CARLET_WIN_HEIGHT
#   define CARLET_WIN_HEIGHT        600
#endif // CARLET_WIN_HEIGHT

#ifndef CARLET_WIN_TITLE
#   define CARLET_WIN_TITLE         "Carlet Simulator"
#endif // CARLET_WIN_TITLE

#ifndef CARLET_ROAD_EDGE_WIDTH
#   define CARLET_ROAD_EDGE_WIDTH   0.1f   // meter, which is 10cm
#endif // CARLET_ROAD_EDGE_WIDTH

#ifndef CARLET_LANELET_WIDTH
#   define CARLET_LANELET_WIDTH     0.1f   // meter, which is 10cm
#endif // CARLET_LANELET_WIDTH

#ifndef CARLET_MAX_GEN_VEH_TRIES
#   define CARLET_MAX_GEN_VEH_TRIES 10
#endif // CARLET_MAX_GEN_VEH_TRIES

#ifndef CARLET_IDM_GEN_MAX_DIST
#   define CARLET_IDM_GEN_MAX_DIST 300.0f
#endif // CARLET_IDM_MAX_DIST

#ifndef CARLET_IDM_GEN_MIN_DIST
#   define CARLET_IDM_GEN_MIN_DIST 100.0f
#endif // CARLET_IDM_MIN_DIST

#ifndef CARLET_AIR_DENSITY
#   define CARLET_AIR_DENSITY           1.293f   // kg/m3
#endif // CARLET_AIR_DENSITY

#ifndef CARLET_ROAD_FRICTION_FACTOR
#   define CARLET_ROAD_FRICTION_FACTOR  0.6f
#endif // CARLET_ROAD_FRICTION_FACTOR

#define CARLET_ARR_LEN(arr)         (sizeof(arr) / sizeof((arr)[0]))


#ifdef _GLIBCXX_OSTREAM
inline std::ostream& operator<<(std::ostream& os, const Vector2& vec)
{
    os << "(" << vec.x << ", " << vec.y << ")";
    return os;
}

inline std::ostream& operator<<(std::ostream& os, const Vector3& vec)
{
    os << "(" << vec.x << ", " << vec.y << ", " << vec.z << ")";
    return os;
}

inline std::ostream& operator<<(std::ostream& os, float* vec)
{
    os << "(" << vec[0] << ", " << vec[1] << ", " << vec[2] << ")";
    return os;
}

inline std::ostream& operator<<(std::ostream& os, const carlet::Veh::State& state)
{
    os << std::fixed << std::setprecision(2) << "State {"
       << "x: " << state.x
       << ", y: " << state.y
       << ", z: " << state.z
       << ", vel: " << state.vel << " (" << carlet::mps_to_kmph(state.vel) << ")"
       << ", accel: " << state.accel
       << ", jerk: " << state.jerk
       << ", yaw: " << state.yaw
       << ", yaw_rate: " << state.yaw_rate
       << ", steer_angle: " << state.steer_angle
       << "}";
    return os;
}
#endif // _GLIBCXX_OSTREAM

namespace carlet {

struct Vector2i
{
    int x;
    int y;
};

struct Vector3i
{
    int x;
    int y;
    int z;
};

const Mesh& gen_veh_mesh(const Vector3& shape);
bool find_lane_info(const std::vector<Road::Lane>& lanes, const Vector3& p, float half_veh_width,
    int& lane_idx, int& waypoint_idx);
bool check_veh_collision(const Veh* a, const Veh* b);

template<typename T>
inline T rand_ab(T a, T b)
{
    assert(b > a);
    const auto rv01{static_cast<float>(rand()) / (static_cast<float>(RAND_MAX) + 1.0f)};
    return rv01 * (b - a) + a;
}

namespace veh_model {

const VehModel tesla {
    .wheel_base         = 2.8f,                     // meter
    .wheel_radius       = 0.33f,                    // meter
    .gc_to_back_axle    = 1.5f,                     // meter
    .max_eps_angle      = deg_to_rad(630.0f),       // rad
    .min_eps_angle      = deg_to_rad(-630.0f),      // rad
    .eps_to_wheel_angle = 14.4,
    .max_accel          = 5.0f,                     // m/s^2
    .Cd                 = 0.23f,                    // Coefficient of Drag
    .mass               = 1836.0f,                  // kg
    .max_hp             = 190.0f,                   // hp
    .hp_loss            = 0.17f,                    // precent of hp
    .gear_friction      = 400.0f,                   // gear friction, newton
    .shape              = Vector3{.x = 4.2f, .y = 1.98f, .z = 1.6f}
}; // tesla

static const VehModel all_veh_models[] {
    tesla
};

inline const VehModel& random() {
    constexpr auto num_models{CARLET_ARR_LEN(all_veh_models)};
    return all_veh_models[rand() % num_models];
}

}; // namespace veh_model

static const Color veh_colors[] {
    YELLOW,  GOLD,    ORANGE,     PINK,    RED,   MAROON,
    GREEN,   LIME,    DARKGREEN,  SKYBLUE, BLUE,  DARKBLUE,
    PURPLE,  VIOLET,  DARKPURPLE, BEIGE,   BROWN, DARKBROWN,
    MAGENTA, RAYWHITE
};

inline const Color& random_color()
{
    constexpr auto num_colors{CARLET_ARR_LEN(veh_colors)};
    return veh_colors[rand() % num_colors];
}

inline const Color& next_color()
{
    constexpr auto num_colors{CARLET_ARR_LEN(veh_colors)};

    static int i{0};
    static std::mutex lock{};
    std::lock_guard<std::mutex> guard{lock};
    return veh_colors[i++ % num_colors];
}

inline void to_raylib_mesh3(float* vec)
{
    const auto x{vec[0]};
    const auto y{vec[1]};
    const auto z{vec[2]};
    vec[0] = -y;
    vec[1] = z;
    vec[2] = -x;
}

inline Vector3 to_raylib(const Vector3& vec)
{
    return Vector3{.x=-vec.y, .y=vec.z, .z=-vec.x};
}

inline int gen_id()
{
    static int cnt{0};
    static std::mutex lock{};

    std::lock_guard<std::mutex> guard{lock};
    int result{cnt};
    cnt = (cnt + 1) % CARLET_MAX_ID;
    return result;
}

template<typename T, size_t N>
struct Intergrator
{
    void append(T v)
    {
        data[idx] = v;
        idx = (idx + 1) % N;
    }
    T sum()
    {
        T res{};
        for (int i = 0; i < N; ++i) res += data[i];
        return res;
    }
private:
    T data[N];
    int idx;
}; // struct Intergrator

Object::Object(const Vector3& shp) : id(gen_id()), shape(shp) {}

Veh::State Veh::State::init_with(float init_x, float init_y, float init_z, float init_vel)
{
    return State{
        .x = init_x,
        .y = init_y,
        .z = init_z,
        .vel = init_vel,
        .accel = 0.0f,
        .jerk = 0.0f,
        .yaw = 0.0f,
        .yaw_rate = 0.0f,
        .steer_angle = 0.0f,
    };
}

Road Road::gen_straight(const Vector3& start_position, const Vector3& end_position, int num_lane, float lane_width)
{
    constexpr auto min_lane_width{2.0f};    // meter
    constexpr auto sample_length{1.0f};     // meter
    constexpr auto half_road_edge_width{CARLET_ROAD_EDGE_WIDTH / 2.0f};

    const auto length{Vector3Distance(start_position, end_position)};
    assert(length > 0.0f && "Bad road length");
    assert(num_lane > 0 && "Bad number of lane");
    assert(lane_width > min_lane_width && "Bad lane width");

    const auto num_samples{max(static_cast<int>(length / sample_length), 1)};
    const auto road_width{num_lane * lane_width};

    Road road;
    road.left_edge.resize(num_samples);
    road.right_edge.resize(num_samples);
    if (num_lane >= 2) {
        road.lanelets.resize(num_lane - 1);
        for (auto& lanelet : road.lanelets) {
            lanelet.resize(num_samples);
        }
    }

    road.lanes.resize(road.lanelets.size() + 1);
    for (auto& lane: road.lanes) {
        lane.resize(num_samples);
    }

    auto calc_center_line{[&start_position, &end_position, length] (float s, Vector3& center_point) -> void {
        center_point.x = (end_position.x - start_position.x) * s / length + start_position.x;
        center_point.y = (end_position.y - start_position.y) * s / length + start_position.y;
        center_point.z = (end_position.z - start_position.z) * s / length + start_position.z;
    }};

    Vector3 sample_center_point{};
    const auto road_heading{std::atan2(end_position.y - start_position.y, end_position.x - start_position.x)};
    const auto cos_road_heading{std::cos(road_heading)};
    const auto sin_road_heading{std::sin(road_heading)};

    for (int i = 0; i < num_samples; ++i) {
        const auto sample_s{sample_length * i};
        calc_center_line(sample_s, sample_center_point);

        const auto left_edge_offset_r{road_width / 2.0f - half_road_edge_width};
        const auto left_edge_offset_l{road_width / 2.0f + half_road_edge_width};
        const auto right_edge_offset_r{-road_width / 2.0f - half_road_edge_width};
        const auto right_edge_offset_l{-road_width / 2.0f + half_road_edge_width};

        road.left_edge.at(i).l.x = sample_center_point.x + left_edge_offset_l * sin_road_heading;
        road.left_edge.at(i).l.y = sample_center_point.y + left_edge_offset_l * cos_road_heading;
        road.left_edge.at(i).l.z = sample_center_point.z;
        road.left_edge.at(i).r.x = sample_center_point.x + left_edge_offset_r * sin_road_heading;
        road.left_edge.at(i).r.y = sample_center_point.y + left_edge_offset_r * cos_road_heading;
        road.left_edge.at(i).r.z = sample_center_point.z;

        road.right_edge.at(i).l.x = sample_center_point.x + right_edge_offset_l * sin_road_heading;
        road.right_edge.at(i).l.y = sample_center_point.y + right_edge_offset_l * cos_road_heading;
        road.right_edge.at(i).l.z = sample_center_point.z;
        road.right_edge.at(i).r.x = sample_center_point.x + right_edge_offset_r * sin_road_heading;
        road.right_edge.at(i).r.y = sample_center_point.y + right_edge_offset_r * cos_road_heading;
        road.right_edge.at(i).r.z = sample_center_point.z;

        if (!road.lanelets.empty()) {
            for (size_t j = 0; j < road.lanelets.size(); ++j) {
                auto& lanelet{road.lanelets.at(j)};
                const auto left_lanelet_offset{-static_cast<int>(j + 1) * lane_width};
                lanelet.at(i).l.x = road.left_edge.at(i).l.x + left_lanelet_offset * sin_road_heading;
                lanelet.at(i).l.y = road.left_edge.at(i).l.y + left_lanelet_offset * cos_road_heading;
                lanelet.at(i).l.z = road.left_edge.at(i).l.z;
                const auto right_lanelet_offset{-CARLET_LANELET_WIDTH};
                lanelet.at(i).r.x = lanelet.at(i).l.x + right_lanelet_offset * sin_road_heading;
                lanelet.at(i).r.y = lanelet.at(i).l.y + right_lanelet_offset * cos_road_heading;
                lanelet.at(i).r.z = lanelet.at(i).l.z;
            }

            for (int j = 0; j <= static_cast<int>(road.lanelets.size()); ++j) {
                const auto left_lanelet_idx{j - 1};
                const auto right_lanelet_idx{j};

                const auto& left_edge_point{left_lanelet_idx < 0
                    ? road.left_edge.at(i).l
                    : road.lanelets.at(left_lanelet_idx).at(i).l};
                const auto& right_edge_point{right_lanelet_idx == road.lanelets.size()
                    ? road.right_edge.at(i).r
                    : road.lanelets.at(right_lanelet_idx).at(i).r};

                road.lanes.at(j).at(i).width = lane_width;
                road.lanes.at(j).at(i).c.x = (left_edge_point.x + right_edge_point.x) / 2.0f;
                road.lanes.at(j).at(i).c.y = (left_edge_point.y + right_edge_point.y) / 2.0f;
                road.lanes.at(j).at(i).c.z = sample_center_point.z;
            }
        } else {
            // single lane road
            road.lanes.at(0).at(i).width = lane_width;
            road.lanes.at(0).at(i).c.x = sample_center_point.x;
            road.lanes.at(0).at(i).c.y = sample_center_point.y;
            road.lanes.at(0).at(i).c.z = sample_center_point.z;
        }
    }

    return road;
}

bool Simulator::is_running()
{
    return !WindowShouldClose();
}

void Simulator::map_to_mesh_model()
{
    static bool mesh_generated{false};
    if (mesh_generated) return;
    mesh_generated = true;

    int num_lanelets{0};
    map_.road_mesh.vertexCount = 0;
    map_.edge_mesh.vertexCount = 0;
    map_.lanelet_mesh.vertexCount = 0;
    for (const auto& road : map_.road_net) {
        map_.edge_mesh.vertexCount += road.left_edge.size() * 2;
        map_.edge_mesh.vertexCount += road.right_edge.size() * 2;
        map_.road_mesh.vertexCount += max(road.right_edge.size(), road.left_edge.size()) * 2; // road

        for (const auto& lanelet : road.lanelets) {
            map_.lanelet_mesh.vertexCount += lanelet.size() * 2;
            ++num_lanelets;
        }
    }

    map_.road_mesh.triangleCount = map_.road_mesh.vertexCount - 2;
    map_.road_mesh.vertices = (float*)RL_CALLOC(3 * map_.road_mesh.vertexCount, sizeof(float)); // Vertex positions
    map_.road_mesh.normals = (float*)RL_CALLOC(3 * map_.road_mesh.vertexCount, sizeof(float)); // Normals
    map_.road_mesh.indices = (unsigned short*)RL_CALLOC(map_.road_mesh.triangleCount * 3, sizeof(unsigned short)); // Index data
    
    map_.edge_mesh.triangleCount = map_.edge_mesh.vertexCount - (2 * 2);
    map_.edge_mesh.vertices = (float*)RL_CALLOC(3 * map_.edge_mesh.vertexCount, sizeof(float)); // Vertex positions
    map_.edge_mesh.normals = (float*)RL_CALLOC(3 * map_.edge_mesh.vertexCount, sizeof(float)); // Normals
    map_.edge_mesh.indices = (unsigned short*)RL_CALLOC(map_.edge_mesh.triangleCount * 3, sizeof(unsigned short)); // Index data
    
    map_.lanelet_mesh.triangleCount = map_.lanelet_mesh.vertexCount - (2 * num_lanelets);
    map_.lanelet_mesh.vertices = (float*)RL_CALLOC(3 * map_.lanelet_mesh.vertexCount, sizeof(float)); // Vertex positions
    map_.lanelet_mesh.normals = (float*)RL_CALLOC(3 * map_.lanelet_mesh.vertexCount, sizeof(float)); // Normals
    map_.lanelet_mesh.indices = (unsigned short*)RL_CALLOC(map_.lanelet_mesh.triangleCount * 3, sizeof(unsigned short)); // Index data

    int v_idx{0};
    int t_idx{0};
    int num_v_last_line{0};
    for (const auto& road : map_.road_net) {
        // road
        int num_v_this_line{0};
        const size_t num_strip{min(road.left_edge.size(), road.right_edge.size())};
        for (size_t i = 0; i < num_strip; ++i) {
            const auto& left_strip{road.left_edge.at(i)};
            const auto& right_strip{road.right_edge.at(i)};
            map_.road_mesh.vertices[v_idx * 6 + 0] = left_strip.l.x;
            map_.road_mesh.vertices[v_idx * 6 + 1] = left_strip.l.y;
            map_.road_mesh.vertices[v_idx * 6 + 2] = left_strip.l.z;
            to_raylib_mesh3(map_.road_mesh.vertices + v_idx * 6);
            map_.road_mesh.vertices[v_idx * 6 + 3] = right_strip.r.x;
            map_.road_mesh.vertices[v_idx * 6 + 4] = right_strip.r.y;
            map_.road_mesh.vertices[v_idx * 6 + 5] = right_strip.r.z;
            to_raylib_mesh3(map_.road_mesh.vertices + v_idx * 6 + 3);
            map_.road_mesh.normals[v_idx * 6 + 0] = 0.0f;
            map_.road_mesh.normals[v_idx * 6 + 1] = 1.0f;
            map_.road_mesh.normals[v_idx * 6 + 2] = 0.0f;
            map_.road_mesh.normals[v_idx * 6 + 3] = 0.0f;
            map_.road_mesh.normals[v_idx * 6 + 4] = 1.0f;
            map_.road_mesh.normals[v_idx * 6 + 5] = 0.0f;
            num_v_this_line += 2;
            ++v_idx;
        }

        for (size_t i = 0; i < num_v_this_line - 2; i += 2) {
            map_.road_mesh.indices[t_idx * 6 + 0] = i;
            map_.road_mesh.indices[t_idx * 6 + 1] = i + 1;
            map_.road_mesh.indices[t_idx * 6 + 2] = i + 2;
            map_.road_mesh.indices[t_idx * 6 + 3] = i + 1;
            map_.road_mesh.indices[t_idx * 6 + 4] = i + 3;
            map_.road_mesh.indices[t_idx * 6 + 5] = i + 2;
            ++t_idx;
        }

        // left road edge
        num_v_this_line = 0;
        num_v_last_line = 0;
        v_idx = 0;
        t_idx = 0;
        for (const auto& strip: road.left_edge) {
            map_.edge_mesh.vertices[v_idx * 6 + 0] = strip.l.x;
            map_.edge_mesh.vertices[v_idx * 6 + 1] = strip.l.y;
            map_.edge_mesh.vertices[v_idx * 6 + 2] = strip.l.z;
            to_raylib_mesh3(map_.edge_mesh.vertices + v_idx * 6);
            map_.edge_mesh.vertices[v_idx * 6 + 3] = strip.r.x;
            map_.edge_mesh.vertices[v_idx * 6 + 4] = strip.r.y;
            map_.edge_mesh.vertices[v_idx * 6 + 5] = strip.r.z;
            to_raylib_mesh3(map_.edge_mesh.vertices + v_idx * 6 + 3);
            map_.edge_mesh.normals[v_idx * 6 + 0] = 0.0f;
            map_.edge_mesh.normals[v_idx * 6 + 1] = 1.0f;
            map_.edge_mesh.normals[v_idx * 6 + 2] = 0.0f;
            map_.edge_mesh.normals[v_idx * 6 + 3] = 0.0f;
            map_.edge_mesh.normals[v_idx * 6 + 4] = 1.0f;
            map_.edge_mesh.normals[v_idx * 6 + 5] = 0.0f;
            num_v_this_line += 2;
            ++v_idx;
        }

        for (size_t i = 0; i < num_v_this_line - 2; i += 2) {
            map_.edge_mesh.indices[t_idx * 6 + 0] = num_v_last_line + i;
            map_.edge_mesh.indices[t_idx * 6 + 1] = num_v_last_line + i + 1;
            map_.edge_mesh.indices[t_idx * 6 + 2] = num_v_last_line + i + 2;
            map_.edge_mesh.indices[t_idx * 6 + 3] = num_v_last_line + i + 1;
            map_.edge_mesh.indices[t_idx * 6 + 4] = num_v_last_line + i + 3;
            map_.edge_mesh.indices[t_idx * 6 + 5] = num_v_last_line + i + 2;
            ++t_idx;
        }

        // right road edge
        num_v_this_line = 0;
        num_v_last_line = v_idx * 2;
        for (const auto& strip: road.right_edge) {
            map_.edge_mesh.vertices[v_idx * 6 + 0] = strip.l.x;
            map_.edge_mesh.vertices[v_idx * 6 + 1] = strip.l.y;
            map_.edge_mesh.vertices[v_idx * 6 + 2] = strip.l.z;
            to_raylib_mesh3(map_.edge_mesh.vertices + v_idx * 6);
            map_.edge_mesh.vertices[v_idx * 6 + 3] = strip.r.x;
            map_.edge_mesh.vertices[v_idx * 6 + 4] = strip.r.y;
            map_.edge_mesh.vertices[v_idx * 6 + 5] = strip.r.z;
            to_raylib_mesh3(map_.edge_mesh.vertices + v_idx * 6 + 3);
            map_.edge_mesh.normals[v_idx * 6 + 0] = 0.0f;
            map_.edge_mesh.normals[v_idx * 6 + 1] = 1.0f;
            map_.edge_mesh.normals[v_idx * 6 + 2] = 0.0f;
            map_.edge_mesh.normals[v_idx * 6 + 3] = 0.0f;
            map_.edge_mesh.normals[v_idx * 6 + 4] = 1.0f;
            map_.edge_mesh.normals[v_idx * 6 + 5] = 0.0f;
            ++v_idx;
            num_v_this_line += 2;
        }

        for (size_t i = 0; i < num_v_this_line - 2; i += 2) {
            map_.edge_mesh.indices[t_idx * 6 + 0] = num_v_last_line + i;
            map_.edge_mesh.indices[t_idx * 6 + 1] = num_v_last_line + i + 1;
            map_.edge_mesh.indices[t_idx * 6 + 2] = num_v_last_line + i + 2;
            map_.edge_mesh.indices[t_idx * 6 + 3] = num_v_last_line + i + 1;
            map_.edge_mesh.indices[t_idx * 6 + 4] = num_v_last_line + i + 3;
            map_.edge_mesh.indices[t_idx * 6 + 5] = num_v_last_line + i + 2;
            ++t_idx;
        }

        v_idx = 0;
        t_idx = 0;
        for (const auto& lanelet : road.lanelets) {
            num_v_this_line = 0;
            num_v_last_line = v_idx * 2;
            for (const auto& strip: lanelet) {
                map_.lanelet_mesh.vertices[v_idx * 6 + 0] = strip.l.x;
                map_.lanelet_mesh.vertices[v_idx * 6 + 1] = strip.l.y;
                map_.lanelet_mesh.vertices[v_idx * 6 + 2] = strip.l.z;
                to_raylib_mesh3(map_.lanelet_mesh.vertices + v_idx * 6);
                map_.lanelet_mesh.vertices[v_idx * 6 + 3] = strip.r.x;
                map_.lanelet_mesh.vertices[v_idx * 6 + 4] = strip.r.y;
                map_.lanelet_mesh.vertices[v_idx * 6 + 5] = strip.r.z;
                to_raylib_mesh3(map_.lanelet_mesh.vertices + v_idx * 6 + 3);
                map_.lanelet_mesh.normals[v_idx * 6 + 0] = 0.0f;
                map_.lanelet_mesh.normals[v_idx * 6 + 1] = 1.0f;
                map_.lanelet_mesh.normals[v_idx * 6 + 2] = 0.0f;
                map_.lanelet_mesh.normals[v_idx * 6 + 3] = 0.0f;
                map_.lanelet_mesh.normals[v_idx * 6 + 4] = 1.0f;
                map_.lanelet_mesh.normals[v_idx * 6 + 5] = 0.0f;
                ++v_idx;
                num_v_this_line += 2;
            }

            for (size_t i = 0; i < num_v_this_line - 2; i += 2) {
                map_.lanelet_mesh.indices[t_idx * 6 + 0] = num_v_last_line + i;
                map_.lanelet_mesh.indices[t_idx * 6 + 1] = num_v_last_line + i + 1;
                map_.lanelet_mesh.indices[t_idx * 6 + 2] = num_v_last_line + i + 2;
                map_.lanelet_mesh.indices[t_idx * 6 + 3] = num_v_last_line + i + 1;
                map_.lanelet_mesh.indices[t_idx * 6 + 4] = num_v_last_line + i + 3;
                map_.lanelet_mesh.indices[t_idx * 6 + 5] = num_v_last_line + i + 2;
                ++t_idx;
            }
        }
    }

    UploadMesh(&map_.road_mesh, false);
    UploadMesh(&map_.edge_mesh, false);
    UploadMesh(&map_.lanelet_mesh, false);
    map_.road_model = LoadModelFromMesh(map_.road_mesh);
    map_.edge_model = LoadModelFromMesh(map_.edge_mesh);
    map_.lanelet_model = LoadModelFromMesh(map_.lanelet_mesh);
}

Simulator::Simulator()
    : first_ctrl_idx_(-1)
{
    InitWindow(CARLET_WIN_WIDTH, CARLET_WIN_HEIGHT, CARLET_WIN_TITLE);
    SetTargetFPS(CARLET_TARGET_FPS);

    camera_.position    = {.x=0.0f, .y=5.0f, .z=0.0f};
    camera_.target      = {.x=0.0f, .y=0.0f, .z=0.0f};
    camera_.up          = {.x=0.0f, .y=1.0f, .z=0.0f};
    camera_.fovy        = 45.0f;
    camera_.projection  = CAMERA_PERSPECTIVE;
}

Simulator::~Simulator()
{
    for (auto v: vehs_) {
        delete v;
    }
    vehs_.clear();
}

void Simulator::update_camera()
{
    camera_.fovy += GetMouseWheelMove() * -5.0f;
    camera_.fovy = clamp(camera_.fovy, 0.0f, 170.0f);

    static int last_left_down_x{-1};
    static int last_left_down_y{-1};
    static int last_right_down_x{-1};
    static int last_right_down_y{-1};

    static Vector3 camera_pos_offset{camera_.position};
    static Vector3 camera_target_offset{camera_.target};

    if (!vehs_.empty() && first_ctrl_idx_ >= 0) {
        // camera follow this first controllable car    
        const auto& veh{vehs_.at(first_ctrl_idx_)};
        camera_.target.z = camera_target_offset.z - veh->state().x - 80;
        camera_.target.x = camera_target_offset.x;
        camera_.target.y = camera_target_offset.y;
        camera_.position.z = camera_pos_offset.z - veh->state().x + 20;
        camera_.position.x = camera_pos_offset.x;
        camera_.position.y = camera_pos_offset.y;
    } else {
        camera_.target.z = camera_target_offset.z;
        camera_.target.x = camera_target_offset.x;
        camera_.target.y = camera_target_offset.y;
        camera_.position.z = camera_pos_offset.z;
        camera_.position.x = camera_pos_offset.x;
        camera_.position.y = camera_pos_offset.y;
    }

    if (IsMouseButtonPressed(MOUSE_LEFT_BUTTON)) {
        last_left_down_x = -1;
        last_left_down_y = -1;
    } else if (IsMouseButtonDown(MOUSE_LEFT_BUTTON)) {
        const auto this_left_down_x{GetMouseX()};
        const auto this_left_down_y{GetMouseY()};
        if (last_left_down_x > 0 || last_left_down_y > 0) {
            const auto delta_x{this_left_down_x - last_left_down_x};
            const auto delta_y{this_left_down_y - last_left_down_y};
            camera_pos_offset.x -= delta_x * 0.03f;
            camera_pos_offset.z -= delta_y * 0.03;
            camera_target_offset.x -= delta_x * 0.03f;
            camera_target_offset.z -= delta_y * 0.03;
        }
        last_left_down_x = this_left_down_x;
        last_left_down_y = this_left_down_y;
    } else if (IsMouseButtonPressed(MOUSE_BUTTON_RIGHT)) {
        last_right_down_x = -1;
        last_right_down_y = -1;
    } else if (IsMouseButtonDown(MOUSE_BUTTON_RIGHT)) {
        const auto this_right_down_x{GetMouseX()};
        const auto this_right_down_y{GetMouseY()};
        if (last_right_down_x > 0 || last_right_down_y > 0) {
            const auto delta_x{this_right_down_x - last_right_down_x};
            const auto delta_y{this_right_down_y - last_right_down_y};
            camera_target_offset.y += delta_y * 0.5f;
            camera_target_offset.x -= delta_x * 0.1f;
        }
        last_right_down_x = this_right_down_x;
        last_right_down_y = this_right_down_y;
    }
    UpdateCamera(&camera_, CAMERA_CUSTOM);
}

void Simulator::render()
{
    update_camera();

    static const Vector3 zero_vec{};
    BeginDrawing();
        ClearBackground(GRAY);
        BeginMode3D(camera_);
            {
                // draw map
                map_to_mesh_model();
                DrawModel(map_.road_model, zero_vec, 1.0f, DARKGRAY);
                DrawModel(map_.edge_model, zero_vec, 1.0f, BLACK);
                DrawModel(map_.lanelet_model, zero_vec, 1.0f, YELLOW);
            }
            {
                // draw vehicles
                for (const auto& veh: vehs_) {
                    if (!veh->valid()) continue;
                    DrawModel(veh->model, zero_vec, 1.0f, veh->color);
                    DrawModelWires(veh->model, zero_vec, 1.0f, WHITE);
                }
            }
        EndMode3D();

        // Draw in 2d space
        {
            // Show veh speed
            if (!vehs_.empty() && first_ctrl_idx_ >= 0) {
                const auto veh{vehs_.at(first_ctrl_idx_)};
                constexpr auto font_size{32};
                constexpr auto buf_size{32};
                constexpr auto str_len{8};   // I guess
                const auto str_position_x{GetScreenWidth() / 2 - (str_len / 2) * font_size / 2};
                char buf[buf_size];
                snprintf(buf, buf_size, "%d KM/H (%.1lf)",
                    static_cast<int>(mps_to_kmph(veh->state().vel)),
                    veh->state().accel);
                DrawText(buf, str_position_x, 10, font_size, RED);
            }
        }

        // draw mini map for the first controllable vehicle
        draw_mini_map();
        // draw control info for the first controllable vehicle
        draw_control_info();
    EndDrawing();
}

void Simulator::draw_control_info()
{
    constexpr Color panel_bg{.r=100, .g=100, .b=100, .a=100};

    const Vector2 panel_position{.x=0, .y=0};
    const Vector2 panel_size{.x=120.0f, .y=200.0f};

    DrawRectangleV(panel_position, panel_size, panel_bg);
    const auto& ego{vehs_.at(0)};

    {
        auto norm_0_360{[] (float angle) -> float {
            while (angle > 360.0f) { angle -= 360.0f; }
            while (angle < -360.0f) { angle += 360.0f; }
            return angle;
        }};

        // draw eps
        constexpr Color inner_color{.r=panel_bg.r, .g=panel_bg.g, .b=panel_bg.b, .a=255};
        constexpr Color outer_color{.r=60, .g=60, .b=60, .a=255};
        const Vector2 center{.x=(panel_position.x + panel_size.x) / 2, .y=40.f};
        const auto inner_radius{25.0f};
        const auto outer_radius{35.0f};
        const auto eps_angle{rad_to_deg(ego->eps_.angle)}; // deg
        const auto start_angle{norm_0_360(50.0f - eps_angle)};
        const auto end_angle{norm_0_360(130.0f - eps_angle)};

        DrawCircleV(center, outer_radius, outer_color);
        DrawCircleV(center, inner_radius, inner_color);
        DrawCircleSector(center, outer_radius, start_angle, end_angle, 1, outer_color);
    }

    // common const for drawing accelerator and brake
    constexpr Color axle_color{.r=10, .g=10, .b=10, .a=255};
    constexpr Color axle_bg_color{.r=150, .g=150, .b=150, .a=255};
    constexpr auto pedal_width{20.0f};
    constexpr auto pedal_high{5.0f};
    constexpr auto axle_width{10.0f};
    constexpr auto axle_high{50.0f};
    constexpr auto base_y{80.0f};

    auto draw_pedal{[&] (const Color pedal_color, float pedal_value, float base_x) -> void {
        const Vector2 axle_bg{.x=base_x, .y=base_y};
        const Vector2 axle{.x=base_x, .y=base_y};
        const Vector2 pedal{
            .x=base_x - (pedal_width - axle_width) / 2,
            .y=base_y - pedal_high + axle_high - pedal_value * axle_high
        };
        const Vector2 axle_size{
            .x=axle_width,
            .y=axle_high - pedal_value * axle_high
        };

        DrawRectangleV(axle_bg, {.x=axle_width, .y=axle_high}, axle_bg_color);
        DrawRectangleV(axle, axle_size, axle_color);
        DrawRectangleV(pedal, {.x=pedal_width, .y=pedal_high}, pedal_color);
    }};

    {
        // draw brake
        constexpr Color pedal_color{.r=150, .g=20, .b=20, .a=255};
        constexpr auto base_x{20.0f};
        draw_pedal(pedal_color, ego->brake_ratio_, base_x);
    }

    {
        // draw accelerator
        constexpr Color pedal_color{.r=20, .g=150, .b=20, .a=255};
        constexpr auto base_x{100.0f};
        draw_pedal(pedal_color, ego->accelerator_ratio_, base_x);
    }
}


void Simulator::draw_mini_map()
{
    constexpr Color panel_bg{.r=100, .g=100, .b=100, .a=100};
    constexpr auto scale{5.0f};
    const Vector2 panel_size{.x=200.0f, .y=static_cast<float>(GetScreenHeight())};
    const Vector2 panel_position{.x=(static_cast<float>(GetScreenWidth()) - panel_size.x), .y=0};
    const Vector2 map_center{
        .x=panel_position.x + panel_size.x * 0.5f,
        .y=panel_position.y + panel_size.y * 0.9f
    };

    if (GetScreenWidth() < panel_size.x) {
        return;
    }

    // Draw background
    DrawRectangleV(panel_position, panel_size, panel_bg);
    const auto& ego{vehs_.at(0)};
    const Veh::SensorData& sensor_data{ego->sensor_data()};

    auto to_mini_map{[&] (float x, float y, float& map_x, float& map_y) -> void {
        map_x = map_center.x - y * scale;
        map_y = map_center.y - x * scale;
    }};

    {
        // draw ego
        float ego_center_x{};
        float ego_center_y{};
        to_mini_map(0, 0, ego_center_x, ego_center_y);

        const Rectangle ego_rec{
            .x=ego_center_x - ego->shape.y * scale / 2,
            .y=ego_center_y,
            .width=ego->shape.y * scale,
            .height=ego->shape.x * scale
        };
        DrawRectanglePro(ego_rec, Vector2{0, 0}, 0.0, RED);
    }

    // draw obstacle
    for (const auto& obst: sensor_data.obsts) {
        float obst_center_x{};
        float obst_center_y{};
        to_mini_map(obst.center.x, obst.center.y, obst_center_x, obst_center_y);

        const Rectangle obst_rec{
            .x=obst_center_x - static_cast<float>(obst.shape.y * scale / 2),
            .y=obst_center_y,
            .width=obst.shape.y * scale,
            .height=obst.shape.x * scale
        };

        DrawRectanglePro(obst_rec, Vector2{0, 0}, 0.0, BLACK);
    }

    // draw lanelet
    auto draw_lanelet{[&](const std::vector<Vector3>& lanelet) -> void {
        std::vector<Vector2> points(lanelet.size());
        for (size_t i = 0; i < lanelet.size(); ++i) {
            const auto& p{lanelet.at(i)};
            to_mini_map(p.x, p.y, points.at(i).x, points.at(i).y);
        }
        DrawLineStrip(points.data(), points.size(), BLACK);
    }};

    if (!sensor_data.lanelets.empty()) {
        const auto& lanelets{sensor_data.lanelets};
        if (lanelets.find(-1) != lanelets.end()) draw_lanelet(lanelets.find(-1)->second);
        if (lanelets.find(-2) != lanelets.end()) draw_lanelet(lanelets.find(-2)->second);
        if (lanelets.find(1) != lanelets.end()) draw_lanelet(lanelets.find(1)->second);
        if (lanelets.find(2) != lanelets.end()) draw_lanelet(lanelets.find(2)->second);
    }
}

int Simulator::create_ctrl_veh(const VehModel& model, int lane_idx)
{
    float init_x{0.0f};
    float init_y{0.0f};

    if (lane_idx >= 0) {
        assert(!map_.road_net.empty());
        const auto& first_road{map_.road_net.at(0)};
        assert(lane_idx < first_road.lanes.size());
        const auto& lane{first_road.lanes.at(lane_idx)};
        assert(!lane.empty());
        init_x = lane.at(0).c.x;
        init_y = lane.at(0).c.y;
    }

    auto veh{new Veh{init_x, init_y, model.shape.z / 2.0f, model, true}};
    veh->model = LoadModelFromMesh(gen_veh_mesh(veh->shape));
    veh->color = next_color();

    vehs_.push_back(veh);
    first_ctrl_idx_ = vehs_.size() - 1;
    return first_ctrl_idx_;
}

Veh* Simulator::get_ctrl_veh(int idx) const
{
    if (idx < 0 || idx >= vehs_.size()) return nullptr;

    auto veh{vehs_.at(idx)};
    if (!veh->controllable_) return nullptr;
    return veh;
}

bool Simulator::collision_with_any_veh(const Veh* veh) const
{
    for (const auto other_veh: vehs_) {
        if (!other_veh->valid()) continue;
        if (check_veh_collision(veh, other_veh)) {
            return true;
        }
    }
    return false;
}

void Simulator::sensing_obsts(Veh* ego)
{
    // setup sensor obstacles 
    ego->sensor_data_.obsts.clear();
    for (const auto v: vehs_) {
        if (v->id == ego->id) continue;
        if (!v->valid()) continue;

        const auto dist{ego->state().distance(v->state())};
        if (dist > CARLET_MAX_SENSOR_RANGE) continue;
        const auto polar_angle{std::atan2(v->state().y - ego->state().y, v->state().x - ego->state().x)};
        Vector3 obst_center{
            .x=static_cast<float>(dist * std::cos(polar_angle)),
            .y=static_cast<float>(dist * std::sin(polar_angle)),
            .z=0.0f,
        };
        Veh::Obstacle obst{
            .center={obst_center},
            .shape={v->shape},
            .heading=normalize_heading<float>(v->state().yaw + ego->state().yaw),
            .vel=static_cast<float>(v->state().vel),
        };
        ego->sensor_data_.obsts.push_back(obst);
    }
}


void Simulator::sensing_lanelets(Veh* ego)
{
    // setup sensor lanelets
    Vector3 veh_position{
        .x=static_cast<float>(ego->state().x),
        .y=static_cast<float>(ego->state().y),
        .z=static_cast<float>(ego->state().z)
    };

    auto& lanelets{ego->sensor_data_.lanelets};
    auto reset_lanelet{[&lanelets](int key) -> void {
        if (lanelets.find(key) == lanelets.end()) {
            lanelets.insert(std::make_pair(key, std::vector<Vector3>{}));
        } else {
            lanelets.at(key).clear();
        }
    }};

    reset_lanelet(-1);
    reset_lanelet(-2);
    reset_lanelet(1);
    reset_lanelet(2);

    auto to_baselink{[ego](const Vector3& w) -> Vector3 {
        Vector3 baselink;
        const auto alpha{std::atan2(w.y - ego->state().y, w.x - ego->state().x)};
        const auto dist{ego->state().distance(w)};
        baselink.x = dist * std::cos(alpha);
        baselink.y = dist * std::sin(alpha);
        baselink.z = w.z;
        return baselink;
    }};

    constexpr auto max_sample_waypoints{100};
    constexpr auto start_sample_waypoints{10};
    for (const auto& road: map_.road_net) {
        if (road.lanes.empty()) {
            continue;
        }

        const auto num_lanes{road.lanes.size()};
        int lane_idx;
        int waypoint_idx;
        if (find_lane_info(road.lanes, veh_position, ego->shape.y / 2.0f,
            lane_idx, waypoint_idx)) {
            const auto left_lanelet{lane_idx == 0
                ? &road.left_edge
                : &road.lanelets.at(lane_idx - 1)};
            const auto left2_lanelet{lane_idx == 0
                ? nullptr
                : lane_idx == 1
                    ? &road.left_edge
                    : &road.lanelets.at(lane_idx - 2)};
            const auto right_lanelet{lane_idx == num_lanes - 1
                ? &road.right_edge
                : &road.lanelets.at(lane_idx)};
            const auto right2_lanelet{lane_idx == num_lanes - 1
                ? nullptr
                : lane_idx == num_lanes - 2
                    ? &road.right_edge
                    : &road.lanelets.at(lane_idx + 1)};

            const auto min_waypoint_idx{max(0, waypoint_idx - start_sample_waypoints)};
            if (left_lanelet) {
                const auto max_waypoint_idx{min(static_cast<int>(left_lanelet->size()),
                    waypoint_idx + max_sample_waypoints)};
                for (int i = min_waypoint_idx; i < max_waypoint_idx; ++i) {
                    lanelets.at(1).push_back(to_baselink(left_lanelet->at(i).r));
                }
            }
            if (left2_lanelet) {
                const auto max_waypoint_idx{min(static_cast<int>(left2_lanelet->size()),
                    waypoint_idx + max_sample_waypoints)};
                for (int i = min_waypoint_idx; i < max_waypoint_idx; ++i) {
                    lanelets.at(2).push_back(to_baselink(left2_lanelet->at(i).r));
                }
            }

            if (right_lanelet) {
                const auto max_waypoint_idx{min(static_cast<int>(right_lanelet->size()),
                    waypoint_idx + max_sample_waypoints)};
                for (int i = min_waypoint_idx; i < max_waypoint_idx; ++i) {
                    lanelets.at(-1).push_back(to_baselink(right_lanelet->at(i).l));
                }
            }
            if (right2_lanelet) {
                const auto max_waypoint_idx{min(static_cast<int>(right2_lanelet->size()),
                    waypoint_idx + max_sample_waypoints)};
                for (int i = min_waypoint_idx; i < max_waypoint_idx; ++i) {
                    lanelets.at(-2).push_back(to_baselink(right2_lanelet->at(i).l));
                }
            }
            break;
        }
    }
}

void Simulator::sensing(Veh* ego, float dt)
{
    constexpr float sensing_period{0.05f};  // sec.
    static float last_sensing_time{0.0f};
    static float next_sensing_time{0.0f};

    last_sensing_time += dt;
    if (last_sensing_time > next_sensing_time) {
        next_sensing_time = last_sensing_time + sensing_period;
        sensing_obsts(ego);
        sensing_lanelets(ego);
    }
}

bool Simulator::step(float dt)
{
    Veh::AngleControl idm_control;
    const Veh::Obs obs{full_obs()};
    const auto& ctrl_veh{vehs_.at(first_ctrl_idx_)};
    const Vector3 ctrl_veh_position{ctrl_veh->state().position()};

    for (auto& veh: vehs_) {
        if (veh->controllable_) {
            veh->step(dt);
            sensing(veh, dt);
            continue;
        }

        // for idm step
        if (veh->idm_plan(obs, idm_control)) {
            veh->idm_act(idm_control);
            veh->step(dt);
            const auto dist_to_ctrl_veh{distance(*ctrl_veh, *veh)};
            if (dist_to_ctrl_veh > CARLET_IDM_GEN_MAX_DIST) {
                if (!veh->idm_reset_position(ctrl_veh_position, this)) {
                    veh->valid_ = false;
                } else {
                    veh->valid_ = true;
                }
            }
        }
    }

    return true;
}

void Simulator::gen_random_vehs(int n, float min_vel, float max_vel)
{
    if (n <= 0) return;

    Vector3 ref_position{};
    if (first_ctrl_idx_ >= 0) {
        ref_position = vehs_.at(first_ctrl_idx_)->state().position();
    }

    for (int i = 0; i < n; ++i) {
        Veh veh{rand_ab(min_vel, max_vel), veh_model::random()};
        if (veh.idm_reset_position(ref_position, this)) {
            // properties for rendering
            veh.color = next_color();
            veh.model = LoadModelFromMesh(gen_veh_mesh(veh.shape));
            vehs_.push_back(new Veh(veh));
        }
    }

    // std::cout << "vehs_.size: " << vehs_.size() << "\n";
    // std::sort(vehs_.begin() + 1, vehs_.end(), [](const Veh* a, const Veh* b) {
    //     return a->state().x > b->state().x;
    // });
    // for (auto veh: vehs_) {
    //     std::cout << veh->state() << "\n";
    // }
}

bool Veh::idm_reset_position(const Vector3& ref_position, const Simulator* simulator)
{
    int n_tries{0};

    const auto& road{simulator->map().road_net.at(0)};
    const auto num_lane{road.lanes.size()};

    while (n_tries < CARLET_MAX_GEN_VEH_TRIES) {
        const auto lane_idx{rand_ab(0ul, num_lane)};
        const auto& target_lane{road.lanes.at(lane_idx)};
        const auto sample_idx{rand_ab(0ul, target_lane.size())};
        const auto& waypoint{target_lane.at(sample_idx)};
        state_ = State::init_with(waypoint.c.x, waypoint.c.y, shape.z / 2.0f, state_.vel);
        // TODO: optimize me

        const auto dist_to_ref{state_.distance(ref_position)};
        if (dist_to_ref > CARLET_IDM_GEN_MAX_DIST || dist_to_ref < CARLET_IDM_GEN_MIN_DIST) {
            continue;
        }

        ++n_tries;
        // collision check
        if (simulator->collision_with_any_veh(this)) {
            if (n_tries > CARLET_MAX_GEN_VEH_TRIES) {
                TraceLog(LOG_WARNING, "Too many vehicles to generate in such a small map, "
                    "generating random vehicles stopped.");
                break;
            }
            continue;
        }

        return true;
    }

    return false;
}

bool Veh::idm_plan(const Veh::Obs& obs, AngleControl& ctrl)
{
    if (controllable_) return false;

    ctrl.eps_angle = 0.0f;
    ctrl.accel = 0.0f;

    const auto preview_length{30.0f};
    const auto steer_pid_p{0.01f};
    const auto cruise_pid_p{0.1f};
    const auto follow_pid_dist_p{0.2f};
    const auto follow_pid_vel_p{0.6f};
    const auto max_cruise_accel{2.0f};
    const auto target_ht{2.5f};

    {
        // longitude
        carlet::Veh* lead{nullptr};
        for (const auto veh: obs.vehs) {
            if (!veh) continue;
            if (veh->id == id) continue;
            if (veh->state().x < state().x) continue;
            if (carlet::abs(veh->state().y - state().y) > 2.0f) continue;
            if (carlet::abs(veh->state().x - state().x) < 2.0f) continue; // ego
            if (lead && veh->state().x > lead->state().x) continue;
            lead = veh;
        }

        if (!lead || lead->state().vel > idm_cruise_vel_) {
            const auto cruise_error{idm_cruise_vel_ - state().vel};
            ctrl.accel = cruise_error * cruise_pid_p;
        } else {
            const auto x_diff{lead->state().x - state().x};
            const auto ht{x_diff / state().vel};
            const auto desire_x{target_ht * state().vel};
            const auto dist_error{x_diff - desire_x};
            const auto vel_error{lead->state().vel - state().vel};
            ctrl.accel = dist_error * follow_pid_dist_p + vel_error * follow_pid_vel_p;
            if (state().vel > idm_cruise_vel_) {
                ctrl.accel = min(0.0f, ctrl.accel);
            }
        }

        ctrl.accel = carlet::min(ctrl.accel, max_cruise_accel);
    }

    {
        // latitude
        float min_dist{3.0f};
        const carlet::Road::LaneSample* curr_waypoint{nullptr};
        const Vector3 ego_preview{
            .x=static_cast<float>(state().x + preview_length * std::cos(state().yaw)),
            .y=static_cast<float>(state().y + preview_length * std::sin(state().yaw)),
            .z=static_cast<float>(state().z)
        };

        for (const auto& road: obs.map.road_net) {
            if (road.lanes.empty()) continue;
            int lane_idx;
            int waypoint_idx;
            if (find_lane_info(road.lanes, ego_preview, shape.y / 2.0f,
                lane_idx, waypoint_idx)) {
                curr_waypoint = &road.lanes.at(lane_idx).at(waypoint_idx);
                break;
            }
        }

        if (curr_waypoint) {
            const auto error{ego_preview.y - curr_waypoint->c.y};
            ctrl.eps_angle = -error * steer_pid_p;
        }
    }

    return true;
}

void Veh::idm_act(const AngleControl& control)
{
    lat_control(control);
    lon_control(control.accel);
}

void Veh::act(const TorqControl& control)
{
    assert(controllable_ &&
        "Bad operation. Try to take a action on uncontrollable vehicle.");
    lat_control(control);
    lon_control(control.accel);
}

void Veh::act(const AngleControl& control)
{
    assert(controllable_ &&
        "Bad operation. Try to take a action on uncontrollable vehicle.");
    lat_control(control);
    lon_control(control.accel);
}

void Veh::lat_control(const TorqControl& control)
{
    const auto eps_angle{clamp(eps_.angle, vm.min_eps_angle, vm.max_eps_angle)};
    float front_wheel_steer{eps_angle / vm.eps_to_wheel_angle};
    front_wheel_steer_ = front_wheel_steer;
}

void Veh::lat_control(const AngleControl& control)
{
    const auto eps_angle{clamp(control.eps_angle, vm.min_eps_angle, vm.max_eps_angle)};
    float front_wheel_steer{eps_angle / vm.eps_to_wheel_angle};
    eps_.angle = eps_angle;
    front_wheel_steer_ = front_wheel_steer;
}

void Veh::lon_control(float accel)
{
    float accelerator_ratio{0.0f};
    float brake_ratio{0.0f};

    const auto accel_error{accel - state().accel};

    float target_force{vm.mass * accel};
    target_force += vm.gear_friction;
    target_force += air_resistance();
    
    if (target_force > 0.0f) {
        // accelerator control
        const auto wheel_torq{target_force * vm.wheel_radius};
        // P = (2 * pi * n * T) / (1000 * 60)
        const auto expect_power{(CARLET_2_PI * wheel_torq * wheel_rot_spd()) / (1000.0f * 60.0f)};
        const auto expect_power_with_loss{expect_power / (1.0f - vm.hp_loss)};
        const auto expect_accelerator_ratio{expect_power_with_loss / hp_to_kw(vm.max_hp)};
        accelerator_ratio = expect_accelerator_ratio + accel_error * 0.001f;
    } else {
        // brake control
        const auto wheel_fri{wheel_friction()};
        const auto expect_brake_ratio{-target_force / wheel_fri};
        brake_ratio = expect_brake_ratio + accel_error * -0.001f;
    }

    accelerator_ratio_ = clamp(accelerator_ratio, 0.0f, 1.0f);
    brake_ratio_ = clamp(brake_ratio, 0.0f, 1.0f);
}

bool Veh::step(float dt)
{
    if (accelerator_ratio_ > 0.0f && brake_ratio_ > 0.0f) {
        TRACELOG(LOG_WARNING, "Bad driving behavior, both accelerator and brake are actived");
        accelerator_ratio_ = 0.0f;
    }

    float force{};
    const auto air_resist{air_resistance()};
    const auto wheel_fri{wheel_friction()}; // newton

    if (accelerator_ratio_ > 0.0f) {
        const auto wheel_torq{wheel_torque(accelerator_ratio_)};
        const auto wheel_driving_force{wheel_torq / vm.wheel_radius};
        force = wheel_driving_force;
    } else {
        force = -brake_ratio_ * wheel_fri;
    }

    force -= air_resist;
    force -= vm.gear_friction;
    const auto accel{force / vm.mass};

    dynamic_act(front_wheel_steer_, accel, dt);
    // setup render model
    model.transform = MatrixRotateY(state().yaw);
    model.transform.m12 = -state().y;
    model.transform.m13 = state().z;
    model.transform.m14 = -state().x;
    return true;
}

float Veh::air_resistance() const
{
    const auto frontal_area{vm.shape.y * vm.shape.z};
    return 0.5f * CARLET_AIR_DENSITY * frontal_area * vm.Cd * pow2(state().vel);
}

float Veh::wheel_friction() const
{
    return vm.mass * CARLET_G * CARLET_ROAD_FRICTION_FACTOR;
}

float Veh::wheel_rot_spd() const
{
    const auto wheel_perimeter{CARLET_2_PI * vm.wheel_radius};
    return state().vel / wheel_perimeter * 60.0f;   // r/min
}

float Veh::wheel_torque(float accelerator_ratio) const
{
    const auto hp{accelerator_ratio * vm.max_hp};
    const auto hp_with_loss{hp * (1.0f - vm.hp_loss)};
    // T = (1000 * 60 * P) / (2 * pi * n)
    const auto wheel_torq{1000.0f * 60.0f * hp_to_kw(hp_with_loss) /
        (CARLET_2_PI * wheel_rot_spd())};
    return wheel_torq;
}

void Veh::dynamic_act(float steer, float accel, float dt)
{
    constexpr auto steer_error{0.0002};

    if (steer > 0) steer += steer_error; else steer -= steer_error;
    state_.steer_angle = steer;
    state_.vel += accel * dt;
    state_.accel = accel;

    const auto slip_angle{std::atan2(vm.gc_to_back_axle * std::tan(state_.steer_angle), vm.wheel_base)};
    const auto vel_angle{slip_angle + state_.yaw};
    const auto dy{state_.vel * std::sin(vel_angle)};
    const auto dx{state_.vel * std::cos(vel_angle)};
    const auto r{vm.wheel_base / (std::tan(state_.steer_angle) * std::cos(slip_angle) + CARLET_EPS)};
    state_.x += dx * dt;
    state_.y += dy * dt;
    state_.yaw_rate = state_.vel / r;
    state_.yaw += state_.yaw_rate * dt;
}

double Veh::State::distance(const Vector3& other) const
{
    return std::sqrt(pow2(x - other.x) + pow2(y - other.y) + pow2(z - other.z));
}

double Veh::State::distance(const State& other) const
{
    return std::sqrt(pow2(x - other.x) + pow2(y - other.y) + pow2(z - other.z));
}

bool check_veh_collision(const Veh* a, const Veh* b)
{
    constexpr float min_veh_space{0.5f}; // meter

    assert(a != nullptr && b != nullptr);
    if (a->id == b->id) return false;

    const auto a_yaw{a->state().yaw};
    const auto a_hw{a->shape.y / 2.0f}; // half width
    const auto a_hl{a->shape.x / 2.0f}; // half length
    const Vector2 a_center{.x=static_cast<float>(a->state().x), .y=static_cast<float>(a->state().y)};

    const auto b_yaw{b->state().yaw};
    const auto b_hw{b->shape.y / 2.0f}; // half width
    const auto b_hl{b->shape.x / 2.0f}; // half length
    const Vector2 b_center{.x=static_cast<float>(b->state().x), .y=static_cast<float>(b->state().y)};

    // quick collision check (naive solution)
    const auto ab_dist{Vector2Distance(a_center, b_center)};
    if (ab_dist > a_hl + b_hl + min_veh_space) {
        return false;
    }
    if (ab_dist < a_hw + b_hw + min_veh_space) {
        return true;
    }

    // 2d AABB check
    const auto a_sin_yaw{std::sin(a_yaw)};
    const auto a_cos_yaw{std::cos(a_yaw)};
    const auto a_aabb_tl_x{a_hl * a_cos_yaw + a_hw * a_sin_yaw + a_center.x};
    const auto a_aabb_tl_y{a_hw * a_cos_yaw + a_hl * a_sin_yaw + a_center.y};
    const auto a_aabb_tr_x{a_hl * a_cos_yaw - a_hw * a_sin_yaw + a_center.x};
    const auto a_aabb_tr_y{-a_hw * a_cos_yaw + a_hl * a_sin_yaw + a_center.y};
    const auto a_aabb_bl_x{-a_hl * a_cos_yaw + a_hw * a_sin_yaw + a_center.x};
    const auto a_aabb_bl_y{a_hw * a_cos_yaw - a_hl * a_sin_yaw + a_center.y};
    const auto a_aabb_br_x{-a_hl * a_cos_yaw - a_hw * a_sin_yaw + a_center.x};
    const auto a_aabb_br_y{-a_hw * a_cos_yaw - a_hl * a_sin_yaw + a_center.y};

    const auto b_sin_yaw{std::sin(b_yaw)};
    const auto b_cos_yaw{std::cos(b_yaw)};
    const auto b_aabb_tl_x{b_hl * b_cos_yaw + b_hw * b_sin_yaw + b_center.x};
    const auto b_aabb_tl_y{b_hw * b_cos_yaw + b_hl * b_sin_yaw + b_center.y};
    const auto b_aabb_tr_x{b_hl * b_cos_yaw - b_hw * b_sin_yaw + b_center.x};
    const auto b_aabb_tr_y{-b_hw * b_cos_yaw + b_hl * b_sin_yaw + b_center.y};
    const auto b_aabb_bl_x{-b_hl * b_cos_yaw + b_hw * b_sin_yaw + b_center.x};
    const auto b_aabb_bl_y{b_hw * b_cos_yaw - b_hl * b_sin_yaw + b_center.y};
    const auto b_aabb_br_x{-b_hl * b_cos_yaw - b_hw * b_sin_yaw + b_center.x};
    const auto b_aabb_br_y{-b_hw * b_cos_yaw - b_hl * b_sin_yaw + b_center.y};

    const auto a_max_x{max(max(a_aabb_tl_x, a_aabb_tr_x), max(a_aabb_bl_x, a_aabb_br_x))};
    const auto b_max_x{max(max(b_aabb_tl_x, b_aabb_tr_x), max(b_aabb_bl_x, b_aabb_br_x))};
    const auto a_min_x{min(min(a_aabb_tl_x, a_aabb_tr_x), min(a_aabb_bl_x, a_aabb_br_x))};
    const auto b_min_x{min(min(b_aabb_tl_x, b_aabb_tr_x), min(b_aabb_bl_x, b_aabb_br_x))};

    const auto a_max_y{max(max(a_aabb_tl_y, a_aabb_tr_y), max(a_aabb_bl_y, a_aabb_br_y))};
    const auto b_max_y{max(max(b_aabb_tl_y, b_aabb_tr_y), max(b_aabb_bl_y, b_aabb_br_y))};
    const auto a_min_y{min(min(a_aabb_tl_y, a_aabb_tr_y), min(a_aabb_bl_y, a_aabb_br_y))};
    const auto b_min_y{min(min(b_aabb_tl_y, b_aabb_tr_y), min(b_aabb_bl_y, b_aabb_br_y))};

    if (min(a_max_x, a_max_x) > max(a_min_x, a_min_x) &&
        min(a_max_y, a_max_y) > max(a_min_y, a_min_y)) {
        return true;
    }

    return false;
}

const Mesh& gen_veh_mesh(const Vector3& shape)
{
    static bool mesh_uploaded{false};
    static Mesh mesh;

    if (!mesh_uploaded) {
        mesh = GenMeshCube(shape.y, shape.z, shape.x);
        UploadMesh(&mesh, false);
        mesh_uploaded = true;
    }
    return mesh;
}

bool find_lane_info(const std::vector<Road::Lane>& lanes, const Vector3& p, float half_veh_width,
    int& lane_idx, int& waypoint_idx)
{
    lane_idx = -1;
    waypoint_idx = -1;

    float min_dist{3.0f};
    for (size_t i = 0; i < lanes.size(); ++i) {
        const auto& lane{lanes.at(i)};
        if (lane.empty()) continue;

        const auto last_idx{static_cast<int>(lane.size()) - 1};
        int low{0};
        int high{last_idx};
        int mid{};
        while (low < high) {
            mid = (low + high) / 2;
            const auto& waypoint_low{lane.at(low)};
            const auto& waypoint_high{lane.at(high)};
            const auto low_dist{Vector3Distance(p, waypoint_low.c)};
            const auto high_dist{Vector3Distance(p, waypoint_high.c)};
            if (low_dist < high_dist) {
                high = max(mid - 1, 0);
            } else {
                low = min(mid + 1, last_idx);
            }
        }

        if (low >= high) {
            const auto& min_waypoint{lane.at(mid)};
            const auto dist{Vector3Distance(p, min_waypoint.c)};
            if (dist < min_dist) {
                min_dist = dist;
                waypoint_idx = mid;
            }
        }

        if (min_dist < (lane.at(0).width / 2.0f + half_veh_width)) {
            // early stop
            lane_idx = i;
            break;
        }
    }

    return (lane_idx >= 0 && waypoint_idx >= 0) ? true : false;
}

} // namespace carlet

#endif // CARLET_CPP_
#endif // CARLET_IMPLEMENTATION