#include <cmath>
#include <cstdint>
#include <iostream>
#include <random>
#include <algorithm>

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

namespace Ad
{
namespace Utils
{

/* kph_to_mps: converts kmph to meters/second */
constexpr float kph_to_mps(const float kph)
{
    return 0.2778 * kph;
}

} // namespace Ad::Utils

namespace Data
{

/* init_ego_vehicle: initializes ego vehicle */
Types::Vehicle init_ego_vehicle()
{
    return Types::Vehicle{
        .id = Consts::EGO_VEHICLE_ID,
        .lane = Types::Lane::Center,
        .speed_mps = Utils::kph_to_mps(135),
        .rel_dist_m = 0,
        };
}

/* init_vehicle_array: initialize an array of vehicles */
template <std::size_t N>
static void init_vehicle_array(std::array<Types::Vehicle, N> &arr, int32_t start_id, Types::Lane lane)
{
    /* init random generators */
    std::mt19937 rng(std::random_device{}());
    std::uniform_real_distribution<double> speed(-150, 150);
    std::uniform_real_distribution<double> dist_gen(0, Consts::VISUALIZE_STEP);

    double dist = static_cast<double> (-Consts::VISUALIZE_STEP) * (arr.size() / 2 + 1);

    for (int32_t i = 0; i < arr.size(); i++)
        arr[i] = Types::Vehicle{
            .id = start_id++,
            .lane = lane, 
            .speed_mps = Utils::kph_to_mps(speed(rng)),
            .rel_dist_m = (dist += Consts::VISUALIZE_STEP + dist_gen(rng))
            };
}              

/* init_vehicles: initializes vehicles */
Types::NeighborVehicles init_vehicles()
{
    auto nv = Types::NeighborVehicles{};

    int32_t id = 0;
    init_vehicle_array(nv.left_lane, id, Types::Lane::Left);
    init_vehicle_array(nv.center_lane, (id += nv.left_lane.size()), Types::Lane::Center);
    init_vehicle_array(nv.right_lane, (id += nv.right_lane.size()), Types::Lane::Right);

    return nv;
}



} // namespace Ad::Data

namespace Visualize
{

/* print_lane: prints lane, helper function for print vehicle */
void print_lane(const Types::Lane lane)
{
    #define if_lane(LANE) if (lane == Types::Lane::LANE) std::cout << #LANE; else
    if_lane(Left)
    if_lane(Center)
    if_lane(Right)
    /* else */
    std::cout << "Unknown";
}

/* print_vehicle: prints vehicle */
void print_vehicle(const Types::Vehicle &vehicle)
{
    std::cout << "Vehicle " << vehicle.id << '\n';
    std::cout << "Lane : ";
    print_lane(vehicle.lane);
    std::cout << '\n';
    std::cout << "speed : " << vehicle.speed_mps << " m/s\n";
    std::cout << "distance : " << vehicle.rel_dist_m << " m\n";
}

/* print_vehicle_array: helper function for print_neghbor_vehicles */
template <size_t N>
static void print_vehicle_array(const std::array<Types::Vehicle, N> &arr)
{
    for (int32_t i = 0; i < arr.size(); i++) {
        print_vehicle(arr[i]);
        std::cout << '\n';
    }
}

/* print_neghbor_vehicles: print neghbor vehicles information */
void print_neighbor_vehicles(const Types::NeighborVehicles &vehicles)
{
    print_vehicle_array(vehicles.left_lane);
    print_vehicle_array(vehicles.center_lane);
    print_vehicle_array(vehicles.right_lane);
}

/* print_scene: prints scene
 * very rough implementation */
void print_scene(const Types::Vehicle &ego_vehicle, 
                 const Types::NeighborVehicles &vehicles)
{
    /* TODO : sort NeighborVehicles arrays */
    double min_dist_d = std::min({vehicles.left_lane[0].rel_dist_m, 
                                 vehicles.center_lane[0].rel_dist_m,
                                 vehicles.right_lane[0].rel_dist_m});

    double max_dist_d = std::max({vehicles.left_lane.back().rel_dist_m, 
                                 vehicles.center_lane.back().rel_dist_m,
                                 vehicles.right_lane.back().rel_dist_m});

    int32_t min_dist = std::floor(min_dist_d / Consts::VISUALIZE_STEP) * Consts::VISUALIZE_STEP; 
    int32_t max_dist = std::ceil(max_dist_d / Consts::VISUALIZE_STEP) * Consts::VISUALIZE_STEP; 

    int32_t li, ci, ri, count;
    li = ci = ri = Consts::VEHICLES_PER_LANE - 1;
    std::cout << "          L   C   R\n";
    for (int32_t dist = max_dist; dist >= min_dist; dist -= Consts::VISUALIZE_STEP) {
        printf("%8d| ", dist);
        char c;
#define HANDLE_LANE(lane, li) \
        for (count = 0; li >= 0 && vehicles.lane[li].rel_dist_m >= dist-Consts::VISUALIZE_STEP; count++, li--) \
            ; \
        c = !count ? ' ' : (count > 1) ? '0' + count : (vehicles.lane[li+1].speed_mps >= 0) ? 'v' : '^'; \
        std::cout << c << " | "
        HANDLE_LANE(left_lane, li);
        HANDLE_LANE(center_lane, ci);
        HANDLE_LANE(right_lane, ri);
        std::cout << '\n';
    }

}

} // namespace Ad::Visualize

} // namespace Ad