#include <boost/geometry.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <iostream>
#include <vector>
#include <cmath>
#include <algorithm>
#include <limits>
#include <unordered_map>
#include <unordered_set>
#include <chrono>
#include <random>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

typedef bg::model::point<float, 2, bg::cs::cartesian> BoostPoint;
typedef bg::model::box<BoostPoint> BoostBox;
typedef std::pair<BoostBox, int> Value;
typedef bg::model::linestring<BoostPoint> BoostLineString;
typedef bg::model::polygon<BoostPoint> BoostPolygon;
typedef bg::model::multi_linestring<BoostLineString> BoostMultiLineString;

// Структура для хранения направляющего вектора линии в 2D-пространстве
struct DirectionVector {
    float x, y; // Компоненты вектора в 2D пространстве
    float azimuth; // Азимут для быстрой фильтрации
    BoostPoint midpoint; // Центральная точка линии
    DirectionVector() : x(0), y(0), azimuth(0), midpoint(BoostPoint(0, 0)) {}
    DirectionVector(const BoostLineString& line) {
        if (line.size() < 2) {
            x = y = azimuth = 0;
            midpoint = BoostPoint(0, 0);
            return;
        }
        
        const BoostPoint& A = line.front();
        const BoostPoint& B = line.back();
        
        // Вычисляем азимут между точками
        float lat1 = bg::get<0>(A) * M_PI / 180.0f;
        float lon1 = bg::get<1>(A) * M_PI / 180.0f;
        float lat2 = bg::get<0>(B) * M_PI / 180.0f;
        float lon2 = bg::get<1>(B) * M_PI / 180.0f;
        
        float dLon = lon2 - lon1;
        float y_comp = std::sin(dLon) * std::cos(lat2);
        float x_comp = std::cos(lat1) * std::sin(lat2) - std::sin(lat1) * std::cos(lat2) * std::cos(dLon);
        azimuth = std::atan2(y_comp, x_comp);
        if (azimuth < 0) azimuth += 2 * M_PI;
        
        // Вычисляем 2D вектор направления (нормализованное направление)
        // Используем проекцию Меркатора для аппроксимации направления в 2D
        x = std::cos(azimuth);
        y = std::sin(azimuth);
        
        // Вычисляем центральную точку линии
        midpoint = BoostPoint(
            (bg::get<0>(A) + bg::get<0>(B)) / 2.0f,
            (bg::get<1>(A) + bg::get<1>(B)) / 2.0f
        );
    }
    
    // Проверка коллинеарности по азимуту
    bool is_roughly_collinear(const DirectionVector& other, float threshold = 0.26f) const {
        float delta_azimuth = std::fabs(azimuth - other.azimuth);
        if (delta_azimuth > M_PI) {
            delta_azimuth = 2 * M_PI - delta_azimuth;
        }
        return delta_azimuth < threshold || delta_azimuth > (M_PI - threshold);
    }
};

// Вычисление точного угла между 2D векторами в градусах
float calculate_exact_angle(const DirectionVector& v1, const DirectionVector& v2) {
    float dot_product = v1.x*v2.x + v1.y*v2.y;
    float v1_length = std::sqrt(v1.x*v1.x + v1.y*v1.y);
    float v2_length = std::sqrt(v2.x*v2.x + v2.y*v2.y);
    
    float cos_angle = dot_product / (v1_length * v2_length);
    cos_angle = std::max(-1.0f, std::min(1.0f, cos_angle));
    
    float angle_rad = std::acos(cos_angle);
    float angle_deg = angle_rad * 180.0f / M_PI;
    
    return angle_deg > 90.0f ? 180.0f - angle_deg : angle_deg;
}

// Вычисление расстояния между двумя точками на сфере (формула гаверсинуса)
float haversine_distance(const BoostPoint& p1, const BoostPoint& p2) {
    const float R = 6371000.0f;
    
    float lat1 = bg::get<0>(p1) * M_PI / 180.0f;
    float lon1 = bg::get<1>(p1) * M_PI / 180.0f;
    float lat2 = bg::get<0>(p2) * M_PI / 180.0f;
    float lon2 = bg::get<1>(p2) * M_PI / 180.0f;
    
    float dLat = lat2 - lat1;
    float dLon = lon2 - lon1;
    
    float a = std::sin(dLat/2) * std::sin(dLat/2) + 
               std::cos(lat1) * std::cos(lat2) * 
               std::sin(dLon/2) * std::sin(dLon/2);
    float c = 2 * std::atan2(std::sqrt(a), std::sqrt(1-a));
    
    return R * c;
}

