#include <limits>

#include "quadtree.h"

static const float BRUSHING_MAX_DIST = 20.0f;

QuadTree::QuadTree(const RectF &bounds)
    : m_bounds(bounds)
    , m_value(-1)
{
}

bool QuadTree::subdivide()
{
    float halfWidth = m_bounds.width() / 2;
    float halfHeight = m_bounds.height() / 2;

    m_nw.reset(new QuadTree(RectF(m_bounds.x(),
                                  m_bounds.y(),
                                  halfWidth,
                                  halfHeight)));
    m_ne.reset(new QuadTree(RectF(m_bounds.x() + halfWidth,
                                  m_bounds.y(),
                                  halfWidth,
                                  halfHeight)));
    m_sw.reset(new QuadTree(RectF(m_bounds.x(),
                                  m_bounds.y() + halfHeight,
                                  halfWidth,
                                  halfHeight)));
    m_se.reset(new QuadTree(RectF(m_bounds.x() + halfWidth,
                                  m_bounds.y() + halfHeight,
                                  halfWidth,
                                  halfHeight)));

    int value = m_value;
    m_value = -1;
    return m_nw->insert(m_x, m_y, value)
        || m_ne->insert(m_x, m_y, value)
        || m_sw->insert(m_x, m_y, value)
        || m_se->insert(m_x, m_y, value);
}

bool QuadTree::insert(float x, float y, int value)
{
    if (!m_bounds.contains(x, y)) {
        return false;
    }

    if (m_nw) {
        return m_nw->insert(x, y, value)
            || m_ne->insert(x, y, value)
            || m_sw->insert(x, y, value)
            || m_se->insert(x, y, value);
    }

    if (m_value >= 0) {
        subdivide();
        return insert(x, y, value);
    }

    m_x = x;
    m_y = y;
    m_value = value;
    return true;
}

int QuadTree::nearestTo(float x, float y) const
{
    if (!m_bounds.contains(x, y)) {
        return -1;
    }

    int q;
    if (m_nw) {
        q = m_nw->nearestTo(x, y);
        if (q >= 0) return q;
        q = m_ne->nearestTo(x, y);
        if (q >= 0) return q;
        q = m_sw->nearestTo(x, y);
        if (q >= 0) return q;
        q = m_se->nearestTo(x, y);
        if (q >= 0) return q;
    }

    float dist = std::numeric_limits<float>::infinity();
    nearestTo(x, y, q, dist);
    if (dist < BRUSHING_MAX_DIST * BRUSHING_MAX_DIST)
        return q;
    return -1;
}

void QuadTree::nearestTo(float x, float y, int &nearest, float &dist) const
{
    if (m_nw) {
        m_nw->nearestTo(x, y, nearest, dist);
        m_ne->nearestTo(x, y, nearest, dist);
        m_sw->nearestTo(x, y, nearest, dist);
        m_se->nearestTo(x, y, nearest, dist);
    } else if (m_value >= 0) {
        float d = (m_x - x)*(m_x - x) + (m_y - y)*(m_y - y);
        if (d < dist) {
            nearest = m_value;
            dist = d;
        }
    }
}

int QuadTree::query(float x, float y) const
{
    if (!m_bounds.contains(x, y)) {
        // There is no way we could find the point
        return -1;
    }

    if (m_nw) {
        int q = -1;
        q = m_nw->query(x, y);
        if (q >= 0) return q;
        q = m_ne->query(x, y);
        if (q >= 0) return q;
        q = m_sw->query(x, y);
        if (q >= 0) return q;
        q = m_se->query(x, y);
        return q;
    }

    return m_value;
}

void QuadTree::query(const RectF &rect, std::vector<int> &result) const
{
    if (!m_bounds.intersects(rect)) {
        return;
    }

    if (m_nw) {
        m_nw->query(rect, result);
        m_ne->query(rect, result);
        m_sw->query(rect, result);
        m_se->query(rect, result);
    } else if (rect.contains(m_x, m_y) && m_value != -1) {
        result.push_back(m_value);
    }
}