#include "utils.h"

#include <maps/libs/common/include/exception.h>

#include <cmath>
#include <unordered_map>

namespace maps::mrc::pos_improvment {

struct XYZ {
    size_t x;
    size_t y;
    size_t z;
};

// 1) divides a sphere into small parts
// 2) counts how many vectors are directed to each sphere part
// 3) finds the part with the maximum sum of vectors lengths
// 4) returns the average vector for that part(using vector length as weight)
// @param histogramDimension sets the number of sphere parts:
// (number of sphere parts) ≈ (4PI * histogramDimension * histogramDimension)
MostPopularDirection findMostPopularDirection(const std::vector<geolib3::Vector3>& vectors,
                                              size_t histogramDimension)
{
    REQUIRE(vectors.size(), "No input data provided");
    size_t dim = histogramDimension;
    constexpr double MIN_VECTOR_LENGTH(0.0001);
    constexpr size_t RADIUS = 3; // radius of neighborhood - number of
                                 // neighboring histogram points that
                                 // should be handled with the target
                                 // histogram point
    std::unordered_map<size_t, size_t> hist; // histogram (sphere parts)
    hist.reserve(2 * dim * dim); // approximate number of used sphere parts

    // converts [-1.0, 1.0] to [RADIUS, dim - RADIUS - 1]
    auto toIndex = [&](double t) -> size_t {
        return RADIUS + 1 + (int)((t + 1) / 2.0 * (dim - RADIUS - RADIUS - 2));
    };

    // converts [[0, dim-1], [0, dim-1], [0, dim-1]] to [0, dim * dim  * dim - 1]
    auto xyzToHistIndex = [&](size_t x, size_t y, size_t z) -> size_t {
        return (x) * (dim) * (dim) + (y) * (dim) + z;
    };

    std::vector<XYZ> indexes;
    std::vector<double> lengths;
    indexes.reserve(vectors.size());
    lengths.reserve(vectors.size());
    for (auto v : vectors) {
        REQUIRE(geolib3::length(v) > MIN_VECTOR_LENGTH,
                "Zero length vector are not allowed");
        lengths.push_back(std::sqrt(geolib3::length(v)));
        v.normalize();
        indexes.push_back({toIndex(v.x()),
                           toIndex(v.y()),
                           toIndex(v.z())});
    }

    // find most popular sphere part
    size_t bestPartIndex = 0;
    size_t bestPartValue = 0;

    for (size_t i = 0; i < indexes.size(); i++) {
        size_t xInt = indexes[i].x;
        size_t yInt = indexes[i].y;
        size_t zInt = indexes[i].z;

        // Also add vectors to all the neighboring sphere parts
        // It fixes the problem when the most polular vector direction
        // is located between two sphere parts
        for (size_t x1 = xInt - RADIUS; x1 <= xInt + RADIUS; x1++) {
            for (size_t y1 = yInt - RADIUS; y1 <= yInt + RADIUS; y1++) {
                for (size_t z1 = zInt - RADIUS; z1 <= zInt + RADIUS; z1++) {
                    size_t index = xyzToHistIndex(x1, y1, z1);
                    hist[index] += lengths[i];
                    if (hist[index] > bestPartValue) {
                        bestPartValue = hist[index];
                        bestPartIndex = index;
                    }
                }
            }
        }
    }

    size_t confidence = 0;
    // the result is the average vector of all the vectors which are
    // directed to the most popular sphere part
    geolib3::Vector3 result(0,0,0);
    for (size_t i = 0; i < indexes.size(); i++) {
        size_t xInt = indexes[i].x;
        size_t yInt = indexes[i].y;
        size_t zInt = indexes[i].z;
        for (size_t x1 = xInt - RADIUS; x1 <= xInt + RADIUS; x1++) {
            for (size_t y1 = yInt - RADIUS; y1 <= yInt + RADIUS; y1++) {
                for (size_t z1 = zInt - RADIUS; z1 <= zInt + RADIUS; z1++) {
                    if (xyzToHistIndex(x1, y1, z1) == bestPartIndex) {
                        result += vectors[i].unit();
                        confidence++;
                    }
                }
            }
        }
    }
    return {UnitVector3(result), confidence / double(vectors.size())};
}

} // namespace maps::mrc::pos_improvment