// Вычисление расстояния между двумя линиями
float calculate_line_distance(const BoostLineString& line1, const BoostLineString& line2) {
    if (line1.size() < 2 || line2.size() < 2) {
        return std::numeric_limits<float>::max();
    }
    
    float d1 = haversine_distance(line1.front(), line2.front());
    float d2 = haversine_distance(line1.back(), line2.back());
    
    float d3 = haversine_distance(line1.front(), line2.back());
    float d4 = haversine_distance(line1.back(), line2.front());
    
    if (d1 + d2 > d3 + d4) {
        return (d3 + d4) / 2.0f;
    } else {
        return (d1 + d2) / 2.0f;
    }
}

// Класс для поиска коллинеарных и близких линий
class CollinearityFinder {
private:
    bgi::rtree<Value, bgi::quadratic<8>> spatial_index;
    std::unordered_map<uint8_t, std::vector<int>> direction_index;
    
    // Храним направляющие векторы, индексируемые по ID линии
    std::unordered_map<int, DirectionVector> directions;
    
    // Храним ссылку на карту линий
    const std::unordered_map<int, BoostLineString>& lines_ref;
    
    // Параметры поиска
    float spatial_filter_expansion = 0.01f;
    float collinearity_threshold = 10.0f;
    float distance_weight = 0.5f;
    
public:
    // Конструктор принимает ссылку на карту линий
    CollinearityFinder(const std::unordered_map<int, BoostLineString>& input_lines)
        : lines_ref(input_lines) {
        preprocess();
    }
    
    void set_distance_weight(float weight) {
        distance_weight = std::max(0.0f, std::min(1.0f, weight));
    }
    
    void set_collinearity_threshold(float threshold) {
        collinearity_threshold = threshold;
    }
    
    void set_spatial_filter_expansion(float expansion) {
        spatial_filter_expansion = expansion;
    }
    
private:
    void preprocess() {
        auto start_time = std::chrono::high_resolution_clock::now();
        
        // Предварительное вычисление направляющих векторов
        for (const auto& pair : lines_ref) {
            int id = pair.first;
            const BoostLineString& line = pair.second;
            
            directions.emplace(id, DirectionVector(line));
            
            // Строим пространственный индекс
            BoostBox box;
            bg::envelope(line, box);
            spatial_index.insert(std::make_pair(box, id));
            
            // Индексируем по азимуту
            uint8_t azimuth_bin = static_cast<uint8_t>(directions[id].azimuth * 9.0f / M_PI) % 18;
            
            direction_index[azimuth_bin].push_back(id);
            
            // Добавляем в противоположный сектор
            uint8_t opposite_bin = (azimuth_bin + 9) % 18;
            direction_index[opposite_bin].push_back(id);
        }
        
        auto end_time = std::chrono::high_resolution_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count();
        std::cout << "Preprocessing completed in " << duration << " ms" << std::endl;
    }
    
public:
    // Поиск наиболее коллинеарной и близкой линии
    std::pair<int, std::pair<float, float>> find_most_collinear_and_close(const BoostLineString& reference_line) {
        auto start_time = std::chrono::high_resolution_clock::now();
        
        DirectionVector reference_vector(reference_line);
        
        // Пространственная фильтрация
        BoostBox query_box;
        bg::envelope(reference_line, query_box);
        
        // Расширяем область поиска
        BoostPoint min_corner = query_box.min_corner();
        BoostPoint max_corner = query_box.max_corner();
        bg::set<0>(min_corner, bg::get<0>(min_corner) - spatial_filter_expansion);
        bg::set<1>(min_corner, bg::get<1>(min_corner) - spatial_filter_expansion);
        bg::set<0>(max_corner, bg::get<0>(max_corner) + spatial_filter_expansion);
        bg::set<1>(max_corner, bg::get<1>(max_corner) + spatial_filter_expansion);
        query_box = BoostBox(min_corner, max_corner);
        
        std::vector<Value> spatial_candidates;
        spatial_index.query(bgi::intersects(query_box), std::back_inserter(spatial_candidates));
        
        // Фильтрация по направлению
        uint8_t azimuth_bin = static_cast<uint8_t>(reference_vector.azimuth * 9.0f / M_PI) % 18;
        
        // Собираем кандидатов по азимуту
        std::unordered_set<int> azimuth_set;
        for (int offset = -1; offset <= 1; ++offset) {
            uint8_t current_bin = (azimuth_bin + offset + 18) % 18; // Защита от отрицательных значений
            
            auto it = direction_index.find(current_bin);
            if (it != direction_index.end()) {
                azimuth_set.insert(it->second.begin(), it->second.end());
            }
        }
        
        // Комбинированная фильтрация
        std::vector<int> candidates;
        candidates.reserve(std::min(spatial_candidates.size(), size_t(100)));
        
        for (const auto& spatial_candidate : spatial_candidates) {
            int id = spatial_candidate.second;
            if (azimuth_set.find(id) != azimuth_set.end() && 
                reference_vector.is_roughly_collinear(directions[id])) {
                candidates.push_back(id);
            }
        }
        
        auto filter_time = std::chrono::high_resolution_clock::now();
        auto filter_duration = std::chrono::duration_cast<std::chrono::milliseconds>(filter_time - start_time).count();
        std::cout << "Filtering reduced candidates from " << lines_ref.size() << " to " << candidates.size() 
                  << " in " << filter_duration << " ms" << std::endl;
        
        // Вычисление комбинированной оценки
        float best_score = std::numeric_limits<float>::max();
        int best_id = -1;
        float best_angle = 0.0f;
        float best_distance = 0.0f;
        
        if (candidates.empty()) {
            std::cout << "No candidates found after filtering" << std::endl;
            return {-1, {std::numeric_limits<float>::max(), std::numeric_limits<float>::max()}};
        }
        
        // Вычисляем длину эталонной линии
        float ref_line_length = 0.0f;
        if (reference_line.size() >= 2) {
            ref_line_length = haversine_distance(reference_line.front(), reference_line.back());
        }
        
        float distance_normalize_factor = (ref_line_length > 0) ? (1.0f / ref_line_length) : 1.0f;
        
        // Обработка кандидатов
        for (int id : candidates) {
            float angle = calculate_exact_angle(reference_vector, directions[id]);
            
            if (angle > collinearity_threshold) {
                continue;
            }
            
            const auto& candidate_line = lines_ref.at(id);
            float distance = calculate_line_distance(reference_line, candidate_line);
            float normalized_distance = distance * distance_normalize_factor;
            float normalized_angle = angle / 90.0f;
            
            float score = (1.0f - distance_weight) * normalized_angle + distance_weight * normalized_distance;
            
            if (score < best_score) {
                best_score = score;
                best_id = id;
                best_angle = angle;
                best_distance = distance;
            }
        }
        
        auto end_time = std::chrono::high_resolution_clock::now();
        auto total_duration = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count();
        std::cout << "Total search completed in " << total_duration << " ms" << std::endl;
        
        return {best_id, {best_angle, best_distance}};
    }
};

