#include <cmath>
#include <cstdint>
#include <iostream>

#include "AdConstants.h"
#include "AdFunctions.h"

namespace Ad
{
namespace Utils
{

constexpr float kph_to_mps(const float kph)
{
    return 0.2778 * kph;
}

} // namespace Ad::Utils

namespace Data
{

Types::Vehicle init_ego_vehicle()
{
    return Types::Vehicle{
        .id = Constants::EGO_VEHICLE_ID,
        .lane = Types::Lane::Center,
        .speed_mps = Utils::kph_to_mps(135),
        .rel_distance_m = 0,
        };
}

} // namespace Ad::Data

namespace Visualize
{

void print_vehicle(const Types::Vehicle vehicle)
{
    std::cout << "Vehicle " << vehicle.id << '\n';
    std::cout << "speed : " << vehicle.speed_mps << " m/s\n";
    std::cout << "distance : " << vehicle.rel_distance_m << " m\n";
}

} // namespace Ad::Visualize

} // namespace Ad