int main() {
    // Эталонная линия
    BoostLineString reference_line;
    bg::append(reference_line, BoostPoint(45.094734f, 37.565922f));
    bg::append(reference_line, BoostPoint(45.094696f, 37.566153f));
    
    // Создаем тестовый набор линий
    std::unordered_map<int, BoostLineString> lines;
    
    std::cout << "Generating test data..." << std::endl;
    const int NUM_LINES = 10000;
    
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<float> lat_dist(45.0f - 0.1f, 45.0f + 0.1f);
    std::uniform_real_distribution<float> lon_dist(37.5f - 0.1f, 37.5f + 0.1f);
    std::uniform_real_distribution<float> angle_dist(0, 2 * M_PI);
    std::uniform_real_distribution<float> length_dist(0.0002f, 0.0012f);
    
    // Генерируем случайные линии
    for (int i = 0; i < NUM_LINES; ++i) {
        float base_lat = lat_dist(gen);
        float base_lon = lon_dist(gen);
        
        BoostLineString line;
        line.reserve(2);
        bg::append(line, BoostPoint(base_lat, base_lon));
        
        float angle = angle_dist(gen);
        float length = length_dist(gen);
        
        bg::append(line, BoostPoint(
            base_lat + length * std::cos(angle),
            base_lon + length * std::sin(angle) / std::cos(base_lat * M_PI / 180.0f)
        ));
        
        lines.emplace(i, std::move(line));
    }
    
    // Добавляем тестовые линии
    BoostLineString collinear_close_line;
    collinear_close_line.reserve(2);
    bg::append(collinear_close_line, BoostPoint(45.094730f, 37.565920f));
    bg::append(collinear_close_line, BoostPoint(45.094692f, 37.566151f));
    lines.emplace(NUM_LINES, std::move(collinear_close_line));
    
    BoostLineString collinear_far_line;
    collinear_far_line.reserve(2);
    bg::append(collinear_far_line, BoostPoint(45.084730f, 37.555920f));
    bg::append(collinear_far_line, BoostPoint(45.084692f, 37.556151f));
    lines.emplace(NUM_LINES + 1, std::move(collinear_far_line));
    
    BoostLineString close_noncollinear_line;
    close_noncollinear_line.reserve(2);
    bg::append(close_noncollinear_line, BoostPoint(45.094734f, 37.565922f));
    bg::append(close_noncollinear_line, BoostPoint(45.094834f, 37.566022f));
    lines.emplace(NUM_LINES + 2, std::move(close_noncollinear_line));
    
    std::cout << "Created " << lines.size() << " test lines" << std::endl;
    
    // Инициализируем поисковик
    CollinearityFinder finder(lines);
    finder.set_distance_weight(0.5f);
    
    // Ищем наиболее коллинеарную и близкую линию
    auto result = finder.find_most_collinear_and_close(reference_line);
    
    std::cout << "Best match: ID=" << result.first 
              << ", angle=" << result.second.first << " degrees"
              << ", distance=" << result.second.second << " meters" << std::endl;
              
    return 0;
}